Config manager fixes

This commit is contained in:
2025-09-30 15:30:10 -04:00
parent 792f4a530a
commit 1ca488b7b9
17 changed files with 328 additions and 205 deletions

View File

@@ -1,4 +1,6 @@
idf_component_register(SRCS "WifiManager.cpp" "mDNSDiscoveryService.cpp" "TCPServer.cpp" "CommunicationRouter.cpp" "MessagingInterface.cpp" "OrientationDetection.cpp"
PRIV_REQUIRES driver esp_event nvs_flash esp_netif esp_wifi espressif__mdns constants config flatbuffers dataLink rmt
REQUIRES ptrQueue
PRIV_REQUIRES driver esp_event nvs_flash esp_netif espressif__mdns constants config flatbuffers dataLink rmt
REQUIRES ptrQueue esp_wifi
INCLUDE_DIRS "include")
target_compile_options(${COMPONENT_LIB} PRIVATE -fexceptions)

View File

@@ -1,16 +1,17 @@
#include "CommunicationRouter.h"
#include <AngleControlMessageBuilder.h>
#include <iostream>
#include "CommunicationRouter.h"
#include "AngleControlMessageBuilder.h"
#include "mDNSDiscoveryService.h"
#include "MPIMessageBuilder.h"
#include "WifiManager.h"
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "Tables.h"
#include "PtrQueue.h"
#include "OrientationDetection.h"
#define TAG "CommunicationRouter"
CommunicationRouter::~CommunicationRouter() {
vTaskDelete(m_router_thread);
}
@@ -24,7 +25,7 @@ CommunicationRouter::~CommunicationRouter() {
while (true) {
const auto buffer = that->m_tcp_rx_queue->dequeue();
std::cout << "routing from tcp" << std::endl;
ESP_LOGD(TAG, "Got message from TCP");
that->route(buffer->data(), buffer->size());
}
}
@@ -35,10 +36,10 @@ CommunicationRouter::~CommunicationRouter() {
const auto channel = params->channel;
delete params;
char buffer[512];
size_t bytes_received = 0;
that->m_data_link_manager->start_receive_frames(channel);
while (true) {
char buffer[512];
// todo: very c style function calls
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);
@@ -46,7 +47,7 @@ CommunicationRouter::~CommunicationRouter() {
// 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;
ESP_LOGI(TAG, "Updating leader");
that->update_leader();
}
@@ -54,12 +55,13 @@ CommunicationRouter::~CommunicationRouter() {
continue;
}
std::cout << "routing message from rmt" << std::endl;
ESP_LOGD(TAG, "Got message from RMT");
that->route(reinterpret_cast<uint8_t *>(buffer), bytes_received);
}
}
int CommunicationRouter::send_msg(char* buffer, const size_t length) const {
ESP_LOGD(TAG, "Got message from application");
route(reinterpret_cast<uint8_t *>(buffer), length);
return 0;
}
@@ -101,17 +103,16 @@ 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;
ESP_LOGD(TAG, "Routing to this module [dest: %d, length: %d]", static_cast<int>(mpi_message->destination()), length);
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;
ESP_LOGD(TAG, "Routing to wifi [dest: %d, length: %d]", static_cast<int>(mpi_message->destination()), length);
this->m_tcp_server->send_msg(reinterpret_cast<char *>(buffer), 512);
} 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;
ESP_LOGD(TAG, "Routing to wireline for wifi [dest: %d, length: %d]", static_cast<int>(mpi_message->destination()), length);
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;
ESP_LOGD(TAG, "Routing to wireline [dest: %d, length: %d]", static_cast<int>(mpi_message->destination()), length);
this->m_data_link_manager->send(mpi_message->destination(), buffer, length, FrameType::MOTOR_TYPE, 0);
}
}

View File

@@ -15,14 +15,14 @@ MessagingInterface::~MessagingInterface() {
vQueueDelete(m_mpi_rx_queue);
vSemaphoreDelete(m_map_semaphore);
for (const auto [_tag, queue] : m_tag_to_queue) {
for (const auto queue: m_tag_to_queue | std::views::values) {
vQueueDelete(queue);
}
}
int MessagingInterface::send(char* buffer, int size, int destination, int tag, bool durable) {
int MessagingInterface::send(char* buffer, const int size, const int destination, const int tag, const bool durable) {
Flatbuffers::MPIMessageBuilder builder;
const auto [mpi_buffer, mpi_size] = builder.build_mpi_message(Messaging::MessageType_PTP, ConfigManager::get_module_id(), destination, sequence_number++, durable, tag, std::vector<uint8_t>(buffer, buffer + size));
const auto [mpi_buffer, mpi_size] = builder.build_mpi_message(Messaging::MessageType_PTP, m_config_manager.get_module_id(), destination, m_sequence_number++, durable, tag, std::vector<uint8_t>(buffer, buffer + size));
m_router->send_msg(static_cast<char *>(mpi_buffer), mpi_size);
return 0;

View File

@@ -2,15 +2,16 @@
// Created by Johnathon Slightham on 2025-07-26.
//
#include <constants/module.h>
#include "constants/module.h"
#include "OrientationDetection.h"
#include "ConfigManager.h"
#include "iostream"
#include <ConfigManager.h>
#include <iostream>
#include <bits/ostream.tcc>
#define TAG "OrientationDetection"
void OrientationDetection::init() {
for (int i = 0; i < MODULE_TO_NUM_CHANNELS_MAP[ConfigManager::get_module_type()]; i++) {
const auto& config_manager = ConfigManager::get_instance();
for (int i = 0; i < MODULE_TO_NUM_CHANNELS_MAP[config_manager.get_module_type()]; i++) {
setup_gpio(static_cast<gpio_num_t>(CHANNEL_TO_0_DEG_MAP[i]));
setup_gpio(static_cast<gpio_num_t>(CHANNEL_TO_90_DEG_MAP[i]));
setup_gpio(static_cast<gpio_num_t>(CHANNEL_TO_180_DEG_MAP[i]));
@@ -20,16 +21,16 @@ void OrientationDetection::init() {
Orientation OrientationDetection::get_orientation(const uint8_t channel) {
if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_90_DEG_MAP[channel]))) {
std::cout << "90deg" << std::endl;
ESP_LOGD(TAG, "90deg");
return Orientation_Deg90;
} else if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_180_DEG_MAP[channel]))) {
std::cout << "180deg" << std::endl;
ESP_LOGD(TAG, "180deg");
return Orientation_Deg180;
} else if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_270_DEG_MAP[channel]))) {
std::cout << "270deg" << std::endl;
ESP_LOGD(TAG, "270deg");
return Orientation_Deg270;
} else {
std::cout << "No orientation detected" << std::endl;
ESP_LOGD(TAG, "No orientation detected");
return Orientation_Deg0;
}
}

View File

@@ -1,24 +1,23 @@
#include <string.h>
#include <iostream>
#include <memory>
#include <esp_log.h>
#include <sys/param.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_event.h"
#include "esp_netif.h"
#include "lwip/err.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include <lwip/netdb.h>
#include <iostream>
#include "lwip/netdb.h"
#include "TCPServer.h"
#include <memory>
#include <bits/shared_ptr_base.h>
#include <constants/app_comms.h>
#include "bits/shared_ptr_base.h"
#include "constants/app_comms.h"
#include "constants/tcp.h"
#define TAG "TCPServer"
// todo: - add message routing to correct client
// - authenticate (don't just return true from the auth function)
// - tx from board
@@ -51,18 +50,18 @@ TCPServer::~TCPServer() {
const auto that = static_cast<TCPServer*>(args);
while (true) {
printf("Attempting to start TCP Server on port %d", that->m_port);
ESP_LOGI(TAG, "Attempting to start TCP Server on port %d", that->m_port);
that->m_server_sock = socket(AF_INET, SOCK_STREAM, IPPROTO_IP);
if (that->m_server_sock < 0) {
printf("Unable to create TCP socket: errno %d\n", errno);
ESP_LOGE(TAG, "Unable to create TCP socket: errno %d\n", errno);
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
constexpr int opt = 1;
setsockopt(that->m_server_sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
printf("Socket created\n");
ESP_LOGI(TAG, "Socket created\n");
sockaddr_in server_addr = {
.sin_family = AF_INET,
@@ -74,18 +73,18 @@ TCPServer::~TCPServer() {
int err = bind(that->m_server_sock, reinterpret_cast<struct sockaddr *>(&server_addr), sizeof(server_addr));
if (0 != err) {
printf("Socket unable to bind: errno %d\n", errno);
ESP_LOGE(TAG, "Socket unable to bind: errno %d\n", errno);
close(that->m_server_sock);
that->m_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
printf("Socket bound to port %d\n", that->m_port);
ESP_LOGI(TAG, "Socket bound to port %d\n", that->m_port);
err = listen(that->m_server_sock, TCP_DEFAULT_LISTEN_BACKLOG);
if (0 != err) {
printf("Error occurred during TCP listen: errno %d\n", errno);
ESP_LOGE(TAG, "Error occurred during TCP listen: errno %d\n", errno);
close(that->m_server_sock);
that->m_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
@@ -97,13 +96,13 @@ TCPServer::~TCPServer() {
socklen_t addr_len = sizeof(client_addr);
int client_sock = accept(that->m_server_sock, reinterpret_cast<struct sockaddr *>(&client_addr), &addr_len);
if (client_sock < 0) {
printf("Unable to accept TCP connection: errno %d\n", errno);
ESP_LOGE(TAG, "Unable to accept TCP connection: errno %d\n", errno);
continue;
}
err = that->authenticate_client(client_sock);
if (0 != err) {
printf("Client failed authentication\n");
ESP_LOGE(TAG, "Client failed authentication\n");
}
setsockopt(client_sock, SOL_SOCKET, SO_KEEPALIVE, &keepAlive, sizeof(int));
@@ -154,18 +153,17 @@ TCPServer::~TCPServer() {
continue;
}
printf("Message size: %ld\n", msg_size);
ESP_LOGD(TAG, "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);
if (int len = recv(sock, buffer->data(), msg_size, MSG_WAITALL); len < 0) {
ESP_LOGE(TAG, "Error occurred during receiving: errno %d\n", errno);
to_remove.emplace_back(sock);
} else if (0 == len) {
printf("Connection closed\n");
ESP_LOGW(TAG, "Connection closed\n");
close(sock);
to_remove.emplace_back(sock);
} else {
printf("TCP Server Received %d bytes\n", len);
ESP_LOGD(TAG, "TCP Server Received %d bytes\n", len);
buffer->resize(len);
that->m_rx_queue->enqueue(std::move(buffer));
}
@@ -219,7 +217,6 @@ int TCPServer::send_msg(char *buffer, uint32_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);
}

View File

@@ -1,30 +1,11 @@
#include "WifiManager.h"
#include <ConfigManager.h>
#include <esp_netif.h>
#include <esp_event.h>
#include <freertos/semphr.h>
#include <cstring>
#include <mDNSDiscoveryService.h>
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "esp_wifi.h"
#include "WifiManager.h"
#include "ConfigManager.h"
#include "constants/wifi.h"
#include "mDNSDiscoveryService.h"
WifiManager::WifiManager() {
esp_netif_init();
esp_wifi_set_storage(WIFI_STORAGE_RAM);
esp_event_loop_create_default();
this->m_mutex = xSemaphoreCreateMutex();
this->m_state = wifi_state::disconnected;
this->m_attempts = 0;
this->m_task = nullptr;
this->m_netif = nullptr;
xTaskCreate(reinterpret_cast<TaskFunction_t>(s_manage), "wifi_task", 3096, this, 5, &m_task);
}
#define TAG "WifiManager"
WifiManager::~WifiManager() {
this->handle_disconnect();
@@ -53,21 +34,21 @@ int WifiManager::disconnect() {
switch (state) {
case wifi_state::connect:
printf("Attempting to connect to wifi in station mode\n");
ESP_LOGI(TAG, "Attempting to connect to wifi in station mode\n");
init_connection();
update_state(wifi_state::connecting);
break;
case wifi_state::connecting:
printf("connecting...\n");
ESP_LOGI(TAG, "connecting...\n");
handle_connecting();
break;
case wifi_state::broadcast:
printf("Attempting to broadcast in softap mode\n");
ESP_LOGI(TAG, "Attempting to broadcast in softap mode\n");
init_softap();
update_state(wifi_state::broadcasting);
break;
case wifi_state::disconnect:
printf("Shutting down wifi\n");
ESP_LOGI(TAG, "Shutting down wifi\n");
handle_disconnect();
update_state(wifi_state::disconnected);
break;
@@ -103,8 +84,8 @@ int WifiManager::init_connection() {
}
};
std::string ssid = ConfigManager::get_wifi_ssid();
std::string pass = ConfigManager::get_wifi_password();
std::string ssid = m_config_manager.get_wifi_ssid();
std::string pass = m_config_manager.get_wifi_password();
std::strncpy(reinterpret_cast<char *>(wifi_configuration.sta.ssid), ssid.c_str(), 32);
std::strncpy(reinterpret_cast<char *>(wifi_configuration.sta.password), pass.c_str(), 64);
wifi_configuration.sta.ssid[31] = '\0';
@@ -190,13 +171,13 @@ void WifiManager::wifi_event_handler(void *event_handler_arg, esp_event_base_t e
const auto that = static_cast<WifiManager *>(event_handler_arg);
if (WIFI_EVENT_STA_START == event_id) {
printf("Station mode started\n");
ESP_LOGI(TAG, "Station mode started\n");
} else if (WIFI_EVENT_STA_CONNECTED == event_id) {
printf("Connected to wifi in station mode\n");
ESP_LOGI(TAG, "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");
ESP_LOGI(TAG, "Station mode shutdown\n");
xSemaphoreTake(that->m_mutex, portMAX_DELAY);
if (that->m_state == wifi_state::connected) {
xSemaphoreGive(that->m_mutex);
@@ -206,13 +187,13 @@ void WifiManager::wifi_event_handler(void *event_handler_arg, esp_event_base_t e
xSemaphoreGive(that->m_mutex);
}
} else if (IP_EVENT_STA_GOT_IP == event_id) {
printf("Got IP as station\n");
ESP_LOGI(TAG, "Got IP as station\n");
} else if (WIFI_EVENT_AP_STACONNECTED == event_id) {
printf("User connected to AP\n");
ESP_LOGI(TAG, "User connected to AP\n");
} else if (WIFI_EVENT_AP_STADISCONNECTED == event_id) {
printf("User disconnected from AP\n");
ESP_LOGI(TAG, "User disconnected from AP\n");
} else if (WIFI_EVENT_AP_START == event_id) {
mDNSDiscoveryService::setup();
printf("AP started\n");
ESP_LOGI(TAG, "AP started\n");
}
}

View File

@@ -36,17 +36,18 @@ public:
CommunicationRouter(const std::function<void(char*, int)> &rx_callback, std::unique_ptr<WifiManager>&& pc_connection)
: m_tcp_rx_queue(std::make_shared<PtrQueue<std::vector<uint8_t>>>(10)),
m_rx_callback(rx_callback),
m_config_manager(ConfigManager::get_instance()),
m_tcp_server(std::make_unique<TCPServer>(TCP_PORT, m_tcp_rx_queue)),
m_data_link_manager(std::make_unique<DataLinkManager>(ConfigManager::get_module_id(), MODULE_TO_NUM_CHANNELS_MAP[ConfigManager::get_module_type()])),
m_data_link_manager(std::make_unique<DataLinkManager>(m_config_manager.get_module_id(), MODULE_TO_NUM_CHANNELS_MAP[m_config_manager.get_module_type()])),
m_pc_connection(std::move(pc_connection)),
m_module_id(ConfigManager::get_module_id()),
m_module_id(m_config_manager.get_module_id()),
m_last_leader_updated(std::chrono::system_clock::now()){
OrientationDetection::init();
update_leader();
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()];
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);
@@ -73,6 +74,7 @@ public:
std::function<void(char*, int)> m_rx_callback;
private:
TaskHandle_t m_router_thread = nullptr;
ConfigManager &m_config_manager;
std::vector<TaskHandle_t> m_link_layer_threads;
std::unique_ptr<TCPServer> m_tcp_server; // todo: dependency injection
std::unique_ptr<DataLinkManager> m_data_link_manager;

View File

@@ -14,8 +14,9 @@
class MessagingInterface {
public:
explicit MessagingInterface(std::unique_ptr<WifiManager>&& pc_connection)
: m_mpi_rx_queue(xQueueCreate(MAX_RX_BUFFER_SIZE, RX_QUEUE_SIZE)),
m_router(std::make_unique<CommunicationRouter>([this](char* buffer, const int size) { handleRecv(buffer, size); }, std::move(pc_connection))),
: m_config_manager(ConfigManager::get_instance()),
m_mpi_rx_queue(xQueueCreate(MAX_RX_BUFFER_SIZE, RX_QUEUE_SIZE)),
m_router(std::make_unique<CommunicationRouter>([this](const char* buffer, const int size) { handleRecv(buffer, size); }, std::move(pc_connection))),
m_map_semaphore(xSemaphoreCreateMutex()) {};
~MessagingInterface();
@@ -31,7 +32,8 @@ private:
void checkOrInsertTag(uint8_t tag);
uint16_t sequence_number = 0;
ConfigManager& m_config_manager;
uint16_t m_sequence_number = 0;
QueueHandle_t m_mpi_rx_queue; // todo: maybe move this down classes more
std::unique_ptr<CommunicationRouter> m_router;
SemaphoreHandle_t m_map_semaphore;

View File

@@ -2,17 +2,27 @@
#define NETWORKMANAGER_H
#include <esp_netif_types.h>
#include <esp_event.h>
#include <esp_netif.h>
#include <esp_wifi.h>
#include <freertos/FreeRTOS.h>
#include <freertos/semphr.h>
#include "esp_event.h"
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "ConfigManager.h"
#include "IWifiManager.h"
class WifiManager final : IWifiManager {
public:
WifiManager();
WifiManager() : m_config_manager(ConfigManager::get_instance()),
m_mutex(xSemaphoreCreateMutex()),
m_state(wifi_state::disconnected) {
esp_netif_init();
esp_wifi_set_storage(WIFI_STORAGE_RAM);
esp_event_loop_create_default();
// todo: move all task metadata to a constants/config file
xTaskCreate(reinterpret_cast<TaskFunction_t>(s_manage), "wifi_task", 3096, this, 5, &m_task);
}
~WifiManager() override;
int connect() override;
int disconnect() override;
@@ -31,11 +41,12 @@ private:
static void wifi_event_handler(void *event_handler_arg, esp_event_base_t event_base, int32_t event_id, void *event_data);
ConfigManager& m_config_manager;
SemaphoreHandle_t m_mutex;
wifi_state m_state;
TaskHandle_t m_task;
unsigned int m_attempts;
esp_netif_t *m_netif;
TaskHandle_t m_task = nullptr;
unsigned int m_attempts = 0;
esp_netif_t *m_netif = nullptr;
};

View File

@@ -18,15 +18,18 @@
// todo: clean this up (strange to be a constructor) also need to add more details, need to add to routing table
void mDNSDiscoveryService::setup() {
mdns_init();
mdns_hostname_set(std::format("botchain-{}", ConfigManager::get_module_id()).c_str());
mdns_instance_name_set(std::format("BotChain Robot Module {}", ConfigManager::get_module_id()).c_str());
const auto& config_manager = ConfigManager::get_instance();
mdns_hostname_set(std::format("botchain-{}", config_manager.get_module_id()).c_str());
mdns_instance_name_set(std::format("BotChain Robot Module {}", config_manager.get_module_id()).c_str());
mdns_service_add(nullptr, "_robotcontrol", "_tcp", TCP_PORT, nullptr, 0);
mdns_service_instance_name_set("_robotcontrol", "_tcp", "Robot Control TCP Server");
mdns_txt_item_t service_txt_data[3] = {
{"module_id",std::to_string(ConfigManager::get_module_id()).c_str()},
{"module_type",std::to_string(ConfigManager::get_module_type()).c_str()},
{"module_id",std::to_string(config_manager.get_module_id()).c_str()},
{"module_type",std::to_string(config_manager.get_module_type()).c_str()},
{"connected_modules",""},
};
@@ -36,6 +39,8 @@ void mDNSDiscoveryService::setup() {
void mDNSDiscoveryService::set_connected_boards(const std::vector<int>& boards) {
std::stringstream ss;
const auto& config_manager = ConfigManager::get_instance();
for (int i = 0; i < boards.size(); i++) {
ss << std::to_string(boards[i]);
if (i < boards.size() - 1) {
@@ -44,8 +49,8 @@ void mDNSDiscoveryService::set_connected_boards(const std::vector<int>& boards)
}
mdns_txt_item_t service_txt_data[3] = {
{"module_id",std::to_string(ConfigManager::get_module_id()).c_str()},
{"module_type",std::to_string(ConfigManager::get_module_type()).c_str()},
{"module_id",std::to_string(config_manager.get_module_id()).c_str()},
{"module_type",std::to_string(config_manager.get_module_type()).c_str()},
{"connected_modules",ss.str().c_str()},
};
mdns_service_txt_set("_robotcontrol", "_tcp", service_txt_data, 3);