Link Layer Frame Send Scheduler, Internal RX Polling, Framework for Generic Frames + Fragmentation

2026-07-08 17:47:21 +02:00 · 2025-12-14 02:51:33 -05:00
parent 0e29470be1
commit 92ddc3faf9
24 changed files with 3735 additions and 818 deletions
--- a/components/dataLink/DataLinkScheduler.cpp
+++ b/components/dataLink/DataLinkScheduler.cpp
@@ -0,0 +1,223 @@
+#include "DataLinkManager.h"
+#include "esp_log.h"
+#include "esp_timer.h"
+#include "freertos/semphr.h"
+
+void DataLinkManager::init_scheduler(){
+    for (int i = 0; i < num_channels; i++){
+        sq_handle[i] = xSemaphoreCreateMutex();
+        async_rx_queue_mutex[i] = xSemaphoreCreateMutex();
+    }
+
+    ESP_LOGI(DEBUG_LINK_TAG, "Starting Frame Scheduler task");
+    xTaskCreate(DataLinkManager::frame_scheduler, "Scheduler", 4096, static_cast<void*>(this), 5, &scheduler_task);
+    xTaskCreate(DataLinkManager::receive_thread_main, "Scheduler", 8192, static_cast<void*>(this), 5, &receive_task);
+}
+
+[[noreturn]] void DataLinkManager::frame_scheduler(void* args){
+    DataLinkManager* link_layer_obj = static_cast<DataLinkManager*>(args);
+    if (link_layer_obj == nullptr){
+        ESP_LOGE(DEBUG_LINK_TAG, "Frame Scheduler failed to start due to invalid pointer");
+        vTaskDelete(nullptr);
+    }
+
+    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));
+        
+    }
+    vTaskDelete(nullptr);
+
+}
+
+/**
+ * @brief Pushes a frame to the scheduler
+ * 
+ * @param frame 
+ * @param channel 
+ * @return esp_err_t 
+ */
+esp_err_t DataLinkManager::push_frame_to_scheduler(SchedulerMetadata frame, uint8_t channel){
+    if (frame.data == nullptr){
+        ESP_LOGE(DEBUG_LINK_TAG, "Frame data is null");
+        return ESP_ERR_INVALID_ARG;
+    }
+
+    if (channel >= num_channels){
+        ESP_LOGE(DEBUG_LINK_TAG, "Invalid channels");
+        return ESP_ERR_INVALID_ARG;
+    }
+
+    if (frame.len == 0){
+        ESP_LOGE(DEBUG_LINK_TAG, "Invalid Frame Length");
+
+        return ESP_ERR_INVALID_ARG;
+    }
+
+    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;
+    }
+
+    // ESP_LOGI(DEBUG_LINK_TAG, "Pushed frame to queue on channel %d", channel);
+
+    return ESP_OK;
+}
+
+esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
+    if (phys_comms == nullptr){
+        ESP_LOGE(DEBUG_LINK_TAG, "Failed to send frame due to no RMT object");
+        return ESP_ERR_INVALID_ARG;
+    }
+
+    if (sq_handle[channel] == nullptr){
+        return ESP_FAIL;
+    }
+    
+    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]);
+    } else {
+        ESP_LOGE(DEBUG_LINK_TAG, "Failed to get mutex when trying to send");
+        //Failed to obtain mutex
+        return ESP_ERR_TIMEOUT;
+    }
+
+    if (frame.data == nullptr){
+        ESP_LOGE(DEBUG_LINK_TAG, "Data array does not exist");
+        return ESP_ERR_INVALID_ARG;
+    }
+
+    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;
+    }
+
+    esp_err_t res;
+    bool isControlFrame = IS_CONTROL_FRAME(static_cast<uint8_t>(frame.header.type_flag));
+
+    size_t frame_size = isControlFrame ? sizeof(ControlFrame) : sizeof(GenericFrame);
+    uint8_t send_data[frame_size];
+
+    if (isControlFrame){
+        //control frame
+
+        res = create_control_frame(frame.data, frame.len, 
+            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;
+        }
+
+        // ESP_LOGI(DEBUG_LINK_TAG, "Sending %d bytes", frame_size);
+    } else {
+        //generic frame
+        
+        if (frame.len > MAX_CONTROL_DATA_LEN){
+            //fragment here
+            uint16_t curr_offset = frame.generic_frame_data_offset;
+            uint16_t fragment_size = 0;
+            
+            //calculate fragment data size
+            if (curr_offset + MAX_CONTROL_DATA_LEN <= frame.len){
+                fragment_size = curr_offset + MAX_CONTROL_DATA_LEN;
+            } else {
+                fragment_size = frame.len - curr_offset;
+            }
+            
+            //create fragment
+            res = create_generic_frame(frame.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); //free entire data for now - will need to implement retries/sliding window soon
+                return res; 
+            }
+
+            //need to schedule the next fragment
+            if (curr_offset != MAX_CONTROL_DATA_LEN){
+                frame.generic_frame_data_offset += fragment_size;
+                frame.header.frag_info += 1; //increment frag_num
+                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); //free entire data for now - will need to implement retries/sliding window soon
+                    return res; 
+                }
+                
+            } else {
+                //Done fragmenting, can free data array
+                vPortFree(frame.data);
+            }
+
+        } else {
+            res = create_generic_frame(frame.data, frame.len, 
+                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");
+            return res;
+        }
+    }
+
+    uint8_t channel_to_route = MAX_CHANNELS;
+    rmt_transmit_config_t config = {
+        .loop_count = 0,
+        .flags = {
+            .eot_level = 0   // typically 0 or 1, depending on your output idle level
+        }
+    };
+    if (frame.header.receiver_id == BROADCAST_ADDR){
+        // printf("Sending on channel %d\n", i);
+        phys_comms->send(send_data, frame_size, &config, channel);
+
+    } else {
+        res = route_frame(frame.header.receiver_id, &channel_to_route);
+
+        if (res != ESP_OK){
+            ESP_LOGE(DEBUG_LINK_TAG, "Failed to find entry for %d", frame.header.receiver_id);
+            return ESP_FAIL;
+        }
+        // ESP_LOGI(DEBUG_LINK_TAG, "Sending message to %d", frame.header.receiver_id);
+        phys_comms->send(send_data, frame_size, &config, channel_to_route);
+    }
+
+    if (res != ESP_OK){
+        ESP_LOGE(DEBUG_LINK_TAG, "Failed to send message");
+        return ESP_FAIL;
+    } else{
+        // printf("Sent message to board %d\n", frame.header.receiver_id);
+    }
+
+    return ESP_OK;
+}