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

update movement set

Browse Source
This commit is contained in:
superkor
2026-01-27 22:17:44 -05:00
parent 60b94b1650
commit 44af33921b
9 changed files with 278 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

76
components/flatbuffers/MovementSetBuilder.cpp
View File

@@ -6,40 +6,72 @@
#ifdef MOVEMENTSETBUILDER
namespace Flatbuffers {
SerializedMessage MovementSetBuilder::build_movement_set(const std::vector<flatbuffers::Offset<Movement::MovementEntry>>& set){
SerializedMessage MovementSetBuilder::build_movement_set(const std::unordered_map<uint8_t, std::vector<MovementEntryInput>>& input){
builder_.Clear();
const auto set_vector = builder_.CreateVector(set);
std::vector<flatbuffers::Offset<Movement::MovementVector>> map_entries;
map_entries.reserve(input.size());
const auto message = Movement::CreateMovementSet(
builder_,
static_cast<uint8_t>(set.size()),
set_vector
// 1. Collect and sort keys (DO NOT sort offsets)
std::vector<uint8_t> keys;
keys.reserve(input.size());
for (const auto& [key, _] : input) {
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);
std::vector<flatbuffers::Offset<Movement::MovementEntry>> fb_entries;
fb_entries.reserve(entries.size());
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_,
key,
movements_vector
)
);
}
// 3. Build the map vector
auto map_vector = builder_.CreateVector(map_entries);
auto message = Movement::CreateMovementSet(
builder_,
map_vector
);
builder_.Finish(message);
return {builder_.GetBufferPointer(), builder_.GetSize()};
}
flatbuffers::Offset<Movement::MovementEntry> MovementSetBuilder::build_movement_entry(uint16_t board_id, uint8_t module_type, uint16_t value_action,
const Movement::ConditionBlob& condition, uint8_t ack, uint16_t ack_ttl_ms, uint16_t post_delay_ms){
return Movement::CreateMovementEntry(
builder_,
board_id,
module_type,
value_action,
&condition,
ack,
ack_ttl_ms,
post_delay_ms
);
return { builder_.GetBufferPointer(), builder_.GetSize() };
}
Movement::ConditionBlob MovementSetBuilder::build_condition_blob(uint16_t value, uint8_t cond, uint8_t module_type, uint8_t in_use){
return Movement::ConditionBlob(value, cond, module_type, in_use);
}
}
#endif //MOVEMENTSETBUILDER

16
components/flatbuffers/TopologyBuilder.cpp
View File

@@ -30,21 +30,7 @@ namespace Flatbuffers{
return { builder_.GetBufferPointer(), builder_.GetSize() };
}
flatbuffers::Offset<Topology::NeighbourBlob> TopologyBuilder::build_neighbour_info(
uint16_t board_id,
const std::vector<Topology::ChannelBoardConn>& connections
) {
auto conn_vec = builder_.CreateVectorOfStructs(connections);
return Topology::CreateNeighbourBlob(
builder_,
board_id,
static_cast<uint8_t>(connections.size()),
conn_vec
);
}
Topology::ChannelBoardConn TopologyBuilder::build_connections(uint8_t channel, uint16_t board_id){
return Topology::ChannelBoardConn(channel, board_id);
}

14
components/flatbuffers/include/MovementSetBuilder.h
View File

@@ -11,14 +11,22 @@
#include "flatbuffers_generated/Movement_generated.h"
#include "flatbuffers/flatbuffers.h"
struct MovementEntryInput {
uint16_t board_id;
uint8_t module_type;
uint16_t value_action;
Movement::ConditionBlob condition;
uint8_t ack;
uint16_t ack_ttl_ms;
uint16_t post_delay_ms;
};
namespace Flatbuffers {
class MovementSetBuilder {
public:
MovementSetBuilder() : builder_(MAX_MOVEMENTS_IN_SET*sizeof(Movement::MovementEntry)) {}
SerializedMessage build_movement_set(const std::vector<flatbuffers::Offset<Movement::MovementEntry>>& set);
flatbuffers::Offset<Movement::MovementEntry> build_movement_entry(uint16_t board_id, uint8_t module_type, uint16_t value_action,
const Movement::ConditionBlob& condition, uint8_t ack, uint16_t ack_ttl_ms, uint16_t post_delay_ms);
SerializedMessage build_movement_set(const std::unordered_map<uint8_t, std::vector<MovementEntryInput>>& input);
Movement::ConditionBlob build_condition_blob(uint16_t value, uint8_t cond, uint8_t module_type, uint8_t in_use);
private:

1
components/flatbuffers/include/TopologyBuilder.h
View File

@@ -18,7 +18,6 @@ namespace Flatbuffers {
Topology::ChannelBoardConn build_connections(uint8_t channel, uint16_t board_id);
private:
flatbuffers::Offset<Topology::NeighbourBlob> build_neighbour_info(uint16_t board_id, const std::vector<Topology::ChannelBoardConn>& connections);
flatbuffers::FlatBufferBuilder builder_;
};
}

102
components/flatbuffers/include/flatbuffers_generated/Movement_generated.h
View File

@@ -20,6 +20,9 @@ struct ConditionBlob;
struct MovementEntry;
struct MovementEntryBuilder;
struct MovementVector;
struct MovementVectorBuilder;
struct MovementSet;
struct MovementSetBuilder;
@@ -165,14 +168,20 @@ inline ::flatbuffers::Offset<MovementEntry> CreateMovementEntry(
return builder_.Finish();
}
struct MovementSet FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
typedef MovementSetBuilder Builder;
struct MovementVector FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
typedef MovementVectorBuilder Builder;
enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
VT_NUM_MOVEMENTS = 4,
VT_KEY = 4,
VT_MOVEMENTS = 6
};
uint8_t num_movements() const {
return GetField<uint8_t>(VT_NUM_MOVEMENTS, 0);
uint8_t key() const {
return GetField<uint8_t>(VT_KEY, 0);
}
bool KeyCompareLessThan(const MovementVector * const o) const {
return key() < o->key();
}
int KeyCompareWithValue(uint8_t _key) const {
return static_cast<int>(key() > _key) - static_cast<int>(key() < _key);
}
const ::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementEntry>> *movements() const {
return GetPointer<const ::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementEntry>> *>(VT_MOVEMENTS);
@@ -180,7 +189,7 @@ struct MovementSet FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
template <bool B = false>
bool Verify(::flatbuffers::VerifierTemplate<B> &verifier) const {
return VerifyTableStart(verifier) &&
VerifyField<uint8_t>(verifier, VT_NUM_MOVEMENTS, 1) &&
VerifyField<uint8_t>(verifier, VT_KEY, 1) &&
VerifyOffset(verifier, VT_MOVEMENTS) &&
verifier.VerifyVector(movements()) &&
verifier.VerifyVectorOfTables(movements()) &&
@@ -188,15 +197,72 @@ struct MovementSet FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
}
};
struct MovementVectorBuilder {
typedef MovementVector Table;
::flatbuffers::FlatBufferBuilder &fbb_;
::flatbuffers::uoffset_t start_;
void add_key(uint8_t key) {
fbb_.AddElement<uint8_t>(MovementVector::VT_KEY, key, 0);
}
void add_movements(::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementEntry>>> movements) {
fbb_.AddOffset(MovementVector::VT_MOVEMENTS, movements);
}
explicit MovementVectorBuilder(::flatbuffers::FlatBufferBuilder &_fbb)
: fbb_(_fbb) {
start_ = fbb_.StartTable();
}
::flatbuffers::Offset<MovementVector> Finish() {
const auto end = fbb_.EndTable(start_);
auto o = ::flatbuffers::Offset<MovementVector>(end);
return o;
}
};
inline ::flatbuffers::Offset<MovementVector> CreateMovementVector(
::flatbuffers::FlatBufferBuilder &_fbb,
uint8_t key = 0,
::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementEntry>>> movements = 0) {
MovementVectorBuilder builder_(_fbb);
builder_.add_movements(movements);
builder_.add_key(key);
return builder_.Finish();
}
inline ::flatbuffers::Offset<MovementVector> CreateMovementVectorDirect(
::flatbuffers::FlatBufferBuilder &_fbb,
uint8_t key = 0,
const std::vector<::flatbuffers::Offset<Movement::MovementEntry>> *movements = nullptr) {
auto movements__ = movements ? _fbb.CreateVector<::flatbuffers::Offset<Movement::MovementEntry>>(*movements) : 0;
return Movement::CreateMovementVector(
_fbb,
key,
movements__);
}
struct MovementSet FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
typedef MovementSetBuilder Builder;
enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
VT_MOVEMENT_MAP = 4
};
const ::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementVector>> *movement_map() const {
return GetPointer<const ::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementVector>> *>(VT_MOVEMENT_MAP);
}
template <bool B = false>
bool Verify(::flatbuffers::VerifierTemplate<B> &verifier) const {
return VerifyTableStart(verifier) &&
VerifyOffset(verifier, VT_MOVEMENT_MAP) &&
verifier.VerifyVector(movement_map()) &&
verifier.VerifyVectorOfTables(movement_map()) &&
verifier.EndTable();
}
};
struct MovementSetBuilder {
typedef MovementSet Table;
::flatbuffers::FlatBufferBuilder &fbb_;
::flatbuffers::uoffset_t start_;
void add_num_movements(uint8_t num_movements) {
fbb_.AddElement<uint8_t>(MovementSet::VT_NUM_MOVEMENTS, num_movements, 0);
}
void add_movements(::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementEntry>>> movements) {
fbb_.AddOffset(MovementSet::VT_MOVEMENTS, movements);
void add_movement_map(::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementVector>>> movement_map) {
fbb_.AddOffset(MovementSet::VT_MOVEMENT_MAP, movement_map);
}
explicit MovementSetBuilder(::flatbuffers::FlatBufferBuilder &_fbb)
: fbb_(_fbb) {
@@ -211,23 +277,19 @@ struct MovementSetBuilder {
inline ::flatbuffers::Offset<MovementSet> CreateMovementSet(
::flatbuffers::FlatBufferBuilder &_fbb,
uint8_t num_movements = 0,
::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementEntry>>> movements = 0) {
::flatbuffers::Offset<::flatbuffers::Vector<::flatbuffers::Offset<Movement::MovementVector>>> movement_map = 0) {
MovementSetBuilder builder_(_fbb);
builder_.add_movements(movements);
builder_.add_num_movements(num_movements);
builder_.add_movement_map(movement_map);
return builder_.Finish();
}
inline ::flatbuffers::Offset<MovementSet> CreateMovementSetDirect(
::flatbuffers::FlatBufferBuilder &_fbb,
uint8_t num_movements = 0,
const std::vector<::flatbuffers::Offset<Movement::MovementEntry>> *movements = nullptr) {
auto movements__ = movements ? _fbb.CreateVector<::flatbuffers::Offset<Movement::MovementEntry>>(*movements) : 0;
std::vector<::flatbuffers::Offset<Movement::MovementVector>> *movement_map = nullptr) {
auto movement_map__ = movement_map ? _fbb.CreateVectorOfSortedTables<Movement::MovementVector>(movement_map) : 0;
return Movement::CreateMovementSet(
_fbb,
num_movements,
movements__);
movement_map__);
}
inline const Movement::MovementSet *GetMovementSet(const void *buf) {

89
components/movements/MovementSetManager.cpp
View File

@@ -9,38 +9,85 @@ MovementSetManager::MovementSetManager(){
ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Failed to open nvs for %s", MOVEMENTS_NVS_NAMESPACE);
return;
}
if (!topology_manager->is_ready()){
ESP_LOGE(MOVEMENTS_DEBUG_TAG, "Topology Manager is not ready!");
return;
}
ready = true;
};
MovementSetManager::~MovementSetManager(){
nvs_close(handle);
}
// esp_err_t MovementSetManager::add_movement(struct MovementEntry& entry, uint8_t& index){
// return ESP_OK;
// }
esp_err_t MovementSetManager::add_movement(Movement::MovementEntry& entry, uint8_t& index){
if (!ready){
return ESP_ERR_INVALID_STATE;
}
// esp_err_t MovementSetManager::remove_movement(uint8_t index){
// return ESP_OK;
// }
if (entry.board_id() == PC_ADDR || entry.board_id() == BROADCAST_ADDR){
return ESP_ERR_INVALID_ARG;
}
// esp_err_t MovementSetManager::update_movement(struct MovementEntry& entry, uint8_t index){
// return ESP_OK;
// }
if (movements.find(index) != movements.end()){
for (Movement::MovementEntry& e : movements[index]){
if (e.board_id() == entry.board_id()){
return ESP_ERR_INVALID_ARG; //only unique board ids per index
}
}
}
movements[index].push_back(entry);
// esp_err_t MovementSetManager::get_curr_movement_set(struct MovementSet& set){
// return ESP_OK;
// }
return ESP_OK;
}
// esp_err_t MovementSetManager::verify_movement_set(){
// return ESP_OK;
// }
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;
}
// esp_err_t MovementSetManager::get_nvs_movement_set(struct MovementSet& set){
// return ESP_OK;
// }
if (movements.find(index) == movements.end()){
return ESP_ERR_INVALID_ARG;
}
// esp_err_t MovementSetManager::write_nvs_movement_set(){
// return ESP_OK;
// }
bool found = false;
for (auto it = movements[index].begin(); it != movements[index].end(); ++it) {
if (it->board_id() == board_id){
movements[index].erase(it);
found = true;
break;
}
}
if (!found){
return ESP_ERR_NOT_FOUND;
}
return ESP_OK;
}
esp_err_t MovementSetManager::update_movement(Movement::MovementEntry& entry, uint8_t index){
return ESP_OK;
}
esp_err_t MovementSetManager::get_curr_movement_set(Movement::MovementSet& set){
return ESP_OK;
}
esp_err_t MovementSetManager::verify_movement_set(){
return ESP_OK;
}
esp_err_t MovementSetManager::get_nvs_movement_set(Movement::MovementSet& set){
return ESP_OK;
}
esp_err_t MovementSetManager::write_nvs_movement_set(){
return ESP_OK;
}
#endif //MOVEMENTS

49
components/movements/TopologyManager.cpp
View File

@@ -17,6 +17,10 @@ TopologyManager::~TopologyManager(){
nvs_close(handle);
}
bool TopologyManager::is_ready(){
return ready;
}
/**
* @brief Creates a `Topology::NeighbourBlob` based on the input parameters and adds the blob to the internal topology map
*
@@ -72,6 +76,14 @@ esp_err_t TopologyManager::add_board_to_topology(const std::vector<std::pair<uin
return ESP_OK;
}
/**
* @brief Removes the board associated with `board_id`
*
* @warning This function does not remove any existing links on other boards on the topology
*
* @param board_id
* @return esp_err_t
*/
esp_err_t TopologyManager::remove_board_from_topology(uint16_t board_id){
if (!ready){
return ESP_ERR_INVALID_STATE;
@@ -86,6 +98,13 @@ esp_err_t TopologyManager::remove_board_from_topology(uint16_t board_id){
return ESP_OK;
}
/**
* @brief Returns a vector of connections that represents the channel connection to a board id from `curr_board_id`
*
* @param connections
* @param curr_board_id
* @return esp_err_t
*/
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;
@@ -106,12 +125,23 @@ esp_err_t TopologyManager::get_board_in_topology(std::vector<std::pair<uint8_t,
return ESP_OK;
}
/**
* @brief Verifies the topology
*
* 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)
*
* @return esp_err_t
*/
esp_err_t TopologyManager::verify_topology(){
if (!ready){
return ESP_ERR_INVALID_STATE;
}
if (topology.size() == 0){
if (topology.size() == 0 || topology.size() >= RIP_MAX_ROUTES){
return ESP_ERR_INVALID_STATE;
}
@@ -187,6 +217,12 @@ esp_err_t TopologyManager::verify_topology(){
return ESP_OK;
}
/**
* @brief Gets the current topology stored in the manager
*
* @param topology
* @return esp_err_t
*/
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;
@@ -204,6 +240,11 @@ esp_err_t TopologyManager::get_curr_topology(std::unordered_map<uint16_t, std::v
return ESP_OK;
}
/**
* @brief Writes the topology stored in the manager onto the board's NVS
*
* @return esp_err_t
*/
esp_err_t TopologyManager::write_nvs_topology(){
if (!ready){
return ESP_ERR_INVALID_STATE;
@@ -237,6 +278,12 @@ esp_err_t TopologyManager::write_nvs_topology(){
return ESP_OK;
}
/**
* @brief Read's the board NVS to retreve the saved topology (if it exists)
*
* @param topology
* @return esp_err_t
*/
esp_err_t TopologyManager::get_nvs_topology(std::unordered_map<uint16_t, std::vector<std::pair<uint8_t, uint16_t>>>& topology){
if (!ready) {
return ESP_ERR_INVALID_STATE;

22
components/movements/include/MovementSetManager.h
View File

@@ -3,6 +3,8 @@
#include "TopologyManager.h"
#include <memory>
#include <vector>
#include "MovementSetBuilder.h"
#include "unordered_map"
#ifdef MOVEMENTS
#pragma once
@@ -11,18 +13,20 @@ class MovementSetManager {
public:
MovementSetManager();
~MovementSetManager();
// esp_err_t add_movement(struct MovementEntry& entry, uint8_t& index);
// esp_err_t remove_movement(uint8_t index);
// esp_err_t update_movement(struct MovementEntry& entry, uint8_t index);
// esp_err_t get_curr_movement_set(struct MovementSet& set);
// esp_err_t verify_movement_set();
// esp_err_t get_nvs_movement_set(struct MovementSet& set);
// esp_err_t write_nvs_movement_set();
esp_err_t add_movement(Movement::MovementEntry& 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 write_nvs_movement_set();
private:
std::unique_ptr<TopologyManager> topology_manager;
// std::vector<struct MovementEntry> movements;
std::unique_ptr<TopologyManager> topology_manager = std::make_unique<TopologyManager>();
std::unordered_map<uint8_t, std::vector<Movement::MovementEntry>> movements;
nvs_handle_t handle;
bool ready = false;
Flatbuffers::MovementSetBuilder builder;
};
#endif //MOVEMENTS

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

@@ -20,6 +20,7 @@ class TopologyManager {
public:
TopologyManager();
~TopologyManager();
bool is_ready();
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);