Sensor data implementation

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<SensorValueInstance>& values) {
+SerializedMessage SensorMessageBuilder::build_sensor_message(std::vector<sensor_value> &values) {
     builder_.Clear();
 
     std::vector<flatbuffers::Offset<void>> values_vec;
     std::vector<uint8_t> sensor_values_vec;
 
-        for (const auto& v : values) {
+    for (const auto &v : values) {
-            values_vec.push_back(Messaging::CreateAngle(builder_, v.angle).Union());
+        std::visit(
-            sensor_values_vec.push_back(Messaging::SensorValue_Angle);
+            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);
+    const auto message =
+        Messaging::CreateSensorMessage(builder_, values_type_fb_vec, values_fb_vec);
 
     builder_.Finish(message);
 
-        return { builder_.GetBufferPointer(), builder_.GetSize() };
+    return {builder_.GetBufferPointer(), builder_.GetSize()};
-    }
 }
+} // namespace Flatbuffers

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{
+struct target_angle {
+    int16_t angle;
+};
+
+struct current_angle {
+    int16_t angle;
+};
+
+typedef std::variant<target_angle, current_angle> sensor_value;
+
+class SensorMessageBuilder {
   public:
-        SensorMessageBuilder() : builder_(128) {}
+    SensorMessageBuilder() : builder_(128) {
+    }
 
-        SerializedMessage build_sensor_message(std::vector<SensorValueInstance>& values);
+    SerializedMessage build_sensor_message(std::vector<sensor_value> &values);
 
   private:
     flatbuffers::FlatBufferBuilder builder_;
-    };
+};
-}
+} // namespace Flatbuffers
 
 #endif //SENSORMESSAGEBUILDER_H

components/flatbuffers/include/Variant.h

@@ -0,0 +1,12 @@
+#ifndef VARIANT_H
+#define VARIANT_H
+
+#include <variant> // NOLINT
+
+template <class... Ts> struct overloaded : Ts... {
+    using Ts::operator()...;
+};
+
+template <class... Ts> overloaded(Ts...) -> overloaded<Ts...>;
+
+#endif // VARIANT_H

components/flatbuffers/include/flatbuffers_generated/SensorMessage_generated.h

@@ -14,31 +14,37 @@
 
 namespace Messaging {
 
-struct Angle;
+struct TargetAngle;
-struct AngleBuilder;
+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] {
+inline const SensorValue (&EnumValuesSensorValue())[3] {
-  static const SensorValue values[] = {SensorValue_NONE, SensorValue_Angle};
+  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<size_t>(e);
   return EnumNamesSensorValue()[index];
@@ -48,8 +54,12 @@ template <typename T> struct SensorValueTraits {
   static const SensorValue enum_value = SensorValue_NONE;
 };
 
-template <> struct SensorValueTraits<Messaging::Angle> {
+template <> struct SensorValueTraits<Messaging::TargetAngle> {
-  static const SensorValue enum_value = SensorValue_Angle;
+  static const SensorValue enum_value = SensorValue_TargetAngle;
+};
+
+template <> struct SensorValueTraits<Messaging::CurrentAngle> {
+  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<void>> *values,
     const ::flatbuffers::Vector<uint8_t> *types);
 
-struct Angle FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
+struct TargetAngle FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
-  typedef AngleBuilder Builder;
+  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 {
+struct TargetAngleBuilder {
-  typedef Angle Table;
+  typedef TargetAngle Table;
   ::flatbuffers::FlatBufferBuilder &fbb_;
   ::flatbuffers::uoffset_t start_;
   void add_value(int16_t value) {
-    fbb_.AddElement<int16_t>(Angle::VT_VALUE, value, 0);
+    fbb_.AddElement<int16_t>(TargetAngle::VT_VALUE, value, 0);
   }
-  explicit AngleBuilder(::flatbuffers::FlatBufferBuilder &_fbb) : fbb_(_fbb) {
+  explicit TargetAngleBuilder(::flatbuffers::FlatBufferBuilder &_fbb)
+      : fbb_(_fbb) {
     start_ = fbb_.StartTable();
   }
-  ::flatbuffers::Offset<Angle> Finish() {
+  ::flatbuffers::Offset<TargetAngle> Finish() {
     const auto end = fbb_.EndTable(start_);
-    auto o = ::flatbuffers::Offset<Angle>(end);
+    auto o = ::flatbuffers::Offset<TargetAngle>(end);
     return o;
   }
 };
 
-inline ::flatbuffers::Offset<Angle>
+inline ::flatbuffers::Offset<TargetAngle>
-CreateAngle(::flatbuffers::FlatBufferBuilder &_fbb, int16_t value = 0) {
+CreateTargetAngle(::flatbuffers::FlatBufferBuilder &_fbb, int16_t value = 0) {
-  AngleBuilder builder_(_fbb);
+  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<int16_t>(VT_VALUE, 0); }
+  bool Verify(::flatbuffers::Verifier &verifier) const {
+    return VerifyTableStart(verifier) &&
+           VerifyField<int16_t>(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<int16_t>(CurrentAngle::VT_VALUE, value, 0);
+  }
+  explicit CurrentAngleBuilder(::flatbuffers::FlatBufferBuilder &_fbb)
+      : fbb_(_fbb) {
+    start_ = fbb_.StartTable();
+  }
+  ::flatbuffers::Offset<CurrentAngle> Finish() {
+    const auto end = fbb_.EndTable(start_);
+    auto o = ::flatbuffers::Offset<CurrentAngle>(end);
+    return o;
+  }
+};
+
+inline ::flatbuffers::Offset<CurrentAngle>
+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: {
+  case SensorValue_TargetAngle: {
-    auto ptr = reinterpret_cast<const Messaging::Angle *>(obj);
+    auto ptr = reinterpret_cast<const Messaging::TargetAngle *>(obj);
+    return verifier.VerifyTable(ptr);
+  }
+  case SensorValue_CurrentAngle: {
+    auto ptr = reinterpret_cast<const Messaging::CurrentAngle *>(obj);
     return verifier.VerifyTable(ptr);
   }
   default:

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<char *>(buffer), 512, PC_ADDR, ACTUATOR_CMD_TAG);
+        m_messaging_interface->recv(reinterpret_cast<char *>(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<LoopManager *>(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<LoopManager *>(args);
     const auto topology_message_builder = std::make_unique<Flatbuffers::TopologyMessageBuilder>();
     while (true) {
-        const auto [module_ids, orientations] =  that->m_messaging_interface->get_physically_connected_modules();
+        const auto [module_ids, orientations] =
-        // todo: this is awful, we can't cast from a vector of orientation to int.... :(
+            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<int8_t> 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_id(), that->m_config_manager.get_module_type(),
-            that->m_config_manager.get_module_type(),
+            module_ids, casted_orientations, that->m_messaging_interface->get_connection_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<char *>(data), size, PC_ADDR, TOPOLOGY_CMD_TAG, false);
+        that->m_messaging_interface->send(static_cast<char *>(data), size, PC_ADDR,
+                                          TOPOLOGY_CMD_TAG, false);
         vTaskDelay(METADATA_PERIOD_MS / portTICK_PERIOD_MS);
     }
 }

main/control/DCMotorActuator.cpp

@@ -1,11 +1,13 @@
 #include <cmath>
 
-#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
@@ -60,7 +62,8 @@ DCMotorActuator::DCMotorActuator() {
     this->m_integral = 0;
     this->m_last_error = 0;
 
-    xTaskCreate(reinterpret_cast<TaskFunction_t>(pid_task), "pid_task", 3072, this, 1, &this->m_pid_task);
+    xTaskCreate(reinterpret_cast<TaskFunction_t>(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<gpio_num_t>(DC_ENCODER_A));
     const int b = gpio_get_level(static_cast<gpio_num_t>(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) {
+void DCMotorActuator::pid_task(char *args) {
-    const auto that = reinterpret_cast<DCMotorActuator*>(args);
+    const auto that = reinterpret_cast<DCMotorActuator *>(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<double>(std::abs(control), 0, 1, MIN_PWM_DUTY, MAX_PWM_DUTY);
+        const auto pwm =
+            util::mapRange<double>(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<Flatbuffers::SensorValueInstance> DCMotorActuator::get_sensor_data() {
+std::vector<Flatbuffers::sensor_value> 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}};
 }

main/control/Servo1Actuator.cpp

@@ -4,11 +4,11 @@
 
 #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
@@ -40,8 +40,8 @@ Servo1Actuator::Servo1Actuator() {
 void Servo1Actuator::actuate(uint8_t *cmd) {
     const auto *angleControlCmd =
         Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd);
-  const auto newDuty = util::mapRange<int32_t>(angleControlCmd->angle(), 0, 180,
+    const auto newDuty =
-                                               LOW_DUTY, HIGH_DUTY);
+        util::mapRange<int32_t>(angleControlCmd->angle(), 0, 180, LOW_DUTY, HIGH_DUTY);
 
     m_target = angleControlCmd->angle();
     std::cout << "actuating to " << angleControlCmd->angle() << std::endl;
@@ -50,6 +50,6 @@ void Servo1Actuator::actuate(uint8_t *cmd) {
     ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0));
 }
 
-std::vector<Flatbuffers::SensorValueInstance> Servo1Actuator::get_sensor_data() {
+std::vector<Flatbuffers::sensor_value> Servo1Actuator::get_sensor_data() {
-    return {{m_target}};
+    return {Flatbuffers::target_angle{(int16_t)m_target}};
 }

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<Flatbuffers::SensorValueInstance> get_sensor_data() override;
+    std::vector<Flatbuffers::sensor_value> get_sensor_data() override;
-private:
+
+  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;

main/include/control/IActuator.h

@@ -11,10 +11,11 @@
 
 
 class IActuator {
-public:
+  public:
-    virtual ~IActuator() {}
+    virtual ~IActuator() {
+    }
     virtual void actuate(uint8_t *cmd) = 0;
-    virtual std::vector<Flatbuffers::SensorValueInstance> get_sensor_data() = 0;
+    virtual std::vector<Flatbuffers::sensor_value> get_sensor_data() = 0;
 };
 
 #endif //IACTUATOR_H

main/include/control/Servo1Actuator.h

@@ -7,16 +7,17 @@
 #ifndef SERVO1ACTUATOR_H
 #define SERVO1ACTUATOR_H
 
-#include <cstdint>
 #include "IActuator.h"
 #include "ISensor.h"
+#include <cstdint>
 
 class Servo1Actuator final : public IActuator {
-public:
+  public:
     Servo1Actuator();
     void actuate(std::uint8_t *cmd) override;
-    std::vector<Flatbuffers::SensorValueInstance> get_sensor_data() override;
+    std::vector<Flatbuffers::sensor_value> get_sensor_data() override;
-private:
+
+  private:
     uint16_t m_target = 90;
 };