botchain/firmware
1
0
Fork 0
mirror of https://github.com/BotChain-Robots/firmware.git synced 2026-07-08 17:47:21 +02:00
Code Issues Packages Projects Releases Wiki Activity

Reduce DataLink latency

Browse Source
This commit is contained in:
Johnathon Slightham
2026-01-27 22:16:24 -05:00
committed by Johnathon Slightham
parent f54063aa8c
commit c14c82ce9c
25 changed files with 801 additions and 757 deletions
Download Patch File Download Diff File Expand all files Collapse all files

85
components/dataLink/DataLinkScheduler.cpp
View File

@@ -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");