botchain/firmware
1
0
Fork 0
mirror of https://github.com/BotChain-Robots/firmware.git synced 2026-07-08 09:37: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 b9c6a99260
commit 2e9004ee34
25 changed files with 801 additions and 757 deletions
Download Patch File Download Diff File Expand all files Collapse all files

217
components/dataLink/DataLinkFrames.cpp
View File

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