From e077c616a325c2fed7733da50d1ebfb4350689d2 Mon Sep 17 00:00:00 2001 From: superkor Date: Sat, 24 Jan 2026 02:05:56 -0500 Subject: [PATCH] able to add, verify, remove, and get current topology --- components/flatbuffers/CMakeLists.txt | 10 +- components/movements/TopologyManager.cpp | 20 ++- .../movements/include/TopologyManager.h | 4 +- main/CMakeLists.txt | 9 + main/main_preprogrammed_test.cpp | 156 +++++++++++++++++- 5 files changed, 189 insertions(+), 10 deletions(-) diff --git a/components/flatbuffers/CMakeLists.txt b/components/flatbuffers/CMakeLists.txt index 85f614f..cee0c94 100644 --- a/components/flatbuffers/CMakeLists.txt +++ b/components/flatbuffers/CMakeLists.txt @@ -1,2 +1,8 @@ -idf_component_register(SRCS "MPIMessageBuilder.cpp" "AngleControlMessageBuilder.cpp" "TopologyMessageBuilder.cpp" "SensorMessageBuilder.cpp" REQUIRES "constants" - INCLUDE_DIRS "include") +idf_component_register(SRCS "MPIMessageBuilder.cpp" + "AngleControlMessageBuilder.cpp" + "TopologyMessageBuilder.cpp" + "SensorMessageBuilder.cpp" + "TopologyBuilder.cpp" + REQUIRES "constants" + INCLUDE_DIRS "include" +) diff --git a/components/movements/TopologyManager.cpp b/components/movements/TopologyManager.cpp index c010f9e..7199263 100644 --- a/components/movements/TopologyManager.cpp +++ b/components/movements/TopologyManager.cpp @@ -35,16 +35,18 @@ TopologyManager::~TopologyManager(){ * * 4. All `board_id` is not unique in the vector - Should be a 1:1 connection between the boards */ -esp_err_t TopologyManager::add_board_to_topology(std::vector>& connections, uint16_t curr_board_id){ +esp_err_t TopologyManager::add_board_to_topology(const std::vector>& connections, uint16_t curr_board_id){ if (!ready){ return ESP_ERR_INVALID_STATE; } if (connections.size() == 0 || connections.size() > MAX_CHANNELS){ + ESP_LOGE(TOPOLOGY_DEBUG_TAG, "vector size is invalid. got size %d", connections.size()); return ESP_ERR_INVALID_ARG; } if (curr_board_id == PC_ADDR || curr_board_id == BROADCAST_ADDR){ + ESP_LOGE(TOPOLOGY_DEBUG_TAG, "curr_board_id is invalid. got size %d", curr_board_id); return ESP_ERR_INVALID_ARG; } @@ -54,8 +56,9 @@ esp_err_t TopologyManager::add_board_to_topology(std::vector conns; for (const std::pair& pair : connections){ - if (pair.second >= MAX_CHANNELS || pair.first == curr_board_id || pair.first == PC_ADDR - || pair.first == BROADCAST_ADDR || check_set.find(pair.first) != check_set.end()){ + if (pair.first >= MAX_CHANNELS || pair.second == curr_board_id || pair.second == PC_ADDR + || pair.second == BROADCAST_ADDR || check_set.find(pair.first) != check_set.end()){ + ESP_LOGE(TOPOLOGY_DEBUG_TAG, "Invalid pair detected. Got: %d -> %d on Ch. %d", curr_board_id, pair.second, pair.first); return ESP_ERR_INVALID_ARG; } @@ -123,6 +126,7 @@ esp_err_t TopologyManager::verify_topology(){ for (const auto& pair : topology){ for (const Topology::ChannelBoardConn& conn : pair.second){ if (topology.find(conn.board_id()) == topology.end()){ + ESP_LOGE(TOPOLOGY_DEBUG_TAG, "Could not find board %d in topology", conn.board_id()); return ESP_ERR_INVALID_STATE; //could not find reciprocal board } @@ -135,6 +139,7 @@ esp_err_t TopologyManager::verify_topology(){ } } if (!found){ + ESP_LOGE(TOPOLOGY_DEBUG_TAG, "Board %d does not have a connection to board %d", conn.board_id(), pair.first); return ESP_ERR_INVALID_STATE; //reciprocal board does not have a connection back to `pair.first` } @@ -152,6 +157,8 @@ esp_err_t TopologyManager::verify_topology(){ uint16_t curr_node = backtrack.top(); backtrack.pop(); + // printf("On board %d\n", curr_node); + if (visited.find(curr_node) != visited.end()) { continue; } @@ -161,19 +168,22 @@ esp_err_t TopologyManager::verify_topology(){ const std::vector& conns = topology[curr_node]; + // printf("Board %d has %d connections\n", curr_node, conns.size()); + for (const Topology::ChannelBoardConn& conn : conns) { uint16_t next = conn.board_id(); - if (visited.find(next) != visited.end()) { + if (visited.find(next) == visited.end()) { + // printf("Found new board %d\n", next); backtrack.push(next); } } } if (count != topology.size()){ + ESP_LOGE(TOPOLOGY_DEBUG_TAG, "Not all boards could be traversed to all other boards. Got %d out of total %d boards", count, topology.size()); return ESP_ERR_INVALID_STATE; } - return ESP_OK; } diff --git a/components/movements/include/TopologyManager.h b/components/movements/include/TopologyManager.h index 9bcab08..a87c1bb 100644 --- a/components/movements/include/TopologyManager.h +++ b/components/movements/include/TopologyManager.h @@ -14,11 +14,13 @@ #include #include +#define TOPOLOGY_DEBUG_TAG "topology" + class TopologyManager { public: TopologyManager(); ~TopologyManager(); - esp_err_t add_board_to_topology(std::vector>& connections, uint16_t curr_board_id); + esp_err_t add_board_to_topology(const std::vector>& connections, uint16_t curr_board_id); esp_err_t remove_board_from_topology(uint16_t board_id); esp_err_t get_board_in_topology(std::vector>& connections, uint16_t curr_board_id); esp_err_t verify_topology(); diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index a50cdc1..be031a9 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -8,11 +8,20 @@ if(DEFINED RMT_TEST AND RMT_TEST EQUAL 1) message(STATUS "Building for testing RMT/Link Layer Functionality with main_rmt_test.cpp") list(REMOVE_ITEM ALL_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/main.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/main_preprogrammed_test.cpp" ) idf_build_set_property(COMPILE_DEFINITIONS RMT_TEST APPEND) +elseif(DEFINED PP_MOVE AND PP_MOVE EQUAL 1) + message(STATUS "Building for testing preprogrammed movements") + list(REMOVE_ITEM ALL_SRCS + "${CMAKE_CURRENT_SOURCE_DIR}/main.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/main_rmt_test.cpp" + ) + idf_build_set_property(COMPILE_DEFINITIONS PP_MOVE APPEND) else() list(REMOVE_ITEM ALL_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/main_rmt_test.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/main_preprogrammed_test.cpp" ) endif() diff --git a/main/main_preprogrammed_test.cpp b/main/main_preprogrammed_test.cpp index 392e1e6..2b36d7c 100644 --- a/main/main_preprogrammed_test.cpp +++ b/main/main_preprogrammed_test.cpp @@ -1,4 +1,4 @@ -#if (defined(RMT_TEST) && RMT_TEST == 0) && (!defined(PP_MOVE) || (defined(PP_MOVE) && PP_MOVE == 1)) +#if (defined(PP_MOVE) && PP_MOVE == 1) && (!defined(RMT_TEST) || (defined(RMT_TEST) && RMT_TEST == 0)) #include #include @@ -16,12 +16,25 @@ #include #include "driver/gptimer.h" #include "esp_log.h" +#include #if !defined(SRC_BOARD) const uint8_t BOARD_ID = 1; #else -const uint8_t BOARD_ID = SRC_BOARD +const uint8_t BOARD_ID = SRC_BOARD; #endif +#define MOVEMENT_DEBUG_TAG "movement" + +[[noreturn]] void restart(){ + for (int i = 5; i >= 0; i--) { + printf("Restarting in %d seconds...\n", i); + vTaskDelay(1000 / portTICK_PERIOD_MS); + } + printf("Restarting now.\n"); + fflush(stdout); + esp_restart(); +} + extern "C" [[noreturn]] void app_main(void) { esp_err_t ret = nvs_flash_init(); @@ -39,7 +52,146 @@ extern "C" [[noreturn]] void app_main(void) { printf("Hello world!\n"); TopologyManager obj = TopologyManager(); + + /** + * @brief test topology + * + * Board | Channel | Connected to + * 1 | 0 | 2 + * 1 | 1 | 100 + * 2 | 0 | 1 + * 2 | 1 | 4 + * 2 | 2 | 7 + * 2 | 3 | 69 + * 4 | 2 | 2 + * 4 | 3 | 100 + * 7 | 0 | 69 + * 7 | 1 | 2 + * 69 | 0 | 2 + * 69 | 1 | 7 + * 100 | 0 | 4 + * 100 | 1 | 1 + */ + + uint8_t succ_count = 0; + + std::vector>, uint16_t>> test_connections = { + { { {0, 2}, {1, 100} }, 1 }, + { { {0, 1}, {1, 4}, {2, 7}, {3, 69} }, 2 }, + { { {2, 2}, {3, 100} }, 4 }, + { { {0, 69}, {1, 2} }, 7 }, + { { {0, 2}, {1, 7} }, 69 }, + { { {0, 4}, {1, 1} }, 100 } + }; + + for (const auto& connections : test_connections){ + ESP_LOGI(MOVEMENT_DEBUG_TAG, "Adding board %d...", connections.second); + ESP_LOGI(MOVEMENT_DEBUG_TAG, "Connections:"); + + for (const auto& c : connections.first){ + ESP_LOGI(MOVEMENT_DEBUG_TAG, "curr board %d -> board %d on channel %d", connections.second, c.second, c.first); + } + + if (obj.add_board_to_topology(connections.first, connections.second) == ESP_OK) succ_count++; + else{ + ESP_LOGE(MOVEMENT_DEBUG_TAG, "Failed to add board %d", connections.second); + } + } + ESP_LOGI(MOVEMENT_DEBUG_TAG, "%d board connections successfully created out of %d", succ_count, test_connections.size()); + + if (succ_count != test_connections.size()){ + restart(); + } + + succ_count = 0; + + esp_err_t res; + + for (const auto& connections : test_connections){ + std::vector> check; + res = obj.get_board_in_topology(check, connections.second); + + if (res != ESP_OK){ + continue; + } + + if (check.size() != connections.first.size()){ + continue; + } + + bool broke = false; + for (uint8_t i = 0; i < check.size(); i++){ + if (check.at(i) != connections.first.at(i)){ + broke = true; + ESP_LOGE(MOVEMENT_DEBUG_TAG, "Board %d in topology is not correct! Failed at connection %d", connections.second, i); + break; + } + } + + if (!broke){ + succ_count++; + } + } + + ESP_LOGI(MOVEMENT_DEBUG_TAG, "%d board connections successfully verified out of %d", succ_count, test_connections.size()); + + res = obj.verify_topology(); + + if (res != ESP_OK){ + ESP_LOGE(MOVEMENT_DEBUG_TAG, "Topology verification incorrectly failed"); + } else { + ESP_LOGI(MOVEMENT_DEBUG_TAG, "Topology verification passed"); + } + + //test removing a board from topology + + uint16_t board_id_to_remove = 2; + + if (obj.remove_board_from_topology(board_id_to_remove) != ESP_OK){ + ESP_LOGE(MOVEMENT_DEBUG_TAG, "Failed to remove board %d", board_id_to_remove); + restart(); + } + + ESP_LOGI(MOVEMENT_DEBUG_TAG, "Successfully removed board %d", board_id_to_remove); + + res = obj.verify_topology(); + + if (res == ESP_OK){ + ESP_LOGE(MOVEMENT_DEBUG_TAG, "Topology verification incorrectly passed"); + } else { + ESP_LOGI(MOVEMENT_DEBUG_TAG, "Topology verification correctly failed"); + } + + std::unordered_map>> test_topology; + + if (obj.get_curr_topology(test_topology) != ESP_OK){ + ESP_LOGE(MOVEMENT_DEBUG_TAG, "Getting current topology failed"); + restart(); + } + + for (const auto& connections : test_connections){ + if (connections.second == board_id_to_remove){ + continue; + } + + const auto& test = connections.first; + + if (test_topology.find(connections.second) == test_topology.end()){ + ESP_LOGE(MOVEMENT_DEBUG_TAG, "Failed to find board %d in topology", connections.second); + restart(); + } + + if (test.size() != test_topology[connections.second].size()){ + ESP_LOGE(MOVEMENT_DEBUG_TAG, "Board %d connections are different in topology", connections.second); + restart(); + } + } + + ESP_LOGI(MOVEMENT_DEBUG_TAG, "Current topology is correct"); + + restart(); + while(true){ //do nothing vTaskDelay(1000 / portTICK_PERIOD_MS);