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
parent 663a7ef262
commit fc25c026b2
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);
}
}