From 9bad7c4159fa201e70430b2b654e2d06352d1b51 Mon Sep 17 00:00:00 2001 From: Johnathon Slightham Date: Sat, 26 Jul 2025 10:55:09 -0400 Subject: [PATCH] Send robot topology to PC --- .../constants/include/constants/module.h | 2 + components/flatbuffers/CMakeLists.txt | 2 +- .../flatbuffers/TopologyMessageBuilder.cpp | 37 +++ .../include/TopologyMessageBuilder.h | 33 +++ .../flatbuffers/include/flatbuffers/README.md | 4 + .../RobotModule_generated.h | 237 +++++++++++++----- .../TopologyMessage_generated.h | 156 ++++++++++++ components/rpc/CommunicationRouter.cpp | 29 +++ components/rpc/MessagingInterface.cpp | 4 + components/rpc/include/CommunicationRouter.h | 13 +- components/rpc/include/MessagingInterface.h | 1 + main/LoopManager.cpp | 26 +- main/include/LoopManager.h | 11 +- main/main.cpp | 5 +- 14 files changed, 493 insertions(+), 67 deletions(-) create mode 100644 components/flatbuffers/TopologyMessageBuilder.cpp create mode 100644 components/flatbuffers/include/TopologyMessageBuilder.h create mode 100644 components/flatbuffers/include/flatbuffers/README.md create mode 100644 components/flatbuffers/include/flatbuffers_generated/TopologyMessage_generated.h diff --git a/components/constants/include/constants/module.h b/components/constants/include/constants/module.h index 3ad49a6..e6a79b6 100644 --- a/components/constants/include/constants/module.h +++ b/components/constants/include/constants/module.h @@ -13,6 +13,8 @@ inline std::unordered_map MODULE_TO_NUM_CHANNELS_MAP {{ModuleType_SPLI #define PC_ADDR 0 +#define MAX_WIRED_CONNECTIONS 4 + #define SERVO_GPIO 1 #define DC_MOTOR_PWM_FWD 1 diff --git a/components/flatbuffers/CMakeLists.txt b/components/flatbuffers/CMakeLists.txt index 7dfcbca..a47fe49 100644 --- a/components/flatbuffers/CMakeLists.txt +++ b/components/flatbuffers/CMakeLists.txt @@ -1,2 +1,2 @@ -idf_component_register(SRCS "MPIMessageBuilder.cpp" "AngleControlMessageBuilder.cpp" +idf_component_register(SRCS "MPIMessageBuilder.cpp" "AngleControlMessageBuilder.cpp" "TopologyMessageBuilder.cpp" INCLUDE_DIRS "include") diff --git a/components/flatbuffers/TopologyMessageBuilder.cpp b/components/flatbuffers/TopologyMessageBuilder.cpp new file mode 100644 index 0000000..dc7cbb3 --- /dev/null +++ b/components/flatbuffers/TopologyMessageBuilder.cpp @@ -0,0 +1,37 @@ +// +// Created by Johnathon Slightham on 2025-06-30. +// + +#include "TopologyMessageBuilder.h" + +#include "SerializedMessage.h" + +namespace Flatbuffers { + SerializedMessage TopologyMessageBuilder::build_topology_message( + const uint8_t module_id, + const ModuleType module_type, + const std::vector& channel_to_module, + const std::vector& orientation_to_module) { + builder_.Clear(); + + const auto orientation_to_module_vector = builder_.CreateVector(orientation_to_module); + const auto channel_to_module_vector = builder_.CreateVector(channel_to_module); + + const auto message = Messaging::CreateTopologyMessage( + builder_, + module_id, + module_type, + channel_to_module.size(), + channel_to_module_vector, + orientation_to_module_vector + ); + + builder_.Finish(message); + + return {builder_.GetBufferPointer(), builder_.GetSize()}; + } + + const Messaging::TopologyMessage* TopologyMessageBuilder::parse_topology_message(const uint8_t* buffer) { + return flatbuffers::GetRoot(buffer); + } +} diff --git a/components/flatbuffers/include/TopologyMessageBuilder.h b/components/flatbuffers/include/TopologyMessageBuilder.h new file mode 100644 index 0000000..e0aeae6 --- /dev/null +++ b/components/flatbuffers/include/TopologyMessageBuilder.h @@ -0,0 +1,33 @@ +// +// Created by Johnathon Slightham on 2025-06-30. +// + +#ifndef TOPOLOGYMESSAGEBUILDER_H +#define TOPOLOGYMESSAGEBUILDER_H + +#include +#include + +#include "SerializedMessage.h" +#include "flatbuffers_generated/TopologyMessage_generated.h" +#include "flatbuffers/flatbuffers.h" + +namespace Flatbuffers { + class TopologyMessageBuilder { + public: + TopologyMessageBuilder() : builder_(1024) {} + + SerializedMessage build_topology_message( + uint8_t module_id, + ModuleType module_type, + const std::vector& channel_to_module, + const std::vector& orientation_to_module); + + static const Messaging::TopologyMessage* parse_topology_message(const uint8_t* buffer); + + private: + flatbuffers::FlatBufferBuilder builder_; + }; +} + +#endif //TOPOLOGYMESSAGEBUILDER_H diff --git a/components/flatbuffers/include/flatbuffers/README.md b/components/flatbuffers/include/flatbuffers/README.md new file mode 100644 index 0000000..444927c --- /dev/null +++ b/components/flatbuffers/include/flatbuffers/README.md @@ -0,0 +1,4 @@ +# Flatbuffers +This folder contains the [Flatbuffer library code](https://github.com/google/flatbuffers/tree/master/include/flatbuffers). +It should never be modified, except when we want to bump the version of Flatbuffers we are using. + diff --git a/components/flatbuffers/include/flatbuffers_generated/RobotModule_generated.h b/components/flatbuffers/include/flatbuffers_generated/RobotModule_generated.h index 7dc8222..012ce86 100644 --- a/components/flatbuffers/include/flatbuffers_generated/RobotModule_generated.h +++ b/components/flatbuffers/include/flatbuffers_generated/RobotModule_generated.h @@ -8,11 +8,14 @@ // 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 && +//static_assert(FLATBUFFERS_VERSION_MAJOR == 25 && // FLATBUFFERS_VERSION_MINOR == 2 && // FLATBUFFERS_VERSION_REVISION == 10, // "Non-compatible flatbuffers version included"); +struct MotorState; +struct MotorStateBuilder; + struct RobotModule; struct RobotModuleBuilder; @@ -52,62 +55,178 @@ inline const char *EnumNameModuleType(ModuleType e) { return EnumNamesModuleType()[index]; } +enum Orientation : int8_t { + Orientation_Deg0 = 0, + Orientation_Deg90 = 1, + Orientation_Deg180 = 2, + Orientation_Deg270 = 3, + Orientation_MIN = Orientation_Deg0, + Orientation_MAX = Orientation_Deg270 +}; + +inline const Orientation (&EnumValuesOrientation())[4] { + static const Orientation values[] = { + Orientation_Deg0, + Orientation_Deg90, + Orientation_Deg180, + Orientation_Deg270 + }; + return values; +} + +inline const char * const *EnumNamesOrientation() { + static const char * const names[5] = { + "Deg0", + "Deg90", + "Deg180", + "Deg270", + nullptr + }; + return names; +} + +inline const char *EnumNameOrientation(Orientation e) { + if (::flatbuffers::IsOutRange(e, Orientation_Deg0, Orientation_Deg270)) return ""; + const size_t index = static_cast(e); + return EnumNamesOrientation()[index]; +} + +enum ModuleState : uint8_t { + ModuleState_NONE = 0, + ModuleState_MotorState = 1, + ModuleState_MIN = ModuleState_NONE, + ModuleState_MAX = ModuleState_MotorState +}; + +inline const ModuleState (&EnumValuesModuleState())[2] { + static const ModuleState values[] = { + ModuleState_NONE, + ModuleState_MotorState + }; + return values; +} + +inline const char * const *EnumNamesModuleState() { + static const char * const names[3] = { + "NONE", + "MotorState", + nullptr + }; + return names; +} + +inline const char *EnumNameModuleState(ModuleState e) { + if (::flatbuffers::IsOutRange(e, ModuleState_NONE, ModuleState_MotorState)) return ""; + const size_t index = static_cast(e); + return EnumNamesModuleState()[index]; +} + +template struct ModuleStateTraits { + static const ModuleState enum_value = ModuleState_NONE; +}; + +template<> struct ModuleStateTraits { + static const ModuleState enum_value = ModuleState_MotorState; +}; + +bool VerifyModuleState(::flatbuffers::Verifier &verifier, const void *obj, ModuleState type); +bool VerifyModuleStateVector(::flatbuffers::Verifier &verifier, const ::flatbuffers::Vector<::flatbuffers::Offset> *values, const ::flatbuffers::Vector *types); + +struct MotorState FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table { + typedef MotorStateBuilder Builder; + enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE { + VT_ANGLE = 4 + }; + int32_t angle() const { + return GetField(VT_ANGLE, 0); + } + bool Verify(::flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyField(verifier, VT_ANGLE, 4) && + verifier.EndTable(); + } +}; + +struct MotorStateBuilder { + typedef MotorState Table; + ::flatbuffers::FlatBufferBuilder &fbb_; + ::flatbuffers::uoffset_t start_; + void add_angle(int32_t angle) { + fbb_.AddElement(MotorState::VT_ANGLE, angle, 0); + } + explicit MotorStateBuilder(::flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + ::flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = ::flatbuffers::Offset(end); + return o; + } +}; + +inline ::flatbuffers::Offset CreateMotorState( + ::flatbuffers::FlatBufferBuilder &_fbb, + int32_t angle = 0) { + MotorStateBuilder builder_(_fbb); + builder_.add_angle(angle); + return builder_.Finish(); +} + 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 + VT_MODULE_TYPE = 6, + VT_CONFIGURATION_TYPE = 8, + VT_CONFIGURATION = 10 }; - int32_t id() const { - return GetField(VT_ID, 0); - } - const ::flatbuffers::String *ip() const { - return GetPointer(VT_IP); - } - const ::flatbuffers::String *hostname() const { - return GetPointer(VT_HOSTNAME); + uint8_t id() const { + return GetField(VT_ID, 0); } ModuleType module_type() const { return static_cast(GetField(VT_MODULE_TYPE, 0)); } - const ::flatbuffers::Vector *connected_module_ids() const { - return GetPointer *>(VT_CONNECTED_MODULE_IDS); + ModuleState configuration_type() const { + return static_cast(GetField(VT_CONFIGURATION_TYPE, 0)); + } + const void *configuration() const { + return GetPointer(VT_CONFIGURATION); + } + template const T *configuration_as() const; + const MotorState *configuration_as_MotorState() const { + return configuration_type() == ModuleState_MotorState ? static_cast(configuration()) : nullptr; } bool Verify(::flatbuffers::Verifier &verifier) const { return VerifyTableStart(verifier) && - VerifyField(verifier, VT_ID, 4) && - VerifyOffset(verifier, VT_IP) && - verifier.VerifyString(ip()) && - VerifyOffset(verifier, VT_HOSTNAME) && - verifier.VerifyString(hostname()) && + VerifyField(verifier, VT_ID, 1) && VerifyField(verifier, VT_MODULE_TYPE, 1) && - VerifyOffset(verifier, VT_CONNECTED_MODULE_IDS) && - verifier.VerifyVector(connected_module_ids()) && + VerifyField(verifier, VT_CONFIGURATION_TYPE, 1) && + VerifyOffset(verifier, VT_CONFIGURATION) && + VerifyModuleState(verifier, configuration(), configuration_type()) && verifier.EndTable(); } }; +template<> inline const MotorState *RobotModule::configuration_as() const { + return configuration_as_MotorState(); +} + struct RobotModuleBuilder { typedef RobotModule Table; ::flatbuffers::FlatBufferBuilder &fbb_; ::flatbuffers::uoffset_t start_; - void add_id(int32_t id) { - fbb_.AddElement(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_id(uint8_t id) { + fbb_.AddElement(RobotModule::VT_ID, id, 0); } void add_module_type(ModuleType module_type) { fbb_.AddElement(RobotModule::VT_MODULE_TYPE, static_cast(module_type), 0); } - void add_connected_module_ids(::flatbuffers::Offset<::flatbuffers::Vector> connected_module_ids) { - fbb_.AddOffset(RobotModule::VT_CONNECTED_MODULE_IDS, connected_module_ids); + void add_configuration_type(ModuleState configuration_type) { + fbb_.AddElement(RobotModule::VT_CONFIGURATION_TYPE, static_cast(configuration_type), 0); + } + void add_configuration(::flatbuffers::Offset configuration) { + fbb_.AddOffset(RobotModule::VT_CONFIGURATION, configuration); } explicit RobotModuleBuilder(::flatbuffers::FlatBufferBuilder &_fbb) : fbb_(_fbb) { @@ -122,37 +241,41 @@ struct RobotModuleBuilder { inline ::flatbuffers::Offset CreateRobotModule( ::flatbuffers::FlatBufferBuilder &_fbb, - int32_t id = 0, - ::flatbuffers::Offset<::flatbuffers::String> ip = 0, - ::flatbuffers::Offset<::flatbuffers::String> hostname = 0, + uint8_t id = 0, ModuleType module_type = ModuleType_SPLITTER, - ::flatbuffers::Offset<::flatbuffers::Vector> connected_module_ids = 0) { + ModuleState configuration_type = ModuleState_NONE, + ::flatbuffers::Offset configuration = 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_configuration(configuration); + builder_.add_configuration_type(configuration_type); builder_.add_module_type(module_type); + builder_.add_id(id); return builder_.Finish(); } -inline ::flatbuffers::Offset CreateRobotModuleDirect( - ::flatbuffers::FlatBufferBuilder &_fbb, - int32_t id = 0, - const char *ip = nullptr, - const char *hostname = nullptr, - ModuleType module_type = ModuleType_SPLITTER, - const std::vector *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(*connected_module_ids) : 0; - return CreateRobotModule( - _fbb, - id, - ip__, - hostname__, - module_type, - connected_module_ids__); +inline bool VerifyModuleState(::flatbuffers::Verifier &verifier, const void *obj, ModuleState type) { + switch (type) { + case ModuleState_NONE: { + return true; + } + case ModuleState_MotorState: { + auto ptr = reinterpret_cast(obj); + return verifier.VerifyTable(ptr); + } + default: return true; + } +} + +inline bool VerifyModuleStateVector(::flatbuffers::Verifier &verifier, const ::flatbuffers::Vector<::flatbuffers::Offset> *values, const ::flatbuffers::Vector *types) { + if (!values || !types) return !values && !types; + if (values->size() != types->size()) return false; + for (::flatbuffers::uoffset_t i = 0; i < values->size(); ++i) { + if (!VerifyModuleState( + verifier, values->Get(i), types->GetEnum(i))) { + return false; + } + } + return true; } inline const RobotModule *GetRobotModule(const void *buf) { diff --git a/components/flatbuffers/include/flatbuffers_generated/TopologyMessage_generated.h b/components/flatbuffers/include/flatbuffers_generated/TopologyMessage_generated.h new file mode 100644 index 0000000..85870a7 --- /dev/null +++ b/components/flatbuffers/include/flatbuffers_generated/TopologyMessage_generated.h @@ -0,0 +1,156 @@ +// automatically generated by the FlatBuffers compiler, do not modify + + +#ifndef FLATBUFFERS_GENERATED_TOPOLOGYMESSAGE_MESSAGING_H_ +#define FLATBUFFERS_GENERATED_TOPOLOGYMESSAGE_MESSAGING_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"); + +#include "RobotModule_generated.h" + +namespace Messaging { + +struct TopologyMessage; +struct TopologyMessageBuilder; + +struct TopologyMessage FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table { + typedef TopologyMessageBuilder Builder; + enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE { + VT_MODULE_ID = 4, + VT_MODULE_TYPE = 6, + VT_NUM_CHANNELS = 8, + VT_CHANNEL_TO_MODULE = 10, + VT_CHANNEL_TO_ORIENTATION = 12 + }; + uint8_t module_id() const { + return GetField(VT_MODULE_ID, 0); + } + ModuleType module_type() const { + return static_cast(GetField(VT_MODULE_TYPE, 0)); + } + uint8_t num_channels() const { + return GetField(VT_NUM_CHANNELS, 0); + } + const ::flatbuffers::Vector *channel_to_module() const { + return GetPointer *>(VT_CHANNEL_TO_MODULE); + } + const ::flatbuffers::Vector *channel_to_orientation() const { + return GetPointer *>(VT_CHANNEL_TO_ORIENTATION); + } + bool Verify(::flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyField(verifier, VT_MODULE_ID, 1) && + VerifyField(verifier, VT_MODULE_TYPE, 1) && + VerifyField(verifier, VT_NUM_CHANNELS, 1) && + VerifyOffset(verifier, VT_CHANNEL_TO_MODULE) && + verifier.VerifyVector(channel_to_module()) && + VerifyOffset(verifier, VT_CHANNEL_TO_ORIENTATION) && + verifier.VerifyVector(channel_to_orientation()) && + verifier.EndTable(); + } +}; + +struct TopologyMessageBuilder { + typedef TopologyMessage Table; + ::flatbuffers::FlatBufferBuilder &fbb_; + ::flatbuffers::uoffset_t start_; + void add_module_id(uint8_t module_id) { + fbb_.AddElement(TopologyMessage::VT_MODULE_ID, module_id, 0); + } + void add_module_type(ModuleType module_type) { + fbb_.AddElement(TopologyMessage::VT_MODULE_TYPE, static_cast(module_type), 0); + } + void add_num_channels(uint8_t num_channels) { + fbb_.AddElement(TopologyMessage::VT_NUM_CHANNELS, num_channels, 0); + } + void add_channel_to_module(::flatbuffers::Offset<::flatbuffers::Vector> channel_to_module) { + fbb_.AddOffset(TopologyMessage::VT_CHANNEL_TO_MODULE, channel_to_module); + } + void add_channel_to_orientation(::flatbuffers::Offset<::flatbuffers::Vector> channel_to_orientation) { + fbb_.AddOffset(TopologyMessage::VT_CHANNEL_TO_ORIENTATION, channel_to_orientation); + } + explicit TopologyMessageBuilder(::flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + ::flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = ::flatbuffers::Offset(end); + return o; + } +}; + +inline ::flatbuffers::Offset CreateTopologyMessage( + ::flatbuffers::FlatBufferBuilder &_fbb, + uint8_t module_id = 0, + ModuleType module_type = ModuleType_SPLITTER, + uint8_t num_channels = 0, + ::flatbuffers::Offset<::flatbuffers::Vector> channel_to_module = 0, + ::flatbuffers::Offset<::flatbuffers::Vector> channel_to_orientation = 0) { + TopologyMessageBuilder builder_(_fbb); + builder_.add_channel_to_orientation(channel_to_orientation); + builder_.add_channel_to_module(channel_to_module); + builder_.add_num_channels(num_channels); + builder_.add_module_type(module_type); + builder_.add_module_id(module_id); + return builder_.Finish(); +} + +inline ::flatbuffers::Offset CreateTopologyMessageDirect( + ::flatbuffers::FlatBufferBuilder &_fbb, + uint8_t module_id = 0, + ModuleType module_type = ModuleType_SPLITTER, + uint8_t num_channels = 0, + const std::vector *channel_to_module = nullptr, + const std::vector *channel_to_orientation = nullptr) { + auto channel_to_module__ = channel_to_module ? _fbb.CreateVector(*channel_to_module) : 0; + auto channel_to_orientation__ = channel_to_orientation ? _fbb.CreateVector(*channel_to_orientation) : 0; + return Messaging::CreateTopologyMessage( + _fbb, + module_id, + module_type, + num_channels, + channel_to_module__, + channel_to_orientation__); +} + +inline const Messaging::TopologyMessage *GetTopologyMessage(const void *buf) { + return ::flatbuffers::GetRoot(buf); +} + +inline const Messaging::TopologyMessage *GetSizePrefixedTopologyMessage(const void *buf) { + return ::flatbuffers::GetSizePrefixedRoot(buf); +} + +inline bool VerifyTopologyMessageBuffer( + ::flatbuffers::Verifier &verifier) { + return verifier.VerifyBuffer(nullptr); +} + +inline bool VerifySizePrefixedTopologyMessageBuffer( + ::flatbuffers::Verifier &verifier) { + return verifier.VerifySizePrefixedBuffer(nullptr); +} + +inline void FinishTopologyMessageBuffer( + ::flatbuffers::FlatBufferBuilder &fbb, + ::flatbuffers::Offset root) { + fbb.Finish(root); +} + +inline void FinishSizePrefixedTopologyMessageBuffer( + ::flatbuffers::FlatBufferBuilder &fbb, + ::flatbuffers::Offset root) { + fbb.FinishSizePrefixed(root); +} + +} // namespace Messaging + +#endif // FLATBUFFERS_GENERATED_TOPOLOGYMESSAGE_MESSAGING_H_ diff --git a/components/rpc/CommunicationRouter.cpp b/components/rpc/CommunicationRouter.cpp index d635a04..4024ace 100644 --- a/components/rpc/CommunicationRouter.cpp +++ b/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> CommunicationRouter::get_physically_connected_modules() const { + std::vector 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 connected_module_ids; + std::vector 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 }; +} diff --git a/components/rpc/MessagingInterface.cpp b/components/rpc/MessagingInterface.cpp index bb7d79b..32cf666 100644 --- a/components/rpc/MessagingInterface.cpp +++ b/components/rpc/MessagingInterface.cpp @@ -64,3 +64,7 @@ void MessagingInterface::checkOrInsertTag(const uint8_t tag) { } xSemaphoreGive(m_map_semaphore); } + +std::pair, std::vector> MessagingInterface::get_physically_connected_modules() const { + return m_router->get_physically_connected_modules(); +} diff --git a/components/rpc/include/CommunicationRouter.h b/components/rpc/include/CommunicationRouter.h index 6c7cc91..9127e56 100644 --- a/components/rpc/include/CommunicationRouter.h +++ b/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 + std::pair, std::vector> get_physically_connected_modules() const; + // todo: does this really need to be here (so i can access from thread)? std::shared_ptr>> m_tcp_rx_queue; std::function m_rx_callback; private: - TaskHandle_t m_router_thread; + TaskHandle_t m_router_thread = nullptr; std::vector m_link_layer_threads; - std::unique_ptr m_tcp_server; + std::unique_ptr m_tcp_server; // todo: dependency injection std::unique_ptr m_data_link_manager; std::unique_ptr m_pc_connection; // todo: change to dependency inject uint8_t m_leader = 0; diff --git a/components/rpc/include/MessagingInterface.h b/components/rpc/include/MessagingInterface.h index f87b183..5f14df0 100644 --- a/components/rpc/include/MessagingInterface.h +++ b/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> get_physically_connected_modules() const; private: void handleRecv(const char* recv_buffer, int recv_size); diff --git a/main/LoopManager.cpp b/main/LoopManager.cpp index 72bb57d..9921c80 100644 --- a/main/LoopManager.cpp +++ b/main/LoopManager.cpp @@ -7,19 +7,39 @@ #include #include #include +#include #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(std::make_unique()); +#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(buffer), 512, PC_ADDR, ACTUATOR_CMD_TAG); + m_messaging_interface->recv(reinterpret_cast(buffer), 512, PC_ADDR, ACTUATOR_CMD_TAG); actuator->actuate(buffer); } } + +[[noreturn]] void LoopManager::metadata_tx_loop(char * args) { + const auto that = reinterpret_cast(args); + const auto topology_message_builder = std::make_unique(); + 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 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(data), size, PC_ADDR, TOPOLOGY_CMD_TAG, true); + vTaskDelay(METADATA_PERIOD_MS / portTICK_PERIOD_MS); + } +} diff --git a/main/include/LoopManager.h b/main/include/LoopManager.h index 52737da..cda0aec 100644 --- a/main/include/LoopManager.h +++ b/main/include/LoopManager.h @@ -5,12 +5,21 @@ #ifndef LOOPMANAGER_H #define LOOPMANAGER_H +#include + +#include + #include "control/IActuator.h" #include "control/ActuatorFactory.h" class LoopManager { public: - [[noreturn]] static void control_loop(); + LoopManager() : m_messaging_interface(std::make_unique(std::make_unique())) {} + [[noreturn]] void control_loop() const; + [[noreturn]] static void metadata_tx_loop(char * args); + +private: + std::unique_ptr m_messaging_interface; private: diff --git a/main/main.cpp b/main/main.cpp index 1c6aac6..0014a47 100644 --- a/main/main.cpp +++ b/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(); + xTaskCreate(reinterpret_cast(LoopManager::metadata_tx_loop), "metadata_tx", 3096, loop_manager.get(), 3, nullptr); + loop_manager->control_loop(); }