botchain/firmware
1
0
Fork 0
mirror of https://github.com/BotChain-Robots/firmware.git synced 2026-07-08 17:47:21 +02:00
Code Issues Packages Projects Releases Wiki Activity

Various bugfixes

Browse Source
This commit is contained in:
Johnathon Slightham
2025-07-12 23:50:53 -04:00
parent 74d4302075
commit c1d9a3a8b4
9 changed files with 47 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
components/rpc/CommunicationRouter.cpp
View File

@@ -24,17 +24,8 @@ CommunicationRouter::~CommunicationRouter() {
const auto that = static_cast<CommunicationRouter *>(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<char *>(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<uint8_t *>(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<uint8_t *>(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<int>(mpi_message->destination()) << ", length: " << length << "]" << std::endl;
this->m_rx_callback(reinterpret_cast<char *>(buffer), 512);
} else if (mpi_message->destination() == PC_ADDR && this->m_leader == m_module_id) {
std::cout << "Routing to wifi [dest:" << static_cast<int>(mpi_message->destination()) << ", length: " << length << "]" << std::endl;
this->m_tcp_server->send_msg(reinterpret_cast<char *>(buffer), 512);
} else {
} else if (mpi_message->destination() == PC_ADDR) {
std::cout << "Routing to wireline for wifi [dest:" << static_cast<int>(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<int>(mpi_message->destination()) << ", length: " << length << "]" << std::endl;
this->m_data_link_manager->send(mpi_message->destination(), buffer, length, FrameType::MOTOR_TYPE, 0);
}
}

15
components/rpc/TCPServer.cpp
View File

@@ -32,7 +32,7 @@ TCPServer::TCPServer(const int port, const std::shared_ptr<PtrQueue<std::vector<
this->m_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<std::vector<uint8_t>>();
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);
}

5
components/rpc/WifiManager.cpp
View File

@@ -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");
}
}

4
components/rpc/include/CommunicationRouter.h
View File

@@ -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]);
}
}

2
components/rpc/include/TCPServer.h
View File

@@ -19,7 +19,7 @@ class TCPServer : IRPCServer {
public:
TCPServer(int port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& 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);

2
components/rpc/include/mDNSDiscoveryService.h
View File

@@ -13,7 +13,7 @@ public:
~mDNSDiscoveryService() = delete;
static void setup();
static void set_connected_boards(std::vector<int>& boards);
static void set_connected_boards(const std::vector<int>& boards);
};
#endif //DISCOVERYSERVICE_H

5
components/rpc/mDNSDiscoveryService.cpp
View File

@@ -12,6 +12,7 @@
#include <string>
#include <format>
#include <iostream>
#include <sstream>
// 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<int>& boards) {
void mDNSDiscoveryService::set_connected_boards(const std::vector<int>& 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 << ',';
}