Work on sensor

components/dataLink/DataLinkScheduler.cpp

@@ -21,11 +21,11 @@ void DataLinkManager::init_scheduler(){
 
 /**
  * @brief Schedules which frame to send
- * 
- * Scheduler: 
+ *
+ * Scheduler:
  * - All frames will be pushed to the back onto a queue
  * - When a generic frame sends a chunk, it will be pushed back to the queue for the next chunk to be sent
- * 
+ *
  * Scheduling may change (above scheduler will lead to starvation of control frames depending on the number of generic frames/fragments to send)
  */
 [[noreturn]] void DataLinkManager::frame_scheduler(void* args){
@@ -41,17 +41,17 @@ void DataLinkManager::init_scheduler(){
             link_layer_obj->scheduler_send(i);
         }
         vTaskDelay(pdMS_TO_TICKS(SCHEDULER_PERIOD_MS));
-        
+
     }
     vTaskDelete(nullptr);
 }
 
 /**
  * @brief Pushes a frame to the scheduler
- * 
- * @param frame 
- * @param channel 
- * @return esp_err_t 
+ *
+ * @param frame
+ * @param channel
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::push_frame_to_scheduler(SchedulerMetadata frame, uint8_t channel){
     if (frame.data == nullptr){
@@ -77,7 +77,7 @@ esp_err_t DataLinkManager::push_frame_to_scheduler(SchedulerMetadata frame, uint
         ESP_LOGE(DEBUG_LINK_TAG, "Invalid scheduler queue handle");
         return ESP_FAIL;
     }
-    
+
     if (xSemaphoreTake(sq_handle[channel], pdMS_TO_TICKS(SCHEDULER_MUTEX_WAIT)) == pdTRUE){
         frame_queue[channel].push(frame);
         xSemaphoreGive(sq_handle[channel]);
@@ -93,9 +93,9 @@ esp_err_t DataLinkManager::push_frame_to_scheduler(SchedulerMetadata frame, uint
 }
 
 /**
- * @brief Scheduler sending the actual frame at the top of the heap on a channel 
- * 
- * @return esp_err_t 
+ * @brief Scheduler sending the actual frame at the top of the heap on a channel
+ *
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
     if (phys_comms == nullptr){
@@ -106,9 +106,9 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
     if (sq_handle[channel] == nullptr){
         return ESP_FAIL;
     }
-    
+
     SchedulerMetadata frame;
-    
+
     if (xSemaphoreTake(sq_handle[channel], pdMS_TO_TICKS(SCHEDULER_MUTEX_WAIT)) == pdTRUE){
         if (frame_queue[channel].empty()){
             xSemaphoreGive(sq_handle[channel]);
@@ -144,7 +144,7 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
     if (isControlFrame){
         //control frame
 
-        res = create_control_frame(frame.data, frame.len, 
+        res = create_control_frame(frame.data, frame.len,
             make_control_frame_from_header(frame.header), send_data, &frame_size);
 
         vPortFree(frame.data);
@@ -160,7 +160,7 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
         //generic frame
         if (frame.len > (MAX_GENERIC_DATA_LEN)){
             //fragment here
-            
+
             if (frame.timeout == 0){
                 frame.timeout = GENERIC_FRAME_MIN_TIMEOUT;
             } else {
@@ -169,7 +169,7 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
                 if (res != ESP_OK){
                     ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment");
                     vPortFree(frame.data);
-                    return res; 
+                    return res;
                 }
                 return ESP_OK;
             }
@@ -235,16 +235,16 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
 
             // ESP_LOGI(DEBUG_LINK_TAG, "frame %d curr offset %d\n", frame.header.seq_num, curr_offset);
             // ESP_LOGI(DEBUG_LINK_TAG, "frame %d fragment size %d\n", frame.header.seq_num, fragment_size);
-            
+
             frame.header.frag_info = (frame.header.frag_info & 0xFFFF0000) | frame.curr_fragment; //increment frag_num
             //create fragment
-            res = create_generic_frame(frame.data, fragment_size, 
+            res = create_generic_frame(frame.data, fragment_size,
                 make_generic_frame_from_header(frame.header), curr_offset, send_data, &frame_size);
 
             if (res != ESP_OK){
                 ESP_LOGE(DEBUG_LINK_TAG, "Failed to create generic frame fragment");
                 vPortFree(frame.data);
-                return res; 
+                return res;
             }
 
             res = scheduler_send_rmt(channel, frame, send_data, frame_size, true);
@@ -255,12 +255,12 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
                 if (res != ESP_OK){
                     ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment");
                     vPortFree(frame.data);
-                }  
-                return res; 
+                }
+                return res;
             }
 
             //need to schedule the next fragment (if total_frags != frag_num)
-            if ((frame.header.frag_info >> 16) > (frame.header.frag_info & 0xFF) || (frame.last_ack != (frame.header.frag_info >> 16) && 
+            if ((frame.header.frag_info >> 16) > (frame.header.frag_info & 0xFF) || (frame.last_ack != (frame.header.frag_info >> 16) &&
             static_cast<FrameType>(GET_TYPE(frame.header.type_flag)) != FrameType::MISC_UDP_GENERIC_TYPE)){
                 // frame.generic_frame_data_offset += fragment_size;
                 // ESP_LOGI(DEBUG_LINK_TAG, "scheduling frame %d with frag_info 0x%X", frame.header.seq_num, frame.header.frag_info);
@@ -268,20 +268,20 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
                 if (res != ESP_OK){
                     ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment");
                     vPortFree(frame.data);
-                    return res; 
-                }                
+                    return res;
+                }
             } 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);
             }
 
         } else {
             //no fragmenting
-            res = create_generic_frame(frame.data, frame.len, 
+            res = create_generic_frame(frame.data, frame.len,
                 make_generic_frame_from_header(frame.header), 0, send_data, &frame_size);
             vPortFree(frame.data);
-            
+
             if (res != ESP_OK){
                 ESP_LOGE(DEBUG_LINK_TAG, "Failed to create generic frame");
                 return res;
@@ -331,22 +331,22 @@ esp_err_t DataLinkManager::scheduler_send_rmt(uint8_t channel, SchedulerMetadata
 
 /**
  * @brief Increases the head of the sliding window associated with the board id and sequence number
- * 
- * @param channel 
+ *
+ * @param channel
  * @param board_id Receiving Board ID (the board who ACK'd)
- * @param seq_num 
- * @param ack_record 
- * @return esp_err_t 
+ * @param seq_num
+ * @param ack_record
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::inc_head_sliding_window(uint8_t channel, uint8_t board_id, uint16_t seq_num, FrameAckRecord* ack_record){
     if (ack_record == NULL){
         return ESP_ERR_INVALID_ARG;
     }
 
-    if (ack_record->total_frags == 0 || ack_record->total_frags > MAX_GENERIC_NUM_FRAG 
+    if (ack_record->total_frags == 0 || ack_record->total_frags > MAX_GENERIC_NUM_FRAG
         || ack_record->last_ack == 0 || ack_record->total_frags < ack_record->last_ack){
         return ESP_ERR_INVALID_ARG;
-    }   
+    }
 
     if (sliding_window_mutex[channel] == NULL){
         return ESP_ERR_INVALID_STATE;
@@ -362,7 +362,7 @@ esp_err_t DataLinkManager::inc_head_sliding_window(uint8_t channel, uint8_t boar
         xSemaphoreGive(sliding_window_mutex[channel]);
         return ESP_ERR_INVALID_ARG;
     }
-    
+
     record.last_ack = ack_record->last_ack;
     if (record.total_frags == 0){
         record.total_frags = ack_record->total_frags;
@@ -375,12 +375,12 @@ esp_err_t DataLinkManager::inc_head_sliding_window(uint8_t channel, uint8_t boar
 
 /**
  * @brief Gets the current record associated with the board id and sequence number from the sliding window
- * 
- * @param channel 
+ *
+ * @param channel
  * @param board_id Receiving Board ID (the board who ACK'd)
- * @param seq_num 
- * @param ack_record 
- * @return esp_err_t 
+ * @param seq_num
+ * @param ack_record
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::get_record_sliding_window(uint8_t channel, uint8_t board_id, uint16_t seq_num, FrameAckRecord* ack_record){
     if (ack_record == NULL){
@@ -415,11 +415,11 @@ esp_err_t DataLinkManager::get_record_sliding_window(uint8_t channel, uint8_t bo
 
 /**
  * @brief Removes the board id + sequence number record fromt the sliding window (map)
- * 
- * @param channel 
+ *
+ * @param channel
  * @param board_id Receiving Board ID (the board who ACK'd)
- * @param seq_num 
- * @return esp_err_t 
+ * @param seq_num
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::complete_record_sliding_window(uint8_t channel, uint8_t board_id, uint16_t seq_num){
     if (sliding_window_mutex[channel] == NULL){
@@ -441,4 +441,4 @@ esp_err_t DataLinkManager::complete_record_sliding_window(uint8_t channel, uint8
     xSemaphoreGive(sliding_window_mutex[channel]);
 
     return ESP_OK;
-}
+}

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 {
-    // todo: impl
+[[noreturn]] void LoopManager::sensor_loop(char * args) {
+    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
+#endif