From c14c82ce9cf4a7e00b19db25896f69a1352d4502 Mon Sep 17 00:00:00 2001
From: Johnathon Slightham <johnathon.slightham@uwaterloo.ca>
Date: Tue, 27 Jan 2026 22:16:24 -0500
Subject: [PATCH] Reduce DataLink latency

---
 components/dataLink/CMakeLists.txt            |   2 +-
 components/dataLink/DataLinkFrames.cpp        | 217 ++++--------
 components/dataLink/DataLinkManager.cpp       | 214 ++++++------
 components/dataLink/DataLinkRIP.cpp           | 100 +++---
 components/dataLink/DataLinkScheduler.cpp     |  85 ++---
 components/dataLink/include/DataLinkManager.h |  94 ++---
 components/dataLink/include/Frames.h          |   8 +-
 components/dataLink/include/Scheduler.h       |  28 +-
 .../ptrQueue/include/BlockingPriorityQueue.h  |  54 +++
 components/ptrQueue/include/BlockingQueue.h   |  53 +++
 components/rmt/include/RMTManager.h           |  17 +-
 components/rpc/CommunicationFactory.cpp       |   4 +-
 components/rpc/CommunicationRouter.cpp        | 251 +++++++-------
 components/rpc/MessagingInterface.cpp         |   5 +-
 components/rpc/OrientationDetection.cpp       |   4 -
 components/rpc/include/CommunicationFactory.h |   6 +-
 components/rpc/include/CommunicationRouter.h  |  16 +-
 components/rpc/include/IRPCServer.h           |   7 +-
 components/rpc/include/MessagingInterface.h   |   9 +-
 components/rpc/include/wireless/TCPServer.h   |  16 +-
 components/rpc/include/wireless/UDPServer.h   |  21 +-
 components/rpc/wireless/TCPServer.cpp         |  11 +-
 components/rpc/wireless/UDPServer.cpp         | 325 +++++++++---------
 main/LoopManager.cpp                          |   8 +-
 main/control/Servo1Actuator.cpp               |   3 +-
 25 files changed, 801 insertions(+), 757 deletions(-)
 create mode 100644 components/ptrQueue/include/BlockingPriorityQueue.h
 create mode 100644 components/ptrQueue/include/BlockingQueue.h

diff --git a/components/dataLink/CMakeLists.txt b/components/dataLink/CMakeLists.txt
index 60a931a..ce67ef1 100644
--- a/components/dataLink/CMakeLists.txt
+++ b/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")
diff --git a/components/dataLink/DataLinkFrames.cpp b/components/dataLink/DataLinkFrames.cpp
index 6c45961..2e2777f 100644
--- a/components/dataLink/DataLinkFrames.cpp
+++ b/components/dataLink/DataLinkFrames.cpp
@@ -1,11 +1,13 @@
 #include "DataLinkManager.h"
 #include "esp_log.h"
+#include <cstring>
+#include <type_traits>
 
 /**
  * @brief Creates a Control Frame from `FrameHeader`
- * 
- * @param header 
- * @return ControlFrame 
+ *
+ * @param header
+ * @return ControlFrame
  */
 ControlFrame make_control_frame_from_header(const FrameHeader& header) {
     ControlFrame frame{};
@@ -21,9 +23,9 @@ ControlFrame make_control_frame_from_header(const FrameHeader& header) {
 
 /**
  * @brief Creates a Generic Frame from `FrameHeader`
- * 
- * @param header 
- * @return GenericFrame 
+ *
+ * @param header
+ * @return GenericFrame
  */
 GenericFrame make_generic_frame_from_header(const FrameHeader& header) {
     GenericFrame frame{};
@@ -40,11 +42,11 @@ GenericFrame make_generic_frame_from_header(const FrameHeader& header) {
 }
 
 /**
- * @brief Store a fragment that has been received 
- * 
- * @param fragment 
+ * @brief Store a fragment that has been received
+ *
+ * @param fragment
  * @param channel
- * @return esp_err_t 
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::store_fragment(GenericFrame* fragment, uint8_t channel){
     if (fragment == nullptr){
@@ -64,7 +66,7 @@ esp_err_t DataLinkManager::store_fragment(GenericFrame* fragment, uint8_t channe
     if (rx_fragment_mutex[channel] == NULL){
         return ESP_FAIL;
     }
-    
+
     if (xSemaphoreTake(rx_fragment_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){
         return ESP_ERR_TIMEOUT;
     }
@@ -106,8 +108,8 @@ esp_err_t DataLinkManager::store_fragment(GenericFrame* fragment, uint8_t channe
 
     if (static_cast<FrameType>(GET_TYPE(fragment->type_flag)) != FrameType::MISC_UDP_GENERIC_TYPE){
         SendAckMetaData data = {
-            .data = {GENERIC_FRAG_ACK_PREAMBLE, static_cast<uint8_t>((last_consec_rx_frag & 0xFF00) >> 8), static_cast<uint8_t>(last_consec_rx_frag & 0xFF), 
-            static_cast<uint8_t>((fragment->total_frag & 0xFF00) >> 8), static_cast<uint8_t>(fragment->total_frag & 0xFF), 
+            .data = {GENERIC_FRAG_ACK_PREAMBLE, static_cast<uint8_t>((last_consec_rx_frag & 0xFF00) >> 8), static_cast<uint8_t>(last_consec_rx_frag & 0xFF),
+            static_cast<uint8_t>((fragment->total_frag & 0xFF00) >> 8), static_cast<uint8_t>(fragment->total_frag & 0xFF),
             static_cast<uint8_t>((fragment->seq_num & 0xFF00) >> 8), static_cast<uint8_t>(fragment->seq_num & 0xFF)},
             .sender_id = fragment->sender_id,
         };
@@ -129,14 +131,14 @@ esp_err_t DataLinkManager::store_fragment(GenericFrame* fragment, uint8_t channe
 
 /**
  * @brief Removes the corresponding entry from `fragment_map` and pushes the data onto `async_receive_queue`
- * 
+ *
  * @param board_id
  * @param sequence_num
- * @return esp_err_t 
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::complete_fragment(uint16_t board_id, uint16_t sequence_num, uint8_t channel){
     Rx_Metadata rx;
-    
+
     if (xSemaphoreTake(rx_fragment_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){
         return ESP_ERR_TIMEOUT;
     }
@@ -151,8 +153,9 @@ 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;
@@ -167,21 +170,21 @@ esp_err_t DataLinkManager::complete_fragment(uint16_t board_id, uint16_t sequenc
     if (xSemaphoreTake(rx_fragment_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){
         return ESP_ERR_TIMEOUT;
     }
-    
+
     if (fragment_map[channel][board_id].find(sequence_num) == fragment_map[channel][board_id].end()){
         xSemaphoreGive(rx_fragment_mutex[channel]);
         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,17 +206,12 @@ 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){
-        vPortFree(combined_data);
+    if (!async_receive_queue->enqueue(std::move(rx), std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS))) {
         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()) {
         fragment_map[channel].erase(board_id);
     }
@@ -225,104 +223,44 @@ esp_err_t DataLinkManager::complete_fragment(uint16_t board_id, uint16_t sequenc
 
 /**
  * @brief Sends an ACK
- * 
+ *
  * @param sender_id This is the board id that is receiving the ACK (the original sender board id)
  * @param data
  * @param data_len
- * @return esp_err_t 
- * 
+ * @return esp_err_t
+ *
  * @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);
-}
-
-/**
- * @brief Checks the channel receive queue for any received frames. If there is, return the first frame's data size
- * 
- * @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;
+    // 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);
+    return send(sender_id, std::move(buffer), FrameType::ACK_TYPE, 0x0);
 }
 
 /**
  * @brief Get the first frame's data
- * 
+ *
  * @param data Char array of the actual combined data
  * @param data_len Combined data length
  * @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){
-    if (data == nullptr || header == nullptr){
-        return ESP_ERR_INVALID_ARG;
+std::optional<std::unique_ptr<std::vector<uint8_t>>> DataLinkManager::async_receive(){
+    auto maybe_top = async_receive_queue->dequeue(std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS));
+    if (!maybe_top) {
+        return std::nullopt;
     }
+    Rx_Metadata top = std::move(*maybe_top);
 
-    if (data_len == 0){
-        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);
-
+    return std::make_optional<std::unique_ptr<std::vector<uint8_t>>>(std::move(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;
 
@@ -332,7 +270,7 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
         // ESP_LOGE(DEBUG_LINK_TAG, "RMT Failed to receive - recieve_rmt");
         return ESP_ERR_TIMEOUT;
     }
-    
+
     if (recv_len > MAX_FRAME_SIZE){
         ESP_LOGE(DEBUG_LINK_TAG, "Received frame is too large to be control or generic");
         return ESP_ERR_INVALID_RESPONSE;
@@ -343,16 +281,18 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
         return ESP_ERR_INVALID_RESPONSE;
     }
 
-    uint8_t message[MAX_FRAME_SIZE];
-    memset(message, 0, sizeof(message));
+    auto message = std::make_unique<std::vector<uint8_t>>();
+    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);
 
@@ -361,17 +301,17 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
         if (message_size != GENERIC_FRAG_ACK_DATA_SIZE || message_size == 0){
             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])),
-            .total_frags = static_cast<uint16_t>((message[3] << 8) | (message[4])),
-            .seq_num = static_cast<uint16_t>((message[5] << 8) | (message[6]))
+            .last_ack = static_cast<uint16_t>((message->data()[1] << 8) | (message->data()[2])),
+            .total_frags = static_cast<uint16_t>((message->data()[3] << 8) | (message->data()[4])),
+            .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);
 
         // if (res == ESP_OK){
@@ -379,7 +319,7 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
         // } else {
         //     ESP_LOGI(DEBUG_LINK_TAG, "Got ACK for seq number %d from board %d but got a lower conseq ack 0x%x%X Total Frag: 0x%X%X", record.seq_num, header.sender_id, message[1], message[2], message[3], message[4]);
         // }
-        
+
         return ESP_OK;
     }
 
@@ -390,36 +330,35 @@ 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
     if (static_cast<FrameType>(GET_TYPE(header.type_flag)) == FrameType::RIP_TABLE_CONTROL){
         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 hops = message[i+1];
+            uint8_t board_id = message->data()[i];
+            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;
-            
+
             res = rip_find_entry(board_id, &entry, true);
             if (res != ESP_OK){
                 return ESP_FAIL;
             }
-            
+
             if (entry == nullptr){
                 printf("rip pointer\n");
                 return ESP_FAIL; //no room for more entries in the table
             }
-            
+
             if (entry->valid == RIP_NEW_ROW){
                 //adding a new entry
                 rip_add_entry(board_id, hops + 1, channel, &entry);
@@ -427,7 +366,7 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
                 //updating an entry
                 rip_update_entry(hops + 1, channel, &entry);
             }
-            
+
             if (GET_FLAG(header.type_flag) == FLAG_DISCOVERY){
                 //discovery -> send routing table
                 // ESP_LOGI(DEBUG_LINK_TAG, "got discovery reply");
@@ -435,10 +374,10 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
                     .info = entry->info,
                     .channel = entry->channel
                 };
-                
+
                 xQueueSendToBack(discovery_tables, &row_queue, (TickType_t)10);
             }
-            
+
         }
         if (message_size == RIP_DISCOVERY_MESSAGE_SIZE){
             res = send_rip_frame(false, header.sender_id);
@@ -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){
-        async_receive_queue[channel].push(metadata);
-        xSemaphoreGive(async_rx_queue_mutex[channel]);
-    } else {
+    if (!async_receive_queue->enqueue(std::move(metadata), std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS))){
         return ESP_ERR_TIMEOUT;
     }
     return ESP_OK;
@@ -507,7 +436,7 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
             res = link_layer_obj->receive_rmt(i);
             res = link_layer_obj->start_receive_frames_rmt(i);
         }
-        
+
         vTaskDelay(pdMS_TO_TICKS(RECEIVE_TASK_PERIOD_MS));
     }
 
@@ -539,12 +468,12 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
             if (xSemaphoreTake(link_layer_obj->send_ack_queue_mutex[channel], pdMS_TO_TICKS(SEND_ACK_MUTEX_WAIT)) != pdTRUE){
                 continue;
             }
-    
+
             if (link_layer_obj->send_ack_queue[channel].empty()){
                 xSemaphoreGive(link_layer_obj->send_ack_queue_mutex[channel]);
                 continue;
             }
-    
+
             SendAckMetaData data = link_layer_obj->send_ack_queue[channel].front();
             link_layer_obj->send_ack_queue[channel].pop();
             link_layer_obj->send_ack(data.sender_id, data.data, GENERIC_FRAG_ACK_DATA_SIZE);
@@ -553,4 +482,4 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
     }
 
     vTaskDelete(nullptr);
-}
\ No newline at end of file
+}
diff --git a/components/dataLink/DataLinkManager.cpp b/components/dataLink/DataLinkManager.cpp
index 29d5d9c..d9ee48f 100644
--- a/components/dataLink/DataLinkManager.cpp
+++ b/components/dataLink/DataLinkManager.cpp
@@ -1,11 +1,16 @@
 #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
- * 
+ *
  * @param board_id Board ID of the current board. Will be written to the NVM under key "board" if not already written.
  */
 DataLinkManager::DataLinkManager(uint8_t board_id, uint8_t num_channels = MAX_CHANNELS){
@@ -28,14 +33,20 @@ 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();
 }
 
 /**
  * @brief Returns if the link layer is ready to receive frames
- * 
- * @return esp_err_t 
+ *
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::ready(){
     return (phys_comms == nullptr || rip_broadcast_task == NULL || rip_ttl_task == NULL || scheduler_task == NULL || receive_task == NULL) ? ESP_FAIL : ESP_OK;
@@ -43,10 +54,10 @@ esp_err_t DataLinkManager::ready(){
 
 /**
  * @brief Atomic function to get and post increment sequence number map
- * 
- * @param board_id 
- * @param seq_num 
- * @return esp_err_t 
+ *
+ * @param board_id
+ * @param seq_num
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::get_inc_sequence_num(uint8_t board_id, uint16_t* seq_num){
     if (seq_num == NULL){
@@ -60,16 +71,16 @@ esp_err_t DataLinkManager::get_inc_sequence_num(uint8_t board_id, uint16_t* seq_
     *seq_num = sequence_num_map[board_id]++;
 
     xSemaphoreGive(sequence_num_map_mutex);
-    
+
     return ESP_OK;
 }
 
 /**
  * @brief Atomic function to get sequence number map
- * 
- * @param board_id 
- * @param seq_num 
- * @return esp_err_t 
+ *
+ * @param board_id
+ * @param seq_num
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::get_sequence_num(uint8_t board_id, uint16_t* seq_num){
     if (seq_num == NULL){
@@ -83,7 +94,7 @@ esp_err_t DataLinkManager::get_sequence_num(uint8_t board_id, uint16_t* seq_num)
     *seq_num = sequence_num_map[board_id];
 
     xSemaphoreGive(sequence_num_map_mutex);
-    
+
     return ESP_OK;
 }
 
@@ -102,15 +113,15 @@ DataLinkManager::~DataLinkManager(){
     if (rip_ttl_task == NULL){
         vTaskDelete(rip_ttl_task);
         rip_ttl_task = NULL;
-    }    
+    }
     if (scheduler_task == NULL){
         vTaskDelete(scheduler_task);
         scheduler_task = NULL;
-    }    
+    }
     if (receive_task == NULL){
         vTaskDelete(receive_task);
         receive_task = NULL;
-    }    
+    }
     if (send_ack_task == NULL){
         vTaskDelete(send_ack_task);
         send_ack_task = NULL;
@@ -136,14 +147,14 @@ esp_err_t DataLinkManager::set_board_id(uint8_t board_id){
         nvs_close(handle);
         return res;
     }
-    
+
     res = nvs_commit(handle);
     if (res != ESP_OK){
         ESP_LOGE(DEBUG_LINK_TAG, "Failed to commit write");
         nvs_close(handle);
         return res;
     }
-    
+
     this_board_id = board_id;
     ESP_LOGI(DEBUG_LINK_TAG, "Successfully wrote %d to NVM", board_id);
 
@@ -170,19 +181,19 @@ esp_err_t DataLinkManager::get_board_id(uint8_t& board_id){
     ESP_LOGI(DEBUG_LINK_TAG, "Successfully got board id %d from NVM", board_id);
 
     nvs_close(handle);
-    
+
     return ESP_OK;
 }
 
 /**
  * @brief Helper function to create a control frame
- * 
- * @param dest_board 
- * @param data 
- * @param data_len 
- * @param type 
- * @param flag 
- * @return esp_err_t 
+ *
+ * @param dest_board
+ * @param data
+ * @param data_len
+ * @param type
+ * @param flag
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::create_control_frame(uint8_t* data, uint16_t data_len, ControlFrame control_frame, uint8_t* send_data, size_t* send_data_len){
     if (data == nullptr){
@@ -198,7 +209,7 @@ esp_err_t DataLinkManager::create_control_frame(uint8_t* data, uint16_t data_len
     if (data_len > MAX_FRAME_SIZE){
         ESP_LOGE(DEBUG_LINK_TAG, "Data for control frame is too large. Maximum size is %d. Current data length is %d", MAX_FRAME_SIZE, data_len);
         return ESP_ERR_INVALID_ARG;
-    } 
+    }
 
     if (send_data == nullptr){
         ESP_LOGE(DEBUG_LINK_TAG, "Invalid pointer for send_data");
@@ -229,7 +240,7 @@ esp_err_t DataLinkManager::create_control_frame(uint8_t* data, uint16_t data_len
     send_data[offset++] = control_frame.type_flag;
     send_data[offset++] = data_len;
     send_data[offset++] = (data_len >> 8) & 0xFF;
-    
+
     memcpy(&send_data[offset], data, data_len);
 
     offset += control_frame.data_len;
@@ -253,14 +264,14 @@ esp_err_t DataLinkManager::create_control_frame(uint8_t* data, uint16_t data_len
 
 /**
  * @brief Helper function to create a generic frame
- * 
- * @param data 
- * @param data_len 
- * @param generic_frame 
- * @param offset 
- * @param send_data 
- * @param send_data_len 
- * @return esp_err_t 
+ *
+ * @param data
+ * @param data_len
+ * @param generic_frame
+ * @param offset
+ * @param send_data
+ * @param send_data_len
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::create_generic_frame(uint8_t* data, uint16_t data_len, GenericFrame generic_frame, uint16_t offset, uint8_t* send_data, size_t* send_data_len){
         if (data == nullptr){
@@ -276,7 +287,7 @@ esp_err_t DataLinkManager::create_generic_frame(uint8_t* data, uint16_t data_len
     if (data_len > MAX_FRAME_SIZE){
         ESP_LOGE(DEBUG_LINK_TAG, "Data for generic frame is too large. Maximum size is %d. Current data length is %d", MAX_FRAME_SIZE, data_len);
         return ESP_ERR_INVALID_ARG;
-    } 
+    }
 
     if (send_data == nullptr){
         ESP_LOGE(DEBUG_LINK_TAG, "Invalid pointer for send_data");
@@ -306,16 +317,16 @@ esp_err_t DataLinkManager::create_generic_frame(uint8_t* data, uint16_t data_len
     send_data[send_data_offset++] = (generic_frame.seq_num >> 8) & 0xFF;
 
     send_data[send_data_offset++] = generic_frame.type_flag;
-    
+
     send_data[send_data_offset++] = generic_frame.total_frag & 0xFF;
     send_data[send_data_offset++] = (generic_frame.total_frag >> 8) & 0xFF;
-    
+
     send_data[send_data_offset++] = generic_frame.frag_num & 0xFF;
     send_data[send_data_offset++] = (generic_frame.frag_num >> 8) & 0xFF;
-    
+
     send_data[send_data_offset++] = data_len;
     send_data[send_data_offset++] = (data_len >> 8) & 0xFF;
-    
+
     memcpy(&send_data[send_data_offset], &data[offset], data_len);
 
     send_data_offset += data_len;
@@ -339,22 +350,22 @@ esp_err_t DataLinkManager::create_generic_frame(uint8_t* data, uint16_t data_len
 
 /**
  * @brief Schedules a frame to be sent via RMT
- * 
+ *
  * @param dest_board 8 bit ID of the destination board
- * @param data 
+ * @param data
  * @param data_len Length of the data in bytes
- * @param type 
- * @return esp_err_t 
+ * @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;
     }
@@ -363,25 +374,15 @@ esp_err_t DataLinkManager::send(uint8_t dest_board, uint8_t* data, uint16_t data
         //If broadcasting generic frames, we don't to spam acks to this board
         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,
-        .len = data_len,
+        .data = std::move(buffer),
         .last_ack = 0,
         .curr_fragment = 0,
         .timeout = 0,
@@ -415,10 +415,9 @@ esp_err_t DataLinkManager::send(uint8_t dest_board, uint8_t* data, uint16_t data
 
     uint8_t channel = 0;
     res = route_frame(dest_board, &channel);
-    
+
     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;
 }
@@ -447,11 +445,11 @@ void DataLinkManager::print_buffer_binary(const uint8_t* buffer, size_t length)
 
 /**
  * @deprecated This function is deprecated. This is replaced by `async_receive_info` and `async_receive`. This function returns `ESP_FAIL`
- * 
+ *
  * @brief Starts the RMT async receive job to start listening for a new frame over a given channel
- * 
- * @param curr_channel 
- * @return esp_err_t 
+ *
+ * @param curr_channel
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::start_receive_frames(uint8_t curr_channel){
     return ESP_FAIL;
@@ -459,9 +457,9 @@ esp_err_t DataLinkManager::start_receive_frames(uint8_t curr_channel){
 
 /**
  * @brief Starts the RMT async receive job to start listening for a new frame over a given channel
- * 
- * @param curr_channel 
- * @return esp_err_t 
+ *
+ * @param curr_channel
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::start_receive_frames_rmt(uint8_t curr_channel){
     if (curr_channel >= num_channels){
@@ -472,29 +470,29 @@ esp_err_t DataLinkManager::start_receive_frames_rmt(uint8_t curr_channel){
 
 /**
  * @deprecated This function is deprecated. This is replaced by `async_receive_info` and `async_receive`. This function returns `ESP_FAIL`
- * 
+ *
  * @brief Receive Control Frame from RMT Physical Layer
- * 
+ *
  * @param data Byte array
  * @param data_len Length of the byte array
  * @param recv_len Length of the received data
  * @param curr_channel Physical channel pair to look at
- * @return esp_err_t 
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::receive(uint8_t* data, size_t data_len, size_t* recv_len, uint8_t curr_channel){
     return ESP_FAIL;
 }
 
 /**
- * @brief 
- * 
- * @param data 
- * @param data_len 
- * @param message 
- * @param message_size 
- * @param header 
- * @return esp_err_t 
- * 
+ * @brief
+ *
+ * @param data
+ * @param data_len
+ * @param message
+ * @param message_size
+ * @param header
+ * @return esp_err_t
+ *
  * @deprecated
  * Will be moved to private function
  */
@@ -527,7 +525,7 @@ esp_err_t DataLinkManager::get_data_from_frame(uint8_t* data, size_t data_len, u
         }
 
         header->data_len = (uint16_t)data[6] | ((uint16_t)data[7] << 8);
-    
+
         if (header->data_len > data_len){
             ESP_LOGE(DEBUG_LINK_TAG, "Mismatch data length in control frame");
             return ESP_ERR_INVALID_RESPONSE;
@@ -546,18 +544,18 @@ esp_err_t DataLinkManager::get_data_from_frame(uint8_t* data, size_t data_len, u
         }
 
         memcpy(message, &data[8], header->data_len);
-    
+
         geneate_crc_16(data, 8*sizeof(uint8_t) + header->data_len, &header->crc_16);
 
         uint16_t crc_calc = ((uint16_t)data[8 + header->data_len] | ((uint16_t)data[9 + header->data_len] << 8));
-    
+
         if (crc_calc != header->crc_16){
             //CRC mismatch
             ESP_LOGE(DEBUG_LINK_TAG, "CRC Mismatch - Control Frame");
             ESP_LOGE(DEBUG_LINK_TAG, "Got 0x%04X but calculated 0x%04X\n", crc_calc, header->crc_16);
             return ESP_ERR_INVALID_CRC;
         }
-    
+
     } else {
         //generic frame
 
@@ -578,7 +576,7 @@ esp_err_t DataLinkManager::get_data_from_frame(uint8_t* data, size_t data_len, u
         }
 
         memcpy(message, &data[12], *message_size);
-        
+
         if (total_frag != 1){
             geneate_crc_16(data, 12*sizeof(uint8_t) + *message_size, &header->crc_16);
         } else {
@@ -586,7 +584,7 @@ esp_err_t DataLinkManager::get_data_from_frame(uint8_t* data, size_t data_len, u
         }
 
         uint16_t crc_calc = ((uint16_t)data[12 + *message_size] | ((uint16_t)data[13 + *message_size] << 8));
-    
+
         if (crc_calc != header->crc_16 && total_frag != 1){
             //CRC mismatch
             ESP_LOGE(DEBUG_LINK_TAG, "CRC Mismatch - Generic Frame");
@@ -594,7 +592,7 @@ esp_err_t DataLinkManager::get_data_from_frame(uint8_t* data, size_t data_len, u
             return ESP_ERR_INVALID_CRC;
         }
     }
-    
+
     // printf("Received Frame Information:\n");
     // printf("%-10s %-12s %-13s %-15s %-12s %-10s %-6s\n",
     // "Preamble", "Sender ID", "Receiver ID", "Sequence Num", "Type+Flag", "Data Len", "CRC");
@@ -609,11 +607,11 @@ esp_err_t DataLinkManager::get_data_from_frame(uint8_t* data, size_t data_len, u
 
 /**
  * @brief This function implements the CRC-16/CCITT algorithm
- * 
- * @param data 
- * @param data_len 
- * @param crc 
- * @return esp_err_t 
+ *
+ * @param data
+ * @param data_len
+ * @param crc
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::geneate_crc_16(uint8_t* data, size_t data_len, uint16_t* crc){
     if (data == nullptr){
@@ -625,27 +623,27 @@ esp_err_t DataLinkManager::geneate_crc_16(uint8_t* data, size_t data_len, uint16
     }
 
     *crc = 0x0;
-    
+
     for (size_t i = 0; i < data_len; i++){
         uint8_t tbl_idx = (*crc >> 8) ^ data[i];
         *crc = (*crc << 8) ^ crc16_table[tbl_idx];
     }
-    
+
     return ESP_OK;
 }
 
 /**
  * @brief Prints to console the encoded frame information from a byte array recevied from RMT
- * 
+ *
  * @note Should only be used for debug purposes
- * 
+ *
  * @warning This function may not be reliable/buggy
- * 
- * @param data 
- * @param data_len 
- * @param message 
- * @param message_len 
- * @return esp_err_t 
+ *
+ * @param data
+ * @param data_len
+ * @param message
+ * @param message_len
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::print_frame_info(uint8_t* data, size_t data_len, uint8_t* message, size_t message_len){
     // printf("Received frame of size %d:\n", data_len);
diff --git a/components/dataLink/DataLinkRIP.cpp b/components/dataLink/DataLinkRIP.cpp
index c0e5584..95114e4 100644
--- a/components/dataLink/DataLinkRIP.cpp
+++ b/components/dataLink/DataLinkRIP.cpp
@@ -4,7 +4,7 @@
 
 /**
  * @brief Initializes the RIP table
- * 
+ *
  */
 void DataLinkManager::init_rip(){
     for (size_t i = 0; i < RIP_MAX_ROUTES; i++){
@@ -54,11 +54,11 @@ esp_err_t DataLinkManager::rip_add_entry(uint8_t board_id, uint8_t hops, uint8_t
     (*entry)->ttl = RIP_TTL_START;
     (*entry)->valid = 1;
 
-    
+
     // ESP_LOGI(DEBUG_LINK_TAG, "board_id %d now has hops %d from channel %d", (*entry)->info.board_id, (*entry)->info.hops, channel);
-    
+
     xSemaphoreGive((*entry)->row_sem);
-    
+
     if (uxQueueMessagesWaiting(manual_broadcasts) == 0){
         bool dummy = true;
         xQueueSend(manual_broadcasts, &dummy, 0); //new row - send broadcast
@@ -108,9 +108,9 @@ esp_err_t DataLinkManager::rip_update_entry(uint8_t new_hop, uint8_t channel, RI
         (*entry)->channel = channel;
         // ESP_LOGI(DEBUG_LINK_TAG, "updated board_id %d now has hops %d from channel %d", (*entry)->info.board_id, (*entry)->info.hops, channel);
     }
-    
+
     (*entry)->ttl = RIP_TTL_START;
-    (*entry)->valid = 1; 
+    (*entry)->valid = 1;
 
     // ESP_LOGI(DEBUG_LINK_TAG, "refreshed board_id %d ttl", (*entry)->info.board_id);
 
@@ -128,10 +128,10 @@ esp_err_t DataLinkManager::rip_update_entry(uint8_t new_hop, uint8_t channel, RI
 /**
  * @brief Finds the board_id in the table if it exists and stores that row in `entry`
  * TODO: use an unordered map instead of an array?
- * 
- * @param board_id 
- * @param entry 
- * @return esp_err_t 
+ *
+ * @param board_id
+ * @param entry
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::rip_find_entry(uint8_t board_id, RIPRow** entry, bool reserve_row = false){
     RIPRow* free_slot = nullptr;
@@ -144,7 +144,7 @@ esp_err_t DataLinkManager::rip_find_entry(uint8_t board_id, RIPRow** entry, bool
             xSemaphoreGive(rip_table[i].row_sem);
             // ESP_LOGI(DEBUG_LINK_TAG, "Found %d in table at row %d", board_id, i);
             return ESP_OK;
-        } 
+        }
         if (rip_table[i].valid == RIP_INVALID_ROW && free_slot == nullptr){
             free_slot = &rip_table[i];
         }
@@ -176,10 +176,10 @@ esp_err_t DataLinkManager::rip_find_entry(uint8_t board_id, RIPRow** entry, bool
 
 /**
  * @brief Returns the associated RIP Table row by row number. Information returned is read only.
- * 
- * @param entry 
- * @param row_num 
- * @return esp_err_t 
+ *
+ * @param entry
+ * @param row_num
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::rip_get_row(RIPRow** entry, uint8_t row_num){
     if (entry == nullptr){
@@ -216,18 +216,16 @@ esp_err_t DataLinkManager::rip_get_row(RIPRow** entry, uint8_t row_num){
 
 /**
  * @brief Sends RIP frame
- * 
+ *
  * @param broadcast True - broadcasts (sends rip table to all available channels); False - sends rip table via routing based on `dest_id`
  * @param dest_id Destination board (requesting board) to send the rip table to (ignored if `broadcast is true`)
- * @return esp_err_t 
+ * @return esp_err_t
  */
 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,43 +233,40 @@ 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);
-                
+
                 if (res != ESP_OK){
                     continue;
                 }
-                
+
                 if (entry == nullptr){
                     continue;
                 }
 
                 // ESP_LOGI(DEBUG_LINK_TAG, "Found entry for board %d with hops %d", entry->info.board_id, entry->info.hops);
-                
+
                 if (entry->channel == channel){
                     //poisoned reverse
-                    rip_message[message_idx++] = entry->info.board_id;
-                    rip_message[message_idx++] = RIP_MAX_HOPS + 1;
+                    rip_message->at(message_idx++) = entry->info.board_id;
+                    rip_message->at(message_idx++) = RIP_MAX_HOPS + 1;
                 } else {
-                    rip_message[message_idx++] = entry->info.board_id;
-                    rip_message[message_idx++] = entry->info.hops;
+                    rip_message->at(message_idx++) = entry->info.board_id;
+                    rip_message->at(message_idx++) = entry->info.hops;
                 }
             }
 
-            uint8_t* send_data = (uint8_t*)pvPortMalloc(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);
+            rip_message->resize(message_idx);
 
             res = get_inc_sequence_num(BROADCAST_ADDR, &seq_num);
             if (res != ESP_OK){
                 ESP_LOGE(DEBUG_LINK_TAG, "Failed atomic get increment sequence number map");
                 return res;
             }
-            
+
             SchedulerMetadata metadata = {
                 .header = {
                     .preamble = START_OF_FRAME,
@@ -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,
-                .len = message_idx,
+                .data = std::move(rip_message),
                 .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[message_idx++] = entry->info.hops;
+            rip_message->data()[message_idx++] = entry->info.board_id;
+            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");
         }
@@ -324,10 +320,10 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
 
 /**
  * @brief Determines which channel to route the frame to, depending on the dest (board) id
- * 
- * @param dest_id 
- * @param channel_to_send 
- * @return esp_err_t 
+ *
+ * @param dest_id
+ * @param channel_to_send
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::route_frame(uint8_t dest_id, uint8_t* channel_to_send){
     RIPRow* entry = nullptr;
@@ -350,12 +346,12 @@ esp_err_t DataLinkManager::route_frame(uint8_t dest_id, uint8_t* channel_to_send
 
 /**
  * @brief Fetches the current routing table at the perspective of the host board
- * 
+ *
  * @details The routing table is based off of RIP
- * 
- * @param table 
- * @param table_size 
- * @return esp_err_t 
+ *
+ * @param table
+ * @param table_size
+ * @return esp_err_t
  */
 esp_err_t DataLinkManager::get_routing_table(RIPRow_public* table, size_t* table_size){
     if (table == nullptr){
@@ -383,7 +379,7 @@ esp_err_t DataLinkManager::get_routing_table(RIPRow_public* table, size_t* table
             table[i].info = rip_table[i].info;
             table[i].channel = rip_table[i].channel;
             curr_size++;
-        } 
+        }
         xSemaphoreGive(rip_table[i].row_sem);
     }
 
@@ -446,7 +442,7 @@ esp_err_t DataLinkManager::get_routing_table(RIPRow_public* table, size_t* table
                     link_layer_obj->rip_table[i].ttl_flush = RIP_FLUSH_COUNT;
                     broadcast = true;
                 }
-            } 
+            }
 
             xSemaphoreGive(link_layer_obj->rip_table[i].row_sem);
         }
@@ -472,4 +468,4 @@ void DataLinkManager::start_rip_tasks(){
     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", 4096, static_cast<void*>(this), 5, &rip_ttl_task);
-} 
\ No newline at end of file
+}
diff --git a/components/dataLink/DataLinkScheduler.cpp b/components/dataLink/DataLinkScheduler.cpp
index b521109..f05e8d1 100644
--- a/components/dataLink/DataLinkScheduler.cpp
+++ b/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(i);
-        }
-        vTaskDelay(pdMS_TO_TICKS(SCHEDULER_PERIOD_MS));
-
+        link_layer_obj->scheduler_send(channel);
     }
+    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){
-        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;
-    }
+    frame_queue[channel]->enqueue(std::move(frame), std::chrono::milliseconds(FRAME_ENQUEUE_TIMEOUT_MS));
 
     // ESP_LOGI(DEBUG_LINK_TAG, "Pushed frame to queue on channel %d", channel);
 
@@ -103,25 +101,15 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
         return ESP_ERR_INVALID_ARG;
     }
 
-    if (sq_handle[channel] == nullptr){
-        return ESP_FAIL;
-    }
+    vTaskDelay(pdMS_TO_TICKS(10));  // the messages cannot be too close together
 
     SchedulerMetadata frame;
 
-    if (xSemaphoreTake(sq_handle[channel], pdMS_TO_TICKS(SCHEDULER_MUTEX_WAIT)) == pdTRUE){
-        if (frame_queue[channel].empty()){
-            xSemaphoreGive(sq_handle[channel]);
-            // 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]);
+    if (auto maybe_frame = frame_queue[channel]->dequeue(std::chrono::milliseconds(FRAME_DEQUEUE_TIMEOUT_MS))) {
+        frame = *maybe_frame;
     } else {
-        ESP_LOGE(DEBUG_LINK_TAG, "Failed to get mutex when trying to send");
-        //Failed to obtain mutex
-        return ESP_ERR_TIMEOUT;
+        // ESP_LOGI(DEBUG_LINK_TAG, "Scheduler queue for channel %d is empty", channel);
+        return ESP_OK;
     }
 
     if (frame.data == nullptr){
@@ -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");
diff --git a/components/dataLink/include/DataLinkManager.h b/components/dataLink/include/DataLinkManager.h
index 59ba9d4..0829efd 100644
--- a/components/dataLink/include/DataLinkManager.h
+++ b/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"
 
@@ -23,43 +25,43 @@ static const char* NVS_BOARD_NAMESPACE = "board";
 
 //look up table for crc
 static const uint16_t crc16_table[256] = {
-    0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, 
-    0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, 
-    0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, 
-    0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, 
-    0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, 
-    0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, 
-    0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49, 
-    0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78, 
-    0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067, 
-    0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, 
-    0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 
-    0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, 
-    0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3, 
-    0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, 
-    0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, 
-    0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0 
-}; 
+    0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
+    0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
+    0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
+    0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
+    0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
+    0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
+    0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
+    0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
+    0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
+    0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
+    0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+    0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
+    0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
+    0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
+    0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
+    0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
+};
 
 #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
- * 
+ *
  * @author Justin Chow
  */
 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);
-        esp_err_t async_receive(uint8_t* data, uint16_t data_len, FrameHeader* header, uint8_t channel);
+        std::optional<std::unique_ptr<std::vector<uint8_t>>> async_receive();
         esp_err_t ready();
         esp_err_t send_ack(uint8_t sender_id, uint8_t* data, uint16_t data_len);
     private:
@@ -75,7 +77,7 @@ class DataLinkManager{
         volatile bool stop_tasks = false; //used by the tasks to know when to stop (set true when DataLinkManager is destroyed)
         TaskHandle_t rip_broadcast_task = NULL;
         TaskHandle_t rip_ttl_task = NULL;
-        
+
         esp_err_t set_board_id(uint8_t board_id);
         esp_err_t get_board_id(uint8_t& board_id);
         void print_binary(uint8_t byte);
@@ -84,7 +86,7 @@ class DataLinkManager{
         esp_err_t geneate_crc_16(uint8_t* data, size_t data_len, uint16_t* crc);
         esp_err_t create_control_frame(uint8_t* data, uint16_t data_len, ControlFrame control_frame, uint8_t* send_data, size_t* send_data_len);
         esp_err_t create_generic_frame(uint8_t* data, uint16_t data_len, GenericFrame generic_frame, uint16_t offset, uint8_t* send_data, size_t* send_data_len);
-        
+
         //==== RIP related functions ====
 
         void init_rip();
@@ -93,29 +95,28 @@ class DataLinkManager{
         esp_err_t rip_add_entry(uint8_t board_id, uint8_t hops, uint8_t channel, RIPRow** entry);
         esp_err_t rip_reset_entry_ttl(uint8_t board_id);
         esp_err_t rip_get_row(RIPRow** entry, uint8_t row_num);
-        
+
         //this is stored locally with metadata `ttl`
         // std::unordered_map<uint8_t, RIPRow> rip_table; //using a hash map to store the routes to other boards - will be used as we scale up
         RIPRow rip_table[RIP_MAX_ROUTES]; //temp using a static array
-        
+
         void start_rip_tasks();
-        esp_err_t send_rip_frame(bool broadcast, uint8_t dest_id); 
+        esp_err_t send_rip_frame(bool broadcast, uint8_t dest_id);
         [[noreturn]] static void rip_broadcast_timer_function(void* args);
         [[noreturn]] static void rip_ttl_decrement_task(void* args);
         QueueHandle_t manual_broadcasts;
 
         QueueHandle_t discovery_tables;
-        
+
         esp_err_t route_frame(uint8_t dest_id, uint8_t* channel_to_send);
 
         //==== Frame Scheduling related functions ====
 
         /**
          * @brief Priority queue for each channel to schedule when to send frames
-         * 
+         *
          */
-        std::priority_queue<SchedulerMetadata, std::vector<SchedulerMetadata>, FrameCompare> frame_queue[MAX_CHANNELS];
-        SemaphoreHandle_t sq_handle[MAX_CHANNELS];
+        std::unique_ptr<BlockingPriorityQueue<SchedulerMetadata, std::vector<SchedulerMetadata>, FrameCompare>> frame_queue[MAX_CHANNELS];
         void init_scheduler();
         esp_err_t push_frame_to_scheduler(SchedulerMetadata frame, uint8_t channel);
         TaskHandle_t scheduler_task = NULL;
@@ -132,13 +133,13 @@ class DataLinkManager{
 
         /**
          * @brief Stores generic frame fragments
-         * 
+         *
          * Mapping:
          * Board ID (of the receiver) -> Sequence number -> Array of Generic Frame Fragments, with size of the number of expected fragments
-         * 
+         *
          * TODO:
          * - Sliding window + ACKs
-         * 
+         *
          */
         std::unordered_map<uint16_t, std::unordered_map<uint16_t, FragmentMetadata>> fragment_map[MAX_CHANNELS];
 
@@ -150,26 +151,26 @@ class DataLinkManager{
         //Async receive
         /**
          * @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);
 
         /**
          * @brief Receive thread entry point
-         * 
-         * @param args 
+         *
+         * @param args
          */
         [[noreturn]] static void receive_thread_main(void* args);
 
         /**
          * @brief Receive bytes from Physical Layer (RMT)
-         * 
+         *
          * @note This replaces the deprecated `receive` function
-         * 
+         *
          * @param channel Physical channel pair to look at
-         * @return esp_err_t 
+         * @return esp_err_t
          */
         esp_err_t receive_rmt(uint8_t channel);
 
@@ -177,10 +178,10 @@ class DataLinkManager{
 
         /**
          * @brief Generic Frame Sliding Window
-         * 
+         *
          * Mapping:
          * Board Id (of the receiver) -> Sequence Number -> FrameAckRecord
-         * 
+         *
          */
         std::unordered_map<uint16_t, std::unordered_map<uint16_t, FrameAckRecord>> sliding_window[MAX_CHANNELS];
 
@@ -194,8 +195,8 @@ class DataLinkManager{
 
         /**
          * @brief Thread for sending acks - Send ACKs on a separate thread to not hold up the receive thread (missing other frames)
-         * 
-         * @param args 
+         *
+         * @param args
          */
         [[noreturn]] static void send_ack_thread_main(void* args);
         TaskHandle_t send_ack_task = NULL;
@@ -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
diff --git a/components/dataLink/include/Frames.h b/components/dataLink/include/Frames.h
index d39e0b5..362f34d 100644
--- a/components/dataLink/include/Frames.h
+++ b/components/dataLink/include/Frames.h
@@ -46,7 +46,7 @@ enum class FrameType : uint8_t {
     DISTANCE_SENSOR_TYPE = 0xA0, //0b1010_0000
     SERVO_TYPE = 0xC0, //0b1100_0000
     MISC_CONTROL_TYPE = 0xD0, //0b1101_0000
-    
+
     //Generic Frames
     MISC_GENERIC_TYPE = 0x00, //0b0000_0000
     MISC_UDP_GENERIC_TYPE = 0x10, // 0b0001_0000 - Same as MISC_GENERIC_TYPE except no ACK frames will be expected
@@ -98,7 +98,7 @@ typedef struct _header{
 
 using Frame = std::variant<ControlFrame, GenericFrame>;
 
-ControlFrame make_control_frame_from_header(const FrameHeader& header); 
+ControlFrame make_control_frame_from_header(const FrameHeader& header);
 
 GenericFrame make_generic_frame_from_header(const FrameHeader& header);
 
@@ -108,9 +108,9 @@ 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;
 
-#endif //DATA_LINK
\ No newline at end of file
+#endif //DATA_LINK
diff --git a/components/dataLink/include/Scheduler.h b/components/dataLink/include/Scheduler.h
index 756e6e7..19b9750 100644
--- a/components/dataLink/include/Scheduler.h
+++ b/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
-    uint16_t len; // length of the actual data
+    std::shared_ptr<std::vector<uint8_t>> data; // the actual data, and length of 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 {
@@ -43,21 +43,21 @@ typedef struct _send_ack_metadata{
 typedef struct _frame_compare {
     /**
      * @brief Uses aging based priority scheduling (linearly increasing priority with time)
-     * 
+     *
      * $P_f = B_f - A_f\alpha$
-     * 
+     *
      * - $P_f$ is the effective priority value (lower comes first)
-     * 
+     *
      * - $B_f$ is the base priority
-     * 
+     *
      * - $A_f$ is the age (amount of time the frame has waited in the queue)
-     * 
+     *
      * - $\alpha$ is the aging factor (rate at which a frame increases priority)
-     * 
-     * @param a 
-     * @param b 
-     * @return true 
-     * @return false 
+     *
+     * @param a
+     * @param b
+     * @return true
+     * @return false
      */
     bool operator()(const SchedulerMetadata& a, const SchedulerMetadata& b) const {
         int64_t now = esp_timer_get_time();
@@ -84,4 +84,4 @@ typedef struct _frame_compare {
     }
 } FrameCompare;
 
-#endif //DATA_LINK
\ No newline at end of file
+#endif //DATA_LINK
diff --git a/components/ptrQueue/include/BlockingPriorityQueue.h b/components/ptrQueue/include/BlockingPriorityQueue.h
new file mode 100644
index 0000000..3e35edf
--- /dev/null
+++ b/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
diff --git a/components/ptrQueue/include/BlockingQueue.h b/components/ptrQueue/include/BlockingQueue.h
new file mode 100644
index 0000000..1f37761
--- /dev/null
+++ b/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
diff --git a/components/rmt/include/RMTManager.h b/components/rmt/include/RMTManager.h
index 172c1d9..59a25d6 100644
--- a/components/rmt/include/RMTManager.h
+++ b/components/rmt/include/RMTManager.h
@@ -26,7 +26,7 @@
 
 /**
  * @brief This struct keeps track of the current byte and bit index of the user data being transmmitted via RMT
- * 
+ *
  */
 typedef struct {
     size_t byte_index; //which byte is currently being encoded when transmitting
@@ -63,16 +63,16 @@ typedef struct _rmt_channel{
     QueueHandle_t rx_queue;
     rmt_symbol_word_t raw_symbols[RECEIVE_BUFFER_SIZE]; //buffer to store the symbols on receive
     rmt_symbol_word_t decoded_recv_symbols[RECEIVE_BUFFER_SIZE]; //allocating some dummy size buffer for decoded string
-    
+
     //General
     uint8_t status;
 } rmt_channel;
 
 /**
  * @brief Class representing the RMT/Physical Layer
- * 
+ *
  * @author Justin Chow
- * 
+ *
  */
 class RMTManager{
     public:
@@ -81,7 +81,7 @@ class RMTManager{
         esp_err_t send(uint8_t* data, size_t size, rmt_transmit_config_t* config, uint8_t channel_num); //temp function to send some string data
         esp_err_t receive(uint8_t* recv_buf, size_t size, size_t* output_size, uint8_t channel_num);
 
-        static size_t encoder_callback(const void* data, size_t data_size, size_t symbols_written, 
+        static size_t encoder_callback(const void* data, size_t data_size, size_t symbols_written,
             size_t symbols_free, rmt_symbol_word_t* symbols, bool* done, void* arg);
 
         static bool rmt_rx_done_callback(rmt_channel_handle_t channel, const rmt_rx_done_event_data_t *edata, void *user_data);
@@ -90,7 +90,7 @@ class RMTManager{
         esp_err_t start_receiving(uint8_t channel_num);
 
         esp_err_t wait_until_send_complete(uint8_t channel_num);
-        
+
     private:
         uint8_t num_channels; //number of channels initalized
         esp_err_t init();
@@ -101,7 +101,7 @@ class RMTManager{
         int convert_symbols_to_char(rmt_symbol_word_t* symbols, size_t num, uint8_t* string, size_t output_num);
 
         [[noreturn]] static void freeMemory(void* args);
-        
+
         rmt_channel channels[MAX_CHANNELS] = {0};
         //=====================TX=====================
 
@@ -119,7 +119,7 @@ class RMTManager{
         //rx_receive_config
         rmt_receive_config_t receive_config = {
             .signal_range_min_ns = 200,
-            .signal_range_max_ns = 200 * 1000, 
+            .signal_range_max_ns = 200 * 1000,
             .flags = {
                 .en_partial_rx = true
             }
@@ -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
diff --git a/components/rpc/CommunicationFactory.cpp b/components/rpc/CommunicationFactory.cpp
index dc7e154..d6f3dbb 100644
--- a/components/rpc/CommunicationFactory.cpp
+++ b/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);
diff --git a/components/rpc/CommunicationRouter.cpp b/components/rpc/CommunicationRouter.cpp
index 43abd2d..1125915 100644
--- a/components/rpc/CommunicationRouter.cpp
+++ b/components/rpc/CommunicationRouter.cpp
@@ -1,3 +1,5 @@
+#include <chrono>
+#include <cstring>
 #include <iostream>
 
 #include "AngleControlMessageBuilder.h"
@@ -14,157 +16,172 @@
 #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); }
-
-// 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
+CommunicationRouter::~CommunicationRouter() {
+    vTaskDelete(m_router_thread);
+}
 
 [[noreturn]] void CommunicationRouter::router_thread(void *args) {
-  const auto that = static_cast<CommunicationRouter *>(args);
+    const auto that = static_cast<CommunicationRouter *>(args);
 
-  while (true) {
-    const auto buffer = that->m_tcp_rx_queue->dequeue();
-    ESP_LOGD(TAG, "Got message from TCP");
-    that->route(buffer->data(), buffer->size());
-  }
+    while (true) {
+        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(std::move(*maybe_buffer));
+        }
+    }
 }
 
 [[noreturn]] void CommunicationRouter::link_layer_thread(void *args) {
-  const auto that = static_cast<CommunicationRouter *>(args);
+    const auto that = static_cast<CommunicationRouter *>(args);
 
-  while (true) {
-    if (std::chrono::system_clock::now() - that->m_last_leader_updated >
-        std::chrono::seconds(2)) {
-      that->m_last_leader_updated = std::chrono::system_clock::now();
-      that->update_leader();
+    while (true) {
+        if (std::chrono::system_clock::now() - that->m_last_leader_updated >
+            std::chrono::seconds(2)) {
+            that->m_last_leader_updated = std::chrono::system_clock::now();
+            that->update_leader();
+        }
+
+        if (auto ptr = that->m_data_link_manager->async_receive()) {
+            that->route(std::move(*ptr));
+        }
     }
-
-    for (uint8_t channel = 0;
-         channel <
-         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;
+    route(reinterpret_cast<uint8_t *>(buffer), length);
+    return 0;
 }
 
 void CommunicationRouter::update_leader() {
-  RIPRow_public table[RIP_MAX_ROUTES];
-  size_t table_size = RIP_MAX_ROUTES;
-  this->m_data_link_manager->get_routing_table(table, &table_size);
+    RIPRow_public table[RIP_MAX_ROUTES];
+    size_t table_size = RIP_MAX_ROUTES;
+    this->m_data_link_manager->get_routing_table(table, &table_size);
 
-  // Leader election (just get the highest id in rip)
-  std::vector<int> connected_module_ids;
-  uint8_t max = m_module_id;
-  for (int i = 0; i < table_size; i++) {
-    const auto id = table[i].info.board_id;
-    connected_module_ids.emplace_back(id);
-    if (id > max) { // todo: change this to be correct
-      max = id;
+    // Leader election (just get the highest id in rip)
+    std::vector<int> connected_module_ids;
+    uint8_t max = m_module_id;
+    for (int i = 0; i < table_size; i++) {
+        const auto id = table[i].info.board_id;
+        connected_module_ids.emplace_back(id);
+        if (id > max) { // todo: change this to be correct
+            max = id;
+        }
     }
-  }
 
-  // Leader has changed, we may need to change PC connection state
-  if (this->m_leader != max) {
-    ESP_LOGI(TAG, "Leader has changed from %d to %d", this->m_leader, max);
-    if (max == m_module_id) {
-      m_pc_connection->connect();
-      m_lossless_server->startup();
-      m_lossy_server->startup();
-    } else if (this->m_leader == m_module_id) {
-      m_pc_connection->disconnect();
-      m_lossless_server->shutdown();
-      m_lossy_server->shutdown();
+    // Leader has changed, we may need to change PC connection state
+    if (this->m_leader != max) {
+        ESP_LOGI(TAG, "Leader has changed from %d to %d", this->m_leader, max);
+        if (max == m_module_id) {
+            m_pc_connection->connect();
+            m_lossless_server->startup();
+            m_lossy_server->startup();
+        } else if (this->m_leader == m_module_id) {
+            m_pc_connection->disconnect();
+            m_lossless_server->shutdown();
+            m_lossy_server->shutdown();
+        }
     }
-  }
 
-  this->m_leader = max;
+    this->m_leader = max;
 
-  if (this->m_leader == m_module_id) {
-    this->m_discovery_service->set_connected_boards(connected_module_ids);
-  }
+    if (this->m_leader == m_module_id) {
+        this->m_discovery_service->set_connected_boards(connected_module_ids);
+    }
 }
 
-void CommunicationRouter::route(uint8_t *buffer, const size_t length) const {
-  flatbuffers::Verifier verifier(buffer, length);
-  // 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);
-      mpi_message->destination() == m_module_id) {
-    this->m_rx_callback(reinterpret_cast<char *>(buffer), 512);
-  } 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);
-    } else {
-      this->m_lossy_server->send_msg(reinterpret_cast<char *>(buffer), 512);
+// Route without trying to copy to heap. Only call if you do not have a unique_ptr.
+// 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 = Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer);
+        mpi_message->destination() == m_module_id) {
+        auto ubuffer = std::make_unique<std::vector<uint8_t>>();
+        ubuffer->resize(size);
+        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(buffer, size);
+        } else {
+            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, std::move(buffer), FrameType::MOTOR_TYPE, 0);
+    } else {
+        this->m_data_link_manager->send(mpi_message->destination(), std::move(buffer), FrameType::MOTOR_TYPE,
+                                        0);
     }
-  } else if (mpi_message->destination() == PC_ADDR) {
-    this->m_data_link_manager->send(this->m_leader, buffer, length,
-                                    FrameType::MOTOR_TYPE, 0);
-  } else {
-    this->m_data_link_manager->send(mpi_message->destination(), buffer, length,
-                                    FrameType::MOTOR_TYPE, 0);
-  }
 }
 
 std::pair<std::vector<uint8_t>, std::vector<Orientation>>
 CommunicationRouter::get_physically_connected_modules() const {
-  std::vector<RIPRow_public> table;
-  table.resize(RIP_MAX_ROUTES);
-  size_t table_size = RIP_MAX_ROUTES * sizeof(RIPRow_public);
-  m_data_link_manager->get_routing_table(table.data(), &table_size);
+    std::vector<RIPRow_public> table;
+    table.resize(RIP_MAX_ROUTES);
+    size_t table_size = RIP_MAX_ROUTES * sizeof(RIPRow_public);
+    m_data_link_manager->get_routing_table(table.data(), &table_size);
 
-  std::vector<uint8_t> connected_module_ids;
-  std::vector<Orientation> connected_module_orientations;
-  connected_module_ids.resize(MAX_WIRED_CONNECTIONS);
-  connected_module_orientations.resize(MAX_WIRED_CONNECTIONS);
+    std::vector<uint8_t> connected_module_ids;
+    std::vector<Orientation> connected_module_orientations;
+    connected_module_ids.resize(MAX_WIRED_CONNECTIONS);
+    connected_module_orientations.resize(MAX_WIRED_CONNECTIONS);
 
-  for (int i = 0; i < MAX_WIRED_CONNECTIONS; i++) {
-    connected_module_ids[i] = 0; // this is not the PC ID here, marking as nc.
-  }
-
-  for (int i = 0; i < table_size; i++) {
-    if (table[i].info.hops == 1 && table[i].channel < MAX_WIRED_CONNECTIONS) {
-      connected_module_ids[table[i].channel] = table[i].info.board_id;
+    for (int i = 0; i < MAX_WIRED_CONNECTIONS; i++) {
+        connected_module_ids[i] = 0; // this is not the PC ID here, marking as nc.
     }
-  }
 
-  if (const auto id = connected_module_ids[0]; 0 == id) {
-    connected_module_orientations[0] = Orientation_Deg0;
-  } else {
-    connected_module_orientations[0] = OrientationDetection::get_orientation(0);
-  }
+    for (int i = 0; i < table_size; i++) {
+        if (table[i].info.hops == 1 && table[i].channel < MAX_WIRED_CONNECTIONS) {
+            connected_module_ids[table[i].channel] = table[i].info.board_id;
+        }
+    }
 
-  return {connected_module_ids, connected_module_orientations};
+    if (const auto id = connected_module_ids[0]; 0 == id) {
+        connected_module_orientations[0] = Orientation_Deg0;
+    } else {
+        connected_module_orientations[0] = OrientationDetection::get_orientation(0);
+    }
+
+    return {connected_module_ids, connected_module_orientations};
 }
 
 [[nodiscard]] uint8_t CommunicationRouter::get_leader() const {
-  return this->m_leader;
+    return this->m_leader;
 }
diff --git a/components/rpc/MessagingInterface.cpp b/components/rpc/MessagingInterface.cpp
index 97b11b9..2f12929 100644
--- a/components/rpc/MessagingInterface.cpp
+++ b/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) {
-    const auto mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(reinterpret_cast<const uint8_t *>(recv_buffer));
+void MessagingInterface::handleRecv(std::unique_ptr<std::vector<uint8_t>>&& buffer) {
+    const auto mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer->data());
 
     checkOrInsertTag(mpi_message->tag());
 
diff --git a/components/rpc/OrientationDetection.cpp b/components/rpc/OrientationDetection.cpp
index bf65bcb..30789cf 100644
--- a/components/rpc/OrientationDetection.cpp
+++ b/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;
     }
 }
diff --git a/components/rpc/include/CommunicationFactory.h b/components/rpc/include/CommunicationFactory.h
index d5c5ac5..7f6be00 100644
--- a/components/rpc/include/CommunicationFactory.h
+++ b/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_lossless_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<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>& rx_queue);
 };
 
 #endif //COMMUNICATIONFACTORY_H
diff --git a/components/rpc/include/CommunicationRouter.h b/components/rpc/include/CommunicationRouter.h
index 7177607..acd9210 100644
--- a/components/rpc/include/CommunicationRouter.h
+++ b/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)
-      : m_tcp_rx_queue(std::make_shared<PtrQueue<std::vector<uint8_t>>>(10)),
-        m_rx_callback(rx_callback),
+      const std::function<void(std::unique_ptr<std::vector<uint8_t>>&&)> &rx_callback)
+      : m_tcp_rx_queue(std::make_shared<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>(MAX_NETWORK_QUEUE_SIZE)),
+        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>>>
-      m_tcp_rx_queue; // todo: this should probably be thread safe
-  std::function<void(char *, int)> m_rx_callback;
+  std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_tcp_rx_queue;
+  std::function<void(std::unique_ptr<std::vector<std::uint8_t>>)> m_rx_callback;
 
 private:
   TaskHandle_t m_router_thread = nullptr;
diff --git a/components/rpc/include/IRPCServer.h b/components/rpc/include/IRPCServer.h
index 5efe3b0..c7081a4 100644
--- a/components/rpc/include/IRPCServer.h
+++ b/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
diff --git a/components/rpc/include/MessagingInterface.h b/components/rpc/include/MessagingInterface.h
index 7fdbab2..862e605 100644
--- a/components/rpc/include/MessagingInterface.h
+++ b/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_router(std::make_unique<CommunicationRouter>([this](const char* buffer, const int size) { handleRecv(buffer, 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](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;
diff --git a/components/rpc/include/wireless/TCPServer.h b/components/rpc/include/wireless/TCPServer.h
index cd63aee..b5bc943 100644
--- a/components/rpc/include/wireless/TCPServer.h
+++ b/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:
-    TCPServer(int port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue);
+  public:
+    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;
diff --git a/components/rpc/include/wireless/UDPServer.h b/components/rpc/include/wireless/UDPServer.h
index 9c76c4f..8c4a6d2 100644
--- a/components/rpc/include/wireless/UDPServer.h
+++ b/components/rpc/include/wireless/UDPServer.h
@@ -6,36 +6,37 @@
 #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:
-    UDPServer(int rx_port, int tx_port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue);
+  public:
+    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();
     [[noreturn]] static void socket_monitor_thread(void *args);
 
-	int m_tx_port;
+    int m_tx_port;
     int m_rx_port;
 
     int m_tx_server_sock;
-	int m_rx_server_sock;
+    int m_rx_server_sock;
 
     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
diff --git a/components/rpc/wireless/TCPServer.cpp b/components/rpc/wireless/TCPServer.cpp
index 07cbd64..c8ca99e 100644
--- a/components/rpc/wireless/TCPServer.cpp
+++ b/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);
diff --git a/components/rpc/wireless/UDPServer.cpp b/components/rpc/wireless/UDPServer.cpp
index 06ab69b..d82e704 100644
--- a/components/rpc/wireless/UDPServer.cpp
+++ b/components/rpc/wireless/UDPServer.cpp
@@ -1,3 +1,4 @@
+#include <chrono>
 #include <cstring>
 #include <memory>
 
@@ -17,205 +18,207 @@
 #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(
-    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(const int rx_port, const int tx_port,
+                     const std::shared_ptr<BlockingQueue<std::unique_ptr<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(); }
+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;
-  }
+    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);
+    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);
-    close(this->m_rx_server_sock);
-    close(this->m_tx_server_sock);
-    this->m_rx_task = nullptr;
-    this->m_rx_server_sock = -1;
-    this->m_tx_server_sock = -1;
-  }
+    ESP_LOGI(TAG, "Shutting down UDP server");
+    if (nullptr != this->m_rx_task) {
+        vTaskDelete(this->m_rx_task);
+        close(this->m_rx_server_sock);
+        close(this->m_tx_server_sock);
+        this->m_rx_task = nullptr;
+        this->m_rx_server_sock = -1;
+        this->m_tx_server_sock = -1;
+    }
 }
 
 [[noreturn]] void UDPServer::socket_monitor_thread(void *args) {
-  const auto that = static_cast<UDPServer *>(args);
+    const auto that = static_cast<UDPServer *>(args);
 
-  while (true) {
-    ESP_LOGI(TAG, "Attempting to start UDP Server on %d", that->m_rx_port);
+    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);
-
-      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;
+        if (!is_network_connected()) {
+            ESP_LOGW(TAG, "Network is disconnected");
+            vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
+            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);
-  }
+        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);
+
+            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), std::chrono::milliseconds(MAX_RX_QUEUE_ENQUEUE_TIMEOUT_MS));
+            }
+        }
+
+        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");
+    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 (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) {
+        return true;
+    }
 
-  if (0 != ip_info.ip.addr) {
-    return true;
-  }
+    if (0 != ip_info.ip.addr) {
+        return true;
+    }
 
-  netif = esp_netif_get_handle_from_ifkey("WIFI_AP_DEF");
+    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 (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) {
+        return true;
+    }
 
-  if (0 != ip_info.ip.addr) {
-    return true;
-  }
+    if (0 != ip_info.ip.addr) {
+        return true;
+    }
 
-  return false;
+    return false;
 }
 
 bool UDPServer::authenticate_client(int sock) {
-  // todo: authentication (key?)
-  return 0;
+    // 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;
-  }
+int UDPServer::send_msg(uint8_t *buffer, size_t len) 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)},
-  };
+    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;
+    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;
+    iovec iov[2];
+    iov[0].iov_base = &size;
+    iov[0].iov_len = 4;
+    iov[1].iov_base = buffer;
+    iov[1].iov_len = size;
 
-  msghdr msg = {};
-  msg.msg_iov = iov;
-  msg.msg_iovlen = 2;
-  msg.msg_name = &mcast_dest;
-  msg.msg_namelen = sizeof(mcast_dest);
+    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);
+    sendmsg(this->m_tx_server_sock, &msg, 0);
 
-  return 0;
+    return 0;
 }
diff --git a/main/LoopManager.cpp b/main/LoopManager.cpp
index 2771dc2..1449ed5 100644
--- a/main/LoopManager.cpp
+++ b/main/LoopManager.cpp
@@ -71,7 +71,9 @@
 void LoopManager::send_sensor_reading(bool durable) const {
     Flatbuffers::SensorMessageBuilder smb{};
     // todo: get data from sensor
-    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);
+    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);
+    }
 }
diff --git a/main/control/Servo1Actuator.cpp b/main/control/Servo1Actuator.cpp
index 02ca7f7..9024c73 100644
--- a/main/control/Servo1Actuator.cpp
+++ b/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));