botchain/firmware
1
0
Fork 0
mirror of https://github.com/BotChain-Robots/firmware.git synced 2026-07-08 17:47:21 +02:00
Code Issues Packages Projects Releases Wiki Activity

able to add, verify, remove, and get current topology

Browse Source
This commit is contained in:
superkor
2026-01-24 02:05:56 -05:00
parent 8a4f4babc6
commit 5124c97b76
5 changed files with 189 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
components/flatbuffers/CMakeLists.txt
View File

@@ -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"
)

20
components/movements/TopologyManager.cpp
View File

@@ -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<std::pair<uint8_t, uint16_t>>& connections, uint16_t curr_board_id){
esp_err_t TopologyManager::add_board_to_topology(const std::vector<std::pair<uint8_t, uint16_t>>& 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<std::pair<uint8_t,
std::vector<Topology::ChannelBoardConn> conns;
for (const std::pair<uint8_t, uint16_t>& 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<Topology::ChannelBoardConn>& 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;
}

4
components/movements/include/TopologyManager.h
View File

@@ -14,11 +14,13 @@
#include <stack>
#include <algorithm>
#define TOPOLOGY_DEBUG_TAG "topology"
class TopologyManager {
public:
TopologyManager();
~TopologyManager();
esp_err_t add_board_to_topology(std::vector<std::pair<uint8_t, uint16_t>>& connections, uint16_t curr_board_id);
esp_err_t add_board_to_topology(const std::vector<std::pair<uint8_t, uint16_t>>& 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<std::pair<uint8_t, uint16_t>>& connections, uint16_t curr_board_id);
esp_err_t verify_topology();

9
main/CMakeLists.txt
View File

@@ -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()

156
main/main_preprogrammed_test.cpp
View File

@@ -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 <cstdio>
#include <memory>
@@ -16,12 +16,25 @@
#include <freertos/semphr.h>
#include "driver/gptimer.h"
#include "esp_log.h"
#include <vector>
#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();
@@ -40,6 +53,145 @@ extern "C" [[noreturn]] void app_main(void) {
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<std::pair<std::vector<std::pair<uint8_t, uint16_t>>, 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<std::pair<uint8_t, uint16_t>> 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<uint16_t, std::vector<std::pair<uint8_t, uint16_t>>> 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);