Reduce DataLink latency

2026-07-08 09:37:21 +02:00 · 2026-01-27 22:16:24 -05:00
parent b9c6a99260
commit 2e9004ee34
25 changed files with 801 additions and 757 deletions
--- 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);