Reduce DataLink latency

components/dataLink/CMakeLists.txt

@@ -1,4 +1,4 @@
 idf_component_register(SRCS "DataLinkManager.cpp" "DataLinkRIP.cpp" "DataLinkScheduler.cpp" "DataLinkFrames.cpp"
                        PRIV_REQUIRES driver esp_event nvs_flash esp_netif rmt
-                       REQUIRES esp_timer
+                       REQUIRES esp_timer ptrQueue
                        INCLUDE_DIRS "include")

components/dataLink/DataLinkFrames.cpp

@@ -1,5 +1,7 @@
 #include "DataLinkManager.h"
 #include "esp_log.h"
+#include <cstring>
+#include <type_traits>
 
 /**
  * @brief Creates a Control Frame from `FrameHeader`
@@ -152,7 +154,8 @@ esp_err_t DataLinkManager::complete_fragment(uint16_t board_id, uint16_t sequenc
     uint16_t total_data_len = metadata.num_fragments_rx*MAX_FRAME_SIZE; //max data size with n fragments
     xSemaphoreGive(rx_fragment_mutex[channel]);
 
-    uint8_t* combined_data = (uint8_t*)pvPortMalloc(total_data_len);
+    auto combined_data = std::make_unique<std::vector<uint8_t>>();
+    combined_data->resize(total_data_len);
     rx.data_len = total_data_len;
     if (combined_data == nullptr){
         return ESP_ERR_NO_MEM;
@@ -173,15 +176,15 @@ esp_err_t DataLinkManager::complete_fragment(uint16_t board_id, uint16_t sequenc
         return ESP_ERR_NOT_FOUND;
     }
 
-    rx.data = combined_data;
     uint16_t prev_index = 0;
     for (size_t i = 0; i < metadata.num_fragments_rx; i++){
-        memcpy(&combined_data[prev_index], metadata.fragments[i].data, metadata.fragments[i].data_len);
+        memcpy(&combined_data->data()[prev_index], metadata.fragments[i].data, metadata.fragments[i].data_len);
         prev_index += metadata.fragments[i].data_len;
     }
 
     xSemaphoreGive(rx_fragment_mutex[channel]);
 
+    rx.data = std::move(combined_data);
     rx.data_len = prev_index;
 
     if (async_rx_queue_mutex[channel] == nullptr){
@@ -203,15 +206,10 @@ esp_err_t DataLinkManager::complete_fragment(uint16_t board_id, uint16_t sequenc
 
     // ESP_LOGI(DEBUG_LINK_TAG, "pushing frame %d onto async rx queue", sequence_num);
 
-    if (xSemaphoreTake(async_rx_queue_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){
+    if (!async_receive_queue->enqueue(std::move(rx), std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS))) {
-        vPortFree(combined_data);
         return ESP_ERR_TIMEOUT;
     }
 
-    async_receive_queue[channel].push(rx);
-
-    xSemaphoreGive(async_rx_queue_mutex[channel]);
-
     fragment_map[channel][board_id].erase(sequence_num);
 
     if (fragment_map[channel][board_id].empty()) {
@@ -234,42 +232,11 @@ esp_err_t DataLinkManager::complete_fragment(uint16_t board_id, uint16_t sequenc
  * @note This may be moved to a private function - Unsure if users should be able to manually send ACKs
  */
 esp_err_t DataLinkManager::send_ack(uint8_t sender_id, uint8_t* data, uint16_t data_len){
-    return send(sender_id, data, data_len, FrameType::ACK_TYPE, 0x0);
+    // todo: change this to take in a unique_ptr
-}
+    auto buffer = std::make_unique<std::vector<uint8_t>>();
-
+    buffer->resize(data_len);
-/**
+    memcpy(buffer->data(), data, data_len);
- * @brief Checks the channel receive queue for any received frames. If there is, return the first frame's data size
+    return send(sender_id, std::move(buffer), FrameType::ACK_TYPE, 0x0);
- * 
- * @param frame_size Size of the data
- * @param header Header information of the combined generic frames
- * 
- * @return esp_err_t 
- */
-esp_err_t DataLinkManager::async_receive_info(uint16_t* frame_size, FrameHeader* header, uint8_t channel){
-    if (frame_size == nullptr || header == nullptr){
-        return ESP_ERR_INVALID_ARG;
-    }
-
-    Rx_Metadata top;
-    if (xSemaphoreTake(async_rx_queue_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){
-        return ESP_ERR_TIMEOUT;
-    }
-    if (async_receive_queue[channel].size() == 0){
-        xSemaphoreGive(async_rx_queue_mutex[channel]);
-
-        *frame_size = 0;
-
-        return ESP_OK;
-    }
-    top = async_receive_queue[channel].front();
-
-    xSemaphoreGive(async_rx_queue_mutex[channel]);
-
-    *frame_size = top.data_len;
-
-    *header = top.header;
-
-    return ESP_OK;
 }
 
 /**
@@ -280,49 +247,20 @@ esp_err_t DataLinkManager::async_receive_info(uint16_t* frame_size, FrameHeader*
  * @param header Header information of returning frame
  *
  */
-esp_err_t DataLinkManager::async_receive(uint8_t* data, uint16_t data_len, FrameHeader* header, uint8_t channel){
+std::optional<std::unique_ptr<std::vector<uint8_t>>> DataLinkManager::async_receive(){
-    if (data == nullptr || header == nullptr){
+    auto maybe_top = async_receive_queue->dequeue(std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS));
-        return ESP_ERR_INVALID_ARG;
+    if (!maybe_top) {
+        return std::nullopt;
     }
+    Rx_Metadata top = std::move(*maybe_top);
 
-    if (data_len == 0){
+    return std::make_optional<std::unique_ptr<std::vector<uint8_t>>>(std::move(top.data));
-        return ESP_ERR_INVALID_ARG;
-    }
-
-    Rx_Metadata top;
-    if (xSemaphoreTake(async_rx_queue_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){
-        return ESP_ERR_TIMEOUT;
-    }
-
-    if (async_receive_queue[channel].size() == 0){
-        xSemaphoreGive(async_rx_queue_mutex[channel]);
-        return ESP_ERR_NOT_FOUND;
-    }
-
-    top = async_receive_queue[channel].front();
-    async_receive_queue[channel].pop();
-
-    xSemaphoreGive(async_rx_queue_mutex[channel]);
-    
-    if (data_len < top.data_len){
-        vPortFree(top.data);
-        return ESP_ERR_INVALID_ARG;
-    }
-
-    *header = top.header;
-
-    memcpy(data, top.data, top.data_len);
-    vPortFree(top.data);
-
     // ESP_LOGI(DEBUG_LINK_TAG, "pushed frame %d onto async queue", header->seq_num);
-
-    return ESP_OK;
 }
 
 esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
     uint16_t data_len = MAX_FRAME_SIZE; //max possible data len
     uint8_t data[data_len];
-    memset(data, 0, data_len);
 
     size_t recv_len = 0;
 
@@ -343,16 +281,18 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
         return ESP_ERR_INVALID_RESPONSE;
     }
 
-    uint8_t message[MAX_FRAME_SIZE];
+    auto message = std::make_unique<std::vector<uint8_t>>();
-    memset(message, 0, sizeof(message));
+    message->resize(MAX_FRAME_SIZE);
+
     size_t message_size = 0;
     FrameHeader header;
 
-    res = get_data_from_frame(data, recv_len, message, &message_size, &header);
+    res = get_data_from_frame(data, recv_len, message->data(), &message_size, &header);
     if (res != ESP_OK){
         // print_buffer_binary(message, message_size);
         return res;
     }
+    message->resize(message_size);
 
     // print_buffer_binary(message, message_size);
 
@@ -362,14 +302,14 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
             return ESP_OK;
         }
 
-        if (message[0] != GENERIC_FRAG_ACK_PREAMBLE){
+        if (message->data()[0] != GENERIC_FRAG_ACK_PREAMBLE){
             return ESP_OK;
         }
 
         FrameAckRecord record = {
-            .last_ack = static_cast<uint16_t>((message[1] << 8) | (message[2])),
+            .last_ack = static_cast<uint16_t>((message->data()[1] << 8) | (message->data()[2])),
-            .total_frags = static_cast<uint16_t>((message[3] << 8) | (message[4])),
+            .total_frags = static_cast<uint16_t>((message->data()[3] << 8) | (message->data()[4])),
-            .seq_num = static_cast<uint16_t>((message[5] << 8) | (message[6]))
+            .seq_num = static_cast<uint16_t>((message->data()[5] << 8) | (message->data()[6]))
         };
 
         res = inc_head_sliding_window(channel, header.sender_id, record.seq_num, &record);
@@ -390,13 +330,12 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
             return ESP_FAIL;
         }
 
-        memcpy(frame.data, message, message_size);
+        memcpy(frame.data, message->data(), message_size);
         esp_err_t res = store_fragment(&frame, channel);
         return res;
     }
 
     //control frame handling: - TODO: clean up :)
-    memcpy(data, message, message_size);
     // ESP_LOGI(DEBUG_LINK_TAG, "Received frame of type 0x%X destined for board %d", GET_TYPE(header.type_flag), header.receiver_id);
 
     //check for a rip frame
@@ -404,8 +343,8 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
         ESP_LOGI(DEBUG_LINK_TAG, "Got a RIP frame");
 
         for (size_t i = 0; i < message_size-1; i+=2){
-            uint8_t board_id = message[i];
+            uint8_t board_id = message->data()[i];
-            uint8_t hops = message[i+1];
+            uint8_t hops = message->data()[i+1];
             // ESP_LOGI(DEBUG_LINK_TAG, "Received: board_id %d and number of hops %d on channel %d", board_id, hops, channel);
 
             RIPRow* entry = nullptr;
@@ -462,27 +401,17 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
     //got frame but not destined for this board
     if (header.receiver_id != this_board_id && header.receiver_id != BROADCAST_ADDR && header.seq_num > seq_num){
         // ESP_LOGI(DEBUG_LINK_TAG, "Sending message to board %d with message %s", header.receiver_id, message);
-        res = send(header.receiver_id, message, message_size, FrameType::MISC_CONTROL_TYPE, 0);
+        res = send(header.receiver_id, std::move(message), FrameType::MISC_CONTROL_TYPE, 0);
         return res;
     }
 
-    uint8_t* metadata_message = (uint8_t*)pvPortMalloc(message_size);
-    if (metadata_message == nullptr){
-        ESP_LOGE(DEBUG_LINK_TAG, "Failed to malloc for receive");
-        return ESP_ERR_NO_MEM;
-    }
-    memcpy(metadata_message, message, message_size);
-
     Rx_Metadata metadata = {
-        .data = metadata_message,
+        .data = std::move(message),
         .data_len = (uint16_t)message_size,
         .header = header
     };
 
-    if (xSemaphoreTake(async_rx_queue_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) == pdTRUE){
+    if (!async_receive_queue->enqueue(std::move(metadata), std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS))){
-        async_receive_queue[channel].push(metadata);
-        xSemaphoreGive(async_rx_queue_mutex[channel]);
-    } else {
         return ESP_ERR_TIMEOUT;
     }
     return ESP_OK;

components/dataLink/DataLinkManager.cpp

@@ -1,7 +1,12 @@
 #include "DataLinkManager.h"
+#include "BlockingQueue.h"
+#include "Frames.h"
 #include "RMTManager.h"
 #include "esp_log.h"
 #include "nvs_flash.h"
+#include <memory>
+
+#define SCHEDULE_QUEUE_SIZE 25
 
 /**
  * @brief Constructs a new Data Link Manager object
@@ -28,6 +33,12 @@ DataLinkManager::DataLinkManager(uint8_t board_id, uint8_t num_channels = MAX_CH
 
     sequence_num_map_mutex = xSemaphoreCreateMutex();
 
+    for (int i = 0; i < MAX_CHANNELS; i++) {
+        frame_queue[i] = std::make_unique<BlockingPriorityQueue<SchedulerMetadata, std::vector<SchedulerMetadata>, FrameCompare>>(SCHEDULE_QUEUE_SIZE);
+    }
+
+    async_receive_queue = std::make_unique<BlockingQueue<Rx_Metadata>>(MAX_RX_QUEUE_SIZE);
+
     init_scheduler();
     init_rip();
 }
@@ -346,15 +357,15 @@ esp_err_t DataLinkManager::create_generic_frame(uint8_t* data, uint16_t data_len
  * @param type
  * @return esp_err_t
  */
-esp_err_t DataLinkManager::send(uint8_t dest_board, uint8_t* data, uint16_t data_len, FrameType type, uint8_t flag){
+esp_err_t DataLinkManager::send(uint8_t dest_board, std::unique_ptr<std::vector<uint8_t>>&& buffer, FrameType type, uint8_t flag){
     bool isControlFrame = IS_CONTROL_FRAME((uint8_t)type);
 
-    if (isControlFrame && data_len > MAX_FRAME_SIZE){
+    if (isControlFrame && buffer->size() > MAX_FRAME_SIZE){
         //Control frames has max data size of MAX_FRAME_SIZE
         return ESP_ERR_INVALID_ARG;
     }
 
-    if (!isControlFrame && data_len > MAX_GENERIC_NUM_FRAG * MAX_GENERIC_DATA_LEN){
+    if (!isControlFrame && buffer->size() > MAX_GENERIC_NUM_FRAG * MAX_GENERIC_DATA_LEN){
         //Generic frames has max MAX_GENERIC_NUM_FRAG fragments, each max size of MAX_GENERIC_DATA_LEN (data size)
         return ESP_ERR_INVALID_ARG;
     }
@@ -364,24 +375,14 @@ esp_err_t DataLinkManager::send(uint8_t dest_board, uint8_t* data, uint16_t data
         return ESP_ERR_INVALID_ARG;
     }
 
-    //save data onto heap
-    uint8_t* saved_data = (uint8_t*)pvPortMalloc(data_len);
-    if (saved_data == nullptr){
-        ESP_LOGE(DEBUG_LINK_TAG, "Failed to malloc in send()");
-        return ESP_ERR_NO_MEM;
-    }
-    memset(saved_data, 0, data_len);
-    memcpy(saved_data, data, data_len); //copy the contents to the heap
-
     //calculate number of fragments required (for generic frames only)
     uint32_t frag_info = 0;
     if (!isControlFrame){
-        if (data_len <= MAX_CONTROL_DATA_LEN){
+        if (buffer->size() <= MAX_CONTROL_DATA_LEN){
             frag_info = (1 << 16); //1 total fragment required
         } else {
-            uint32_t total_frags = (data_len + MAX_GENERIC_DATA_LEN - 1) / MAX_GENERIC_DATA_LEN;
+            uint32_t total_frags = (buffer->size() + MAX_GENERIC_DATA_LEN - 1) / MAX_GENERIC_DATA_LEN;
             frag_info = (total_frags) << 16;
-
         }
     }
 
@@ -401,13 +402,12 @@ esp_err_t DataLinkManager::send(uint8_t dest_board, uint8_t* data, uint16_t data
             .seq_num = seq_num,
             .type_flag = (uint8_t)((static_cast<uint8_t>(type) & 0xF0) | (flag & 0xF)),
             .frag_info = frag_info,
-            .data_len = data_len,
+            .data_len = (uint16_t)buffer->size(),
             .crc_16 = 0,
         },
         .generic_frame_data_offset = 0,
         .enqueue_time_ns = 0,
-        .data = saved_data,
+        .data = std::move(buffer),
-        .len = data_len,
         .last_ack = 0,
         .curr_fragment = 0,
         .timeout = 0,
@@ -418,7 +418,6 @@ esp_err_t DataLinkManager::send(uint8_t dest_board, uint8_t* data, uint16_t data
 
     if (res != ESP_OK){
         // ESP_LOGE(DEBUG_LINK_TAG, "Failed to route message to board %d", dest_board);
-        vPortFree(saved_data);
         return res;
     }
 
@@ -426,7 +425,6 @@ esp_err_t DataLinkManager::send(uint8_t dest_board, uint8_t* data, uint16_t data
 
     if (res != ESP_OK){
         ESP_LOGE(DEBUG_LINK_TAG, "Failed to push frame to scheduler queue");
-        vPortFree(saved_data);
     }
     return res;
 }

components/dataLink/DataLinkRIP.cpp

@@ -225,9 +225,7 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
     //use the control frame for the demo (as the number of rows increase, we will need to use the generic frame)
     //data will be [board_id (1), hops (1), board_id (2), hops (2), ...]
 
-    uint8_t rip_message[RIP_MAX_ROUTES*2] = {};
     uint16_t message_idx = 0;
-
     esp_err_t res;
 
     RIPRow* entry = nullptr;
@@ -235,6 +233,9 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
 
     if(broadcast){
         for (size_t channel = 0; channel < num_channels; channel++){
+            auto rip_message = std::make_unique<std::vector<uint8_t>>();
+            rip_message->resize(RIP_MAX_ROUTES * 2);
+
             for (size_t i = 0; i < RIP_MAX_ROUTES; i++){
                 res = rip_get_row(&entry, i);
 
@@ -250,21 +251,15 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
 
                 if (entry->channel == channel){
                     //poisoned reverse
-                    rip_message[message_idx++] = entry->info.board_id;
+                    rip_message->at(message_idx++) = entry->info.board_id;
-                    rip_message[message_idx++] = RIP_MAX_HOPS + 1;
+                    rip_message->at(message_idx++) = RIP_MAX_HOPS + 1;
                 } else {
-                    rip_message[message_idx++] = entry->info.board_id;
+                    rip_message->at(message_idx++) = entry->info.board_id;
-                    rip_message[message_idx++] = entry->info.hops;
+                    rip_message->at(message_idx++) = entry->info.hops;
                 }
             }
 
-            uint8_t* send_data = (uint8_t*)pvPortMalloc(message_idx);
+            rip_message->resize(message_idx);
-            if (send_data == nullptr){
-                ESP_LOGE(DEBUG_LINK_TAG, "Failed to malloc when trying to send rip frame broadcast on channel %d", channel);
-                continue;
-            }
-            memset(send_data, 0, message_idx);
-            memcpy(send_data, rip_message, message_idx);
 
             res = get_inc_sequence_num(BROADCAST_ADDR, &seq_num);
             if (res != ESP_OK){
@@ -284,8 +279,7 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
                 },
                 .generic_frame_data_offset = 0,
                 .enqueue_time_ns = 0,
-                .data = send_data,
+                .data = std::move(rip_message),
-                .len = message_idx,
                 .last_ack = 0,
                 .curr_fragment = 0,
                 .timeout = 0,
@@ -296,9 +290,11 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
                 ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule rip frame from send_rip_frame for channel %d", channel);
             }
             message_idx = 0;
-            memset(rip_message, 0, sizeof(rip_message));
         }
     } else {
+        auto rip_message = std::make_unique<std::vector<uint8_t>>();
+        rip_message->resize(RIP_MAX_ROUTES * 2);
+
         for (size_t i = 0; i < RIP_MAX_ROUTES; i++){
             res = rip_get_row(&entry, i);
 
@@ -309,11 +305,11 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
             if (entry == nullptr){
                 continue;
             }
-            rip_message[message_idx++] = entry->info.board_id;
+            rip_message->data()[message_idx++] = entry->info.board_id;
-            rip_message[message_idx++] = entry->info.hops;
+            rip_message->data()[message_idx++] = entry->info.hops;
         }
         ESP_LOGI(DEBUG_LINK_TAG, "replying to discovery request to board %d", dest_id);
-        res = send(dest_id, rip_message, message_idx, FrameType::RIP_TABLE_CONTROL, FLAG_DISCOVERY);
+        res = send(dest_id, std::move(rip_message), FrameType::RIP_TABLE_CONTROL, FLAG_DISCOVERY);
         if (res != ESP_OK){
             ESP_LOGE(DEBUG_LINK_TAG, "Failed to send rip frame from send_rip_frame");
         }

components/dataLink/DataLinkScheduler.cpp

@@ -1,20 +1,30 @@
+#include <chrono>
+
 #include "DataLinkManager.h"
 #include "esp_log.h"
 #include "esp_timer.h"
+#include "freertos/projdefs.h"
 #include "freertos/semphr.h"
 #include "esp_random.h"
+#include "portmacro.h"
+
+#define FRAME_DEQUEUE_TIMEOUT_MS 2000
+#define FRAME_ENQUEUE_TIMEOUT_MS 50
 
 void DataLinkManager::init_scheduler(){
     for (int i = 0; i < num_channels; i++){
-        sq_handle[i] = xSemaphoreCreateMutex();
         async_rx_queue_mutex[i] = xSemaphoreCreateMutex();
         rx_fragment_mutex[i] = xSemaphoreCreateMutex();
         sliding_window_mutex[i] = xSemaphoreCreateMutex();
         send_ack_queue_mutex[i] = xSemaphoreCreateMutex();
+
+        ESP_LOGI(DEBUG_LINK_TAG, "Starting Frame Scheduler task for channel %d", i);
+        auto args = (frame_scheduler_args*)malloc(sizeof(frame_scheduler_args));
+        args->channel_id = i;
+        args->that = this;
+        xTaskCreate(DataLinkManager::frame_scheduler, "Scheduler", 4096, static_cast<void*>(args), 4, &scheduler_task);
     }
 
-    ESP_LOGI(DEBUG_LINK_TAG, "Starting Frame Scheduler task");
-    xTaskCreate(DataLinkManager::frame_scheduler, "Scheduler", 4096, static_cast<void*>(this), 4, &scheduler_task);
     xTaskCreate(DataLinkManager::receive_thread_main, "Receiver", 8192, static_cast<void*>(this), 5, &receive_task);
     xTaskCreate(DataLinkManager::send_ack_thread_main, "Send ACKs", 8192, static_cast<void*>(this), 5, &send_ack_task);
 }
@@ -29,7 +39,10 @@ void DataLinkManager::init_scheduler(){
  * Scheduling may change (above scheduler will lead to starvation of control frames depending on the number of generic frames/fragments to send)
  */
 [[noreturn]] void DataLinkManager::frame_scheduler(void* args){
-    DataLinkManager* link_layer_obj = static_cast<DataLinkManager*>(args);
+    const auto parsed_args = static_cast<frame_scheduler_args*>(args);
+    uint8_t channel = parsed_args->channel_id;
+    DataLinkManager* link_layer_obj = parsed_args->that;
+
     if (link_layer_obj == nullptr){
         ESP_LOGE(DEBUG_LINK_TAG, "Frame Scheduler failed to start due to invalid pointer");
         vTaskDelete(nullptr);
@@ -37,12 +50,9 @@ void DataLinkManager::init_scheduler(){
 
     ESP_LOGI(DEBUG_LINK_TAG, "Starting Frame Scheduler task");
     while(!link_layer_obj->stop_tasks){
-        for (uint8_t i = 0; i < link_layer_obj->num_channels; i++){
+        link_layer_obj->scheduler_send(channel);
-            link_layer_obj->scheduler_send(i);
-        }
-        vTaskDelay(pdMS_TO_TICKS(SCHEDULER_PERIOD_MS));
-
     }
+    free(args);
     vTaskDelete(nullptr);
 }
 
@@ -64,7 +74,7 @@ esp_err_t DataLinkManager::push_frame_to_scheduler(SchedulerMetadata frame, uint
         return ESP_ERR_INVALID_ARG;
     }
 
-    if (frame.len == 0){
+    if (frame.data->size() == 0){
         ESP_LOGE(DEBUG_LINK_TAG, "Invalid Frame Length");
 
         return ESP_ERR_INVALID_ARG;
@@ -73,19 +83,7 @@ esp_err_t DataLinkManager::push_frame_to_scheduler(SchedulerMetadata frame, uint
     int64_t now = esp_timer_get_time();
     frame.enqueue_time_ns = now;
 
-    if (sq_handle[channel] == nullptr){
+    frame_queue[channel]->enqueue(std::move(frame), std::chrono::milliseconds(FRAME_ENQUEUE_TIMEOUT_MS));
-        ESP_LOGE(DEBUG_LINK_TAG, "Invalid scheduler queue handle");
-        return ESP_FAIL;
-    }
-
-    if (xSemaphoreTake(sq_handle[channel], pdMS_TO_TICKS(SCHEDULER_MUTEX_WAIT)) == pdTRUE){
-        frame_queue[channel].push(frame);
-        xSemaphoreGive(sq_handle[channel]);
-    } else {
-        //Failed to obtain mutex
-        ESP_LOGE(DEBUG_LINK_TAG, "Failed to get mutex");
-        return ESP_ERR_TIMEOUT;
-    }
 
     // ESP_LOGI(DEBUG_LINK_TAG, "Pushed frame to queue on channel %d", channel);
 
@@ -103,26 +101,16 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
         return ESP_ERR_INVALID_ARG;
     }
 
-    if (sq_handle[channel] == nullptr){
+    vTaskDelay(pdMS_TO_TICKS(10));  // the messages cannot be too close together
-        return ESP_FAIL;
-    }
 
     SchedulerMetadata frame;
 
-    if (xSemaphoreTake(sq_handle[channel], pdMS_TO_TICKS(SCHEDULER_MUTEX_WAIT)) == pdTRUE){
+    if (auto maybe_frame = frame_queue[channel]->dequeue(std::chrono::milliseconds(FRAME_DEQUEUE_TIMEOUT_MS))) {
-        if (frame_queue[channel].empty()){
+        frame = *maybe_frame;
-            xSemaphoreGive(sq_handle[channel]);
+    } else {
         // ESP_LOGI(DEBUG_LINK_TAG, "Scheduler queue for channel %d is empty", channel);
         return ESP_OK;
     }
-        frame = frame_queue[channel].top();
-        frame_queue[channel].pop();
-        xSemaphoreGive(sq_handle[channel]);
-    } else {
-        ESP_LOGE(DEBUG_LINK_TAG, "Failed to get mutex when trying to send");
-        //Failed to obtain mutex
-        return ESP_ERR_TIMEOUT;
-    }
 
     if (frame.data == nullptr){
         ESP_LOGE(DEBUG_LINK_TAG, "Data array does not exist");
@@ -131,7 +119,6 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
 
     if (this_board_id == PC_ADDR){
         ESP_LOGE(DEBUG_LINK_TAG, "This board is not assigned a board id");
-        vPortFree(frame.data);
         return ESP_ERR_INVALID_ARG;
     }
 
@@ -144,11 +131,9 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
     if (isControlFrame){
         //control frame
 
-        res = create_control_frame(frame.data, frame.len,
+        res = create_control_frame(frame.data->data(), frame.data->size(),
             make_control_frame_from_header(frame.header), send_data, &frame_size);
 
-        vPortFree(frame.data);
-
         if (res != ESP_OK){
             ESP_LOGE(DEBUG_LINK_TAG, "Failed to create control frame");
             return res;
@@ -158,7 +143,7 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
         return scheduler_send_rmt(channel, frame, send_data, frame_size, false);
     } else {
         //generic frame
-        if (frame.len > (MAX_GENERIC_DATA_LEN)){
+        if (frame.data->size() > (MAX_GENERIC_DATA_LEN)){
             //fragment here
 
             if (frame.timeout == 0){
@@ -168,7 +153,6 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
                 res = push_frame_to_scheduler(frame, channel);
                 if (res != ESP_OK){
                     ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment");
-                    vPortFree(frame.data);
                     return res;
                 }
                 return ESP_OK;
@@ -184,7 +168,6 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
 
                 if (res != ESP_OK){
                     ESP_LOGE(DEBUG_LINK_TAG, "Failed to get sliding window ack record for board id %d seq num %d", frame.header.receiver_id, frame.header.seq_num);
-                    vPortFree(frame.data);
                     return res;
                 }
 
@@ -206,7 +189,6 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
                         //all acks received, can simply exit
                         // ESP_LOGI(DEBUG_LINK_TAG, "All acks recevied for board id %d seq num %d", frame.header.receiver_id, frame.header.seq_num);
                         complete_record_sliding_window(channel, frame.header.receiver_id, frame.header.seq_num);
-                        vPortFree(frame.data);
                         return ESP_OK;
                     }
 
@@ -228,7 +210,7 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
             if (frame.curr_fragment != (frame.header.frag_info >> 16)) {
                 fragment_size = MAX_GENERIC_DATA_LEN;
             } else {
-                fragment_size = frame.len - (MAX_GENERIC_DATA_LEN * (frame.curr_fragment-1));
+                fragment_size = frame.data->size() - (MAX_GENERIC_DATA_LEN * (frame.curr_fragment-1));
             }
 
             uint16_t curr_offset = MAX_GENERIC_DATA_LEN * (frame.curr_fragment - 1);
@@ -238,12 +220,11 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
 
             frame.header.frag_info = (frame.header.frag_info & 0xFFFF0000) | frame.curr_fragment; //increment frag_num
             //create fragment
-            res = create_generic_frame(frame.data, fragment_size,
+            res = create_generic_frame(frame.data->data(), fragment_size,
                 make_generic_frame_from_header(frame.header), curr_offset, send_data, &frame_size);
 
             if (res != ESP_OK){
                 ESP_LOGE(DEBUG_LINK_TAG, "Failed to create generic frame fragment");
-                vPortFree(frame.data);
                 return res;
             }
 
@@ -254,7 +235,6 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
                 res = push_frame_to_scheduler(frame, channel);
                 if (res != ESP_OK){
                     ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment");
-                    vPortFree(frame.data);
                 }
                 return res;
             }
@@ -267,20 +247,17 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
                 res = push_frame_to_scheduler(frame, channel);
                 if (res != ESP_OK){
                     ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment");
-                    vPortFree(frame.data);
                     return res;
                 }
             } else {
                 //Done fragmenting, can free data array
                 // ESP_LOGI(DEBUG_LINK_TAG, "finished fragmenting seq num %d frag_info 0x%X", frame.header.seq_num, frame.header.frag_info);
-                vPortFree(frame.data);
             }
 
         } else {
             //no fragmenting
-            res = create_generic_frame(frame.data, frame.len,
+            res = create_generic_frame(frame.data->data(), frame.data->size(),
                 make_generic_frame_from_header(frame.header), 0, send_data, &frame_size);
-            vPortFree(frame.data);
 
             if (res != ESP_OK){
                 ESP_LOGE(DEBUG_LINK_TAG, "Failed to create generic frame");

components/dataLink/include/DataLinkManager.h

@@ -11,6 +11,8 @@
 #include "Frames.h"
 #include "Tables.h"
 #include "RMTManager.h"
+#include "BlockingQueue.h"
+#include "BlockingPriorityQueue.h"
 #include <unordered_map>
 #include "Scheduler.h"
 
@@ -43,6 +45,7 @@ static const uint16_t crc16_table[256] = {
 
 #define ASYNC_QUEUE_WAIT_TICKS 100
 #define SEQUENCE_NUM_MAP_MUTEX_MAX_WAIT_MS 50
+#define MAX_RX_QUEUE_SIZE 100
 
 /**
  * @brief Class to represent the Data Link Layer
@@ -53,13 +56,12 @@ class DataLinkManager{
     public:
         DataLinkManager(uint8_t board_id, uint8_t num_channels);
         ~DataLinkManager();
-        esp_err_t send(uint8_t dest_board, uint8_t* data, uint16_t data_len, FrameType type, uint8_t flag);
+        esp_err_t send(uint8_t dest_board, std::unique_ptr<std::vector<uint8_t>>&& buffer, FrameType type, uint8_t flag);
         esp_err_t start_receive_frames(uint8_t curr_channel);
         esp_err_t receive(uint8_t* data, size_t data_len, size_t* recv_len, uint8_t curr_channel);
         esp_err_t print_frame_info(uint8_t* data, size_t data_len, uint8_t* message, size_t message_len);
         esp_err_t get_routing_table(RIPRow_public* table, size_t* table_size);
-        esp_err_t async_receive_info(uint16_t* frame_size, FrameHeader* header, uint8_t channel);
+        std::optional<std::unique_ptr<std::vector<uint8_t>>> async_receive();
-        esp_err_t async_receive(uint8_t* data, uint16_t data_len, FrameHeader* header, uint8_t channel);
         esp_err_t ready();
         esp_err_t send_ack(uint8_t sender_id, uint8_t* data, uint16_t data_len);
     private:
@@ -114,8 +116,7 @@ class DataLinkManager{
          * @brief Priority queue for each channel to schedule when to send frames
          *
          */
-        std::priority_queue<SchedulerMetadata, std::vector<SchedulerMetadata>, FrameCompare> frame_queue[MAX_CHANNELS];
+        std::unique_ptr<BlockingPriorityQueue<SchedulerMetadata, std::vector<SchedulerMetadata>, FrameCompare>> frame_queue[MAX_CHANNELS];
-        SemaphoreHandle_t sq_handle[MAX_CHANNELS];
         void init_scheduler();
         esp_err_t push_frame_to_scheduler(SchedulerMetadata frame, uint8_t channel);
         TaskHandle_t scheduler_task = NULL;
@@ -152,7 +153,7 @@ class DataLinkManager{
          * @brief Queue to store complete received frame data
          *
          */
-        std::queue<Rx_Metadata> async_receive_queue[MAX_CHANNELS];
+        std::unique_ptr<BlockingQueue<Rx_Metadata>> async_receive_queue;
 
         esp_err_t start_receive_frames_rmt(uint8_t curr_channel);
 
@@ -204,4 +205,9 @@ class DataLinkManager{
         std::queue<SendAckMetaData> send_ack_queue[MAX_CHANNELS];
 };
 
+struct frame_scheduler_args {
+    uint8_t channel_id;
+    DataLinkManager* that;
+};
+
 #endif //DATA_LINK

components/dataLink/include/Frames.h

@@ -108,7 +108,7 @@ typedef struct _fragment_metadata {
 } FragmentMetadata;
 
 typedef struct _receive_metadata{
-    uint8_t* data;
+    std::unique_ptr<std::vector<uint8_t>> data;
     uint16_t data_len;
     FrameHeader header;
 } Rx_Metadata;

components/dataLink/include/Scheduler.h

@@ -4,7 +4,7 @@
 #include <cstdint>
 
 #define SCHEDULER_MUTEX_WAIT 10 //max time duration to wait
-#define SCHEDULER_PERIOD_MS 140
+#define SCHEDULER_PERIOD_MS 10
 #define RECEIVE_TASK_PERIOD_MS 2
 
 #define GENERIC_FRAME_SLIDING_WINDOW_SIZE 5 //defines the maximum size of the sliding window before resending previously un-ack'd fragments
@@ -20,13 +20,13 @@ typedef struct _frame_scheduler_metadata {
     FrameHeader header; //header of the frame
     uint16_t generic_frame_data_offset; //For data greater than MAX_GENERIC_DATA_LEN to keep track of fragment positions
     int64_t enqueue_time_ns; //when the frame has been first enqueued into the priority queue
-    uint8_t* data; //dyanmically allocated memory - contains the actual data
+    std::shared_ptr<std::vector<uint8_t>> data; // the actual data, and length of data
-    uint16_t len; // length of the actual data
 
     //sliding window
     uint16_t last_ack; //fragment number represnting the last ack'd fragment (from rx) - head
     uint16_t curr_fragment; //fragment number of the current fragment being sent
     uint32_t timeout;
+
 } SchedulerMetadata;
 
 typedef struct _frame_ack_record {

components/ptrQueue/include/BlockingPriorityQueue.h

@@ -0,0 +1,54 @@
+#ifndef BLOCKINGPRIORITYQUEUE_H
+#define BLOCKINGPRIORITYQUEUE_H
+
+#include <chrono>
+#include <condition_variable>
+#include <mutex>
+#include <optional>
+#include <queue>
+#include <vector>
+#include <functional>
+
+template <typename T,
+          typename Container = std::vector<T>,
+          typename Compare = std::less<typename Container::value_type>>
+class BlockingPriorityQueue {
+  public:
+    explicit BlockingPriorityQueue(const size_t capacity) : m_capacity(capacity) {
+    }
+
+    // Enqueue with timeout. Returns true on success, false on timeout.
+    bool enqueue(T &&item, std::chrono::milliseconds max_wait) {
+        std::unique_lock lock(m_mutex);
+        if (!m_cond_not_full.wait_for(lock, max_wait,
+                                      [this]() { return m_queue.size() < m_capacity; })) {
+            return false;
+        }
+
+        m_queue.push(std::move(item));
+        m_cond_not_empty.notify_one();
+        return true;
+    }
+
+    // Dequeue with timeout. Returns optional<T> (empty on timeout).
+    std::optional<T> dequeue(std::chrono::milliseconds max_wait) {
+        std::unique_lock lock(m_mutex);
+        if (!m_cond_not_empty.wait_for(lock, max_wait, [this]() { return !m_queue.empty(); })) {
+            return std::nullopt;
+        }
+
+        T item = std::move(m_queue.top());
+        m_queue.pop();
+        m_cond_not_full.notify_one();
+        return item;
+    }
+
+  private:
+    std::priority_queue<T, Container, Compare> m_queue;
+    size_t m_capacity;
+    std::mutex m_mutex;
+    std::condition_variable m_cond_not_empty;
+    std::condition_variable m_cond_not_full;
+};
+
+#endif // BLOCKINGPRIORITYQUEUE_H

components/ptrQueue/include/BlockingQueue.h

@@ -0,0 +1,53 @@
+//
+// Created by Johnathon Slightham on 2025-07-10.
+//
+
+#ifndef BLOCKINGQUEUE_H
+#define BLOCKINGQUEUE_H
+
+#include <chrono>
+#include <condition_variable>
+#include <mutex>
+#include <optional>
+#include <queue>
+
+template <typename T> class BlockingQueue {
+  public:
+    explicit BlockingQueue(const size_t capacity) : m_capacity(capacity) {
+    }
+
+    // Enqueue with timeout. Returns true on success, false on timeout.
+    bool enqueue(T &&item, std::chrono::milliseconds max_wait) {
+        std::unique_lock lock(m_mutex);
+        if (!m_cond_not_full.wait_for(lock, max_wait,
+                                      [this]() { return m_queue.size() < m_capacity; })) {
+            return false;
+        }
+
+        m_queue.push(std::move(item));
+        m_cond_not_empty.notify_one();
+        return true;
+    }
+
+    // Dequeue with timeout. Returns optional<T> (empty on timeout).
+    std::optional<T> dequeue(std::chrono::milliseconds max_wait) {
+        std::unique_lock lock(m_mutex);
+        if (!m_cond_not_empty.wait_for(lock, max_wait, [this]() { return !m_queue.empty(); })) {
+            return std::nullopt;
+        }
+
+        T item = std::move(m_queue.front());
+        m_queue.pop();
+        m_cond_not_full.notify_one();
+        return item;
+    }
+
+  private:
+    std::queue<T> m_queue;
+    size_t m_capacity;
+    std::mutex m_mutex;
+    std::condition_variable m_cond_not_empty;
+    std::condition_variable m_cond_not_full;
+};
+
+#endif // BLOCKINGQUEUE_H

components/rmt/include/RMTManager.h

@@ -151,5 +151,6 @@ static const GPIO_Channel_Pair gpio_channel_pairs[MAX_CHANNELS] = {
         .rx_pin = GPIO_NUM_15
     }
 }; //todo: use these pairs directly instead of the two arrays in the class definition above
+   // but ensure to update them first!!!
 
 #endif //RMT_COMMUNICATIONS

components/rpc/CommunicationFactory.cpp

@@ -30,7 +30,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) {
+std::unique_ptr<IRPCServer> CommunicationFactory::create_lossy_server(const CommunicationMethod type, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>& rx_queue) {
     switch (type) {
         case Wireless:
             return std::make_unique<UDPServer>(RECV_PORT, SEND_PORT, rx_queue);
@@ -39,7 +39,7 @@ std::unique_ptr<IRPCServer> CommunicationFactory::create_lossy_server(const Comm
     }
 }
 
-std::unique_ptr<IRPCServer> CommunicationFactory::create_lossless_server(const CommunicationMethod type, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue) {
+std::unique_ptr<IRPCServer> CommunicationFactory::create_lossless_server(const CommunicationMethod type, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>& rx_queue) {
     switch (type) {
         case Wireless:
             return std::make_unique<TCPServer>(TCP_PORT, rx_queue);

components/rpc/CommunicationRouter.cpp

@@ -1,3 +1,5 @@
+#include <chrono>
+#include <cstring>
 #include <iostream>
 
 #include "AngleControlMessageBuilder.h"
@@ -14,19 +16,21 @@
 #include "include/wireless/mDNSDiscoveryService.h"
 
 #define TAG "CommunicationRouter"
+#define MAX_RX_BUFFER_SIZE 1024
+#define WIRELESS_DEQUEUE_TIMEOUT_MS 3000
 
-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<CommunicationRouter *>(args);
 
     while (true) {
-    const auto buffer = that->m_tcp_rx_queue->dequeue();
+        if( auto maybe_buffer = that->m_tcp_rx_queue->dequeue(std::chrono::milliseconds(WIRELESS_DEQUEUE_TIMEOUT_MS))) {
             ESP_LOGD(TAG, "Got message from TCP");
-    that->route(buffer->data(), buffer->size());
+            that->route(std::move(*maybe_buffer));
+        }
     }
 }
 
@@ -40,31 +44,13 @@ CommunicationRouter::~CommunicationRouter() { vTaskDelete(m_router_thread); }
             that->update_leader();
         }
 
-    for (uint8_t channel = 0;
+        if (auto ptr = that->m_data_link_manager->async_receive()) {
-         channel <
+            that->route(std::move(*ptr));
-         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;
         }
-
-      std::vector<uint8_t> 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<uint8_t *>(buffer), length);
     return 0;
 }
@@ -106,31 +92,62 @@ void CommunicationRouter::update_leader() {
     }
 }
 
-void CommunicationRouter::route(uint8_t *buffer, const size_t length) const {
+// Route without trying to copy to heap. Only call if you do not have a unique_ptr.
-  flatbuffers::Verifier verifier(buffer, length);
+// To handle the case of writing directly from control -> TCP/UDP, nothing has to touch the heap.
+void CommunicationRouter::route(uint8_t *buffer, size_t size) const {
+    flatbuffers::Verifier verifier(buffer, size);
     // 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 =
+    if (const auto &mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer);
-          Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer);
         mpi_message->destination() == m_module_id) {
-    this->m_rx_callback(reinterpret_cast<char *>(buffer), 512);
+        auto ubuffer = std::make_unique<std::vector<uint8_t>>();
-  } else if (mpi_message->destination() == PC_ADDR &&
+        ubuffer->resize(size);
-             this->m_leader == m_module_id) {
+        memcpy(ubuffer->data(), buffer, size);
+        this->m_rx_callback(std::move(ubuffer));
+    } 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<char *>(buffer), 512);
+            this->m_lossless_server->send_msg(buffer, size);
         } else {
-      this->m_lossy_server->send_msg(reinterpret_cast<char *>(buffer), 512);
+            this->m_lossy_server->send_msg(buffer, size);
+        }
+    } else {
+        const auto dest = mpi_message->destination() == PC_ADDR ? this->m_leader : mpi_message->destination();
+
+        auto u_buffer = std::make_unique<std::vector<uint8_t>>();
+        u_buffer->resize(size);
+        memcpy(u_buffer->data(), buffer, size);
+
+        this->m_data_link_manager->send(dest, std::move(u_buffer), FrameType::MOTOR_TYPE, 0);
+    }
+}
+
+// Route heap messages
+void CommunicationRouter::route(std::unique_ptr<std::vector<uint8_t>>&& buffer) const {
+    flatbuffers::Verifier verifier(buffer->data(), buffer->size());
+    // 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->data());
+        mpi_message->destination() == m_module_id) {
+        this->m_rx_callback(std::move(buffer));
+    } else if (mpi_message->destination() == PC_ADDR && this->m_leader == m_module_id) {
+        if (mpi_message->is_durable()) {
+            this->m_lossless_server->send_msg(buffer->data(), buffer->size());
+        } else {
+            this->m_lossy_server->send_msg(buffer->data(), buffer->size());
         }
     } else if (mpi_message->destination() == PC_ADDR) {
-    this->m_data_link_manager->send(this->m_leader, buffer, length,
+        this->m_data_link_manager->send(this->m_leader, std::move(buffer), FrameType::MOTOR_TYPE, 0);
-                                    FrameType::MOTOR_TYPE, 0);
     } else {
-    this->m_data_link_manager->send(mpi_message->destination(), buffer, length,
+        this->m_data_link_manager->send(mpi_message->destination(), std::move(buffer), FrameType::MOTOR_TYPE,
-                                    FrameType::MOTOR_TYPE, 0);
+                                        0);
     }
 }
 

components/rpc/MessagingInterface.cpp

@@ -13,7 +13,6 @@
 #include "MPIMessageBuilder.h"
 
 MessagingInterface::~MessagingInterface() {
-    vQueueDelete(m_mpi_rx_queue);
     vSemaphoreDelete(m_map_semaphore);
 
     for (const auto queue: m_tag_to_queue | std::views::values) {
@@ -52,8 +51,8 @@ int MessagingInterface::sendrecv(char* send_buffer, const int send_size, const i
 }
 
 // todo: when handleRecv returns, remove from queue (from router)
-void MessagingInterface::handleRecv(const char* recv_buffer, int recv_size) {
+void MessagingInterface::handleRecv(std::unique_ptr<std::vector<uint8_t>>&& buffer) {
-    const auto mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(reinterpret_cast<const uint8_t *>(recv_buffer));
+    const auto mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer->data());
 
     checkOrInsertTag(mpi_message->tag());
 

components/rpc/OrientationDetection.cpp

@@ -21,16 +21,12 @@ 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]))) {
-        ESP_LOGD(TAG, "90deg");
         return Orientation_Deg90;
     } else if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_180_DEG_MAP[channel]))) {
-        ESP_LOGD(TAG, "180deg");
         return Orientation_Deg180;
     } else if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_270_DEG_MAP[channel]))) {
-        ESP_LOGD(TAG, "270deg");
         return Orientation_Deg270;
     } else {
-        ESP_LOGD(TAG, "No orientation detected");
         return Orientation_Deg0;
     }
 }

components/rpc/include/CommunicationFactory.h

@@ -13,15 +13,15 @@
 #include "IConnectionManager.h"
 #include "IDiscoveryService.h"
 #include "IRPCServer.h"
-#include "PtrQueue.h"
+#include "BlockingQueue.h"
 #include "enums.h"
 
 class CommunicationFactory {
 public:
     static std::unique_ptr<IConnectionManager> create_connection_manager(CommunicationMethod type);
     static std::unique_ptr<IDiscoveryService> create_discovery_service(CommunicationMethod type);
-    static std::unique_ptr<IRPCServer> create_lossy_server(CommunicationMethod type, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>> &rx_queue);
+    static std::unique_ptr<IRPCServer> create_lossy_server(CommunicationMethod type, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue);
-    static std::unique_ptr<IRPCServer> create_lossless_server(CommunicationMethod type, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue);
+    static std::unique_ptr<IRPCServer> create_lossless_server(CommunicationMethod type, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>& rx_queue);
 };
 
 #endif //COMMUNICATIONFACTORY_H

components/rpc/include/CommunicationRouter.h

@@ -21,13 +21,15 @@
 #include "wireless/TCPServer.h"
 #include "wireless/WifiManager.h"
 
+#define MAX_NETWORK_QUEUE_SIZE 10
+
 class CommunicationRouter {
 
 public:
   explicit CommunicationRouter(
-      const std::function<void(char *, int)> &rx_callback)
+      const std::function<void(std::unique_ptr<std::vector<uint8_t>>&&)> &rx_callback)
-      : m_tcp_rx_queue(std::make_shared<PtrQueue<std::vector<uint8_t>>>(10)),
+      : m_tcp_rx_queue(std::make_shared<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>(MAX_NETWORK_QUEUE_SIZE)),
-        m_rx_callback(rx_callback),
+        m_rx_callback(std::move(rx_callback)),
         m_config_manager(ConfigManager::get_instance()),
         m_pc_connection(CommunicationFactory::create_connection_manager(
             m_config_manager.get_communication_method())),
@@ -56,15 +58,15 @@ public:
   [[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;
+  void route(std::unique_ptr<std::vector<uint8_t>>&& buffer) const;
+  void route(uint8_t* buffer, size_t size) const;
   [[nodiscard]] std::pair<std::vector<uint8_t>, std::vector<Orientation>>
   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<PtrQueue<std::vector<uint8_t>>>
+  std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_tcp_rx_queue;
-      m_tcp_rx_queue; // todo: this should probably be thread safe
+  std::function<void(std::unique_ptr<std::vector<std::uint8_t>>)> m_rx_callback;
-  std::function<void(char *, int)> m_rx_callback;
 
 private:
   TaskHandle_t m_router_thread = nullptr;

components/rpc/include/IRPCServer.h

@@ -5,12 +5,15 @@
 #ifndef IRPCSERVER_H
 #define IRPCSERVER_H
 
+#include <memory>
+#include <vector>
+
 class IRPCServer {
-public:
+  public:
     virtual ~IRPCServer() = default;
     virtual void startup() = 0;
     virtual void shutdown() = 0;
-    virtual int send_msg(char* buffer, uint32_t length) const = 0;
+    virtual int send_msg(uint8_t *buffer, size_t size) const = 0;
 };
 
 #endif //IRPCSERVER_H

components/rpc/include/MessagingInterface.h

@@ -9,6 +9,7 @@
 #include <unordered_map>
 #include <flatbuffers_generated/TopologyMessage_generated.h>
 
+#include "BlockingQueue.h"
 #include "constants/app_comms.h"
 #include "CommunicationRouter.h"
 
@@ -16,8 +17,8 @@ class MessagingInterface {
 public:
     explicit MessagingInterface()
         : m_config_manager(ConfigManager::get_instance()),
-            m_mpi_rx_queue(xQueueCreate(MAX_RX_BUFFER_SIZE, RX_QUEUE_SIZE)),
+            m_mpi_rx_queue(std::make_unique<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>(RX_QUEUE_SIZE)),
-            m_router(std::make_unique<CommunicationRouter>([this](const char* buffer, const int size) { handleRecv(buffer, size); })),
+            m_router(std::make_unique<CommunicationRouter>([this](std::unique_ptr<std::vector<uint8_t>>&& buffer) { handleRecv(std::move(buffer)); })),
             m_map_semaphore(xSemaphoreCreateMutex()) {};
 
     ~MessagingInterface();
@@ -31,13 +32,13 @@ public:
     uint8_t get_leader() const;
 
 private:
-    void handleRecv(const char* recv_buffer, int recv_size);
+    void handleRecv(std::unique_ptr<std::vector<uint8_t>>&& buffer);
 
     void checkOrInsertTag(uint8_t tag);
 
     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<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_mpi_rx_queue;
     std::unique_ptr<CommunicationRouter> m_router;
     SemaphoreHandle_t m_map_semaphore;
     std::unordered_map<uint8_t, QueueHandle_t> m_tag_to_queue;

components/rpc/include/wireless/TCPServer.h

@@ -6,22 +6,22 @@
 #define TCPSERVER_H
 
 #include <memory>
-#include <vector>
 #include <unordered_set>
+#include <vector>
 
-#include "freertos/FreeRTOS.h"
 #include "IRPCServer.h"
-#include "PtrQueue.h"
+#include "BlockingQueue.h"
+#include "freertos/FreeRTOS.h"
 
 class TCPServer final : public IRPCServer {
-public:
+  public:
-    TCPServer(int port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue);
+    TCPServer(int port, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue);
     ~TCPServer() override;
     void startup() override;
     void shutdown() override;
-    int send_msg(char* buffer, uint32_t length) const override;
+    int send_msg(uint8_t* buffer, size_t size) const override;
 
-private:
+  private:
     bool authenticate_client(int client_sock);
 
     static bool is_network_connected();
@@ -34,7 +34,7 @@ private:
     TaskHandle_t m_task;
     TaskHandle_t m_rx_task;
 
-    std::shared_ptr<PtrQueue<std::vector<uint8_t>>> m_rx_queue;
+    std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_rx_queue;
 
     SemaphoreHandle_t m_mutex;
     std::unordered_set<int> m_clients;

components/rpc/include/wireless/UDPServer.h

@@ -6,22 +6,23 @@
 #define UDPSERVER_H
 
 #include <memory>
-#include <vector>
 #include <unordered_set>
+#include <vector>
 
-#include "freertos/FreeRTOS.h"
 #include "IRPCServer.h"
-#include "PtrQueue.h"
+#include "BlockingQueue.h"
+#include "freertos/FreeRTOS.h"
 
 class UDPServer final : public IRPCServer {
-public:
+  public:
-    UDPServer(int rx_port, int tx_port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue);
+    UDPServer(int rx_port, int tx_port,
+              const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue);
     ~UDPServer() override;
     void startup() override;
     void shutdown() override;
-    int send_msg(char* buffer, uint32_t length) const override;
+    int send_msg(uint8_t *buffer, size_t size) const override;
 
-private:
+  private:
     bool authenticate_client(int client_sock);
 
     static bool is_network_connected();
@@ -35,7 +36,7 @@ private:
 
     TaskHandle_t m_rx_task;
 
-    std::shared_ptr<PtrQueue<std::vector<uint8_t>>> m_rx_queue;
+    std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_rx_queue;
 };
 
 #endif //UDPSERVER_H

components/rpc/wireless/TCPServer.cpp

@@ -1,3 +1,4 @@
+#include <chrono>
 #include <memory>
 
 #include "bits/shared_ptr_base.h"
@@ -15,6 +16,8 @@
 #include "sys/param.h"
 #include "wireless/TCPServer.h"
 
+#define RX_QUEUE_ENQUEUE_TIMEOUT_MS 50  // must be small to ensure we drain TCP buffer
+
 #define TAG "TCPServer"
 
 #define MIN(X, Y) (((X) < (Y)) ? (X) : (Y))
@@ -24,7 +27,7 @@
 //       - tx from board
 
 TCPServer::TCPServer(const int port,
-                     const std::shared_ptr<PtrQueue<std::vector<uint8_t>>> &rx_queue) {
+                     const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue) {
     this->m_port = port;
     this->m_mutex = xSemaphoreCreateMutex();
     this->m_clients = std::unordered_set<int>();
@@ -215,7 +218,7 @@ void TCPServer::shutdown() {
                     } else {
                         ESP_LOGD(TAG, "TCP Server Received %d bytes\n", len);
                         buffer->resize(len);
-                        that->m_rx_queue->enqueue(std::move(buffer));
+                        that->m_rx_queue->enqueue(std::move(buffer), std::chrono::milliseconds(RX_QUEUE_ENQUEUE_TIMEOUT_MS));
                     }
                 }
             }
@@ -260,11 +263,13 @@ bool TCPServer::authenticate_client(int sock) {
     return 0;
 }
 
-int TCPServer::send_msg(char *buffer, const uint32_t length) const {
+int TCPServer::send_msg(uint8_t *buffer, size_t size) const {
     if (!is_network_connected()) {
         return -1;
     }
 
+    const auto length = (uint32_t)size;
+
     for (const auto client_sock : m_clients) {
         send(client_sock, &length, 4, 0);
         send(client_sock, buffer, length, 0);

components/rpc/wireless/UDPServer.cpp

@@ -1,3 +1,4 @@
+#include <chrono>
 #include <cstring>
 #include <memory>
 
@@ -17,14 +18,14 @@
 #include "wireless/UDPServer.h"
 
 #define TAG "UDPServer"
+#define MAX_RX_QUEUE_ENQUEUE_TIMEOUT_MS 50
 
 #define MIN(X, Y) (((X) < (Y)) ? (X) : (Y))
 
 // todo: - authenticate
 
-UDPServer::UDPServer(
+UDPServer::UDPServer(const int rx_port, const int tx_port,
-    const int rx_port, const int tx_port,
+                     const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue) {
-    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;
@@ -33,7 +34,9 @@ UDPServer::UDPServer(
     this->m_tx_server_sock = 0;
 }
 
-UDPServer::~UDPServer() { this->shutdown(); }
+UDPServer::~UDPServer() {
+    this->shutdown();
+}
 
 void UDPServer::startup() {
     ESP_LOGI(TAG, "Starting UDP server on port %d", this->m_rx_port);
@@ -90,8 +93,8 @@ void UDPServer::shutdown() {
         }
 
         int reuse = 1;
-    if (setsockopt(that->m_rx_server_sock, SOL_SOCKET, SO_REUSEADDR, &reuse,
+        if (setsockopt(that->m_rx_server_sock, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)) <
-                   sizeof(reuse)) < 0) {
+            0) {
             ESP_LOGE(TAG, "Failed to set SO_REUSEADDR. Error %d", errno);
             close(that->m_rx_server_sock);
             close(that->m_tx_server_sock);
@@ -104,8 +107,8 @@ void UDPServer::shutdown() {
         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,
+        int ret =
-                   sizeof(struct sockaddr_in));
+            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);
@@ -119,8 +122,8 @@ void UDPServer::shutdown() {
         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,
+        if (setsockopt(that->m_rx_server_sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &imreq,
-                   &imreq, sizeof(struct ip_mreq)) < 0) {
+                       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);
@@ -135,8 +138,8 @@ void UDPServer::shutdown() {
             auto buffer = std::make_unique<std::vector<uint8_t>>();
             buffer->resize(MAX_RX_BUFFER_SIZE + 4);
 
-      if (int len = recvfrom(that->m_rx_server_sock, buffer->data(),
+            if (int len = recvfrom(that->m_rx_server_sock, buffer->data(), MAX_RX_BUFFER_SIZE, 0,
-                             MAX_RX_BUFFER_SIZE, 0, nullptr, nullptr);
+                                   nullptr, nullptr);
                 len < 0) {
                 ESP_LOGE(TAG, "Error occurred during receiving: errno %d", errno);
             } else if (len < 4 || len > MAX_RX_BUFFER_SIZE) {
@@ -149,7 +152,7 @@ void UDPServer::shutdown() {
                 }
                 buffer->erase(buffer->begin(), buffer->begin() + 4); // todo: copying
                 buffer->resize(msg_size);
-        that->m_rx_queue->enqueue(std::move(buffer));
+                that->m_rx_queue->enqueue(std::move(buffer), std::chrono::milliseconds(MAX_RX_QUEUE_ENQUEUE_TIMEOUT_MS));
             }
         }
 
@@ -190,7 +193,7 @@ bool UDPServer::authenticate_client(int sock) {
     return 0;
 }
 
-int UDPServer::send_msg(char *buffer, const uint32_t length) const {
+int UDPServer::send_msg(uint8_t *buffer, size_t len) const {
     if (!is_network_connected() || m_tx_server_sock == -1) {
         return -1;
     }
@@ -201,13 +204,13 @@ int UDPServer::send_msg(char *buffer, const uint32_t length) const {
         .sin_addr = {.s_addr = inet_addr(SEND_MCAST)},
     };
 
-  uint32_t size = length;
+    uint32_t size = (uint32_t)len;
 
     iovec iov[2];
     iov[0].iov_base = &size;
     iov[0].iov_len = 4;
     iov[1].iov_base = buffer;
-  iov[1].iov_len = length;
+    iov[1].iov_len = size;
 
     msghdr msg = {};
     msg.msg_iov = iov;

main/LoopManager.cpp

@@ -71,7 +71,9 @@
 void LoopManager::send_sensor_reading(bool durable) const {
     Flatbuffers::SensorMessageBuilder smb{};
     // todo: get data from sensor
+    if (m_actuator) {
         auto data = m_actuator->get_sensor_data();
         const auto [ptr, size] = smb.build_sensor_message(data);
         m_messaging_interface->send(reinterpret_cast<char *>(ptr), size, PC_ADDR, SENSOR_TAG, durable);
+    }
 }

main/control/Servo1Actuator.cpp

@@ -9,6 +9,7 @@
 #include "driver/ledc.h"
 #include "flatbuffers_generated/SensorMessage_generated.h"
 #include "util/number_utils.h"
+#include "esp_log.h"
 
 #define LOW_DUTY 200
 #define HIGH_DUTY 1000
@@ -44,7 +45,7 @@ void Servo1Actuator::actuate(uint8_t *cmd) {
         util::mapRange<int32_t>(angleControlCmd->angle(), 0, 180, LOW_DUTY, HIGH_DUTY);
 
     m_target = angleControlCmd->angle();
-    std::cout << "actuating to " << angleControlCmd->angle() << std::endl;
+    ESP_LOGI("TMP", "actuating to %d", angleControlCmd->angle());
 
     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));