UDP implementation

components/constants/include/constants/udp.h

@@ -0,0 +1,18 @@
+//
+// Created by Johnathon Slightham on 2025-12-21.
+//
+
+#ifndef UDP_H
+#define UDP_H
+
+#define SLEEP_AFTER_FAIL_MS         5000
+
+#define NO_CLIENT_SLEEP_MS          400
+
+#define RECV_MCAST                  "239.1.1.1"
+#define RECV_PORT                   3101
+
+#define SEND_MCAST                  "239.1.1.2"
+#define SEND_PORT                   3100
+
+#endif //UDP_H

components/dataLink/DataLinkRIP.cpp

@@ -452,5 +452,5 @@ void DataLinkManager::start_rip_tasks(){
     ESP_LOGI(DEBUG_LINK_TAG, "Starting RIP Broadcast task");
     xTaskCreate(DataLinkManager::rip_broadcast_timer_function, "RIPBroadcast", 4096, static_cast<void*>(this), 5, &rip_broadcast_task);
     ESP_LOGI(DEBUG_LINK_TAG, "Starting RIP TTL task");
-    xTaskCreate(DataLinkManager::rip_ttl_decrement_task, "RIPTTL", 2048, static_cast<void*>(this), 5, &rip_ttl_task);
+    xTaskCreate(DataLinkManager::rip_ttl_decrement_task, "RIPTTL", 4096, static_cast<void*>(this), 5, &rip_ttl_task);
 }

components/rpc/CommunicationFactory.cpp

@@ -6,8 +6,10 @@
 
 #include "CommunicationFactory.h"
 #include "constants/tcp.h"
+#include "constants/udp.h"
 #include "wireless/mDNSDiscoveryService.h"
 #include "wireless/TCPServer.h"
+#include "wireless/UDPServer.h"
 #include "wireless/WifiManager.h"
 
 std::unique_ptr<IConnectionManager> CommunicationFactory::create_connection_manager(const CommunicationMethod type) {
@@ -31,7 +33,7 @@ std::unique_ptr<IDiscoveryService> CommunicationFactory::create_discovery_servic
 std::unique_ptr<IRPCServer> CommunicationFactory::create_lossy_server(const CommunicationMethod type, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue) {
     switch (type) {
         case Wireless:
-            return std::make_unique<TCPServer>(TCP_PORT, rx_queue); // todo: replace with udp server
+            return std::make_unique<UDPServer>(RECV_PORT, SEND_PORT, rx_queue);
         default:
             return nullptr;
     }

components/rpc/CommunicationRouter.cpp

@@ -85,11 +85,11 @@ void CommunicationRouter::update_leader() {
         if (max == m_module_id) {
             m_pc_connection->connect();
             m_lossless_server->startup();
-            // todo: BTS-22 add lossy server
+            m_lossy_server->startup();
         } else if (this->m_leader == m_module_id) {
             m_pc_connection->disconnect();
             m_lossless_server->shutdown();
-            // todo: BTS-22 add lossy server
+            m_lossless_server->shutdown();
         }
     }
 
@@ -113,7 +113,11 @@ void CommunicationRouter::route(uint8_t* buffer, const size_t length) const {
         this->m_rx_callback(reinterpret_cast<char *>(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<int>(mpi_message->destination()), length);
+        if (mpi_message->is_durable()) {
             this->m_lossless_server->send_msg(reinterpret_cast<char *>(buffer), 512);
+        } else {
+            this->m_lossy_server->send_msg(reinterpret_cast<char *>(buffer), 512);
+        }
     } else if (mpi_message->destination() == PC_ADDR) {
         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);

components/rpc/include/CommunicationRouter.h

@@ -37,6 +37,7 @@ public:
             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<DataLinkManager>(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()),
@@ -44,14 +45,14 @@ public:
         OrientationDetection::init();
         update_leader();
 
-        xTaskCreate(router_thread, "communication_router", 4096, this, 3, &this->m_router_thread);
+        xTaskCreate(router_thread, "router", 4096, this, 3, &this->m_router_thread);
 
-        const auto num_channels = MODULE_TO_NUM_CHANNELS_MAP[m_config_manager.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);
+        // this->m_link_layer_threads.resize(num_channels);
-        for (int i = 0; i < num_channels; i++) {
+        // for (int i = 0; i < num_channels; i++) {
-            auto *params = new link_layer_thread_params(this, 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]);
+        //     xTaskCreate(link_layer_thread, "router_rmt", 4096, params, 3, &this->m_link_layer_threads[i]);
-        }
+        // }
     }
 
     ~CommunicationRouter();
@@ -65,7 +66,7 @@ public:
     [[nodiscard]] uint8_t get_leader() const;
 
     // todo: does this really need to be here (so i can access from thread)?
-    std::shared_ptr<PtrQueue<std::vector<uint8_t>>> m_tcp_rx_queue;
+    std::shared_ptr<PtrQueue<std::vector<uint8_t>>> m_tcp_rx_queue; // todo: this should probably be thread safe
     std::function<void(char*, int)> m_rx_callback;
 private:
     TaskHandle_t m_router_thread = nullptr;
@@ -73,6 +74,7 @@ private:
     std::unique_ptr<IConnectionManager> m_pc_connection;
     std::vector<TaskHandle_t> m_link_layer_threads;
     std::unique_ptr<IRPCServer> m_lossless_server;
+    std::unique_ptr<IRPCServer> m_lossy_server;
     std::unique_ptr<DataLinkManager> m_data_link_manager;
     uint8_t m_leader = 0;
     uint8_t m_module_id;

components/rpc/include/wireless/UDPServer.h

@@ -0,0 +1,41 @@
+//
+// Created by Johnathon Slightham on 2025-05-25.
+//
+
+#ifndef UDPSERVER_H
+#define UDPSERVER_H
+
+#include <memory>
+#include <vector>
+#include <unordered_set>
+
+#include "freertos/FreeRTOS.h"
+#include "IRPCServer.h"
+#include "PtrQueue.h"
+
+class UDPServer final : public IRPCServer {
+public:
+    UDPServer(int rx_port, int tx_port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue);
+    ~UDPServer() override;
+    void startup() override;
+    void shutdown() override;
+    int send_msg(char* buffer, uint32_t length) const override;
+
+private:
+    bool authenticate_client(int client_sock);
+
+    static bool is_network_connected();
+    [[noreturn]] static void socket_monitor_thread(void *args);
+
+	int m_tx_port;
+    int m_rx_port;
+
+    int m_tx_server_sock;
+	int m_rx_server_sock;
+
+    TaskHandle_t m_rx_task;
+
+    std::shared_ptr<PtrQueue<std::vector<uint8_t>>> m_rx_queue;
+};
+
+#endif //UDPSERVER_H

components/rpc/wireless/TCPServer.cpp

@@ -84,7 +84,7 @@ void TCPServer::shutdown() {
 
         constexpr int opt = 1;
         setsockopt(that->m_server_sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
-        ESP_LOGI(TAG, "Socket created\n");
+        ESP_LOGI(TAG, "Socket created");
 
         sockaddr_in server_addr = {
             .sin_family = AF_INET,

components/rpc/wireless/UDPServer.cpp

@@ -0,0 +1,214 @@
+#include <memory>
+#include <cstring>
+
+#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 "wireless/UDPServer.h"
+#include "bits/shared_ptr_base.h"
+#include "constants/app_comms.h"
+#include "constants/udp.h"
+#include "esp_netif.h"
+
+#define TAG "UDPServer"
+
+#define MIN(X, Y) (((X) < (Y)) ? (X) : (Y))
+
+// todo: - authenticate
+
+UDPServer::UDPServer(const int rx_port, const int tx_port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue) {
+    this->m_rx_port = rx_port;
+	this->m_tx_port = tx_port;
+    this->m_rx_task = nullptr;
+    this->m_rx_queue = rx_queue;
+    this->m_rx_server_sock = 0;
+	this->m_tx_server_sock = 0;
+}
+
+UDPServer::~UDPServer() {
+    this->shutdown();
+}
+
+void UDPServer::startup() {
+    ESP_LOGI(TAG, "Starting UDP server on port %d", this->m_rx_port);
+    if (nullptr != this->m_rx_task) {
+        ESP_LOGW(TAG, "Attempted to start UDP server when already started, ignoring start request");
+        return;
+    }
+
+    xTaskCreate(socket_monitor_thread, "udp_rx", 4096, this, 5, &this->m_rx_task);
+}
+
+void UDPServer::shutdown() {
+    ESP_LOGI(TAG, "Shutting down UDP server");
+    if (nullptr != this->m_rx_task) {
+        vTaskDelete(this->m_rx_task);
+    }
+}
+
+[[noreturn]] void UDPServer::socket_monitor_thread(void *args) {
+    const auto that = static_cast<UDPServer *>(args);
+
+    while(true) {
+        ESP_LOGI(TAG, "Attempting to start UDP Server on %d", that->m_rx_port);
+
+        if (!is_network_connected()) {
+            ESP_LOGW(TAG, "Network is disconnected");
+            vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
+            continue;
+        }
+
+        sockaddr_in saddr = {0};
+        sockaddr_in from_addr = {0};
+
+        that->m_rx_server_sock = socket(AF_INET, SOCK_DGRAM, 0);
+        if (that->m_rx_server_sock == -1) {
+            ESP_LOGE(TAG, "Create UDP socket fail");
+            vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
+            continue;
+        }
+
+        that->m_tx_server_sock = socket(AF_INET, SOCK_DGRAM, 0);
+        if (that->m_tx_server_sock < 0) {
+            ESP_LOGE(TAG, "Unable to create UDP tx socket: errno %d", errno);
+            close(that->m_rx_server_sock);
+            that->m_rx_server_sock = -1;
+            vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
+            continue;
+        }
+
+        int reuse = 1;
+        if (setsockopt(that->m_rx_server_sock, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)) < 0) {
+            ESP_LOGE(TAG, "Failed to set SO_REUSEADDR. Error %d", errno);
+            close(that->m_rx_server_sock);
+            close(that->m_tx_server_sock);
+            that->m_rx_server_sock = -1;
+            that->m_tx_server_sock = -1;
+            vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
+            continue;
+        }
+
+        saddr.sin_family = AF_INET;
+        saddr.sin_port = htons(that->m_rx_port);
+        saddr.sin_addr.s_addr = htonl(INADDR_ANY);
+        int ret = bind(that->m_rx_server_sock, (struct sockaddr *)&saddr, sizeof(struct sockaddr_in));
+        if (ret < 0) {
+            ESP_LOGE(TAG, "Failed to bind socket. Error %d", errno);
+            close(that->m_rx_server_sock);
+            close(that->m_tx_server_sock);
+            that->m_rx_server_sock = -1;
+            that->m_tx_server_sock = -1;
+            vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
+            continue;
+        }
+
+        struct ip_mreq imreq = {};
+        imreq.imr_multiaddr.s_addr = inet_addr(RECV_MCAST);
+        imreq.imr_interface.s_addr = htonl(INADDR_ANY);
+        if(setsockopt(that->m_rx_server_sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &imreq, sizeof(struct ip_mreq)) < 0) {
+            ESP_LOGE(TAG, "Failed to set IP_ADD_MEMBERSHIP. Error %d", errno);
+            close(that->m_rx_server_sock);
+            close(that->m_tx_server_sock);
+            that->m_rx_server_sock = -1;
+            that->m_tx_server_sock = -1;
+            vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
+            continue;
+        }
+
+        uint32_t msg_size;
+        while(is_network_connected()) {
+            auto buffer = std::make_unique<std::vector<uint8_t>>();
+            buffer->resize(MAX_RX_BUFFER_SIZE + 4);
+
+            ESP_LOGI(TAG, "Before rx");
+            if (int len = recvfrom(that->m_rx_server_sock, buffer->data(), MAX_RX_BUFFER_SIZE, 0, nullptr, nullptr); len < 0) {
+                ESP_LOGE(TAG, "Error occurred during receiving: errno %d", errno);
+            } else if (len < 4 || len > MAX_RX_BUFFER_SIZE) {
+				ESP_LOGE(TAG, "Got illegal message size");
+			} else {
+            	msg_size = *reinterpret_cast<uint32_t*>(buffer->data());
+                if (msg_size > len - 4) {
+                    ESP_LOGW(TAG, "Message size incorrect");
+                    continue;
+                }
+				buffer->erase(buffer->begin(), buffer->begin() + 4); // todo: copying
+            	buffer->resize(msg_size);
+            	that->m_rx_queue->enqueue(std::move(buffer));
+			}
+        }
+
+        ESP_LOGW(TAG, "Network disconnected");
+		close(that->m_tx_server_sock);
+		that->m_tx_server_sock = -1;
+        vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
+    }
+}
+
+bool UDPServer::is_network_connected() {
+    esp_netif_ip_info_t ip_info;
+    esp_netif_t *netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
+
+    if (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) {
+        return true;
+    }
+
+    if (0 != ip_info.ip.addr) {
+        return true;
+    }
+
+    netif = esp_netif_get_handle_from_ifkey("WIFI_AP_DEF");
+
+    if (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) {
+        return true;
+    }
+
+    if (0 != ip_info.ip.addr) {
+        return true;
+    }
+
+    return false;
+}
+
+bool UDPServer::authenticate_client(int sock) {
+    // todo: authentication (key?)
+    return 0;
+}
+
+int UDPServer::send_msg(char *buffer, const uint32_t length) const {
+    if (!is_network_connected() || m_tx_server_sock == -1) {
+        return -1;
+    }
+
+	sockaddr_in mcast_dest = {
+    	.sin_family = AF_INET,
+    	.sin_port = htons(m_tx_port),
+    	.sin_addr = {
+			.s_addr = inet_addr(SEND_MCAST)
+		},
+	};
+
+	uint32_t size = length;
+
+	iovec iov[2];
+	iov[0].iov_base = &size;
+	iov[0].iov_len = 4;
+	iov[1].iov_base = buffer;
+	iov[1].iov_len = length;
+
+	msghdr msg = {};
+	msg.msg_iov = iov;
+	msg.msg_iovlen = 2;
+	msg.msg_name = &mcast_dest;
+	msg.msg_namelen = sizeof(mcast_dest);
+
+	sendmsg(this->m_tx_server_sock, &msg, 0);
+
+    return 0;
+}

components/rpc/wireless/WifiManager.cpp

@@ -108,6 +108,8 @@ int WifiManager::init_connection() {
     esp_wifi_start();
     esp_wifi_connect();
 
+    esp_netif_set_default_netif(esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"));
+
     return 0;
 }
 
@@ -199,6 +201,8 @@ int WifiManager::init_softap() {
 
     esp_wifi_start();
 
+   	esp_netif_set_default_netif(esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"));
+
     return 0;
 }
 
@@ -214,12 +218,12 @@ 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) {
-        ESP_LOGI(TAG, "Station mode started\n");
+        ESP_LOGI(TAG, "Station mode started");
     } else if (WIFI_EVENT_STA_CONNECTED == event_id) {
-        ESP_LOGI(TAG, "Connected to wifi in station mode\n");
+        ESP_LOGI(TAG, "Connected to wifi in station mode");
         that->update_state(wifi_state::connected);
     } else if (WIFI_EVENT_STA_DISCONNECTED == event_id) {
-        ESP_LOGI(TAG, "Station mode shutdown\n");
+        ESP_LOGI(TAG, "Station mode shutdown");
         xSemaphoreTake(that->m_mutex, portMAX_DELAY);
         if (that->m_state == wifi_state::connected) {
             xSemaphoreGive(that->m_mutex);
@@ -228,12 +232,12 @@ 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) {
-        ESP_LOGI(TAG, "Got IP as station\n");
+        ESP_LOGI(TAG, "Got IP as station");
     } else if (WIFI_EVENT_AP_STACONNECTED == event_id) {
-        ESP_LOGI(TAG, "User connected to AP\n");
+        ESP_LOGI(TAG, "User connected to AP");
     } else if (WIFI_EVENT_AP_STADISCONNECTED == event_id) {
-        ESP_LOGI(TAG, "User disconnected from AP\n");
+        ESP_LOGI(TAG, "User disconnected from AP");
     } else if (WIFI_EVENT_AP_START == event_id) {
-        ESP_LOGI(TAG, "AP started\n");
+        ESP_LOGI(TAG, "AP started");
     }
 }

main/LoopManager.cpp

@@ -14,6 +14,7 @@
 
 #define ACTUATOR_CMD_TAG 5
 #define TOPOLOGY_CMD_TAG 6
+#define METADATA_RX_TAG 7
 
 #define METADATA_PERIOD_MS 1000
 
@@ -51,7 +52,18 @@
             casted_orientations,
             that->m_messaging_interface->get_connection_type(),
             that->m_messaging_interface->get_leader());
-        that->m_messaging_interface->send(static_cast<char *>(data), size, PC_ADDR, TOPOLOGY_CMD_TAG, true);
+        that->m_messaging_interface->send(static_cast<char *>(data), size, PC_ADDR, TOPOLOGY_CMD_TAG, false);
         vTaskDelay(METADATA_PERIOD_MS / portTICK_PERIOD_MS);
     }
 }
+
+[[noreturn]] void LoopManager::metadata_rx_loop(char *args) {
+    const auto that = reinterpret_cast<LoopManager *>(args);
+    const auto buffer = std::make_unique<std::vector<char>>();
+    buffer->resize(512);
+    while (true) {
+        that->m_messaging_interface->recv(buffer->data(), 512, PC_ADDR, METADATA_RX_TAG);
+
+
+    }
+}

main/control/ActuatorFactory.cpp

@@ -9,7 +9,8 @@
 #include "control/Servo1Actuator.h"
 #include "flatbuffers_generated/RobotModule_generated.h"
 
-std::unique_ptr<IActuator> ActuatorFactory::create_actuator(const ModuleType type) {
+std::unique_ptr<IActuator>
+ActuatorFactory::create_actuator(const ModuleType type) {
   switch (type) {
   case ModuleType_SERVO_1:
     return std::make_unique<Servo1Actuator>();

main/control/Servo1Actuator.cpp

@@ -3,10 +3,10 @@
 //
 
 #include "control/Servo1Actuator.h"
-#include "util/number_utils.h"
-#include "driver/ledc.h"
-#include "constants/module.h"
 #include "AngleControlMessageBuilder.h"
+#include "constants/module.h"
+#include "driver/ledc.h"
+#include "util/number_utils.h"
 
 #define LOW_DUTY 200
 #define HIGH_DUTY 1000
@@ -36,8 +36,12 @@ Servo1Actuator::Servo1Actuator() {
 }
 
 void Servo1Actuator::actuate(uint8_t *cmd) {
-    const auto* angleControlCmd = Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd);
+  const auto *angleControlCmd =
-    const auto newDuty = util::mapRange<int32_t>(angleControlCmd->angle(), 0, 180, LOW_DUTY, HIGH_DUTY);
+      Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd);
+  const auto newDuty = util::mapRange<int32_t>(angleControlCmd->angle(), 0, 180,
+                                               LOW_DUTY, HIGH_DUTY);
+
+  std::cout << "actuating to " << angleControlCmd->angle() << std::endl;
 
   ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, newDuty));
   ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0));