mirror of
https://github.com/BotChain-Robots/firmware.git
synced 2026-07-08 17:47:21 +02:00
566 lines
20 KiB
C++
566 lines
20 KiB
C++
#include "DataLinkManager.h"
|
||
#include "esp_log.h"
|
||
#include <cstring>
|
||
#include <type_traits>
|
||
|
||
/**
|
||
* @brief Creates a Control Frame from `FrameHeader`
|
||
*
|
||
* @param header
|
||
* @return ControlFrame
|
||
*/
|
||
ControlFrame make_control_frame_from_header(const FrameHeader& header) {
|
||
ControlFrame frame{};
|
||
frame.preamble = header.preamble;
|
||
frame.sender_id = header.sender_id;
|
||
frame.receiver_id = header.receiver_id;
|
||
frame.seq_num = header.seq_num;
|
||
frame.type_flag = header.type_flag;
|
||
frame.data_len = header.data_len;
|
||
frame.crc_16 = header.crc_16;
|
||
return frame;
|
||
}
|
||
|
||
/**
|
||
* @brief Creates a Generic Frame from `FrameHeader`
|
||
*
|
||
* @param header
|
||
* @return GenericFrame
|
||
*/
|
||
GenericFrame make_generic_frame_from_header(const FrameHeader& header) {
|
||
GenericFrame frame{};
|
||
frame.preamble = header.preamble;
|
||
frame.sender_id = header.sender_id;
|
||
frame.receiver_id = header.receiver_id;
|
||
frame.seq_num = header.seq_num;
|
||
frame.type_flag = header.type_flag;
|
||
frame.total_frag = (header.frag_info >> 16) & 0xFFFF;
|
||
frame.frag_num = (header.frag_info) & 0xFFFF;
|
||
frame.data_len = header.data_len;
|
||
frame.crc_16 = header.crc_16;
|
||
return frame;
|
||
}
|
||
|
||
/**
|
||
* @brief Store a fragment that has been received
|
||
*
|
||
* @param fragment
|
||
* @param channel
|
||
* @return esp_err_t
|
||
*/
|
||
esp_err_t DataLinkManager::store_fragment(GenericFrame* fragment, uint8_t channel){
|
||
if (fragment == nullptr){
|
||
return ESP_ERR_INVALID_ARG;
|
||
}
|
||
|
||
if (fragment->data_len == 0){
|
||
return ESP_ERR_INVALID_ARG;
|
||
}
|
||
|
||
if (fragment->receiver_id != this_board_id){
|
||
return ESP_ERR_INVALID_ARG;
|
||
}
|
||
|
||
// ESP_LOGI(DEBUG_LINK_TAG, "got frame %d, fragment %d of %d", fragment->seq_num, fragment->frag_num, fragment->total_frag);
|
||
|
||
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;
|
||
}
|
||
|
||
if (fragment_map[channel][fragment->receiver_id].find(fragment->seq_num) == fragment_map[channel][fragment->receiver_id].end()){
|
||
FragmentMetadata& metadata = fragment_map[channel][fragment->receiver_id][fragment->seq_num];
|
||
metadata.num_fragments_rx = 0;
|
||
|
||
metadata.fragments.reserve(fragment->total_frag);
|
||
for (uint16_t i = 0; i < fragment->total_frag; i++){
|
||
metadata.fragments.push_back(GenericFrame{});
|
||
}
|
||
}
|
||
|
||
FragmentMetadata& metadata = fragment_map[channel][fragment->receiver_id][fragment->seq_num];
|
||
if ((fragment->frag_num == 0) || (fragment->frag_num - 1) >= metadata.fragments.size()){
|
||
xSemaphoreGive(rx_fragment_mutex[channel]);
|
||
return ESP_ERR_INVALID_STATE;
|
||
}
|
||
|
||
if (metadata.fragments[fragment->frag_num-1].data_len == 0){
|
||
metadata.fragments[fragment->frag_num-1] = *fragment;
|
||
metadata.num_fragments_rx++;
|
||
// ESP_LOGI(DEBUG_LINK_TAG, "store frame %d fragment %d success; got %d out of %d", fragment->seq_num, fragment->frag_num, metadata.num_fragments_rx, metadata.fragments.size());
|
||
}
|
||
|
||
uint16_t last_consec_rx_frag = 0;
|
||
if (static_cast<FrameType>(GET_TYPE(fragment->type_flag)) != FrameType::MISC_UDP_GENERIC_TYPE){
|
||
for (; last_consec_rx_frag < fragment->total_frag; last_consec_rx_frag++){
|
||
if (metadata.fragments[last_consec_rx_frag].data_len == 0){
|
||
//found missing fragment
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
size_t metadata_fragment_size = metadata.fragments.size();
|
||
xSemaphoreGive(rx_fragment_mutex[channel]);
|
||
|
||
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),
|
||
static_cast<uint8_t>((fragment->seq_num & 0xFF00) >> 8), static_cast<uint8_t>(fragment->seq_num & 0xFF)},
|
||
.sender_id = fragment->sender_id,
|
||
};
|
||
if (xSemaphoreTake(send_ack_queue_mutex[channel], pdMS_TO_TICKS(SEND_ACK_MUTEX_WAIT)) != pdTRUE){
|
||
return ESP_FAIL;
|
||
}
|
||
|
||
send_ack_queue[channel].push(data);
|
||
xSemaphoreGive(send_ack_queue_mutex[channel]);
|
||
}
|
||
|
||
if (metadata.num_fragments_rx == metadata_fragment_size){
|
||
//all fragments received
|
||
return complete_fragment(fragment->receiver_id, fragment->seq_num, channel);
|
||
}
|
||
|
||
return ESP_OK;
|
||
}
|
||
|
||
/**
|
||
* @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
|
||
*/
|
||
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;
|
||
}
|
||
|
||
if (fragment_map[channel][board_id].find(sequence_num) == fragment_map[channel][board_id].end()){
|
||
return ESP_ERR_NOT_FOUND;
|
||
}
|
||
|
||
FragmentMetadata& metadata = fragment_map[channel][board_id][sequence_num];
|
||
if (metadata.num_fragments_rx != metadata.fragments.size()){
|
||
return ESP_ERR_INVALID_STATE;
|
||
}
|
||
// Compute the exact total size from actual fragment data lengths, not the
|
||
// theoretical maximum (num_fragments * MAX_GENERIC_DATA_LEN). The last fragment
|
||
// is almost always shorter, so over-allocating would leave garbage bytes at the
|
||
// end that consumers reading combined_data->size() would see.
|
||
uint16_t total_data_len = 0;
|
||
for (size_t i = 0; i < metadata.num_fragments_rx; i++){
|
||
total_data_len += metadata.fragments[i].data_len;
|
||
}
|
||
xSemaphoreGive(rx_fragment_mutex[channel]);
|
||
|
||
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;
|
||
}
|
||
|
||
if (rx_fragment_mutex[channel] == NULL){
|
||
return ESP_FAIL;
|
||
}
|
||
|
||
// ESP_LOGI(DEBUG_LINK_TAG, "completing %d fragments for frame %d", metadata.num_fragments_rx, sequence_num);
|
||
|
||
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;
|
||
}
|
||
|
||
uint16_t prev_index = 0;
|
||
for (size_t i = 0; i < metadata.num_fragments_rx; i++){
|
||
memcpy(&combined_data->data()[prev_index], metadata.fragments[i].data, metadata.fragments[i].data_len);
|
||
prev_index += metadata.fragments[i].data_len;
|
||
}
|
||
combined_data->resize(prev_index); // ensure vector size == actual data (no garbage tail bytes)
|
||
|
||
xSemaphoreGive(rx_fragment_mutex[channel]);
|
||
|
||
rx.data = std::move(combined_data);
|
||
rx.data_len = prev_index;
|
||
|
||
if (async_rx_queue_mutex[channel] == nullptr){
|
||
return ESP_FAIL;
|
||
}
|
||
|
||
GenericFrame frame = metadata.fragments[0];
|
||
|
||
rx.header = {
|
||
.preamble = START_OF_FRAME,
|
||
.sender_id = frame.sender_id,
|
||
.receiver_id = frame.receiver_id,
|
||
.seq_num = frame.seq_num,
|
||
.type_flag = frame.type_flag,
|
||
.frag_info = (uint32_t)((frame.total_frag << 16) | frame.frag_num),
|
||
.data_len = prev_index,
|
||
.crc_16 = 0
|
||
};
|
||
|
||
// ESP_LOGI(DEBUG_LINK_TAG, "pushing frame %d onto async rx queue", sequence_num);
|
||
|
||
if (!async_receive_queue->enqueue(std::move(rx), std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS))) {
|
||
return ESP_ERR_TIMEOUT;
|
||
}
|
||
|
||
fragment_map[channel][board_id].erase(sequence_num);
|
||
|
||
if (fragment_map[channel][board_id].empty()) {
|
||
fragment_map[channel].erase(board_id);
|
||
}
|
||
|
||
// ESP_LOGI(DEBUG_LINK_TAG, "frame %d pushed success", sequence_num);
|
||
|
||
return ESP_OK;
|
||
}
|
||
|
||
/**
|
||
* @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
|
||
*
|
||
* @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){
|
||
// 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
|
||
*
|
||
*/
|
||
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);
|
||
|
||
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);
|
||
}
|
||
|
||
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];
|
||
|
||
size_t recv_len = 0;
|
||
|
||
esp_err_t res = phys_comms->receive(data, data_len, &recv_len, channel);
|
||
|
||
if (res != ESP_OK){
|
||
// 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;
|
||
}
|
||
|
||
if (recv_len < CONTROL_FRAME_OVERHEAD) {
|
||
//Frame is too small
|
||
return ESP_ERR_INVALID_RESPONSE;
|
||
}
|
||
|
||
// ---- LUT fast-path for control frames ----
|
||
// Raw wire layout for control frames:
|
||
// [0] preamble
|
||
// [1] sender_id
|
||
// [2] receiver_id
|
||
// [3..4] seq_num (LE)
|
||
// [5] type_flag
|
||
// [6..7] wire_data_len (LE) -- includes CONTROL_FRAME_HASH_SIZE
|
||
// [8..11] 4-byte FNV-1a hash (LE)
|
||
// [12..] actual payload
|
||
// [-2..-1] CRC-16
|
||
bool is_control = IS_CONTROL_FRAME(data[5]);
|
||
if (is_control && recv_len >= (size_t)(CONTROL_FRAME_OVERHEAD + CONTROL_FRAME_HASH_SIZE)){
|
||
uint32_t peeked_hash = ((uint32_t)data[8] ) |
|
||
((uint32_t)data[9] << 8) |
|
||
((uint32_t)data[10] << 16) |
|
||
((uint32_t)data[11] << 24);
|
||
|
||
std::vector<uint8_t> cached_message;
|
||
FrameHeader cached_header;
|
||
|
||
if (lut_lookup(peeked_hash, cached_message, cached_header)){
|
||
// Cache hit: replay the previously validated payload directly onto the receive queue.
|
||
// Skip CRC checking, hash verification, data-length checks, and full parsing.
|
||
|
||
auto message = std::make_unique<std::vector<uint8_t>>(cached_message);
|
||
|
||
// Update per-frame dynamic fields from the raw bytes (sender/receiver/seq differ per transmission)
|
||
cached_header.sender_id = data[1];
|
||
cached_header.receiver_id = data[2];
|
||
cached_header.seq_num = (uint16_t)data[3] | ((uint16_t)data[4] << 8);
|
||
|
||
// RIP control frames are handled internally – replay them too
|
||
if (static_cast<FrameType>(GET_TYPE(cached_header.type_flag)) == FrameType::RIP_TABLE_CONTROL){
|
||
// Re-run the RIP update using the cached message
|
||
for (size_t i = 0; i < cached_message.size() - 1; i += 2){
|
||
uint8_t board_id = cached_message[i];
|
||
uint8_t hops = cached_message[i + 1];
|
||
RIPRow* entry = nullptr;
|
||
if (rip_find_entry(board_id, &entry, true) != ESP_OK || entry == nullptr){
|
||
continue;
|
||
}
|
||
if (entry->valid == RIP_NEW_ROW){
|
||
rip_add_entry(board_id, hops + 1, channel, &entry);
|
||
} else {
|
||
rip_update_entry(hops + 1, channel, &entry);
|
||
}
|
||
}
|
||
return ESP_OK;
|
||
}
|
||
|
||
// Data is already validated – enqueue directly, no further checks needed.
|
||
Rx_Metadata metadata = {
|
||
.data = std::move(message),
|
||
.data_len = (uint16_t)cached_message.size(),
|
||
.header = cached_header
|
||
};
|
||
|
||
if (!async_receive_queue->enqueue(std::move(metadata), std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS))){
|
||
return ESP_ERR_TIMEOUT;
|
||
}
|
||
return ESP_OK;
|
||
}
|
||
// Cache miss – fall through to full validation below
|
||
// ESP_LOGI("TMP", "Control frame LUT cache MISS - hash=0x%08lX", peeked_hash);
|
||
}
|
||
// ---- end LUT fast-path ----
|
||
|
||
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->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);
|
||
|
||
//push control frame onto async_receive_queue
|
||
if (static_cast<FrameType>(GET_TYPE(header.type_flag)) == FrameType::ACK_TYPE){
|
||
if (message_size != GENERIC_FRAG_ACK_DATA_SIZE || message_size == 0){
|
||
return ESP_OK;
|
||
}
|
||
|
||
if (message->data()[0] != GENERIC_FRAG_ACK_PREAMBLE){
|
||
return ESP_OK;
|
||
}
|
||
|
||
FrameAckRecord record = {
|
||
.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);
|
||
|
||
return ESP_OK;
|
||
}
|
||
|
||
if (!IS_CONTROL_FRAME(header.type_flag)){
|
||
//Handle generic frame fragment
|
||
GenericFrame frame = make_generic_frame_from_header(header);
|
||
if (message_size > MAX_FRAME_SIZE){
|
||
return ESP_FAIL;
|
||
}
|
||
|
||
memcpy(frame.data, message->data(), message_size);
|
||
esp_err_t res = store_fragment(&frame, channel);
|
||
return res;
|
||
}
|
||
|
||
// Control frame fully validated – store in LUT for future replays
|
||
// Extract the hash that was embedded at data[8..11] (already validated in get_data_from_frame)
|
||
{
|
||
uint32_t validated_hash = ((uint32_t)data[8] ) |
|
||
((uint32_t)data[9] << 8) |
|
||
((uint32_t)data[10] << 16) |
|
||
((uint32_t)data[11] << 24);
|
||
lut_insert(validated_hash, message->data(), message_size, header);
|
||
}
|
||
|
||
//control frame handling: - TODO: clean up :)
|
||
// 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->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);
|
||
} else {
|
||
//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");
|
||
RIPRow_public row_queue = {
|
||
.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);
|
||
if (res != ESP_OK){
|
||
ESP_LOGE(DEBUG_LINK_TAG, "Failed to send back rip table to board %d", header.sender_id);
|
||
return res;
|
||
}
|
||
}
|
||
|
||
return res;
|
||
}
|
||
|
||
uint16_t seq_num = 0;
|
||
|
||
res = get_sequence_num(header.receiver_id, &seq_num);
|
||
|
||
if (res != ESP_OK){
|
||
return res;
|
||
}
|
||
|
||
//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, std::move(message), FrameType::MISC_CONTROL_TYPE, 0);
|
||
return res;
|
||
}
|
||
|
||
Rx_Metadata metadata = {
|
||
.data = std::move(message),
|
||
.data_len = (uint16_t)message_size,
|
||
.header = header
|
||
};
|
||
|
||
if (!async_receive_queue->enqueue(std::move(metadata), std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS))){
|
||
return ESP_ERR_TIMEOUT;
|
||
}
|
||
return ESP_OK;
|
||
}
|
||
|
||
[[noreturn]] void DataLinkManager::receive_thread_main(void* args){
|
||
DataLinkManager* link_layer_obj = static_cast<DataLinkManager*>(args);
|
||
if (link_layer_obj == nullptr || link_layer_obj->manual_broadcasts == nullptr){
|
||
ESP_LOGE(DEBUG_LINK_TAG, "Receive thread failed to start due to invalid pointer");
|
||
vTaskDelete(nullptr);
|
||
}
|
||
|
||
ESP_LOGI(DEBUG_LINK_TAG, "Starting Receive thread task");
|
||
|
||
esp_err_t res;
|
||
|
||
for (uint8_t i = 0; i < link_layer_obj->num_channels; i++){
|
||
res = link_layer_obj->start_receive_frames_rmt(i);
|
||
}
|
||
while(!link_layer_obj->stop_tasks){
|
||
for (uint8_t i = 0; i < link_layer_obj->num_channels; i++){
|
||
res = link_layer_obj->receive_rmt(i);
|
||
res = link_layer_obj->start_receive_frames_rmt(i);
|
||
}
|
||
|
||
vTaskDelay(pdMS_TO_TICKS(RECEIVE_TASK_PERIOD_MS));
|
||
}
|
||
|
||
vTaskDelete(nullptr);
|
||
}
|
||
|
||
[[noreturn]] void DataLinkManager::send_ack_thread_main(void* args){
|
||
DataLinkManager* link_layer_obj = static_cast<DataLinkManager*>(args);
|
||
if (link_layer_obj == nullptr || link_layer_obj->manual_broadcasts == nullptr){
|
||
ESP_LOGE(DEBUG_LINK_TAG, "Send Ack thread failed to start due to invalid pointer");
|
||
vTaskDelete(nullptr);
|
||
}
|
||
|
||
for (uint8_t i = 0; i < link_layer_obj->num_channels; i++){
|
||
if (link_layer_obj->send_ack_queue_mutex[i] == NULL){
|
||
ESP_LOGE(DEBUG_LINK_TAG, "%d send ack queue mutex is null!", i);
|
||
vTaskDelete(nullptr);
|
||
}
|
||
}
|
||
|
||
ESP_LOGI(DEBUG_LINK_TAG, "Starting Send ACK task");
|
||
|
||
esp_err_t res;
|
||
|
||
while(!link_layer_obj->stop_tasks){
|
||
for (uint8_t channel = 0; channel < link_layer_obj->num_channels; channel++){
|
||
vTaskDelay(pdMS_TO_TICKS(SEND_ACK_PERIOD_MS));
|
||
|
||
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);
|
||
xSemaphoreGive(link_layer_obj->send_ack_queue_mutex[channel]);
|
||
}
|
||
}
|
||
|
||
vTaskDelete(nullptr);
|
||
}
|