Send robot topology to PC

components/rpc/CommunicationRouter.cpp

@@ -117,3 +117,32 @@ void CommunicationRouter::route(uint8_t* buffer, const size_t length) const {
         this->m_data_link_manager->send(mpi_message->destination(), buffer, length, FrameType::MOTOR_TYPE, 0);
     }
 }
+
+std::pair<std::vector<uint8_t>, std::vector<Orientation>> CommunicationRouter::get_physically_connected_modules() const {
+    std::vector<RIPRow_public> table;
+    table.resize(RIP_MAX_ROUTES);
+    size_t table_size = RIP_MAX_ROUTES * sizeof(RIPRow_public);
+    m_data_link_manager->get_routing_table(table.data(), &table_size);
+
+    std::vector<uint8_t> connected_module_ids;
+    std::vector<Orientation> connected_module_orientations;
+    connected_module_ids.resize(MAX_WIRED_CONNECTIONS);
+    connected_module_orientations.resize(MAX_WIRED_CONNECTIONS);
+
+    for (int i = 0; i < MAX_WIRED_CONNECTIONS; i++) {
+        connected_module_ids[i] = 0; // this is not the PC ID here, marking as nc.
+    }
+
+    for (int i = 0; i < table_size; i++) {
+        if (table[i].info.hops == 1 && table[i].channel < MAX_WIRED_CONNECTIONS) {
+            connected_module_ids[table[i].channel] = table[i].info.board_id;
+        }
+    }
+
+    // todo: get orientation (temporary)
+    for (int i = 0; i < MAX_WIRED_CONNECTIONS; i++) {
+        connected_module_orientations[i] = Orientation_Deg0;
+    }
+
+    return { connected_module_ids, connected_module_orientations };
+}

components/rpc/MessagingInterface.cpp

@@ -64,3 +64,7 @@ void MessagingInterface::checkOrInsertTag(const uint8_t tag) {
     }
     xSemaphoreGive(m_map_semaphore);
 }
+
+std::pair<std::vector<uint8_t>, std::vector<Orientation>> MessagingInterface::get_physically_connected_modules() const {
+    return m_router->get_physically_connected_modules();
+}

components/rpc/include/CommunicationRouter.h

@@ -18,7 +18,9 @@
 #include "constants/tcp.h"
 #include "constants/module.h"
 
-#include"PtrQueue.h"
+#include "flatbuffers_generated/TopologyMessage_generated.h"
+
+#include "PtrQueue.h"
 
 class CommunicationRouter {
 
@@ -44,7 +46,7 @@ public:
 
         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++) {
+        for (int i = 0; i < num_channels; i++) {
             auto *params = new link_layer_thread_params(this, i);
             xTaskCreate(link_layer_thread, "communication_router_rmt", 4096, params, 3, &this->m_link_layer_threads[i]);
         }
@@ -61,13 +63,16 @@ public:
 
     void route(uint8_t *buffer, size_t length) const;
 
+    // pair of <module, mount orientation>
+    std::pair<std::vector<uint8_t>, std::vector<Orientation>> get_physically_connected_modules() const;
+
     // todo: does this really need to be here (so i can access from thread)?
     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;
+    TaskHandle_t m_router_thread = nullptr;
     std::vector<TaskHandle_t> m_link_layer_threads;
-    std::unique_ptr<TCPServer> m_tcp_server;
+    std::unique_ptr<TCPServer> m_tcp_server; // todo: dependency injection
     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;

components/rpc/include/MessagingInterface.h

@@ -24,6 +24,7 @@ public:
     int broadcast(char* buffer, int size, int root, bool durable);
     int recv(char* buffer, int size, int source, int tag);
     int sendrecv(char* send_buffer, int send_size, int dest, int send_tag, char* recv_buffer, int recv_size, int recv_tag);
+    std::pair<std::vector<uint8_t>, std::vector<Orientation>> get_physically_connected_modules() const;
 
 private:
     void handleRecv(const char* recv_buffer, int recv_size);