Integrate RPC with RMT

components/config/CMakeLists.txt

@@ -1,3 +1,4 @@
 idf_component_register(SRCS "ConfigManager.cpp"
-                       PRIV_REQUIRES constants nvs_flash
+                        PRIV_REQUIRES constants nvs_flash
-                       INCLUDE_DIRS "include")
+                        REQUIRES flatbuffers
+                        INCLUDE_DIRS "include")

components/config/ConfigManager.cpp

@@ -26,6 +26,7 @@ void ConfigManager::init_config() {
     nvs_close(config_handle);
 }
 
+// todo: we should probably cache some of these things
 uint16_t ConfigManager::get_module_id() {
     nvs_handle config_handle;
     nvs_open(NVS_FLASH_NAMESPACE, NVS_READONLY, &config_handle);

components/config/include/ConfigManager.h

@@ -6,6 +6,7 @@
 #define CONFIGMANAGER_H
 
 #include "constants/config.h"
+#include "flatbuffers_generated/RobotModule_generated.h"
 
 class ConfigManager {
 public:

components/constants/CMakeLists.txt

@@ -1 +1 @@
-idf_component_register(INCLUDE_DIRS "include")
+idf_component_register(INCLUDE_DIRS "include" PRIV_REQUIRES "flatbuffers")

components/constants/include/constants/config.h

@@ -12,9 +12,4 @@
 #define MODULE_ID_KEY           "module_id"
 #define MODULE_TYPE_KEY         "module_type"
 
-
-enum ModuleType { splitter }; // todo: maybe this should be flatbuffer
-
-
-
 #endif //CONFIG_H

components/constants/include/constants/module.h

@@ -0,0 +1,16 @@
+//
+// Created by Johnathon Slightham on 2025-07-12.
+//
+
+#ifndef MODULE_H
+#define MODULE_H
+
+#include <unordered_map>
+
+#include "flatbuffers_generated/RobotModule_generated.h"
+
+inline std::unordered_map<int, int> MODULE_TO_NUM_CHANNELS_MAP {{ModuleType_SPLITTER, 4}, {ModuleType_SERVO_1, 2}, {ModuleType_DC_MOTOR, 1}};
+
+#define PC_ADDR 0
+
+#endif //MODULE_H

components/dataLink/DataLinkManager.cpp

@@ -16,11 +16,11 @@ DataLinkManager::DataLinkManager(uint8_t board_id, uint8_t num_channels = MAX_CH
         return;
     }
 
-    if (get_board_id(this_board_id) != ESP_OK){
+    // if (get_board_id(this_board_id) != ESP_OK){
         //failed to read from NVM for board id under key "board". Will write a new entry
         this_board_id = board_id;
         set_board_id(this_board_id);
-    }
+    // }
 
     // if (this_board_id != board_id){
     //     //NVM board id is different from `board_id` -> update entry to the new board id

components/flatbuffers/include/flatbuffers_generated/RobotModule_generated.h

@@ -0,0 +1,188 @@
+// automatically generated by the FlatBuffers compiler, do not modify
+
+
+#ifndef FLATBUFFERS_GENERATED_ROBOTMODULE_H_
+#define FLATBUFFERS_GENERATED_ROBOTMODULE_H_
+
+#include "flatbuffers/flatbuffers.h"
+
+// Ensure the included flatbuffers.h is the same version as when this file was
+// generated, otherwise it may not be compatible.
+// static_assert(FLATBUFFERS_VERSION_MAJOR == 25 &&
+//              FLATBUFFERS_VERSION_MINOR == 2 &&
+//              FLATBUFFERS_VERSION_REVISION == 10,
+//             "Non-compatible flatbuffers version included");
+
+struct RobotModule;
+struct RobotModuleBuilder;
+
+enum ModuleType : int8_t {
+  ModuleType_SPLITTER = 0,
+  ModuleType_SERVO_1 = 1,
+  ModuleType_DC_MOTOR = 2,
+  ModuleType_BATTERY = 3,
+  ModuleType_MIN = ModuleType_SPLITTER,
+  ModuleType_MAX = ModuleType_BATTERY
+};
+
+inline const ModuleType (&EnumValuesModuleType())[4] {
+  static const ModuleType values[] = {
+    ModuleType_SPLITTER,
+    ModuleType_SERVO_1,
+    ModuleType_DC_MOTOR,
+    ModuleType_BATTERY
+  };
+  return values;
+}
+
+inline const char * const *EnumNamesModuleType() {
+  static const char * const names[5] = {
+    "SPLITTER",
+    "SERVO_1",
+    "DC_MOTOR",
+    "BATTERY",
+    nullptr
+  };
+  return names;
+}
+
+inline const char *EnumNameModuleType(ModuleType e) {
+  if (::flatbuffers::IsOutRange(e, ModuleType_SPLITTER, ModuleType_BATTERY)) return "";
+  const size_t index = static_cast<size_t>(e);
+  return EnumNamesModuleType()[index];
+}
+
+struct RobotModule FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
+  typedef RobotModuleBuilder Builder;
+  enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
+    VT_ID = 4,
+    VT_IP = 6,
+    VT_HOSTNAME = 8,
+    VT_MODULE_TYPE = 10,
+    VT_CONNECTED_MODULE_IDS = 12
+  };
+  int32_t id() const {
+    return GetField<int32_t>(VT_ID, 0);
+  }
+  const ::flatbuffers::String *ip() const {
+    return GetPointer<const ::flatbuffers::String *>(VT_IP);
+  }
+  const ::flatbuffers::String *hostname() const {
+    return GetPointer<const ::flatbuffers::String *>(VT_HOSTNAME);
+  }
+  ModuleType module_type() const {
+    return static_cast<ModuleType>(GetField<int8_t>(VT_MODULE_TYPE, 0));
+  }
+  const ::flatbuffers::Vector<int32_t> *connected_module_ids() const {
+    return GetPointer<const ::flatbuffers::Vector<int32_t> *>(VT_CONNECTED_MODULE_IDS);
+  }
+  bool Verify(::flatbuffers::Verifier &verifier) const {
+    return VerifyTableStart(verifier) &&
+           VerifyField<int32_t>(verifier, VT_ID, 4) &&
+           VerifyOffset(verifier, VT_IP) &&
+           verifier.VerifyString(ip()) &&
+           VerifyOffset(verifier, VT_HOSTNAME) &&
+           verifier.VerifyString(hostname()) &&
+           VerifyField<int8_t>(verifier, VT_MODULE_TYPE, 1) &&
+           VerifyOffset(verifier, VT_CONNECTED_MODULE_IDS) &&
+           verifier.VerifyVector(connected_module_ids()) &&
+           verifier.EndTable();
+  }
+};
+
+struct RobotModuleBuilder {
+  typedef RobotModule Table;
+  ::flatbuffers::FlatBufferBuilder &fbb_;
+  ::flatbuffers::uoffset_t start_;
+  void add_id(int32_t id) {
+    fbb_.AddElement<int32_t>(RobotModule::VT_ID, id, 0);
+  }
+  void add_ip(::flatbuffers::Offset<::flatbuffers::String> ip) {
+    fbb_.AddOffset(RobotModule::VT_IP, ip);
+  }
+  void add_hostname(::flatbuffers::Offset<::flatbuffers::String> hostname) {
+    fbb_.AddOffset(RobotModule::VT_HOSTNAME, hostname);
+  }
+  void add_module_type(ModuleType module_type) {
+    fbb_.AddElement<int8_t>(RobotModule::VT_MODULE_TYPE, static_cast<int8_t>(module_type), 0);
+  }
+  void add_connected_module_ids(::flatbuffers::Offset<::flatbuffers::Vector<int32_t>> connected_module_ids) {
+    fbb_.AddOffset(RobotModule::VT_CONNECTED_MODULE_IDS, connected_module_ids);
+  }
+  explicit RobotModuleBuilder(::flatbuffers::FlatBufferBuilder &_fbb)
+        : fbb_(_fbb) {
+    start_ = fbb_.StartTable();
+  }
+  ::flatbuffers::Offset<RobotModule> Finish() {
+    const auto end = fbb_.EndTable(start_);
+    auto o = ::flatbuffers::Offset<RobotModule>(end);
+    return o;
+  }
+};
+
+inline ::flatbuffers::Offset<RobotModule> CreateRobotModule(
+    ::flatbuffers::FlatBufferBuilder &_fbb,
+    int32_t id = 0,
+    ::flatbuffers::Offset<::flatbuffers::String> ip = 0,
+    ::flatbuffers::Offset<::flatbuffers::String> hostname = 0,
+    ModuleType module_type = ModuleType_SPLITTER,
+    ::flatbuffers::Offset<::flatbuffers::Vector<int32_t>> connected_module_ids = 0) {
+  RobotModuleBuilder builder_(_fbb);
+  builder_.add_connected_module_ids(connected_module_ids);
+  builder_.add_hostname(hostname);
+  builder_.add_ip(ip);
+  builder_.add_id(id);
+  builder_.add_module_type(module_type);
+  return builder_.Finish();
+}
+
+inline ::flatbuffers::Offset<RobotModule> CreateRobotModuleDirect(
+    ::flatbuffers::FlatBufferBuilder &_fbb,
+    int32_t id = 0,
+    const char *ip = nullptr,
+    const char *hostname = nullptr,
+    ModuleType module_type = ModuleType_SPLITTER,
+    const std::vector<int32_t> *connected_module_ids = nullptr) {
+  auto ip__ = ip ? _fbb.CreateString(ip) : 0;
+  auto hostname__ = hostname ? _fbb.CreateString(hostname) : 0;
+  auto connected_module_ids__ = connected_module_ids ? _fbb.CreateVector<int32_t>(*connected_module_ids) : 0;
+  return CreateRobotModule(
+      _fbb,
+      id,
+      ip__,
+      hostname__,
+      module_type,
+      connected_module_ids__);
+}
+
+inline const RobotModule *GetRobotModule(const void *buf) {
+  return ::flatbuffers::GetRoot<RobotModule>(buf);
+}
+
+inline const RobotModule *GetSizePrefixedRobotModule(const void *buf) {
+  return ::flatbuffers::GetSizePrefixedRoot<RobotModule>(buf);
+}
+
+inline bool VerifyRobotModuleBuffer(
+    ::flatbuffers::Verifier &verifier) {
+  return verifier.VerifyBuffer<RobotModule>(nullptr);
+}
+
+inline bool VerifySizePrefixedRobotModuleBuffer(
+    ::flatbuffers::Verifier &verifier) {
+  return verifier.VerifySizePrefixedBuffer<RobotModule>(nullptr);
+}
+
+inline void FinishRobotModuleBuffer(
+    ::flatbuffers::FlatBufferBuilder &fbb,
+    ::flatbuffers::Offset<RobotModule> root) {
+  fbb.Finish(root);
+}
+
+inline void FinishSizePrefixedRobotModuleBuffer(
+    ::flatbuffers::FlatBufferBuilder &fbb,
+    ::flatbuffers::Offset<RobotModule> root) {
+  fbb.FinishSizePrefixed(root);
+}
+
+#endif  // FLATBUFFERS_GENERATED_ROBOTMODULE_H_

components/ptrQueue/CMakeLists.txt

@@ -0,0 +1 @@
+idf_component_register(INCLUDE_DIRS "include")

components/ptrQueue/include/PtrQueue.h

@@ -0,0 +1,43 @@
+//
+// Created by Johnathon Slightham on 2025-07-12.
+//
+
+#ifndef PTRQUEUE_H
+#define PTRQUEUE_H
+
+#include <memory>
+#include <iostream>
+
+// Wrapped FreeRTOS queue to support unique_ptr
+
+template <typename T>
+class PtrQueue {
+public:
+    explicit PtrQueue(UBaseType_t queueLength)
+        : queue(xQueueCreate(queueLength, sizeof(T*))) {}
+
+    ~PtrQueue() {
+        if (queue) {
+            vQueueDelete(queue);
+        }
+    }
+
+    bool enqueue(std::unique_ptr<T>&& item, const TickType_t timeout = portMAX_DELAY) {
+        T* rawPtr = item.release();  // Release ownership and get raw pointer
+        return xQueueSendToBack(queue, &rawPtr, timeout) == pdPASS;
+    }
+
+    std::unique_ptr<T> dequeue(const TickType_t timeout = portMAX_DELAY) {
+        T* rawPtr = nullptr;
+        if (xQueueReceive(queue, &rawPtr, timeout) == pdPASS) {
+            std::unique_ptr<T> ptr(rawPtr);
+            return ptr;
+        }
+        return nullptr;
+    }
+
+private:
+    QueueHandle_t queue;
+};
+
+#endif //PTRQUEUE_H

components/rpc/CMakeLists.txt

@@ -1,3 +1,4 @@
 idf_component_register(SRCS "WifiManager.cpp" "mDNSDiscoveryService.cpp" "TCPServer.cpp" "CommunicationRouter.cpp" "MessagingInterface.cpp"
-                       PRIV_REQUIRES driver esp_event nvs_flash esp_netif esp_wifi espressif__mdns constants config flatbuffers
+                        PRIV_REQUIRES driver esp_event nvs_flash esp_netif esp_wifi espressif__mdns constants config flatbuffers dataLink rmt
-                       INCLUDE_DIRS "include")
+                        REQUIRES ptrQueue
+                        INCLUDE_DIRS "include")

components/rpc/CommunicationRouter.cpp

@@ -1,26 +1,115 @@
 #include "CommunicationRouter.h"
 
 #include <iostream>
+#include "mDNSDiscoveryService.h"
+#include "MPIMessageBuilder.h"
+#include "WifiManager.h"
+
 #include "freertos/FreeRTOS.h"
 #include "freertos/queue.h"
 
+#include "Tables.h"
+
+#include "PtrQueue.h"
+
 CommunicationRouter::~CommunicationRouter() {
     vTaskDelete(m_router_thread);
-    vQueueDelete(m_tcp_rx_queue);
 }
 
+// todo: we really need to change all char to uint8_t everywhere
+// todo: get rid of copying going on, need to pass around sharedptrs/uniqueptrs
+
+// todo: this needs to be combined with the 4 rmt threads
 [[noreturn]] void CommunicationRouter::router_thread(void *args) {
     const auto that = static_cast<CommunicationRouter *>(args);
 
-    // todo: change to queue set
-    char buffer[512];
     while (true) {
-        xQueueReceive(that->m_tcp_rx_queue, buffer, portMAX_DELAY);
+        // todo: strange to have this here, but i dont want 4 threads calling it or a thread just for it.
-        that->m_rx_callback(buffer, 512);
+        if (std::chrono::system_clock::now() - that->m_last_leader_updated > std::chrono::seconds(15)) {
-        std::cout << "callback" << std::endl;
+            that->update_leader();
+        }
+
+        const auto buffer = that->m_tcp_rx_queue->dequeue();
+
+        std::cout << "dequeued buffer" << std::endl;
+
+        that->m_rx_callback(reinterpret_cast<char *>(buffer->data()), buffer->size());
+        std::cout << "WiFi callback" << std::endl;
+    }
+}
+
+[[noreturn]] void CommunicationRouter::link_layer_thread(void *args) {
+    const auto* params = static_cast<link_layer_thread_params *>(args);
+    const auto that = params->router;
+    const auto channel = params->channel;
+    delete params;
+
+    char buffer[512];
+    size_t bytes_received = 0;
+    that->m_data_link_manager->start_receive_frames(channel);
+    while (true) {
+        // todo: very c style function calls
+        const auto err = that->m_data_link_manager->receive(reinterpret_cast<uint8_t *>(buffer), 512, &bytes_received, channel);
+        that->m_data_link_manager->start_receive_frames(channel);
+
+        if (ESP_OK != err) {
+            continue;
+        }
+
+        that->route(reinterpret_cast<uint8_t *>(buffer), bytes_received);
+
+        std::cout << "RMT callback" << std::endl;
     }
 }
 
 int CommunicationRouter::send_msg(char* buffer, const size_t length) const {
-    return this->m_tcp_server->send_msg(buffer, length);
+    route(reinterpret_cast<uint8_t *>(buffer), length);
+    return 0;
+}
+
+// todo: the number of things this is doing in so many different places is crazy...
+void CommunicationRouter::update_leader() {
+    RIPRow_public table[RIP_MAX_ROUTES];
+    size_t table_size = RIP_MAX_ROUTES;
+    this->m_data_link_manager->get_routing_table(table, &table_size);
+
+    // Leader election (just get the highest id in rip)
+    std::vector<int> connected_module_ids;
+    uint8_t max = m_module_id;
+    for (int i = 0; i < table_size; i++) {
+        const auto id = table[i].info.board_id;
+        connected_module_ids.emplace_back(id);
+        if (max > id) {
+            max = id;
+        }
+    }
+
+    // Leader has changed, we may need to change PC connection state
+    if (this->m_leader != max) {
+        if (max == m_module_id) {
+            m_pc_connection->connect();
+        } else if (this->m_leader == m_module_id) {
+            m_pc_connection->disconnect();
+        }
+    }
+
+    this->m_leader = max;
+
+    if (this->m_leader == m_module_id) {
+        mDNSDiscoveryService::set_connected_boards(connected_module_ids);
+    }
+
+    this->m_last_leader_updated = std::chrono::system_clock::now();
+}
+
+void CommunicationRouter::route(uint8_t* buffer, const size_t length) const {
+    const auto& mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer);
+
+    if (mpi_message->destination() == m_module_id) {
+        this->m_rx_callback(reinterpret_cast<char *>(buffer), 512);
+    } else if (mpi_message->destination() == PC_ADDR && this->m_leader == m_module_id) {
+        this->m_tcp_server->send_msg(reinterpret_cast<char *>(buffer), 512);
+    } else {
+        this->m_data_link_manager->send(mpi_message->destination(), buffer, length, FrameType::MOTOR_TYPE, 0);
+    }
 }

components/rpc/TCPServer.cpp

@@ -13,7 +13,9 @@
 
 #include "TCPServer.h"
 
+#include <memory>
 #include <bits/shared_ptr_base.h>
+#include <constants/app_comms.h>
 
 #include "constants/tcp.h"
 
@@ -21,7 +23,7 @@
 //       - authenticate (don't just return true from the auth function)
 //       - tx from board
 
-TCPServer::TCPServer(const int port, const QueueHandle_t rx_queue) {
+TCPServer::TCPServer(const int port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue) {
     this->m_port = port;
     this->m_mutex = xSemaphoreCreateMutex();
     this->m_clients = std::unordered_set<int>();
@@ -143,9 +145,10 @@ TCPServer::~TCPServer() {
             for (int sock : that->m_clients) {
                 if (FD_ISSET(sock, &readfds)) {
                     // Handle socket
-                    char buffer[512];
+                    auto buffer = std::make_unique<std::vector<uint8_t>>();
-                    int len = recv(sock, buffer, sizeof(buffer) - 1, 0); // temp: for the null terminator
+                    buffer->resize(MAX_RX_BUFFER_SIZE);
 
+                    int len = recv(sock, buffer->data(), MAX_RX_BUFFER_SIZE, 0); // temp: for the null terminator
                     if (len < 0) {
                         printf("Error occurred during receiving: errno %d\n", errno);
                         to_remove.emplace_back(sock);
@@ -155,7 +158,8 @@ TCPServer::~TCPServer() {
                         to_remove.emplace_back(sock);
                     } else {
                         printf("TCP Server Received %d bytes\n", len);
-                        xQueueSendToBack(that->m_rx_queue, buffer, 0);
+                        buffer->resize(len);
+                        that->m_rx_queue->enqueue(std::move(buffer));
                     }
                 }
             }

components/rpc/WifiManager.cpp

@@ -4,6 +4,7 @@
 #include <esp_event.h>
 #include <freertos/semphr.h>
 #include <cstring>
+#include <mDNSDiscoveryService.h>
 
 #include "freertos/FreeRTOS.h"
 #include "freertos/semphr.h"
@@ -188,6 +189,7 @@ void WifiManager::wifi_event_handler(void *event_handler_arg, esp_event_base_t e
     } else if (WIFI_EVENT_STA_CONNECTED == event_id) {
         printf("Connected to wifi in station mode\n");
         that->update_state(wifi_state::connected);
+        mDNSDiscoveryService::setup();
     } else if (WIFI_EVENT_STA_DISCONNECTED == event_id) {
         printf("Station mode shutdown\n");
         xSemaphoreTake(that->m_mutex, portMAX_DELAY);
@@ -204,5 +206,8 @@ void WifiManager::wifi_event_handler(void *event_handler_arg, esp_event_base_t e
         printf("User connected to AP\n");
     } else if (WIFI_EVENT_AP_STADISCONNECTED == event_id) {
         printf("User disconnected from AP\n");
+    } else if (WIFI_EVENT_AP_START == event_id) {
+        mDNSDiscoveryService::setup();
+        printf("AP started\n");
     }
 }

components/rpc/include/CommunicationRouter.h

@@ -5,39 +5,74 @@
 #ifndef COMMUNICATIONROUTER_H
 #define COMMUNICATIONROUTER_H
 
+#include <ConfigManager.h>
 #include <functional>
 #include <memory>
+#include <chrono>
+#include <WifiManager.h>
 
 #include "freertos/FreeRTOS.h"
-#include "freertos/queue.h"
 
 #include "TCPServer.h"
+#include "DataLinkManager.h"
 #include "constants/tcp.h"
-#include "constants/app_comms.h"
+#include "constants/module.h"
+
+#include"PtrQueue.h"
 
 class CommunicationRouter {
+
+    class link_layer_thread_params {
+    public:
+        link_layer_thread_params(CommunicationRouter* router, const uint8_t channel) : router(router), channel(channel) {};
+        CommunicationRouter *router;
+        uint8_t channel;
+    };
+
 public:
-    explicit CommunicationRouter(const std::function<void(char*, int)> &rx_callback)
+    CommunicationRouter(const std::function<void(char*, int)> &rx_callback, std::unique_ptr<WifiManager>&& pc_connection)
-        : m_tcp_rx_queue(xQueueCreate(RX_QUEUE_SIZE, MAX_RX_BUFFER_SIZE)),
+        : m_tcp_rx_queue(std::make_shared<PtrQueue<std::vector<uint8_t>>>(10)),
             m_rx_callback(rx_callback),
-            m_tcp_server(std::make_unique<TCPServer>(TCP_PORT, m_tcp_rx_queue)) {
+            m_tcp_server(std::make_unique<TCPServer>(TCP_PORT, m_tcp_rx_queue)),
+            m_data_link_manager(std::make_unique<DataLinkManager>(ConfigManager::get_module_id(), MODULE_TO_NUM_CHANNELS_MAP[ConfigManager::get_module_type()])),
+            m_pc_connection(std::move(pc_connection)),
+            m_module_id(ConfigManager::get_module_id()),
+            m_last_leader_updated(std::chrono::system_clock::now()){
+        update_leader();
+
         xTaskCreate(router_thread, "communication_router", 2048, this, 3, &this->m_router_thread);
+
+        const auto num_channels = MODULE_TO_NUM_CHANNELS_MAP[ConfigManager::get_module_type()];
+        this->m_link_layer_threads.resize(num_channels);
+        for (uint8_t i = 0; i < num_channels; i++) {
+            auto *params = new link_layer_thread_params(this, i);
+            xTaskCreate(link_layer_thread, "communication_router_rmt", 3096, params, 3, &this->m_link_layer_threads[i]);
+        }
     }
 
     ~CommunicationRouter();
 
     [[noreturn]] static void router_thread(void *args);
+    [[noreturn]] static void link_layer_thread(void *args);
 
     int send_msg(char* buffer, size_t length) const;
 
+    void update_leader();
+
+    void route(uint8_t *buffer, size_t length) const;
+
     // todo: does this really need to be here (so i can access from thread)?
-    QueueHandle_t m_tcp_rx_queue;
+    std::shared_ptr<PtrQueue<std::vector<uint8_t>>> m_tcp_rx_queue;
     std::function<void(char*, int)> m_rx_callback;
 private:
-
     TaskHandle_t m_router_thread;
+    std::vector<TaskHandle_t> m_link_layer_threads;
     std::unique_ptr<TCPServer> m_tcp_server;
-
+    std::unique_ptr<DataLinkManager> m_data_link_manager;
+    std::unique_ptr<WifiManager> m_pc_connection; // todo: change to dependency inject
+    uint8_t m_leader = 0;
+    uint8_t m_module_id;
+    std::chrono::time_point<std::chrono::system_clock> m_last_leader_updated;
 };
 
 #endif //COMMUNICATIONROUTER_H

components/rpc/include/MessagingInterface.h

@@ -7,14 +7,15 @@
 
 #include <memory>
 #include <unordered_map>
+#include <constants/app_comms.h>
 
 #include "CommunicationRouter.h"
 
 class MessagingInterface {
 public:
-    MessagingInterface()
+    explicit MessagingInterface(std::unique_ptr<WifiManager>&& pc_connection)
         : m_mpi_rx_queue(xQueueCreate(MAX_RX_BUFFER_SIZE, RX_QUEUE_SIZE)),
-            m_router(std::make_unique<CommunicationRouter>([this](char* buffer, const int size) { handleRecv(buffer, size); })),
+            m_router(std::make_unique<CommunicationRouter>([this](char* buffer, const int size) { handleRecv(buffer, size); }, std::move(pc_connection))),
             m_map_semaphore(xSemaphoreCreateMutex()) {};
 
     ~MessagingInterface();

components/rpc/include/TCPServer.h

@@ -5,6 +5,7 @@
 #ifndef TCPSERVER_H
 #define TCPSERVER_H
 
+#include <memory>
 #include <vector>
 #include <unordered_set>
 
@@ -12,9 +13,11 @@
 
 #include "IRPCServer.h"
 
+#include "PtrQueue.h"
+
 class TCPServer : IRPCServer {
 public:
-    TCPServer(int port, QueueHandle_t rx_queue);
+    TCPServer(int port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue);
     ~TCPServer();
     int send_msg(char* buffer, size_t length) const;
 
@@ -31,7 +34,7 @@ private:
     TaskHandle_t m_task;
     TaskHandle_t m_rx_task;
 
-    QueueHandle_t m_rx_queue;
+    std::shared_ptr<PtrQueue<std::vector<uint8_t>>> m_rx_queue;
 
     SemaphoreHandle_t m_mutex;
     std::unordered_set<int> m_clients;

components/rpc/include/mDNSDiscoveryService.h

@@ -5,12 +5,15 @@
 #ifndef DISCOVERYSERVICE_H
 #define DISCOVERYSERVICE_H
 
+#include <vector>
+
 class mDNSDiscoveryService final {
 public:
     mDNSDiscoveryService() = delete;
     ~mDNSDiscoveryService() = delete;
 
     static void setup();
+    static void set_connected_boards(const std::vector<int>& boards);
 };
 
 #endif //DISCOVERYSERVICE_H

components/rpc/mDNSDiscoveryService.cpp

@@ -12,6 +12,7 @@
 
 #include <string>
 #include <format>
+#include <sstream>
 
 // todo: clean this up (strange to be a constructor) also need to add more details, need to add to routing table
 void mDNSDiscoveryService::setup() {
@@ -25,8 +26,26 @@ void mDNSDiscoveryService::setup() {
     mdns_txt_item_t service_txt_data[3] = {
         {"module_id",std::to_string(ConfigManager::get_module_id()).c_str()},
         {"module_type",std::to_string(ConfigManager::get_module_type()).c_str()},
-           {"connected_modules","2,3,4,5,6,7,8,9"},
+           {"connected_modules",""},
     };
 
     mdns_service_txt_set("_robotcontrol", "_tcp", service_txt_data, 3);
 }
+
+void mDNSDiscoveryService::set_connected_boards(const std::vector<int>& boards) {
+    std::stringstream ss;
+
+    for (int i = 0; i < boards.size(); i++) {
+        ss << boards[i];
+        if (i < boards.size() - 1) {
+            ss << ',';
+        }
+    }
+
+    mdns_txt_item_t service_txt_data[3] = {
+        {"module_id",std::to_string(ConfigManager::get_module_id()).c_str()},
+        {"module_type",std::to_string(ConfigManager::get_module_type()).c_str()},
+           {"connected_modules",ss.str().c_str()},
+    };
+    mdns_service_txt_set("_robotcontrol", "_tcp", service_txt_data, 3);
+}

main/LoopManager.cpp

@@ -11,7 +11,7 @@
 #include "esp_log.h"
 
 [[noreturn]] void LoopManager::control_loop() {
-    const auto messaging_interface = std::make_unique<MessagingInterface>();
+    const auto messaging_interface = std::make_unique<MessagingInterface>(std::make_unique<WifiManager>());
 
     char buffer[512];
     while (true) {
@@ -20,7 +20,7 @@
 
 
         char s[21] = {'H', 'e', 'l', 'l', 'o', ' ', 'f', 'r', 'o', 'm', ' ', 't', 'h', 'e', ' ', 'B', 'O', 'A', 'R', 'D', '!' };
-        messaging_interface->send(s, 21, 1, 2, true);
+        messaging_interface->send(s, 21, 5, 2, true);
 
         ESP_LOGI("MEM", "Free internal RAM: %d", heap_caps_get_free_size(MALLOC_CAP_8BIT));
         ESP_LOGI("MEM", "Free PSRAM: %d", heap_caps_get_free_size(MALLOC_CAP_SPIRAM));

main/link_layer_main.cpp

@@ -185,29 +185,7 @@ void multi_transceiver(void* arg) {
         iteration++;
         if (iteration == 10){
             iteration = 0;
-            // if (!receive_only){
-            //     matrix_size = RIP_MAX_ROUTES;
-            //     res = obj->get_network_toplogy(matrix, &matrix_size);
-            //     if (res != ESP_OK){
-            //         ESP_LOGE("multi", "Failed to get topology");
-            //     } else {
-            //         for (int i = 0; i < matrix_size; i++){
-            //             printf("Table for board %d:\n", matrix[i].board_id);
-            //             printf("board_id\t\tHops\t\tChannel\n");
-            //             for (int j = 0; j < matrix[i].size; j++){
-            //                 printf("%d\t\t%d\t\t%d\n", matrix[i].table[j].info.board_id, matrix[i].table[j].info.hops, matrix[i].table[j].channel);
-            //             }
-            //             printf("=====\n");
 
-            //             //reset matrix
-            //             matrix[i].size = RIP_MAX_ROUTES;
-            //         }
-            //     }
-            // }
-        }
-        
-        // vTaskDelay(1000 /    portTICK_PERIOD_MS); // wait 1 second before trying to send again
-        //reset temp buffers
         memset(recv_buf, 0, DATA_SIZE_TEST);
         memset(send_buf, 0, DATA_SIZE_TEST);
         

main/main.cpp

@@ -1,17 +1,13 @@
-//mdns and other stuff main file
 #include <cstdio>
 #include <memory>
 
 #include "sdkconfig.h"
 #include "freertos/FreeRTOS.h"
-#include "freertos/task.h"
-#include "freertos/queue.h"
 
 #include "WifiManager.h"
 #include "mDNSDiscoveryService.h"
 #include "TCPServer.h"
 #include "ConfigManager.h"
-#include "constants/tcp.h"
 #include "LoopManager.h"
 #include "esp_log.h"
 
@@ -20,11 +16,5 @@ extern "C" [[noreturn]] void app_main(void) {
     ESP_LOGI("MEM", "Free PSRAM: %d", heap_caps_get_free_size(MALLOC_CAP_SPIRAM));
 
     ConfigManager::init_config();
-
-    const auto manager = std::make_unique<WifiManager>();
-    manager->connect();
-
-    mDNSDiscoveryService::setup();
-
     LoopManager::control_loop();
 }