Work on sensor

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