Work on sensor

components/dataLink/DataLinkScheduler.cpp

@@ -272,7 +272,7 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
                 }
             } else {
                 //Done fragmenting, can free data array
-                ESP_LOGI(DEBUG_LINK_TAG, "finished fragmenting seq num %d frag_info 0x%X", frame.header.seq_num, frame.header.frag_info);
+                // ESP_LOGI(DEBUG_LINK_TAG, "finished fragmenting seq num %d frag_info 0x%X", frame.header.seq_num, frame.header.frag_info);
                 vPortFree(frame.data);
             }
 

components/flatbuffers/CMakeLists.txt

@@ -1,2 +1,2 @@
-idf_component_register(SRCS "MPIMessageBuilder.cpp" "AngleControlMessageBuilder.cpp" "TopologyMessageBuilder.cpp"
+idf_component_register(SRCS "MPIMessageBuilder.cpp" "AngleControlMessageBuilder.cpp" "TopologyMessageBuilder.cpp" "SensorMessageBuilder.cpp"
         INCLUDE_DIRS "include")

components/flatbuffers/SensorMessageBuilder.cpp

@@ -0,0 +1,31 @@
+//
+// Created by Johnathon Slightham on 2025-06-30.
+//
+
+#include "SensorMessageBuilder.h"
+#include "SerializedMessage.h"
+#include "flatbuffers_generated/SensorMessage_generated.h"
+
+namespace Flatbuffers {
+
+    SerializedMessage SensorMessageBuilder::build_sensor_message(std::vector<SensorValueInstance>& values) {
+        builder_.Clear();
+
+        std::vector<flatbuffers::Offset<void>> values_vec;
+        std::vector<uint8_t> 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() };
+    }
+}

components/flatbuffers/include/SensorMessageBuilder.h

@@ -0,0 +1,24 @@
+
+#ifndef SENSORMESSAGEBUILDER_H
+#define SENSORMESSAGEBUILDER_H
+
+#include "SerializedMessage.h"
+#include "flatbuffers_generated/SensorMessage_generated.h"
+
+namespace Flatbuffers {
+    struct SensorValueInstance {
+        uint16_t angle; // todo: change to a variant
+    };
+
+    class SensorMessageBuilder{
+    public:
+        SensorMessageBuilder() : builder_(128) {}
+
+        SerializedMessage build_sensor_message(std::vector<SensorValueInstance>& values);
+
+    private:
+        flatbuffers::FlatBufferBuilder builder_;
+    };
+}
+
+#endif //SENSORMESSAGEBUILDER_H

components/flatbuffers/include/SerializedMessage.h

@@ -5,10 +5,12 @@
 #ifndef SERIALIZEDMESSAGE_H
 #define SERIALIZEDMESSAGE_H
 
+#include <cstdint>
+
 namespace Flatbuffers {
     struct SerializedMessage {
         void* data;
-        size_t size;
+        std::size_t size;
     };
 }
 

components/rpc/CommunicationRouter.cpp

@@ -35,9 +35,8 @@ CommunicationRouter::~CommunicationRouter() { vTaskDelete(m_router_thread); }
 
   while (true) {
     if (std::chrono::system_clock::now() - that->m_last_leader_updated >
-        std::chrono::seconds(15)) {
+        std::chrono::seconds(2)) {
       that->m_last_leader_updated = std::chrono::system_clock::now();
-      ESP_LOGI(TAG, "Updating leader");
       that->update_leader();
     }
 
@@ -88,6 +87,7 @@ void CommunicationRouter::update_leader() {
 
   // Leader has changed, we may need to change PC connection state
   if (this->m_leader != max) {
+    ESP_LOGI(TAG, "Leader has changed from %d to %d", this->m_leader, max);
     if (max == m_module_id) {
       m_pc_connection->connect();
       m_lossless_server->startup();

main/LoopManager.cpp

@@ -2,35 +2,37 @@
 // Created by Johnathon Slightham on 2025-07-05.
 //
 
-
-#include <iostream>
 #include <memory>
 
 #include "LoopManager.h"
-#include "MessagingInterface.h"
+#include "SensorMessageBuilder.h"
 #include "TopologyMessageBuilder.h"
-#include "control/ActuatorFactory.h"
-#include "esp_log.h"
 
 #define ACTUATOR_CMD_TAG 5
 #define TOPOLOGY_CMD_TAG 6
 #define METADATA_RX_TAG 7
+#define SENSOR_TAG 8
 
 #define METADATA_PERIOD_MS 1000
+#define SENSOR_DATA_PERIOD_MS 1000
 
 [[noreturn]] void LoopManager::control_loop() const {
-    const auto actuator = ActuatorFactory::create_actuator(m_config_manager.get_module_type()); // todo: this needs to be moved higher up with one factory that returns shared ptr for both actuator and sensor.
-
     uint8_t buffer[512];
     while (true) {
         m_messaging_interface->recv(reinterpret_cast<char *>(buffer), 512, PC_ADDR, ACTUATOR_CMD_TAG);
-        actuator->actuate(buffer);
+        m_actuator->actuate(buffer);
+        send_sensor_reading(false);
     }
 }
 
 
-[[noreturn]] void LoopManager::sensor_loop() const {
+[[noreturn]] void LoopManager::sensor_loop(char * args) {
-    // todo: impl
+    const auto that = reinterpret_cast<LoopManager *>(args);
+
+    while (true) {
+        that->send_sensor_reading(true);
+        vTaskDelay(SENSOR_DATA_PERIOD_MS / portTICK_PERIOD_MS);
+    }
 }
 
 [[noreturn]] void LoopManager::metadata_tx_loop(char * args) {
@@ -63,7 +65,13 @@
     buffer->resize(512);
     while (true) {
         that->m_messaging_interface->recv(buffer->data(), 512, PC_ADDR, METADATA_RX_TAG);
-
-
     }
 }
+
+void LoopManager::send_sensor_reading(bool durable) const {
+    Flatbuffers::SensorMessageBuilder smb{};
+    // todo: get data from sensor
+    auto data = m_actuator->get_sensor_data();
+    const auto [ptr, size] = smb.build_sensor_message(data);
+    m_messaging_interface->send(reinterpret_cast<char *>(ptr), size, PC_ADDR, SENSOR_TAG, durable);
+}

main/control/DCMotorActuator.cpp

@@ -1,3 +1,5 @@
+#include <cmath>
+
 #include "control/DCMotorActuator.h"
 #include "esp_attr.h"
 #include "util/number_utils.h"
@@ -104,9 +106,9 @@ void DCMotorActuator::pid_task(char* args) {
     const auto that = reinterpret_cast<DCMotorActuator*>(args);
 
     while (true) {
-        const double degrees = (encoder_ticks * 360.0) / (GEAR_RATIO * TICKS_PER_ROTATION);
+        that->m_current_angle = (encoder_ticks * 360.0) / (GEAR_RATIO * TICKS_PER_ROTATION);
 
-        const double error = degrees - that->m_target_angle;
+        const double error = that->m_current_angle - that->m_target_angle;
         that->m_integral += error * KI;
         const double detivative = (error - that->m_last_error) * KD;
         that->m_last_error = error;
@@ -146,3 +148,8 @@ void DCMotorActuator::pid_task(char* args) {
         vTaskDelay(75 / portTICK_PERIOD_MS);
     }
 }
+
+std::vector<Flatbuffers::SensorValueInstance> 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)}};
+}

main/control/Servo1Actuator.cpp

@@ -6,7 +6,9 @@
 #include "AngleControlMessageBuilder.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
@@ -41,8 +43,13 @@ void Servo1Actuator::actuate(uint8_t *cmd) {
   const auto newDuty = util::mapRange<int32_t>(angleControlCmd->angle(), 0, 180,
                                                LOW_DUTY, HIGH_DUTY);
 
+  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));
 }
+
+std::vector<Flatbuffers::SensorValueInstance> Servo1Actuator::get_sensor_data() {
+    return {{m_target}};
+}

main/include/LoopManager.h

@@ -8,19 +8,25 @@
 #include <memory>
 
 #include "MessagingInterface.h"
+#include "control/ActuatorFactory.h"
+#include "control/IActuator.h"
 
 class LoopManager {
 public:
     LoopManager() : m_config_manager(ConfigManager::get_instance()),
-        m_messaging_interface(std::make_unique<MessagingInterface>()) {}
+        m_messaging_interface(std::make_unique<MessagingInterface>()),
+       m_actuator(ActuatorFactory::create_actuator(m_config_manager.get_module_type())) {}
     [[noreturn]] void control_loop() const;                     // gets control commands
-    [[noreturn]] void sensor_loop() const;                      // sends sensor data commands continually
+    [[noreturn]] static void sensor_loop(char * args);          // sends sensor data commands continually
     [[noreturn]] static void metadata_tx_loop(char * args);     // sends metadata continually (low duty cycle)
     [[noreturn]] static void metadata_rx_loop(char * args);     // gets other commands from PC (ie. f/w updates, nvs updates)
 
 private:
     ConfigManager& m_config_manager;
     std::unique_ptr<MessagingInterface> m_messaging_interface;
+    std::unique_ptr<IActuator> m_actuator;
+
+    void send_sensor_reading(bool durable) const;
 };
 
 #endif //LOOPMANAGER_H

main/include/control/DCMotorActuator.h

@@ -12,10 +12,12 @@ public:
     DCMotorActuator();
     ~DCMotorActuator() override;
     void actuate(uint8_t *cmd) override;
+    std::vector<Flatbuffers::SensorValueInstance> get_sensor_data() override;
 private:
     void setup_encoder();
     static void pid_task(char* args);
 
+    double m_current_angle;
     int64_t m_target_angle;
     TaskHandle_t m_pid_task;
 

main/include/control/IActuator.h

@@ -5,10 +5,16 @@
 #ifndef IACTUATOR_H
 #define IACTUATOR_H
 
+#include <vector>
+
+#include "SensorMessageBuilder.h"
+
+
 class IActuator {
 public:
     virtual ~IActuator() {}
     virtual void actuate(uint8_t *cmd) = 0;
+    virtual std::vector<Flatbuffers::SensorValueInstance> get_sensor_data() = 0;
 };
 
 #endif //IACTUATOR_H

main/include/control/Servo1Actuator.h

@@ -15,6 +15,9 @@ class Servo1Actuator final : public IActuator {
 public:
     Servo1Actuator();
     void actuate(std::uint8_t *cmd) override;
+    std::vector<Flatbuffers::SensorValueInstance> get_sensor_data() override;
+private:
+    uint16_t m_target = 90;
 };
 
 #endif //SERVO1ACTUATOR_H

main/main.cpp

@@ -18,6 +18,8 @@ extern "C" [[noreturn]] void app_main(void) {
     const auto loop_manager = std::make_unique<LoopManager>();
     xTaskCreate(reinterpret_cast<TaskFunction_t>(LoopManager::metadata_tx_loop),
         "metadata_tx", 3096, loop_manager.get(), 3, nullptr);
+    xTaskCreate(reinterpret_cast<TaskFunction_t>(LoopManager::sensor_loop),
+            "sensor_tx", 3096, loop_manager.get(), 3, nullptr);
     loop_manager->control_loop();
 }
 #endif