From 780eff2852ff83a8db6e2179b8344278fad3f5fa Mon Sep 17 00:00:00 2001 From: superkor Date: Wed, 28 Jan 2026 00:47:22 -0500 Subject: [PATCH] movement set read write to nvs working --- .gitignore | 1 + components/flatbuffers/CMakeLists.txt | 1 + components/flatbuffers/MovementSetBuilder.cpp | 56 +++---- .../flatbuffers/include/MovementSetBuilder.h | 20 ++- components/movements/CMakeLists.txt | 2 +- components/movements/MovementSetManager.cpp | 151 +++++++++++++++--- .../movements/include/MovementManager.h | 2 + .../movements/include/MovementSetManager.h | 34 +++- main/README.md | 2 + main/main_preprogrammed_test.cpp | 77 +++++++++ 10 files changed, 288 insertions(+), 58 deletions(-) diff --git a/.gitignore b/.gitignore index f9f513f..0bba8b7 100644 --- a/.gitignore +++ b/.gitignore @@ -5,5 +5,6 @@ sdkconfig.old .vscode/* /.cache test/build/* +build_movement build_A build_B \ No newline at end of file diff --git a/components/flatbuffers/CMakeLists.txt b/components/flatbuffers/CMakeLists.txt index cee0c94..b951fa9 100644 --- a/components/flatbuffers/CMakeLists.txt +++ b/components/flatbuffers/CMakeLists.txt @@ -3,6 +3,7 @@ idf_component_register(SRCS "MPIMessageBuilder.cpp" "TopologyMessageBuilder.cpp" "SensorMessageBuilder.cpp" "TopologyBuilder.cpp" + "MovementSetBuilder.cpp" REQUIRES "constants" INCLUDE_DIRS "include" ) diff --git a/components/flatbuffers/MovementSetBuilder.cpp b/components/flatbuffers/MovementSetBuilder.cpp index 8b9c571..7751637 100644 --- a/components/flatbuffers/MovementSetBuilder.cpp +++ b/components/flatbuffers/MovementSetBuilder.cpp @@ -6,7 +6,7 @@ #ifdef MOVEMENTSETBUILDER namespace Flatbuffers { - SerializedMessage MovementSetBuilder::build_movement_set(const std::unordered_map>& input){ + SerializedMessage MovementSetBuilder::build_movement_set(const std::unordered_map>& input){ builder_.Clear(); std::vector> map_entries; @@ -17,50 +17,50 @@ namespace Flatbuffers { keys.reserve(input.size()); for (const auto& [key, _] : input) { - keys.push_back(key); + keys.push_back(key); } std::sort(keys.begin(), keys.end()); // 2. Build MovementVector entries in key order for (uint8_t key : keys) { - const auto& entries = input.at(key); + const auto& entries = input.at(key); - std::vector> fb_entries; - fb_entries.reserve(entries.size()); + std::vector> fb_entries; + fb_entries.reserve(entries.size()); - for (const auto& e : entries) { - fb_entries.push_back( - Movement::CreateMovementEntry( + for (const auto& e : entries) { + fb_entries.push_back( + Movement::CreateMovementEntry( + builder_, + e.board_id, + e.module_type, + e.value_action, + &e.condition, + e.ack, + e.ack_ttl_ms, + e.post_delay_ms + ) + ); + } + + auto movements_vector = builder_.CreateVector(fb_entries); + + map_entries.push_back( + Movement::CreateMovementVector( builder_, - e.board_id, - e.module_type, - e.value_action, - &e.condition, - e.ack, - e.ack_ttl_ms, - e.post_delay_ms + key, + movements_vector ) ); } - auto movements_vector = builder_.CreateVector(fb_entries); - - map_entries.push_back( - Movement::CreateMovementVector( - builder_, - key, - movements_vector - ) - ); - } - // 3. Build the map vector auto map_vector = builder_.CreateVector(map_entries); auto message = Movement::CreateMovementSet( - builder_, - map_vector + builder_, + map_vector ); builder_.Finish(message); diff --git a/components/flatbuffers/include/MovementSetBuilder.h b/components/flatbuffers/include/MovementSetBuilder.h index eb63a2c..338cb66 100644 --- a/components/flatbuffers/include/MovementSetBuilder.h +++ b/components/flatbuffers/include/MovementSetBuilder.h @@ -11,22 +11,36 @@ #include "flatbuffers_generated/Movement_generated.h" #include "flatbuffers/flatbuffers.h" -struct MovementEntryInput { +struct MovementEntryData { uint16_t board_id; uint8_t module_type; uint16_t value_action; - Movement::ConditionBlob condition; + Movement::ConditionBlob condition; // struct is fine to reuse uint8_t ack; uint16_t ack_ttl_ms; uint16_t post_delay_ms; + + bool operator==(const MovementEntryData& other) const { + return board_id == other.board_id && + module_type == other.module_type && + value_action == other.value_action && + condition.value() == other.condition.value() && + condition.cond() == other.condition.cond() && + condition.module_type() == other.condition.module_type() && + condition.in_use() == other.condition.in_use() && + ack == other.ack && + ack_ttl_ms == other.ack_ttl_ms && + post_delay_ms == other.post_delay_ms; + } }; + namespace Flatbuffers { class MovementSetBuilder { public: MovementSetBuilder() : builder_(MAX_MOVEMENTS_IN_SET*sizeof(Movement::MovementEntry)) {} - SerializedMessage build_movement_set(const std::unordered_map>& input); + SerializedMessage build_movement_set(const std::unordered_map>& input); Movement::ConditionBlob build_condition_blob(uint16_t value, uint8_t cond, uint8_t module_type, uint8_t in_use); private: diff --git a/components/movements/CMakeLists.txt b/components/movements/CMakeLists.txt index 75904a6..8a2b731 100644 --- a/components/movements/CMakeLists.txt +++ b/components/movements/CMakeLists.txt @@ -1,4 +1,4 @@ -idf_component_register(SRCS "MovementManager.cpp" "MovementManager.cpp" "TopologyManager.cpp" +idf_component_register(SRCS "MovementManager.cpp" "MovementSetManager.cpp" "TopologyManager.cpp" PRIV_REQUIRES esp_event nvs_flash rmt dataLink flatbuffers # REQUIRES esp_timer INCLUDE_DIRS "include") diff --git a/components/movements/MovementSetManager.cpp b/components/movements/MovementSetManager.cpp index 6ab6fa0..77ac9a1 100644 --- a/components/movements/MovementSetManager.cpp +++ b/components/movements/MovementSetManager.cpp @@ -10,11 +10,6 @@ MovementSetManager::MovementSetManager(){ return; } - if (!topology_manager->is_ready()){ - ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Topology Manager is not ready!"); - return; - } - ready = true; }; @@ -22,28 +17,51 @@ MovementSetManager::~MovementSetManager(){ nvs_close(handle); } -esp_err_t MovementSetManager::add_movement(Movement::MovementEntry& entry, uint8_t& index){ +/** + * @brief Adds a movement entry into the movements map + * + * @param entry Movement entry information + * @param index Sequence number of the movements + * @return esp_err_t + */ +esp_err_t MovementSetManager::add_movement(MovementEntryData& entry, uint8_t index){ if (!ready){ return ESP_ERR_INVALID_STATE; } - if (entry.board_id() == PC_ADDR || entry.board_id() == BROADCAST_ADDR){ + if (entry.board_id == PC_ADDR || entry.board_id == BROADCAST_ADDR){ return ESP_ERR_INVALID_ARG; } if (movements.find(index) != movements.end()){ - for (Movement::MovementEntry& e : movements[index]){ - if (e.board_id() == entry.board_id()){ + for (MovementEntryData& e : movements[index]){ + if (e.board_id == entry.board_id){ return ESP_ERR_INVALID_ARG; //only unique board ids per index } } } + + if (entry.post_delay_ms == 0){ + //we don't want 0ms delay in between the movements, so we will add the default delay + entry.post_delay_ms = DEFAULT_DELAY_MS; + } + + if (entry.ack != static_cast(ACK_VALUES::NO_ACK) && entry.ack_ttl_ms == 0){ + entry.ack_ttl_ms = DEFAULT_ACK_TTL_MS; + } movements[index].push_back(entry); return ESP_OK; } +/** + * @brief Removes a movement entry from the sequence, given the board id + * + * @param index Sequence number + * @param board_id + * @return esp_err_t + */ esp_err_t MovementSetManager::remove_movement(uint8_t index, uint8_t board_id){ if (board_id == PC_ADDR || board_id == BROADCAST_ADDR){ return ESP_ERR_INVALID_ARG; @@ -53,10 +71,14 @@ esp_err_t MovementSetManager::remove_movement(uint8_t index, uint8_t board_id){ return ESP_ERR_INVALID_ARG; } + if (!ready){ + return ESP_ERR_INVALID_STATE; + } + bool found = false; for (auto it = movements[index].begin(); it != movements[index].end(); ++it) { - if (it->board_id() == board_id){ + if (it->board_id == board_id){ movements[index].erase(it); found = true; break; @@ -70,23 +92,116 @@ esp_err_t MovementSetManager::remove_movement(uint8_t index, uint8_t board_id){ return ESP_OK; } -esp_err_t MovementSetManager::update_movement(Movement::MovementEntry& entry, uint8_t index){ +esp_err_t MovementSetManager::get_curr_movement_set(std::unordered_map>& set){ + if (!ready){ + return ESP_ERR_INVALID_STATE; + } + + set = movements; return ESP_OK; } -esp_err_t MovementSetManager::get_curr_movement_set(Movement::MovementSet& set){ - return ESP_OK; -} +esp_err_t MovementSetManager::get_nvs_movement_set(std::unordered_map>& set){ + if (!ready){ + return ESP_ERR_INVALID_STATE; + } -esp_err_t MovementSetManager::verify_movement_set(){ - return ESP_OK; -} + size_t size = 0; + esp_err_t res = nvs_get_u32(handle, MOVEMENTS_NVS_SET_SIZE_KEY, reinterpret_cast(&size)); -esp_err_t MovementSetManager::get_nvs_movement_set(Movement::MovementSet& set){ + if (res != ESP_OK) { + ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Failed to read data size"); + return res; + } + + std::vector buffer(size); + + res = nvs_get_blob(handle, MOVEMENTS_NVS_SET_KEY, buffer.data(), &size); + + if (res != ESP_OK) { + ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Failed to read blob"); + return res; + } + + // Verify the FlatBuffer + flatbuffers::Verifier verifier(buffer.data(), buffer.size()); + if (!Movement::VerifyMovementSetBuffer(verifier)) { + ESP_LOGE(MOVEMENTS_DEBUG_TAG, "FlatBuffer verification failed"); + return ESP_ERR_INVALID_ARG; + } + + // Parse root + const Movement::MovementSet* movement_set = Movement::GetMovementSet(buffer.data()); + + movements.clear(); + + const auto* map = movement_set->movement_map(); + if (!map) { + return ESP_OK; // empty set + } + + // Iterate map entries + for (const Movement::MovementVector* mv : *map) { + uint8_t key = mv->key(); + + auto& vec = movements[key]; + vec.clear(); + + const auto* entries = mv->movements(); + if (!entries) { + continue; + } + + vec.reserve(entries->size()); + + // Must iterate over Movement::MovementEntry* (FlatBuffers table) + for (const Movement::MovementEntry* e : *entries) { + vec.push_back({ + e->board_id(), + e->module_type(), + e->value_action(), + *e->condition(), // copy struct + e->ack(), + e->ack_ttl_ms(), + e->post_delay_ms() + }); + } + } + + set = movements; + return ESP_OK; } esp_err_t MovementSetManager::write_nvs_movement_set(){ + if (!ready){ + return ESP_ERR_INVALID_STATE; + } + + Flatbuffers::SerializedMessage m = builder.build_movement_set(movements); + + esp_err_t res = nvs_set_u32(handle, MOVEMENTS_NVS_SET_SIZE_KEY, static_cast(m.size)); + + res = nvs_commit(handle); + + if (res != ESP_OK) { + ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Failed to commit data size"); + return res; + } + + res = nvs_set_blob(handle, MOVEMENTS_NVS_SET_KEY, m.data, m.size); + + if (res != ESP_OK){ + ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Failed to write to nvs"); + return res; + } + + res = nvs_commit(handle); + if (res != ESP_OK) { + ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Failed to commit topology"); + return res; + } + return ESP_OK; } diff --git a/components/movements/include/MovementManager.h b/components/movements/include/MovementManager.h index dbc071c..583e57d 100644 --- a/components/movements/include/MovementManager.h +++ b/components/movements/include/MovementManager.h @@ -7,6 +7,8 @@ #define MOVEMENTS_NVS_KEY "key" #define MOVEMENTS_NVS_TOPOLOGY_KEY "topology" #define MOVEMENTS_NVS_TOPOLOGY_DATA_SIZE_KEY "data_size" +#define MOVEMENTS_NVS_SET_KEY "mvt_set" +#define MOVEMENTS_NVS_SET_SIZE_KEY "mvt_data_size" #define MOVEMENTS_DEBUG_TAG "movements" class MovementManager { diff --git a/components/movements/include/MovementSetManager.h b/components/movements/include/MovementSetManager.h index c286d1a..024e465 100644 --- a/components/movements/include/MovementSetManager.h +++ b/components/movements/include/MovementSetManager.h @@ -1,29 +1,47 @@ #include "MovementManager.h" #include "nvs_flash.h" -#include "TopologyManager.h" #include #include #include "MovementSetBuilder.h" #include "unordered_map" +#include "constants/rmt.h" +#include "constants/datalink.h" #ifdef MOVEMENTS #pragma once +#define DEFAULT_DELAY_MS 50 //some random value that i am using to default to for the delay after a movement has been completed +#define DEFAULT_ACK_TTL_MS 100 +#define CONDITION_BLOB_NOT_IN_USE 0 +#define CONDITION_BLOB_IN_USE 1 + +enum class ACK_VALUES : uint8_t { + NO_ACK, //no acks required + ACK, //acks required to be sent and received properly within ttl + ACK_NO_FAIL, //acks required to be send but not necessarily required to be received properly within ttl +}; + +enum class CONDITION_VALUES : uint8_t { + LT, + LEQ, + GT, + GEQ, + EQ, + NE, +}; + class MovementSetManager { public: MovementSetManager(); ~MovementSetManager(); - esp_err_t add_movement(Movement::MovementEntry& entry, uint8_t& index); + esp_err_t add_movement(MovementEntryData& entry, uint8_t index); esp_err_t remove_movement(uint8_t index, uint8_t board_id); - esp_err_t update_movement(Movement::MovementEntry& entry, uint8_t index); - esp_err_t get_curr_movement_set(Movement::MovementSet& set); - esp_err_t verify_movement_set(); - esp_err_t get_nvs_movement_set(Movement::MovementSet& set); + esp_err_t get_curr_movement_set(std::unordered_map>& set); + esp_err_t get_nvs_movement_set(std::unordered_map>& set); esp_err_t write_nvs_movement_set(); private: - std::unique_ptr topology_manager = std::make_unique(); - std::unordered_map> movements; + std::unordered_map> movements; nvs_handle_t handle; bool ready = false; Flatbuffers::MovementSetBuilder builder; diff --git a/main/README.md b/main/README.md index 0e1298e..079d63b 100644 --- a/main/README.md +++ b/main/README.md @@ -42,6 +42,8 @@ To run the sample code, add the compiler variable `PP_MOVE=1` when compiling. You can use the `SRC_BOARD` compiler variable to set a custom board id. +Sample usage: `idf.py -B ./build_movement build -D SRC_BOARD=101 -D PP_MOVE=1` + The sample topology used to test: ![Sample Topology Used for Testing](images/sample_topology.png) diff --git a/main/main_preprogrammed_test.cpp b/main/main_preprogrammed_test.cpp index 0d73426..dc9d39b 100644 --- a/main/main_preprogrammed_test.cpp +++ b/main/main_preprogrammed_test.cpp @@ -10,6 +10,9 @@ #include "constants/datalink.h" #include "TopologyManager.h" +#include "MovementSetManager.h" +#include "MovementSetBuilder.h" +#include "flatbuffers_generated/RobotModule_generated.h" #include #include @@ -238,6 +241,80 @@ extern "C" [[noreturn]] void app_main(void) { ESP_LOGI(MOVEMENT_DEBUG_TAG, "Current topology is correct"); + //Testing Movements + + MovementSetManager movement_set_mgr = MovementSetManager(); + + std::unordered_map> movements = { + {0, { + {69, (uint8_t)ModuleType_DC_MOTOR, 0, Movement::ConditionBlob(0,0,0,0), (uint8_t)ACK_VALUES::NO_ACK, 0, 0}, + {67, (uint8_t)ModuleType_SERVO_1, 0, Movement::ConditionBlob(0,0,0,0), (uint8_t)ACK_VALUES::NO_ACK, 0, 0}, + }}, + {1, { + {198, (uint8_t)ModuleType_BATTERY, 0, Movement::ConditionBlob(0,0,0,0), (uint8_t)ACK_VALUES::NO_ACK, 0, 0} + }}, + {2, { + {56, (uint8_t)ModuleType_SERVO_1, 0, Movement::ConditionBlob(0,0,0,0), (uint8_t)ACK_VALUES::NO_ACK, 0, 0}, + {69, (uint8_t)ModuleType_DC_MOTOR, 0, Movement::ConditionBlob(0,0,0,0), (uint8_t)ACK_VALUES::NO_ACK, 0, 0}, + {26, (uint8_t)ModuleType_SERVO_2, 0, Movement::ConditionBlob(0,0,0,0), (uint8_t)ACK_VALUES::NO_ACK, 0, 0}, + }}, + {3, { + {69, (uint8_t)ModuleType_DC_MOTOR, 0, Movement::ConditionBlob(0,0,0,0), (uint8_t)ACK_VALUES::NO_ACK, 0, 0}, + {67, (uint8_t)ModuleType_SERVO_1, 0, Movement::ConditionBlob(0,0,0,0), (uint8_t)ACK_VALUES::NO_ACK, 0, 0}, + }}, + }; + + for (auto& [index, movement_vector] : movements){ + for (auto& movement : movement_vector){ + res = movement_set_mgr.add_movement(movement, index); + if (res != ESP_OK){ + ESP_LOGE(MOVEMENT_DEBUG_TAG, "Failed to insert movement for board id %d at index %d", movement.board_id, index); + restart(); + } + } + } + + res = movement_set_mgr.write_nvs_movement_set(); + + if (res != ESP_OK){ + ESP_LOGE(MOVEMENT_DEBUG_TAG, "Failed to write movement set to nvs"); + restart(); + } + + std::unordered_map> movements_test; + + res = movement_set_mgr.get_nvs_movement_set(movements_test); + + if (res != ESP_OK){ + ESP_LOGE(MOVEMENT_DEBUG_TAG, "Failed to get movement set from nvs"); + restart(); + } + + for (const auto& [key, vec] : movements) { + auto it = movements_test.find(key); + if (it == movements_test.end()) { + ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Missing key %d", key); + restart(); + } + + const auto& vec2 = it->second; + if (vec.size() != vec2.size()) { + ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Size mismatch at key %d: %d vs %d", key, vec.size(), vec2.size()); + restart(); + } + + for (size_t i = 0; i < vec.size(); ++i) { + if (!(vec[i] == vec2[i])) { + ESP_LOGE(MOVEMENTS_DEBUG_TAG, + "Mismatch at key %d index %d: board_id %d vs %d", + key, i, vec[i].board_id, vec2[i].board_id); + restart(); + } + } + } + + ESP_LOGI(MOVEMENTS_DEBUG_TAG, "Successfully got movement set from nvs correctly"); + restart(); while(true){