From 3b898b51b4a87c5e7ae08658e6c7fe100c508082 Mon Sep 17 00:00:00 2001 From: Johnathon Slightham Date: Thu, 15 Jan 2026 01:24:57 -0500 Subject: [PATCH] Sensor data implementation --- .../flatbuffers/SensorMessageBuilder.cpp | 46 +++++---- .../include/SensorMessageBuilder.h | 30 +++--- components/flatbuffers/include/Variant.h | 12 +++ .../SensorMessage_generated.h | 98 ++++++++++++++----- main/LoopManager.cpp | 24 ++--- main/control/DCMotorActuator.cpp | 48 +++++---- main/control/Servo1Actuator.cpp | 58 +++++------ main/include/control/DCMotorActuator.h | 11 ++- main/include/control/IActuator.h | 7 +- main/include/control/Servo1Actuator.h | 9 +- 10 files changed, 218 insertions(+), 125 deletions(-) create mode 100644 components/flatbuffers/include/Variant.h diff --git a/components/flatbuffers/SensorMessageBuilder.cpp b/components/flatbuffers/SensorMessageBuilder.cpp index 8e00fa8..f40d687 100644 --- a/components/flatbuffers/SensorMessageBuilder.cpp +++ b/components/flatbuffers/SensorMessageBuilder.cpp @@ -4,28 +4,40 @@ #include "SensorMessageBuilder.h" #include "SerializedMessage.h" +#include "Variant.h" #include "flatbuffers_generated/SensorMessage_generated.h" namespace Flatbuffers { - SerializedMessage SensorMessageBuilder::build_sensor_message(std::vector& values) { - builder_.Clear(); +SerializedMessage SensorMessageBuilder::build_sensor_message(std::vector &values) { + builder_.Clear(); - std::vector> values_vec; - std::vector sensor_values_vec; + std::vector> values_vec; + std::vector sensor_values_vec; - for (const auto& v : values) { - values_vec.push_back(Messaging::CreateAngle(builder_, v.angle).Union()); - sensor_values_vec.push_back(Messaging::SensorValue_Angle); - } - - auto values_fb_vec = builder_.CreateVector(values_vec); - const auto values_type_fb_vec = builder_.CreateVector(sensor_values_vec); - - const auto message = Messaging::CreateSensorMessage(builder_, values_type_fb_vec, values_fb_vec); - - builder_.Finish(message); - - return { builder_.GetBufferPointer(), builder_.GetSize() }; + for (const auto &v : values) { + std::visit( + overloaded{ + [&](target_angle a) { + values_vec.push_back(Messaging::CreateTargetAngle(builder_, a.angle).Union()); + sensor_values_vec.push_back(Messaging::SensorValue_TargetAngle); + }, + [&](current_angle a) { + values_vec.push_back(Messaging::CreateCurrentAngle(builder_, a.angle).Union()); + sensor_values_vec.push_back(Messaging::SensorValue_CurrentAngle); + }, + }, + v); } + + auto values_fb_vec = builder_.CreateVector(values_vec); + const auto values_type_fb_vec = builder_.CreateVector(sensor_values_vec); + + const auto message = + Messaging::CreateSensorMessage(builder_, values_type_fb_vec, values_fb_vec); + + builder_.Finish(message); + + return {builder_.GetBufferPointer(), builder_.GetSize()}; } +} // namespace Flatbuffers diff --git a/components/flatbuffers/include/SensorMessageBuilder.h b/components/flatbuffers/include/SensorMessageBuilder.h index 2c8be27..7cf3cef 100644 --- a/components/flatbuffers/include/SensorMessageBuilder.h +++ b/components/flatbuffers/include/SensorMessageBuilder.h @@ -6,19 +6,27 @@ #include "flatbuffers_generated/SensorMessage_generated.h" namespace Flatbuffers { - struct SensorValueInstance { - uint16_t angle; // todo: change to a variant - }; - class SensorMessageBuilder{ - public: - SensorMessageBuilder() : builder_(128) {} +struct target_angle { + int16_t angle; +}; - SerializedMessage build_sensor_message(std::vector& values); +struct current_angle { + int16_t angle; +}; - private: - flatbuffers::FlatBufferBuilder builder_; - }; -} +typedef std::variant sensor_value; + +class SensorMessageBuilder { + public: + SensorMessageBuilder() : builder_(128) { + } + + SerializedMessage build_sensor_message(std::vector &values); + + private: + flatbuffers::FlatBufferBuilder builder_; +}; +} // namespace Flatbuffers #endif //SENSORMESSAGEBUILDER_H diff --git a/components/flatbuffers/include/Variant.h b/components/flatbuffers/include/Variant.h new file mode 100644 index 0000000..e8e51f6 --- /dev/null +++ b/components/flatbuffers/include/Variant.h @@ -0,0 +1,12 @@ +#ifndef VARIANT_H +#define VARIANT_H + +#include // NOLINT + +template struct overloaded : Ts... { + using Ts::operator()...; +}; + +template overloaded(Ts...) -> overloaded; + +#endif // VARIANT_H diff --git a/components/flatbuffers/include/flatbuffers_generated/SensorMessage_generated.h b/components/flatbuffers/include/flatbuffers_generated/SensorMessage_generated.h index d83a7bf..455fa23 100644 --- a/components/flatbuffers/include/flatbuffers_generated/SensorMessage_generated.h +++ b/components/flatbuffers/include/flatbuffers_generated/SensorMessage_generated.h @@ -14,31 +14,37 @@ namespace Messaging { -struct Angle; -struct AngleBuilder; +struct TargetAngle; +struct TargetAngleBuilder; + +struct CurrentAngle; +struct CurrentAngleBuilder; struct SensorMessage; struct SensorMessageBuilder; enum SensorValue : uint8_t { SensorValue_NONE = 0, - SensorValue_Angle = 1, + SensorValue_TargetAngle = 1, + SensorValue_CurrentAngle = 2, SensorValue_MIN = SensorValue_NONE, - SensorValue_MAX = SensorValue_Angle + SensorValue_MAX = SensorValue_CurrentAngle }; -inline const SensorValue (&EnumValuesSensorValue())[2] { - static const SensorValue values[] = {SensorValue_NONE, SensorValue_Angle}; +inline const SensorValue (&EnumValuesSensorValue())[3] { + static const SensorValue values[] = { + SensorValue_NONE, SensorValue_TargetAngle, SensorValue_CurrentAngle}; return values; } inline const char *const *EnumNamesSensorValue() { - static const char *const names[3] = {"NONE", "Angle", nullptr}; + static const char *const names[4] = {"NONE", "TargetAngle", "CurrentAngle", + nullptr}; return names; } inline const char *EnumNameSensorValue(SensorValue e) { - if (::flatbuffers::IsOutRange(e, SensorValue_NONE, SensorValue_Angle)) + if (::flatbuffers::IsOutRange(e, SensorValue_NONE, SensorValue_CurrentAngle)) return ""; const size_t index = static_cast(e); return EnumNamesSensorValue()[index]; @@ -48,8 +54,12 @@ template struct SensorValueTraits { static const SensorValue enum_value = SensorValue_NONE; }; -template <> struct SensorValueTraits { - static const SensorValue enum_value = SensorValue_Angle; +template <> struct SensorValueTraits { + static const SensorValue enum_value = SensorValue_TargetAngle; +}; + +template <> struct SensorValueTraits { + static const SensorValue enum_value = SensorValue_CurrentAngle; }; bool VerifySensorValue(::flatbuffers::Verifier &verifier, const void *obj, @@ -59,8 +69,8 @@ bool VerifySensorValueVector( const ::flatbuffers::Vector<::flatbuffers::Offset> *values, const ::flatbuffers::Vector *types); -struct Angle FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table { - typedef AngleBuilder Builder; +struct TargetAngle FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table { + typedef TargetAngleBuilder Builder; enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE { VT_VALUE = 4 }; @@ -71,26 +81,64 @@ struct Angle FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table { } }; -struct AngleBuilder { - typedef Angle Table; +struct TargetAngleBuilder { + typedef TargetAngle Table; ::flatbuffers::FlatBufferBuilder &fbb_; ::flatbuffers::uoffset_t start_; void add_value(int16_t value) { - fbb_.AddElement(Angle::VT_VALUE, value, 0); + fbb_.AddElement(TargetAngle::VT_VALUE, value, 0); } - explicit AngleBuilder(::flatbuffers::FlatBufferBuilder &_fbb) : fbb_(_fbb) { + explicit TargetAngleBuilder(::flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { start_ = fbb_.StartTable(); } - ::flatbuffers::Offset Finish() { + ::flatbuffers::Offset Finish() { const auto end = fbb_.EndTable(start_); - auto o = ::flatbuffers::Offset(end); + auto o = ::flatbuffers::Offset(end); return o; } }; -inline ::flatbuffers::Offset -CreateAngle(::flatbuffers::FlatBufferBuilder &_fbb, int16_t value = 0) { - AngleBuilder builder_(_fbb); +inline ::flatbuffers::Offset +CreateTargetAngle(::flatbuffers::FlatBufferBuilder &_fbb, int16_t value = 0) { + TargetAngleBuilder builder_(_fbb); + builder_.add_value(value); + return builder_.Finish(); +} + +struct CurrentAngle FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table { + typedef CurrentAngleBuilder Builder; + enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE { + VT_VALUE = 4 + }; + int16_t value() const { return GetField(VT_VALUE, 0); } + bool Verify(::flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyField(verifier, VT_VALUE, 2) && verifier.EndTable(); + } +}; + +struct CurrentAngleBuilder { + typedef CurrentAngle Table; + ::flatbuffers::FlatBufferBuilder &fbb_; + ::flatbuffers::uoffset_t start_; + void add_value(int16_t value) { + fbb_.AddElement(CurrentAngle::VT_VALUE, value, 0); + } + explicit CurrentAngleBuilder(::flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + ::flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = ::flatbuffers::Offset(end); + return o; + } +}; + +inline ::flatbuffers::Offset +CreateCurrentAngle(::flatbuffers::FlatBufferBuilder &_fbb, int16_t value = 0) { + CurrentAngleBuilder builder_(_fbb); builder_.add_value(value); return builder_.Finish(); } @@ -171,8 +219,12 @@ inline bool VerifySensorValue(::flatbuffers::Verifier &verifier, case SensorValue_NONE: { return true; } - case SensorValue_Angle: { - auto ptr = reinterpret_cast(obj); + case SensorValue_TargetAngle: { + auto ptr = reinterpret_cast(obj); + return verifier.VerifyTable(ptr); + } + case SensorValue_CurrentAngle: { + auto ptr = reinterpret_cast(obj); return verifier.VerifyTable(ptr); } default: diff --git a/main/LoopManager.cpp b/main/LoopManager.cpp index 9a56f9f..2771dc2 100644 --- a/main/LoopManager.cpp +++ b/main/LoopManager.cpp @@ -19,14 +19,14 @@ [[noreturn]] void LoopManager::control_loop() const { uint8_t buffer[512]; while (true) { - m_messaging_interface->recv(reinterpret_cast(buffer), 512, PC_ADDR, ACTUATOR_CMD_TAG); + m_messaging_interface->recv(reinterpret_cast(buffer), 512, PC_ADDR, + ACTUATOR_CMD_TAG); m_actuator->actuate(buffer); send_sensor_reading(false); } } - -[[noreturn]] void LoopManager::sensor_loop(char * args) { +[[noreturn]] void LoopManager::sensor_loop(char *args) { const auto that = reinterpret_cast(args); while (true) { @@ -35,12 +35,14 @@ } } -[[noreturn]] void LoopManager::metadata_tx_loop(char * args) { +[[noreturn]] void LoopManager::metadata_tx_loop(char *args) { const auto that = reinterpret_cast(args); const auto topology_message_builder = std::make_unique(); while (true) { - const auto [module_ids, orientations] = that->m_messaging_interface->get_physically_connected_modules(); - // todo: this is awful, we can't cast from a vector of orientation to int.... :( + const auto [module_ids, orientations] = + that->m_messaging_interface->get_physically_connected_modules(); + // todo: this is awful, we can't cast from a vector of orientation to + // int.... :( std::vector casted_orientations{}; casted_orientations.reserve(orientations.size()); for (const auto orientation : orientations) { @@ -48,13 +50,11 @@ } const auto [data, size] = topology_message_builder->build_topology_message( - that->m_config_manager.get_module_id(), - that->m_config_manager.get_module_type(), - module_ids, - casted_orientations, - that->m_messaging_interface->get_connection_type(), + that->m_config_manager.get_module_id(), that->m_config_manager.get_module_type(), + module_ids, casted_orientations, that->m_messaging_interface->get_connection_type(), that->m_messaging_interface->get_leader()); - that->m_messaging_interface->send(static_cast(data), size, PC_ADDR, TOPOLOGY_CMD_TAG, false); + that->m_messaging_interface->send(static_cast(data), size, PC_ADDR, + TOPOLOGY_CMD_TAG, false); vTaskDelay(METADATA_PERIOD_MS / portTICK_PERIOD_MS); } } diff --git a/main/control/DCMotorActuator.cpp b/main/control/DCMotorActuator.cpp index 94edc9d..0195468 100644 --- a/main/control/DCMotorActuator.cpp +++ b/main/control/DCMotorActuator.cpp @@ -1,11 +1,13 @@ #include -#include "control/DCMotorActuator.h" -#include "esp_attr.h" -#include "util/number_utils.h" -#include "driver/ledc.h" -#include "constants/module.h" #include "AngleControlMessageBuilder.h" +#include "SensorMessageBuilder.h" +#include "constants/module.h" +#include "control/DCMotorActuator.h" +#include "driver/ledc.h" +#include "esp_attr.h" +#include "flatbuffers_generated/SensorMessage_generated.h" +#include "util/number_utils.h" #define LOW_DUTY 200 #define HIGH_DUTY 1000 @@ -42,14 +44,14 @@ DCMotorActuator::DCMotorActuator() { ESP_ERROR_CHECK(ledc_channel_config(&fwd_ledc_channel)); ledc_channel_config_t rev_ledc_channel = { - .gpio_num = DC_MOTOR_PWM_REV, - .speed_mode = LEDC_LOW_SPEED_MODE, - .channel = REV_CHANNEL, - .intr_type = LEDC_INTR_DISABLE, - .timer_sel = LEDC_TIMER_0, - .duty = 0, - .hpoint = 0, - }; + .gpio_num = DC_MOTOR_PWM_REV, + .speed_mode = LEDC_LOW_SPEED_MODE, + .channel = REV_CHANNEL, + .intr_type = LEDC_INTR_DISABLE, + .timer_sel = LEDC_TIMER_0, + .duty = 0, + .hpoint = 0, + }; ESP_ERROR_CHECK(ledc_channel_config(&rev_ledc_channel)); @@ -60,7 +62,8 @@ DCMotorActuator::DCMotorActuator() { this->m_integral = 0; this->m_last_error = 0; - xTaskCreate(reinterpret_cast(pid_task), "pid_task", 3072, this, 1, &this->m_pid_task); + xTaskCreate(reinterpret_cast(pid_task), "pid_task", 3072, this, 1, + &this->m_pid_task); } DCMotorActuator::~DCMotorActuator() { @@ -70,7 +73,7 @@ DCMotorActuator::~DCMotorActuator() { volatile int32_t encoder_ticks = 0; volatile int8_t direction = 0; -static void IRAM_ATTR encoder_isr_handler(void* arg) { +static void IRAM_ATTR encoder_isr_handler(void *arg) { const int a = gpio_get_level(static_cast(DC_ENCODER_A)); const int b = gpio_get_level(static_cast(DC_ENCODER_B)); @@ -98,12 +101,13 @@ void DCMotorActuator::setup_encoder() { } void DCMotorActuator::actuate(uint8_t *cmd) { - const auto* angleControlCmd = Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd); + const auto *angleControlCmd = + Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd); this->m_target_angle = angleControlCmd->angle(); } -void DCMotorActuator::pid_task(char* args) { - const auto that = reinterpret_cast(args); +void DCMotorActuator::pid_task(char *args) { + const auto that = reinterpret_cast(args); while (true) { that->m_current_angle = (encoder_ticks * 360.0) / (GEAR_RATIO * TICKS_PER_ROTATION); @@ -119,7 +123,8 @@ void DCMotorActuator::pid_task(char* args) { } else if (control < -1) { control = -1; } - const auto pwm = util::mapRange(std::abs(control), 0, 1, MIN_PWM_DUTY, MAX_PWM_DUTY); + const auto pwm = + util::mapRange(std::abs(control), 0, 1, MIN_PWM_DUTY, MAX_PWM_DUTY); if (std::abs(control) < DEADZONE) { ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL, 0)); @@ -149,7 +154,8 @@ void DCMotorActuator::pid_task(char* args) { } } -std::vector DCMotorActuator::get_sensor_data() { +std::vector DCMotorActuator::get_sensor_data() { // todo: this really needs to return a int32, should also return two sensor data items, one for target one for current - return {{(uint16_t)(m_current_angle)}}; + return {Flatbuffers::target_angle{(int16_t)m_current_angle}, + Flatbuffers::current_angle{(int16_t)m_current_angle}}; } diff --git a/main/control/Servo1Actuator.cpp b/main/control/Servo1Actuator.cpp index acc941a..02ca7f7 100644 --- a/main/control/Servo1Actuator.cpp +++ b/main/control/Servo1Actuator.cpp @@ -4,52 +4,52 @@ #include "control/Servo1Actuator.h" #include "AngleControlMessageBuilder.h" +#include "SensorMessageBuilder.h" #include "constants/module.h" #include "driver/ledc.h" #include "flatbuffers_generated/SensorMessage_generated.h" #include "util/number_utils.h" -#include "SensorMessageBuilder.h" #define LOW_DUTY 200 #define HIGH_DUTY 1000 #define PWM_FREQ 50 // 4khz Servo1Actuator::Servo1Actuator() { - ledc_timer_config_t ledc_timer = { - .speed_mode = LEDC_LOW_SPEED_MODE, - .duty_resolution = LEDC_TIMER_13_BIT, - .timer_num = LEDC_TIMER_0, - .freq_hz = PWM_FREQ, - .clk_cfg = LEDC_AUTO_CLK, - }; - ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer)); + ledc_timer_config_t ledc_timer = { + .speed_mode = LEDC_LOW_SPEED_MODE, + .duty_resolution = LEDC_TIMER_13_BIT, + .timer_num = LEDC_TIMER_0, + .freq_hz = PWM_FREQ, + .clk_cfg = LEDC_AUTO_CLK, + }; + ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer)); - ledc_channel_config_t ledc_channel = { - .gpio_num = SERVO_GPIO, - .speed_mode = LEDC_LOW_SPEED_MODE, - .channel = LEDC_CHANNEL_0, - .intr_type = LEDC_INTR_DISABLE, - .timer_sel = LEDC_TIMER_0, - .duty = (HIGH_DUTY + LOW_DUTY) / 2, // move motor to midpoint initially - .hpoint = 0, - }; + ledc_channel_config_t ledc_channel = { + .gpio_num = SERVO_GPIO, + .speed_mode = LEDC_LOW_SPEED_MODE, + .channel = LEDC_CHANNEL_0, + .intr_type = LEDC_INTR_DISABLE, + .timer_sel = LEDC_TIMER_0, + .duty = (HIGH_DUTY + LOW_DUTY) / 2, // move motor to midpoint initially + .hpoint = 0, + }; - ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel)); + ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel)); } void Servo1Actuator::actuate(uint8_t *cmd) { - const auto *angleControlCmd = - Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd); - const auto newDuty = util::mapRange(angleControlCmd->angle(), 0, 180, - LOW_DUTY, HIGH_DUTY); + const auto *angleControlCmd = + Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd); + const auto newDuty = + util::mapRange(angleControlCmd->angle(), 0, 180, LOW_DUTY, HIGH_DUTY); - m_target = angleControlCmd->angle(); - std::cout << "actuating to " << angleControlCmd->angle() << std::endl; + m_target = angleControlCmd->angle(); + std::cout << "actuating to " << angleControlCmd->angle() << std::endl; - ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, newDuty)); - ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0)); + ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, newDuty)); + ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0)); } -std::vector Servo1Actuator::get_sensor_data() { - return {{m_target}}; +std::vector Servo1Actuator::get_sensor_data() { + return {Flatbuffers::target_angle{(int16_t)m_target}}; } diff --git a/main/include/control/DCMotorActuator.h b/main/include/control/DCMotorActuator.h index 80bc28c..bc2178c 100644 --- a/main/include/control/DCMotorActuator.h +++ b/main/include/control/DCMotorActuator.h @@ -3,19 +3,20 @@ #ifndef DCMOTORACTUATOR_H #define DCMOTORACTUATOR_H +#include "IActuator.h" #include "freertos/FreeRTOS.h" #include "freertos/task.h" -#include "IActuator.h" class DCMotorActuator final : public IActuator { -public: + public: DCMotorActuator(); ~DCMotorActuator() override; void actuate(uint8_t *cmd) override; - std::vector get_sensor_data() override; -private: + std::vector get_sensor_data() override; + + private: void setup_encoder(); - static void pid_task(char* args); + static void pid_task(char *args); double m_current_angle; int64_t m_target_angle; diff --git a/main/include/control/IActuator.h b/main/include/control/IActuator.h index 886a8da..d6ec2c4 100644 --- a/main/include/control/IActuator.h +++ b/main/include/control/IActuator.h @@ -11,10 +11,11 @@ class IActuator { -public: - virtual ~IActuator() {} + public: + virtual ~IActuator() { + } virtual void actuate(uint8_t *cmd) = 0; - virtual std::vector get_sensor_data() = 0; + virtual std::vector get_sensor_data() = 0; }; #endif //IACTUATOR_H diff --git a/main/include/control/Servo1Actuator.h b/main/include/control/Servo1Actuator.h index 4475372..daa5ebe 100644 --- a/main/include/control/Servo1Actuator.h +++ b/main/include/control/Servo1Actuator.h @@ -7,16 +7,17 @@ #ifndef SERVO1ACTUATOR_H #define SERVO1ACTUATOR_H -#include #include "IActuator.h" #include "ISensor.h" +#include class Servo1Actuator final : public IActuator { -public: + public: Servo1Actuator(); void actuate(std::uint8_t *cmd) override; - std::vector get_sensor_data() override; -private: + std::vector get_sensor_data() override; + + private: uint16_t m_target = 90; };