diff --git a/components/rpc/CommunicationRouter.cpp b/components/rpc/CommunicationRouter.cpp index 96507c3..8c21838 100644 --- a/components/rpc/CommunicationRouter.cpp +++ b/components/rpc/CommunicationRouter.cpp @@ -1,162 +1,163 @@ #include -#include "CommunicationRouter.h" #include "AngleControlMessageBuilder.h" -#include "include/wireless/mDNSDiscoveryService.h" +#include "CommunicationRouter.h" #include "MPIMessageBuilder.h" -#include "include/wireless/WifiManager.h" -#include "freertos/FreeRTOS.h" -#include "Tables.h" -#include "PtrQueue.h" #include "OrientationDetection.h" +#include "PtrQueue.h" +#include "Tables.h" +#include "constants/module.h" +#include "freertos/FreeRTOS.h" +#include "freertos/idf_additions.h" +#include "freertos/projdefs.h" +#include "include/wireless/WifiManager.h" +#include "include/wireless/mDNSDiscoveryService.h" #define TAG "CommunicationRouter" -CommunicationRouter::~CommunicationRouter() { - vTaskDelete(m_router_thread); -} +CommunicationRouter::~CommunicationRouter() { vTaskDelete(m_router_thread); } // 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 [[noreturn]] void CommunicationRouter::router_thread(void *args) { - const auto that = static_cast(args); + const auto that = static_cast(args); - while (true) { - const auto buffer = that->m_tcp_rx_queue->dequeue(); - ESP_LOGD(TAG, "Got message from TCP"); - that->route(buffer->data(), buffer->size()); - } + while (true) { + const auto buffer = that->m_tcp_rx_queue->dequeue(); + ESP_LOGD(TAG, "Got message from TCP"); + that->route(buffer->data(), buffer->size()); + } } [[noreturn]] void CommunicationRouter::link_layer_thread(void *args) { - const auto* params = static_cast(args); - const auto that = params->router; - const auto channel = params->channel; - delete params; + const auto that = static_cast(args); - size_t bytes_received = 0; - that->m_data_link_manager->start_receive_frames(channel); - while (true) { - char buffer[512]; - const auto err = that->m_data_link_manager->receive(reinterpret_cast(buffer), 512, &bytes_received, channel); - that->m_data_link_manager->start_receive_frames(channel); + while (true) { + for (uint8_t channel = 0; + channel < + MODULE_TO_NUM_CHANNELS_MAP[that->m_config_manager.get_module_type()]; + channel++) { + uint16_t frame_size = 0; + FrameHeader frame_header{}; + if (ESP_OK != that->m_data_link_manager->async_receive_info( + &frame_size, &frame_header, channel) || + 0 == frame_size) { + continue; + } - // todo: do we only want one thread ever doing this? - if (std::chrono::system_clock::now() - that->m_last_leader_updated > std::chrono::seconds(15)) { - that->m_last_leader_updated = std::chrono::system_clock::now(); - ESP_LOGI(TAG, "Updating leader"); - that->update_leader(); - } - - if (ESP_OK != err || bytes_received < 1) { - continue; - } - - ESP_LOGD(TAG, "Got message from RMT"); - that->route(reinterpret_cast(buffer), bytes_received); + std::vector data{}; + data.resize(frame_size); + that->m_data_link_manager->async_receive(data.data(), frame_size, + &frame_header, channel); + that->route(data.data(), frame_size); } + + vTaskDelay(pdMS_TO_TICKS(50)); + } } -int CommunicationRouter::send_msg(char* buffer, const size_t length) const { - ESP_LOGD(TAG, "Got message from application"); - route(reinterpret_cast(buffer), length); - return 0; +int CommunicationRouter::send_msg(char *buffer, const size_t length) const { + ESP_LOGD(TAG, "Got message from application"); + route(reinterpret_cast(buffer), length); + return 0; } 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); + 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 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 (id > max) { // todo: change this to be correct - max = id; - } + // Leader election (just get the highest id in rip) + std::vector 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 (id > max) { // todo: change this to be correct + 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(); - m_lossless_server->startup(); - m_lossy_server->startup(); - } else if (this->m_leader == m_module_id) { - m_pc_connection->disconnect(); - m_lossless_server->shutdown(); - m_lossless_server->shutdown(); - } + // 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(); + m_lossless_server->startup(); + m_lossy_server->startup(); + } else if (this->m_leader == m_module_id) { + m_pc_connection->disconnect(); + m_lossless_server->shutdown(); + m_lossless_server->shutdown(); } + } - this->m_leader = max; + this->m_leader = max; - if (this->m_leader == m_module_id) { - this->m_discovery_service->set_connected_boards(connected_module_ids); - } + if (this->m_leader == m_module_id) { + this->m_discovery_service->set_connected_boards(connected_module_ids); + } } -void CommunicationRouter::route(uint8_t* buffer, const size_t length) const { - flatbuffers::Verifier verifier(buffer, length); - // This could be moved to just be called on wireline data to save cpu cycles. - if (bool ok = Messaging::VerifyMPIMessageBuffer(verifier); !ok) { - ESP_LOGW(TAG, "route: got an invalid MPI message, disregarding"); - return; - } +void CommunicationRouter::route(uint8_t *buffer, const size_t length) const { + flatbuffers::Verifier verifier(buffer, length); + // This could be moved to just be called on wireline data to save cpu cycles. + if (bool ok = Messaging::VerifyMPIMessageBuffer(verifier); !ok) { + ESP_LOGW(TAG, "route: got an invalid MPI message, disregarding"); + return; + } - if (const auto& mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer); mpi_message->destination() == m_module_id) { - ESP_LOGD(TAG, "Routing to this module [dest: %d, length: %d]", static_cast(mpi_message->destination()), length); - this->m_rx_callback(reinterpret_cast(buffer), 512); - } else if (mpi_message->destination() == PC_ADDR && this->m_leader == m_module_id) { - ESP_LOGD(TAG, "Routing to wifi [dest: %d, length: %d]", static_cast(mpi_message->destination()), length); - if (mpi_message->is_durable()) { - this->m_lossless_server->send_msg(reinterpret_cast(buffer), 512); - } else { - this->m_lossy_server->send_msg(reinterpret_cast(buffer), 512); - } - } else if (mpi_message->destination() == PC_ADDR) { - ESP_LOGD(TAG, "Routing to wireline for wifi [dest: %d, length: %d]", static_cast(mpi_message->destination()), length); - this->m_data_link_manager->send(this->m_leader, buffer, length, FrameType::MOTOR_TYPE, 0); - }else { - ESP_LOGD(TAG, "Routing to wireline [dest: %d, length: %d]", static_cast(mpi_message->destination()), length); - 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; - } - } - - if (const auto id = connected_module_ids[0]; 0 == id) { - connected_module_orientations[0] = Orientation_Deg0; + if (const auto &mpi_message = + Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer); + mpi_message->destination() == m_module_id) { + this->m_rx_callback(reinterpret_cast(buffer), 512); + } else if (mpi_message->destination() == PC_ADDR && + this->m_leader == m_module_id) { + if (mpi_message->is_durable()) { + this->m_lossless_server->send_msg(reinterpret_cast(buffer), 512); } else { - connected_module_orientations[0] = OrientationDetection::get_orientation(0); + this->m_lossy_server->send_msg(reinterpret_cast(buffer), 512); } + } else if (mpi_message->destination() == PC_ADDR) { + this->m_data_link_manager->send(this->m_leader, buffer, length, + FrameType::MOTOR_TYPE, 0); + } else { + this->m_data_link_manager->send(mpi_message->destination(), buffer, length, + FrameType::MOTOR_TYPE, 0); + } +} - return { connected_module_ids, connected_module_orientations }; +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; + } + } + + if (const auto id = connected_module_ids[0]; 0 == id) { + connected_module_orientations[0] = Orientation_Deg0; + } else { + connected_module_orientations[0] = OrientationDetection::get_orientation(0); + } + + return {connected_module_ids, connected_module_orientations}; } [[nodiscard]] uint8_t CommunicationRouter::get_leader() const { - return this->m_leader; + return this->m_leader; } diff --git a/components/rpc/include/CommunicationRouter.h b/components/rpc/include/CommunicationRouter.h index 05c1546..7177607 100644 --- a/components/rpc/include/CommunicationRouter.h +++ b/components/rpc/include/CommunicationRouter.h @@ -7,79 +7,77 @@ #ifndef COMMUNICATIONROUTER_H #define COMMUNICATIONROUTER_H +#include #include #include -#include #include "CommunicationFactory.h" -#include "IDiscoveryService.h" #include "ConfigManager.h" -#include "OrientationDetection.h" -#include "wireless/WifiManager.h" -#include "wireless/TCPServer.h" #include "DataLinkManager.h" -#include "constants/module.h" +#include "IDiscoveryService.h" +#include "OrientationDetection.h" #include "PtrQueue.h" +#include "constants/module.h" +#include "wireless/TCPServer.h" +#include "wireless/WifiManager.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 &rx_callback) - : m_tcp_rx_queue(std::make_shared>>(10)), - m_rx_callback(rx_callback), - m_config_manager(ConfigManager::get_instance()), - m_pc_connection(CommunicationFactory::create_connection_manager(m_config_manager.get_communication_method())), - m_lossless_server(CommunicationFactory::create_lossless_server(m_config_manager.get_communication_method(), m_tcp_rx_queue)), - m_lossy_server(CommunicationFactory::create_lossy_server(m_config_manager.get_communication_method(), m_tcp_rx_queue)), - m_data_link_manager(std::make_unique(m_config_manager.get_module_id(), MODULE_TO_NUM_CHANNELS_MAP[m_config_manager.get_module_type()])), - m_module_id(m_config_manager.get_module_id()), - m_last_leader_updated(std::chrono::system_clock::now()), - m_discovery_service(CommunicationFactory::create_discovery_service(m_config_manager.get_communication_method())){ - OrientationDetection::init(); - update_leader(); + explicit CommunicationRouter( + const std::function &rx_callback) + : m_tcp_rx_queue(std::make_shared>>(10)), + m_rx_callback(rx_callback), + m_config_manager(ConfigManager::get_instance()), + m_pc_connection(CommunicationFactory::create_connection_manager( + m_config_manager.get_communication_method())), + m_lossless_server(CommunicationFactory::create_lossless_server( + m_config_manager.get_communication_method(), m_tcp_rx_queue)), + m_lossy_server(CommunicationFactory::create_lossy_server( + m_config_manager.get_communication_method(), m_tcp_rx_queue)), + m_data_link_manager(std::make_unique( + m_config_manager.get_module_id(), + MODULE_TO_NUM_CHANNELS_MAP[m_config_manager.get_module_type()])), + m_module_id(m_config_manager.get_module_id()), + m_last_leader_updated(std::chrono::system_clock::now()), + m_discovery_service(CommunicationFactory::create_discovery_service( + m_config_manager.get_communication_method())) { + OrientationDetection::init(); + update_leader(); - xTaskCreate(router_thread, "router", 4096, this, 3, &this->m_router_thread); + xTaskCreate(router_thread, "router", 4096, this, 3, &this->m_router_thread); + xTaskCreate(link_layer_thread, "router_rmt", 4096, this, 3, + &this->m_link_layer_thread); + } - // const auto num_channels = MODULE_TO_NUM_CHANNELS_MAP[m_config_manager.get_module_type()]; - // this->m_link_layer_threads.resize(num_channels); - // for (int i = 0; i < num_channels; i++) { - // auto *params = new link_layer_thread_params(this, i); - // xTaskCreate(link_layer_thread, "router_rmt", 4096, params, 3, &this->m_link_layer_threads[i]); - // } - } + ~CommunicationRouter(); - ~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; + [[nodiscard]] std::pair, std::vector> + get_physically_connected_modules() const; + [[nodiscard]] uint8_t get_leader() const; - [[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; - [[nodiscard]] std::pair, std::vector> get_physically_connected_modules() const; - [[nodiscard]] uint8_t get_leader() const; + // todo: does this really need to be here (so i can access from thread)? + std::shared_ptr>> + m_tcp_rx_queue; // todo: this should probably be thread safe + std::function m_rx_callback; - // todo: does this really need to be here (so i can access from thread)? - std::shared_ptr>> m_tcp_rx_queue; // todo: this should probably be thread safe - std::function m_rx_callback; private: - TaskHandle_t m_router_thread = nullptr; - ConfigManager &m_config_manager; - std::unique_ptr m_pc_connection; - std::vector m_link_layer_threads; - std::unique_ptr m_lossless_server; - std::unique_ptr m_lossy_server; - std::unique_ptr m_data_link_manager; - uint8_t m_leader = 0; - uint8_t m_module_id; - std::chrono::time_point m_last_leader_updated; - std::unique_ptr m_discovery_service; + TaskHandle_t m_router_thread = nullptr; + TaskHandle_t m_link_layer_thread = nullptr; + ConfigManager &m_config_manager; + std::unique_ptr m_pc_connection; + std::unique_ptr m_lossless_server; + std::unique_ptr m_lossy_server; + std::unique_ptr m_data_link_manager; + uint8_t m_leader = 0; + uint8_t m_module_id; + std::chrono::time_point m_last_leader_updated; + std::unique_ptr m_discovery_service; }; -#endif //COMMUNICATIONROUTER_H +#endif // COMMUNICATIONROUTER_H diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 1ff55ba..260c604 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -4,15 +4,15 @@ file(GLOB_RECURSE ALL_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/*.S" ) -if(DEFINED RMT_TEST AND RMT_TEST EQUAL 1) +if(DEFINED RMT_TEST AND RMT_TEST EQUAL 1) message(STATUS "Building for testing RMT/Link Layer Functionality with main_rmt_test.cpp") list(REMOVE_ITEM ALL_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/main.cpp" ) - idf_build_set_property(COMPILE_DEFINITIONS RMT_TEST APPEND) + idf_build_set_property(COMPILE_DEFINITIONS RMT_TEST APPEND) else() list(REMOVE_ITEM ALL_SRCS - "${CMAKE_CURRENT_SOURCE_DIR}/main.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/main_rmt_test.cpp" ) endif() @@ -34,4 +34,3 @@ if(DEFINED RECEIVE_ONLY AND RECEIVE_ONLY) message(STATUS "Building for receive only") idf_build_set_property(COMPILE_DEFINITIONS RECEIVE_ONLY=${RECEIVE_ONLY} APPEND) endif() -