mirror of
https://github.com/BotChain-Robots/firmware.git
synced 2026-07-08 17:47:21 +02:00
updates to topology with flatbuffers
This commit is contained in:
superkor
@@ -17,70 +17,182 @@ TopologyManager::~TopologyManager(){
|
||||
nvs_close(handle);
|
||||
}
|
||||
|
||||
// esp_err_t TopologyManager::add_board_to_topology(struct NeighbourBlob& blob){
|
||||
// if (!ready){
|
||||
// return ESP_ERR_INVALID_STATE;
|
||||
// }
|
||||
/**
|
||||
* @brief Creates a `Topology::NeighbourBlob` based on the input parameters and adds the blob to the internal topology map
|
||||
*
|
||||
* @param connections This is a vector of pairs (uint8_t: `channel number`, uint16_t: `board_id`) representing the connection from the `curr_board_id` to `board_id` via `channel number`
|
||||
* @param curr_board_id This is the board id, from its perspective, has the connections to the other boards in the vector `connections`
|
||||
* @return esp_err_t
|
||||
*
|
||||
* @note
|
||||
* Connections to fail:
|
||||
*
|
||||
* 1. Any of the `board_id` in the vector is `PC_ADDR` or `BROADCAST_ADDR` - Reserved addresses
|
||||
*
|
||||
* 2. Any of the `board_id` in the vector is `curr_board_id` - No loopback connections are allowed
|
||||
*
|
||||
* 3. Any of the `channel` in the vector is greater than or equal to `MAX_CHANNELS` - Channels are 0-indexed and there are at most 4 channels
|
||||
*
|
||||
* 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){
|
||||
if (!ready){
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
// if (blob.num_connections > MAX_CHANNELS || blob.curr_board_id == PC_ADDR || blob.curr_board_id == BROADCAST_ADDR){
|
||||
// return ESP_ERR_INVALID_ARG;
|
||||
// }
|
||||
if (connections.size() == 0 || connections.size() > MAX_CHANNELS){
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
|
||||
// for (uint8_t i = 0; i < blob.num_connections; i++){
|
||||
// ChannelBoardConn connection = blob.neighbour_connections[i];
|
||||
// if (connection.channel > MAX_CHANNELS || connection.board_id == PC_ADDR
|
||||
// || connection == BROADCAST_ADDR || connection.board_id == blob.curr_board_id){
|
||||
// return ESP_ERR_INVALID_ARG;
|
||||
// }
|
||||
// }
|
||||
if (curr_board_id == PC_ADDR || curr_board_id == BROADCAST_ADDR){
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
|
||||
// topology[blob.curr_board_id] = blob;
|
||||
//verify the connections in the vector
|
||||
std::unordered_set<uint8_t> check_set;
|
||||
|
||||
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()){
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
|
||||
check_set.insert(pair.first);
|
||||
|
||||
conns.push_back(builder.build_connections(pair.first, pair.second));
|
||||
}
|
||||
|
||||
topology[curr_board_id] = conns;
|
||||
|
||||
// return ESP_OK;
|
||||
// }
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
// esp_err_t TopologyManager::remove_board_from_topology(uint16_t board_id){
|
||||
// if (!ready){
|
||||
// return ESP_ERR_INVALID_STATE;
|
||||
// }
|
||||
esp_err_t TopologyManager::remove_board_from_topology(uint16_t board_id){
|
||||
if (!ready){
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
// if (topology.find(board_id) == topology.end()){
|
||||
// return ESP_ERR_NOT_FOUND;
|
||||
// }
|
||||
if (topology.find(board_id) == topology.end()){
|
||||
return ESP_ERR_NOT_FOUND;
|
||||
}
|
||||
|
||||
// topology.erase(board_id);
|
||||
topology.erase(board_id);
|
||||
|
||||
// return ESP_OK;
|
||||
// }
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
// esp_err_t TopologyManager::get_board_in_topology(struct NeighbourBlob& blob){
|
||||
// if (!ready){
|
||||
// return ESP_ERR_INVALID_STATE;
|
||||
// }
|
||||
esp_err_t TopologyManager::get_board_in_topology(std::vector<std::pair<uint8_t, uint16_t>>& connections, uint16_t curr_board_id){
|
||||
if (!ready){
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
// if (topology.find(board_id) == topology.end()){
|
||||
// return ESP_ERR_NOT_FOUND;
|
||||
// }
|
||||
if (topology.find(curr_board_id) == topology.end()){
|
||||
return ESP_ERR_NOT_FOUND;
|
||||
}
|
||||
|
||||
// blob = topology.find(board_id);
|
||||
std::vector<Topology::ChannelBoardConn> conn = topology[curr_board_id];
|
||||
|
||||
// return ESP_OK;
|
||||
// }
|
||||
connections.clear();
|
||||
|
||||
// esp_err_t TopologyManager::verify_topology(){
|
||||
// if (!ready){
|
||||
// return ESP_ERR_INVALID_STATE;
|
||||
// }
|
||||
// return ESP_OK;
|
||||
// }
|
||||
for (const Topology::ChannelBoardConn c : conn){
|
||||
connections.push_back(std::pair<uint8_t, uint16_t>(c.channel(), c.board_id()));
|
||||
}
|
||||
|
||||
// esp_err_t TopologyManager::get_curr_topology(struct Topology& topology){
|
||||
// if (!ready){
|
||||
// return ESP_ERR_INVALID_STATE;
|
||||
// }
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
// return ESP_OK;
|
||||
// }
|
||||
esp_err_t TopologyManager::verify_topology(){
|
||||
if (!ready){
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
if (topology.size() == 0){
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
/**
|
||||
* Conditions to fail:
|
||||
* 1. Any board that is referenced in the topology does not have a reciprocal connection (eg. A -> B and B -> A)
|
||||
* 2. The topology contains 2 separate graphs (a board in the topology should be in the same graph with some sort of path to all other boards)
|
||||
*/
|
||||
|
||||
//condition 1
|
||||
|
||||
for (const auto& pair : topology){
|
||||
for (const Topology::ChannelBoardConn& conn : pair.second){
|
||||
if (topology.find(conn.board_id()) == topology.end()){
|
||||
return ESP_ERR_INVALID_STATE; //could not find reciprocal board
|
||||
}
|
||||
|
||||
std::vector<Topology::ChannelBoardConn> topology_board_conn = topology[conn.board_id()];
|
||||
bool found = false;
|
||||
for (const Topology::ChannelBoardConn& c : topology_board_conn){
|
||||
if (c.board_id() == pair.first){
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found){
|
||||
return ESP_ERR_INVALID_STATE; //reciprocal board does not have a connection back to `pair.first`
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
//condition 2 - dfs
|
||||
std::unordered_set<uint16_t> visited;
|
||||
std::stack<uint16_t> backtrack;
|
||||
uint32_t count = 0;
|
||||
|
||||
backtrack.push(topology.begin()->first);
|
||||
|
||||
while (!backtrack.empty()){
|
||||
uint16_t curr_node = backtrack.top();
|
||||
backtrack.pop();
|
||||
|
||||
if (visited.find(curr_node) != visited.end()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
visited.insert(curr_node);
|
||||
count++;
|
||||
|
||||
const std::vector<Topology::ChannelBoardConn>& conns = topology[curr_node];
|
||||
|
||||
for (const Topology::ChannelBoardConn& conn : conns) {
|
||||
uint16_t next = conn.board_id();
|
||||
if (visited.find(next) != visited.end()) {
|
||||
backtrack.push(next);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (count != topology.size()){
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t TopologyManager::get_curr_topology(std::unordered_map<uint16_t, std::vector<std::pair<uint8_t, uint16_t>>>& topology){
|
||||
if (!ready){
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
}
|
||||
|
||||
topology.clear();
|
||||
|
||||
for (const auto& [key, value] : this->topology){
|
||||
topology[key] = std::vector<std::pair<uint8_t, uint16_t>>();
|
||||
for (const Topology::ChannelBoardConn c : value){
|
||||
topology[key].push_back(std::pair<uint8_t, uint16_t>(c.channel(), c.board_id()));
|
||||
}
|
||||
}
|
||||
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
// esp_err_t TopologyManager::write_nvs_topology(){
|
||||
// if (!ready){
|
||||
|
||||
Reference in New Issue
Block a user