Send robot topology to PC

main/LoopManager.cpp

@@ -7,19 +7,39 @@
 #include <iostream>
 #include <memory>
 #include <MessagingInterface.h>
+#include <TopologyMessageBuilder.h>
 
 #include "esp_log.h"
 
 #define ACTUATOR_CMD_TAG 5
+#define TOPOLOGY_CMD_TAG 6
 
-[[noreturn]] void LoopManager::control_loop() {
-    const auto messaging_interface = std::make_unique<MessagingInterface>(std::make_unique<WifiManager>());
+#define METADATA_PERIOD_MS 5000
 
+[[noreturn]] void LoopManager::control_loop() const {
     const auto actuator = ActuatorFactory::create_actuator(ConfigManager::get_module_type());
 
     uint8_t buffer[512];
     while (true) {
-        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);
         actuator->actuate(buffer);
     }
 }
+
+[[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();
+        // 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) {
+            casted_orientations.emplace_back(orientation);
+        }
+
+        const auto [data, size] = topology_message_builder->build_topology_message(ConfigManager::get_module_id(), ConfigManager::get_module_type(), module_ids, casted_orientations);
+        that->m_messaging_interface->send(static_cast<char *>(data), size, PC_ADDR, TOPOLOGY_CMD_TAG, true);
+        vTaskDelay(METADATA_PERIOD_MS / portTICK_PERIOD_MS);
+    }
+}

main/include/LoopManager.h

@@ -5,12 +5,21 @@
 #ifndef LOOPMANAGER_H
 #define LOOPMANAGER_H
 
+#include <memory>
+
+#include <MessagingInterface.h>
+
 #include "control/IActuator.h"
 #include "control/ActuatorFactory.h"
 
 class LoopManager {
 public:
-    [[noreturn]] static void control_loop();
+    LoopManager() : m_messaging_interface(std::make_unique<MessagingInterface>(std::make_unique<WifiManager>())) {}
+    [[noreturn]] void control_loop() const;
+    [[noreturn]] static void metadata_tx_loop(char * args);
+
+private:
+    std::unique_ptr<MessagingInterface> m_messaging_interface;
 
 private:
 

main/main.cpp

@@ -17,5 +17,8 @@ extern "C" [[noreturn]] void app_main(void) {
 
     ConfigManager::init_config();
 
-    LoopManager::control_loop();
+
+    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);
+    loop_manager->control_loop();
 }