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

movement set read write to nvs working

Browse Source
This commit is contained in:
superkor
2026-01-28 00:47:22 -05:00
parent 4c2fabcd11
commit 255840abf1
10 changed files with 288 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -5,5 +5,6 @@ sdkconfig.old
.vscode/*
/.cache
test/build/*
build_movement
build_A
build_B

1
components/flatbuffers/CMakeLists.txt
View File

@@ -3,6 +3,7 @@ idf_component_register(SRCS "MPIMessageBuilder.cpp"
"TopologyMessageBuilder.cpp"
"SensorMessageBuilder.cpp"
"TopologyBuilder.cpp"
"MovementSetBuilder.cpp"
REQUIRES "constants"
INCLUDE_DIRS "include"
)

56
components/flatbuffers/MovementSetBuilder.cpp
View File

@@ -6,7 +6,7 @@
#ifdef MOVEMENTSETBUILDER
namespace Flatbuffers {
SerializedMessage MovementSetBuilder::build_movement_set(const std::unordered_map<uint8_t, std::vector<MovementEntryInput>>& input){
SerializedMessage MovementSetBuilder::build_movement_set(const std::unordered_map<uint8_t, std::vector<MovementEntryData>>& input){
builder_.Clear();
std::vector<flatbuffers::Offset<Movement::MovementVector>> 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<flatbuffers::Offset<Movement::MovementEntry>> fb_entries;
fb_entries.reserve(entries.size());
std::vector<flatbuffers::Offset<Movement::MovementEntry>> 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);

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

@@ -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<uint8_t, std::vector<MovementEntryInput>>& input);
SerializedMessage build_movement_set(const std::unordered_map<uint8_t, std::vector<MovementEntryData>>& input);
Movement::ConditionBlob build_condition_blob(uint16_t value, uint8_t cond, uint8_t module_type, uint8_t in_use);
private:

2
components/movements/CMakeLists.txt
View File

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

151
components/movements/MovementSetManager.cpp
View File

@@ -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<uint8_t>(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<uint8_t, std::vector<MovementEntryData>>& 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<uint8_t, std::vector<MovementEntryData>>& 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<uint32_t*>(&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<uint8_t> 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<uint32_t>(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;
}

2
components/movements/include/MovementManager.h
View File

@@ -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 {

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

@@ -1,29 +1,47 @@
#include "MovementManager.h"
#include "nvs_flash.h"
#include "TopologyManager.h"
#include <memory>
#include <vector>
#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<uint8_t, std::vector<MovementEntryData>>& set);
esp_err_t get_nvs_movement_set(std::unordered_map<uint8_t, std::vector<MovementEntryData>>& set);
esp_err_t write_nvs_movement_set();
private:
std::unique_ptr<TopologyManager> topology_manager = std::make_unique<TopologyManager>();
std::unordered_map<uint8_t, std::vector<Movement::MovementEntry>> movements;
std::unordered_map<uint8_t, std::vector<MovementEntryData>> movements;
nvs_handle_t handle;
bool ready = false;
Flatbuffers::MovementSetBuilder builder;

2
main/README.md
View File

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

77
main/main_preprogrammed_test.cpp
View File

@@ -10,6 +10,9 @@
#include "constants/datalink.h"
#include "TopologyManager.h"
#include "MovementSetManager.h"
#include "MovementSetBuilder.h"
#include "flatbuffers_generated/RobotModule_generated.h"
#include <esp_netif.h>
#include <esp_event.h>
@@ -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<uint8_t, std::vector<MovementEntryData>> 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<uint8_t, std::vector<MovementEntryData>> 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){