From c1d9a3a8b4760c2969faf7b9fe5d44326ef97879 Mon Sep 17 00:00:00 2001 From: Johnathon Slightham Date: Sat, 12 Jul 2025 23:50:53 -0400 Subject: [PATCH] Various bugfixes --- components/rpc/CommunicationRouter.cpp | 38 ++++++++++--------- components/rpc/TCPServer.cpp | 15 ++++++-- components/rpc/WifiManager.cpp | 5 +++ components/rpc/include/CommunicationRouter.h | 4 +- components/rpc/include/TCPServer.h | 2 +- components/rpc/include/mDNSDiscoveryService.h | 2 +- components/rpc/mDNSDiscoveryService.cpp | 5 ++- main/LoopManager.cpp | 7 ++-- main/main.cpp | 1 - 9 files changed, 47 insertions(+), 32 deletions(-) diff --git a/components/rpc/CommunicationRouter.cpp b/components/rpc/CommunicationRouter.cpp index cbdbeea..8dffba6 100644 --- a/components/rpc/CommunicationRouter.cpp +++ b/components/rpc/CommunicationRouter.cpp @@ -24,17 +24,8 @@ CommunicationRouter::~CommunicationRouter() { const auto that = static_cast(args); while (true) { - // todo: strange to have this here, but i dont want 4 threads calling it or a thread just for it. - if (std::chrono::system_clock::now() - that->m_last_leader_updated > std::chrono::seconds(15)) { - that->update_leader(); - } - const auto buffer = that->m_tcp_rx_queue->dequeue(); - - std::cout << "dequeued buffer" << std::endl; - - that->m_rx_callback(reinterpret_cast(buffer->data()), buffer->size()); - std::cout << "WiFi callback" << std::endl; + that->route(buffer->data(), buffer->size()); } } @@ -52,13 +43,18 @@ CommunicationRouter::~CommunicationRouter() { 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); - if (ESP_OK != err) { + // 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(); + std::cout << "Updating leader" << std::endl; + that->update_leader(); + } + + if (ESP_OK != err || bytes_received < 1) { continue; } that->route(reinterpret_cast(buffer), bytes_received); - - std::cout << "RMT callback" << std::endl; } } @@ -79,7 +75,7 @@ void CommunicationRouter::update_leader() { 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) { + if (id > max) { // todo: change this to be correct max = id; } } @@ -95,19 +91,25 @@ void CommunicationRouter::update_leader() { this->m_leader = max; - mDNSDiscoveryService::set_connected_boards(connected_module_ids); - - this->m_last_leader_updated = std::chrono::system_clock::now(); + if (this->m_leader == m_module_id) { + mDNSDiscoveryService::set_connected_boards(connected_module_ids); + } } 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) { + std::cout << "Routing to this module [dest:" << static_cast(mpi_message->destination()) << ", length: " << length << "]" << std::endl; this->m_rx_callback(reinterpret_cast(buffer), 512); } else if (mpi_message->destination() == PC_ADDR && this->m_leader == m_module_id) { + std::cout << "Routing to wifi [dest:" << static_cast(mpi_message->destination()) << ", length: " << length << "]" << std::endl; this->m_tcp_server->send_msg(reinterpret_cast(buffer), 512); - } else { + } else if (mpi_message->destination() == PC_ADDR) { + std::cout << "Routing to wireline for wifi [dest:" << static_cast(mpi_message->destination()) << ", length: " << length << "]" << std::endl; + this->m_data_link_manager->send(this->m_leader, buffer, length, FrameType::MOTOR_TYPE, 0); + }else { + std::cout << "Routing to wireline [dest:" << static_cast(mpi_message->destination()) << ", length: " << length << "]" << std::endl; this->m_data_link_manager->send(mpi_message->destination(), buffer, length, FrameType::MOTOR_TYPE, 0); } } diff --git a/components/rpc/TCPServer.cpp b/components/rpc/TCPServer.cpp index 012f370..b20ad9d 100644 --- a/components/rpc/TCPServer.cpp +++ b/components/rpc/TCPServer.cpp @@ -32,7 +32,7 @@ TCPServer::TCPServer(const int port, const std::shared_ptrm_rx_queue = rx_queue; this->m_server_sock = 0; - xTaskCreate(tcp_server_task, "tcp_accept_server", 2048, this, 5, &this->m_task); + xTaskCreate(tcp_server_task, "tcp_accept_server", 3072, this, 5, &this->m_task); xTaskCreate(socket_monitor_thread, "tcp_rx", 4096, this, 5, &this->m_rx_task); } @@ -148,7 +148,15 @@ TCPServer::~TCPServer() { auto buffer = std::make_unique>(); buffer->resize(MAX_RX_BUFFER_SIZE); - int len = recv(sock, buffer->data(), MAX_RX_BUFFER_SIZE, 0); // temp: for the null terminator + uint32_t msg_size = 0; + recv(sock, &msg_size, 4, MSG_WAITALL); + if (msg_size < 1 || msg_size > 512) { + continue; + } + + printf("Message size: %ld\n", msg_size); + + int len = recv(sock, buffer->data(), msg_size, MSG_WAITALL); if (len < 0) { printf("Error occurred during receiving: errno %d\n", errno); to_remove.emplace_back(sock); @@ -203,7 +211,7 @@ bool TCPServer::authenticate_client(int sock) { return 0; } -int TCPServer::send_msg(char *buffer, size_t length) const { +int TCPServer::send_msg(char *buffer, uint32_t length) const { // todo: should we assign a unique rank to each pc? if (!is_network_connected()) { @@ -212,6 +220,7 @@ int TCPServer::send_msg(char *buffer, size_t length) const { for (const auto client_sock : m_clients) { std::cout << "sending tcp" << std::endl; + send(client_sock, &length, 4, 0); send(client_sock, buffer, length, 0); } diff --git a/components/rpc/WifiManager.cpp b/components/rpc/WifiManager.cpp index e87c841..c4f3efb 100644 --- a/components/rpc/WifiManager.cpp +++ b/components/rpc/WifiManager.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #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"); } } diff --git a/components/rpc/include/CommunicationRouter.h b/components/rpc/include/CommunicationRouter.h index 8b34397..6c7cc91 100644 --- a/components/rpc/include/CommunicationRouter.h +++ b/components/rpc/include/CommunicationRouter.h @@ -40,13 +40,13 @@ public: m_last_leader_updated(std::chrono::system_clock::now()){ update_leader(); - xTaskCreate(router_thread, "communication_router", 2048, this, 3, &this->m_router_thread); + xTaskCreate(router_thread, "communication_router", 4096, 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]); + xTaskCreate(link_layer_thread, "communication_router_rmt", 4096, params, 3, &this->m_link_layer_threads[i]); } } diff --git a/components/rpc/include/TCPServer.h b/components/rpc/include/TCPServer.h index ee70333..7a9cd03 100644 --- a/components/rpc/include/TCPServer.h +++ b/components/rpc/include/TCPServer.h @@ -19,7 +19,7 @@ class TCPServer : IRPCServer { public: TCPServer(int port, const std::shared_ptr>>& rx_queue); ~TCPServer(); - int send_msg(char* buffer, size_t length) const; + int send_msg(char* buffer, uint32_t length) const; private: bool authenticate_client(int client_sock); diff --git a/components/rpc/include/mDNSDiscoveryService.h b/components/rpc/include/mDNSDiscoveryService.h index 9f63e97..5695e80 100644 --- a/components/rpc/include/mDNSDiscoveryService.h +++ b/components/rpc/include/mDNSDiscoveryService.h @@ -13,7 +13,7 @@ public: ~mDNSDiscoveryService() = delete; static void setup(); - static void set_connected_boards(std::vector& boards); + static void set_connected_boards(const std::vector& boards); }; #endif //DISCOVERYSERVICE_H diff --git a/components/rpc/mDNSDiscoveryService.cpp b/components/rpc/mDNSDiscoveryService.cpp index 325bff2..aab7041 100644 --- a/components/rpc/mDNSDiscoveryService.cpp +++ b/components/rpc/mDNSDiscoveryService.cpp @@ -12,6 +12,7 @@ #include #include +#include #include // todo: clean this up (strange to be a constructor) also need to add more details, need to add to routing table @@ -32,11 +33,11 @@ void mDNSDiscoveryService::setup() { mdns_service_txt_set("_robotcontrol", "_tcp", service_txt_data, 3); } -void mDNSDiscoveryService::set_connected_boards(std::vector& boards) { +void mDNSDiscoveryService::set_connected_boards(const std::vector& boards) { std::stringstream ss; for (int i = 0; i < boards.size(); i++) { - ss << boards[i]; + ss << std::to_string(boards[i]); if (i < boards.size() - 1) { ss << ','; } diff --git a/main/LoopManager.cpp b/main/LoopManager.cpp index 6691d19..f389af2 100644 --- a/main/LoopManager.cpp +++ b/main/LoopManager.cpp @@ -16,11 +16,10 @@ char buffer[512]; while (true) { messaging_interface->recv(buffer, 512, 0, 1); - std::cout << buffer << std::endl; + //std::cout << buffer << std::endl; - - 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, 5, 2, true); + std::string s = std::format("num {} bo", ConfigManager::get_module_id()); + messaging_interface->send(s.data(), s.size(), 0, 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)); diff --git a/main/main.cpp b/main/main.cpp index 445ba8a..89ad2c6 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -16,6 +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(); - mDNSDiscoveryService::setup(); LoopManager::control_loop(); }