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

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

Browse Source
This commit is contained in:
superkor
2025-12-14 02:51:33 -05:00
parent 17606bc98d
commit fe2f396817
24 changed files with 3735 additions and 818 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
components/dataLink/CMakeLists.txt
View File

@@ -1,3 +1,4 @@
idf_component_register(SRCS "DataLinkManager.cpp"
idf_component_register(SRCS "DataLinkManager.cpp" "DataLinkRIP.cpp" "DataLinkScheduler.cpp" "DataLinkFrames.cpp"
PRIV_REQUIRES driver esp_event nvs_flash esp_netif rmt
REQUIRES esp_timer
INCLUDE_DIRS "include")

377
components/dataLink/DataLinkFrames.cpp Normal file
View File

@@ -0,0 +1,377 @@
#include "DataLinkManager.h"
#include "esp_log.h"
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;
}
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;
}
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;
}
if (fragment_map[fragment->receiver_id].find(fragment->seq_num) == fragment_map[fragment->receiver_id].end()){
FragmentMetadata& metadata = fragment_map[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[fragment->receiver_id][fragment->seq_num];
if (fragment->frag_num >= metadata.fragments.size()){
return ESP_ERR_INVALID_STATE;
}
if (metadata.fragments[fragment->frag_num].data_len ==0){
metadata.fragments[fragment->frag_num] = *fragment;
metadata.num_fragments_rx++;
}
if (metadata.num_fragments_rx == metadata.fragments.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){
if (fragment_map[board_id].find(sequence_num) == fragment_map[board_id].end()){
return ESP_ERR_NOT_FOUND;
}
FragmentMetadata& metadata = fragment_map[board_id][sequence_num];
if (metadata.num_fragments_rx != metadata.fragments.size()){
return ESP_ERR_INVALID_STATE;
}
Rx_Metadata rx;
uint16_t total_data_len = metadata.num_fragments_rx*MAX_CONTROL_DATA_LEN; //max data size with n fragments
uint8_t* combined_data = (uint8_t*)pvPortMalloc(total_data_len);
rx.data_len = total_data_len;
if (combined_data == nullptr){
return ESP_ERR_NO_MEM;
}
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);
prev_index += metadata.fragments[i].data_len;
}
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
};
if (xSemaphoreTake(async_rx_queue_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){
vPortFree(combined_data);
return ESP_ERR_TIMEOUT;
}
async_receive_queue[channel].push(rx);
xSemaphoreGive(async_rx_queue_mutex[channel]);
fragment_map[board_id].erase(sequence_num);
if (fragment_map[board_id].empty()) {
fragment_map.erase(board_id);
}
return ESP_OK;
}
/**
* @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;
}
/**
* @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;
}
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 ESP_OK;
}
esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
uint16_t data_len = MAX_CONTROL_DATA_LEN+GENERIC_FRAME_OVERHEAD; //max possible data len
uint8_t data[data_len];
memset(data, 0, 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_CONTROL_DATA_LEN + GENERIC_FRAME_OVERHEAD){
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;
}
uint8_t message[MAX_CONTROL_DATA_LEN];
memset(message, 0, sizeof(message));
size_t message_size = 0;
FrameHeader header;
res = get_data_from_frame(data, recv_len, message, &message_size, &header);
if (res != ESP_OK){
// print_buffer_binary(message, message_size);
return res;
}
// print_buffer_binary(message, message_size);
if (!IS_CONTROL_FRAME(header.type_flag)){
//Handle generic frame fragment
GenericFrame frame = make_generic_frame_from_header(header);
if (message_size > MAX_CONTROL_DATA_LEN){
return ESP_FAIL;
}
memcpy(frame.data, message, 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){
// printf("Got a RIP frame\n");
for (size_t i = 0; i < message_size-1; i+=2){
uint8_t board_id = message[i];
uint8_t hops = message[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;
}
}
}
//got frame but not destined for this board
if (header.receiver_id != this_board_id && header.receiver_id != BROADCAST_ADDR && header.seq_num > sequence_num_map[header.receiver_id]){
// 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::DEBUG_CONTROL_TYPE, 0);
return res;
}
//push control frame onto async_receive_queue
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_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 {
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(SCHEDULER_PERIOD_MS));
}
vTaskDelete(nullptr);
}

1046
components/dataLink/DataLinkManager.cpp
View File

File diff suppressed because it is too large Load Diff

456
components/dataLink/DataLinkRIP.cpp Normal file
View File

@@ -0,0 +1,456 @@
#include "DataLinkManager.h"
#include "esp_log.h"
#include "freertos/semphr.h"
/**
* @brief Initializes the RIP table
*
*/
void DataLinkManager::init_rip(){
for (size_t i = 0; i < RIP_MAX_ROUTES; i++){
rip_table[i] = {
.info = {
.board_id = BROADCAST_ADDR, //invalid addr
.hops = RIP_MAX_HOPS + 1, //infinite
},
.channel = MAX_CHANNELS + 1, //invalid channels
.ttl = 0,
.valid = RIP_INVALID_ROW,
.ttl_flush = 0,
.row_sem = NULL
};
rip_table[i].row_sem = xSemaphoreCreateMutexStatic(&rip_table[i].mutex_buf);
}
//add the self route to the table
rip_table[0].info = {
.board_id = this_board_id,
.hops = 0,
};
rip_table[0].channel = MAX_CHANNELS + 1;
rip_table[0].ttl = RIP_TTL_START;
rip_table[0].valid = 1;
discovery_tables = xQueueCreate(RIP_MAX_ROUTES, sizeof(RIPRow_public));
start_rip_tasks();
}
esp_err_t DataLinkManager::rip_add_entry(uint8_t board_id, uint8_t hops, uint8_t channel, RIPRow** entry){
if (entry == nullptr){
return ESP_FAIL;
}
if (xSemaphoreTake((*entry)->row_sem, (TickType_t)RIP_MAX_SEM_WAIT) != pdTRUE){
return ESP_FAIL;
}
(*entry)->channel = channel;
(*entry)->info = {
.board_id = board_id,
.hops = hops
};
(*entry)->ttl = RIP_TTL_START;
(*entry)->valid = 1;
// ESP_LOGI(DEBUG_LINK_TAG, "board_id %d now has hops %d from channel %d", (*entry)->info.board_id, (*entry)->info.hops, channel);
xSemaphoreGive((*entry)->row_sem);
if (uxQueueMessagesWaiting(manual_broadcasts) == 0){
bool dummy = true;
xQueueSend(manual_broadcasts, &dummy, 0); //new row - send broadcast
}
return ESP_OK;
}
esp_err_t DataLinkManager::rip_reset_entry_ttl(uint8_t board_id){
RIPRow* entry = nullptr;
esp_err_t res;
res = rip_find_entry(board_id, &entry, false);
if (res != ESP_OK){
return ESP_FAIL;
}
if (entry == nullptr){
return ESP_FAIL; //board doesn't exist
}
if (xSemaphoreTake(entry->row_sem, (TickType_t)RIP_MAX_SEM_WAIT) != pdTRUE){
return ESP_FAIL;
}
entry->ttl = RIP_TTL_START;
xSemaphoreGive(entry->row_sem);
return ESP_OK;
}
esp_err_t DataLinkManager::rip_update_entry(uint8_t new_hop, uint8_t channel, RIPRow** entry){
if (entry == nullptr){
return ESP_FAIL; //board doesn't exist
}
if (xSemaphoreTake((*entry)->row_sem, (TickType_t)RIP_MAX_SEM_WAIT) != pdTRUE){
return ESP_FAIL;
}
uint8_t old_hops = (*entry)->info.hops;
if ((*entry)->info.hops >= new_hop && (*entry)->info.hops != RIP_MAX_HOPS + 1){ //no count to infinity if path is invalid
(*entry)->info.hops = new_hop;
(*entry)->channel = channel;
// ESP_LOGI(DEBUG_LINK_TAG, "updated board_id %d now has hops %d from channel %d", (*entry)->info.board_id, (*entry)->info.hops, channel);
}
(*entry)->ttl = RIP_TTL_START;
(*entry)->valid = 1;
// ESP_LOGI(DEBUG_LINK_TAG, "refreshed board_id %d ttl", (*entry)->info.board_id);
xSemaphoreGive((*entry)->row_sem);
if (uxQueueMessagesWaiting(manual_broadcasts) == 0 && old_hops > new_hop && old_hops != RIP_MAX_HOPS + 1){
//if hops were changed, send broadcast (if there isn't already one manual broadcast request pending)
bool dummy = true;
xQueueSend(manual_broadcasts, &dummy, 0);
}
return ESP_OK;
}
/**
* @brief Finds the board_id in the table if it exists and stores that row in `entry`
* TODO: use an unordered map instead of an array?
*
* @param board_id
* @param entry
* @return esp_err_t
*/
esp_err_t DataLinkManager::rip_find_entry(uint8_t board_id, RIPRow** entry, bool reserve_row = false){
RIPRow* free_slot = nullptr;
for (size_t i = 0; i < RIP_MAX_ROUTES; i++){
if (xSemaphoreTake(rip_table[i].row_sem, (TickType_t)RIP_MAX_SEM_WAIT) != pdTRUE){
return ESP_FAIL;
}
if (rip_table[i].valid == RIP_VALID_ROW && rip_table[i].info.board_id == board_id){
*entry = &rip_table[i];
xSemaphoreGive(rip_table[i].row_sem);
// ESP_LOGI(DEBUG_LINK_TAG, "Found %d in table at row %d", board_id, i);
return ESP_OK;
}
if (rip_table[i].valid == RIP_INVALID_ROW && free_slot == nullptr){
free_slot = &rip_table[i];
}
xSemaphoreGive(rip_table[i].row_sem);
}
// ESP_LOGI(DEBUG_LINK_TAG, "Finished looking for %d in table", board_id);
if (!reserve_row){
return ESP_OK;
}
if (free_slot != nullptr){
if (xSemaphoreTake(free_slot->row_sem, RIP_MAX_SEM_WAIT) != pdTRUE) {
return ESP_FAIL;
}
// IMPORTANT: Mark it as taken so others don't grab it
free_slot->valid = RIP_NEW_ROW; // Or some other init state
free_slot->info.board_id = board_id;
*entry = free_slot;
xSemaphoreGive(free_slot->row_sem);
// ESP_LOGI(DEBUG_LINK_TAG, "Reserved new entry for board %d", board_id);
}
return ESP_OK;
}
/**
* @brief Returns the associated RIP Table row by row number. Information returned is read only.
*
* @param entry
* @param row_num
* @return esp_err_t
*/
esp_err_t DataLinkManager::rip_get_row(RIPRow** entry, uint8_t row_num){
if (entry == nullptr){
return ESP_ERR_INVALID_ARG;
}
if (row_num >= RIP_MAX_ROUTES){
return ESP_ERR_INVALID_ARG;
}
if (xSemaphoreTake(rip_table[row_num].row_sem, (TickType_t)RIP_MAX_SEM_WAIT) != pdTRUE) return ESP_ERR_TIMEOUT;
if (rip_table[row_num].valid == RIP_INVALID_ROW){
xSemaphoreGive(rip_table[row_num].row_sem);
return ESP_ERR_INVALID_STATE;
}
if (rip_table[row_num].info.hops == RIP_MAX_HOPS + 1){
//invalid hop, decrement counter
rip_table[row_num].ttl_flush--;
if (rip_table[row_num].ttl_flush == 0){
rip_table[row_num].valid = RIP_INVALID_ROW;
xSemaphoreGive(rip_table[row_num].row_sem);
return ESP_FAIL;
}
}
*entry = &rip_table[row_num];
xSemaphoreGive(rip_table[row_num].row_sem);
return ESP_OK;
}
/**
* @brief Sends RIP frame
*
* @param broadcast True - broadcasts (sends rip table to all available channels); False - sends rip table via routing based on `dest_id`
* @param dest_id Destination board (requesting board) to send the rip table to (ignored if `broadcast is true`)
* @return esp_err_t
*/
esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
//use the control frame for the demo (as the number of rows increase, we will need to use the generic frame)
//data will be [board_id (1), hops (1), board_id (2), hops (2), ...]
uint8_t rip_message[RIP_MAX_ROUTES*2] = {};
uint16_t message_idx = 0;
esp_err_t res;
RIPRow* entry = nullptr;
if(broadcast){
for (size_t channel = 0; channel < num_channels; channel++){
for (size_t i = 0; i < RIP_MAX_ROUTES; i++){
res = rip_get_row(&entry, i);
if (res != ESP_OK){
continue;
}
if (entry == nullptr){
continue;
}
// ESP_LOGI(DEBUG_LINK_TAG, "Found entry for board %d with hops %d", entry->info.board_id, entry->info.hops);
if (entry->channel == channel){
//poisoned reverse
rip_message[message_idx++] = entry->info.board_id;
rip_message[message_idx++] = RIP_MAX_HOPS + 1;
} else {
rip_message[message_idx++] = entry->info.board_id;
rip_message[message_idx++] = entry->info.hops;
}
}
uint8_t* send_data = (uint8_t*)pvPortMalloc(message_idx);
if (send_data == nullptr){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to malloc when trying to send rip frame broadcast on channel %d", channel);
continue;
}
memset(send_data, 0, message_idx);
memcpy(send_data, rip_message, message_idx);
SchedulerMetadata metadata = {
.header = {
.preamble = START_OF_FRAME,
.sender_id = this_board_id,
.receiver_id = BROADCAST_ADDR,
.seq_num = sequence_num_map[BROADCAST_ADDR]++,
.type_flag = static_cast<uint8_t>(FrameType::RIP_TABLE_CONTROL),
.data_len = message_idx,
.crc_16 = 0,
},
.generic_frame_data_offset = 0,
.enqueue_time_ns = 0,
.data = send_data,
.len = message_idx,
};
res = push_frame_to_scheduler(metadata, channel);
if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule rip frame from send_rip_frame for channel %d", channel);
}
message_idx = 0;
memset(rip_message, 0, sizeof(rip_message));
}
} else {
for (size_t i = 0; i < RIP_MAX_ROUTES; i++){
res = rip_get_row(&entry, i);
if (res != ESP_OK){
continue;
}
if (entry == nullptr){
continue;
}
rip_message[message_idx++] = entry->info.board_id;
rip_message[message_idx++] = entry->info.hops;
}
ESP_LOGI(DEBUG_LINK_TAG, "replying to discovery request to board %d", dest_id);
res = send(dest_id, rip_message, message_idx, FrameType::RIP_TABLE_CONTROL, FLAG_DISCOVERY);
if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to send rip frame from send_rip_frame");
}
}
return ESP_OK;
}
/**
* @brief Determines which channel to route the frame to, depending on the dest (board) id
*
* @param dest_id
* @param channel_to_send
* @return esp_err_t
*/
esp_err_t DataLinkManager::route_frame(uint8_t dest_id, uint8_t* channel_to_send){
RIPRow* entry = nullptr;
esp_err_t res;
res = rip_find_entry(dest_id, &entry, false);
if (entry == nullptr){
return ESP_ERR_NOT_FOUND;
}
if (res != ESP_OK){
return res;
}
*channel_to_send = entry->channel;
return ESP_OK;
}
esp_err_t DataLinkManager::get_routing_table(RIPRow_public* table, size_t* table_size){
if (table == nullptr){
ESP_LOGE(DEBUG_LINK_TAG, "Invalid table pointer");
return ESP_FAIL;
}
if (table_size == nullptr){
ESP_LOGE(DEBUG_LINK_TAG, "Invalid table size pointer");
return ESP_FAIL;
}
if (*table_size < RIP_MAX_ROUTES){
ESP_LOGE(DEBUG_LINK_TAG, "Invalid table size (must be greater than %d)", RIP_MAX_ROUTES);
return ESP_FAIL;
}
size_t curr_size = 0;
for (size_t i = 0; i < RIP_MAX_ROUTES; i++){
if (xSemaphoreTake(rip_table[i].row_sem, (TickType_t)RIP_MAX_SEM_WAIT) != pdTRUE){
return ESP_FAIL;
}
if (rip_table[i].valid == RIP_VALID_ROW){
table[i].info = rip_table[i].info;
table[i].channel = rip_table[i].channel;
curr_size++;
}
xSemaphoreGive(rip_table[i].row_sem);
}
*table_size = curr_size;
return ESP_OK;
}
[[noreturn]] void DataLinkManager::rip_broadcast_timer_function(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, "RIP Broadacst task failed to start due to invalid pointer");
vTaskDelete(nullptr);
}
ESP_LOGI(DEBUG_LINK_TAG, "Starting RIP broadcast task");
esp_err_t res;
while(!link_layer_obj->stop_tasks){
bool dummy;
xQueueReceive(link_layer_obj->manual_broadcasts, &dummy, pdMS_TO_TICKS(RIP_BROADCAST_INTERVAL)); //wait up to RIP_BROADCAST_INTERVAL ms
ESP_LOGI(DEBUG_LINK_TAG, "Broadcasting table..."); //debug
res = link_layer_obj->send_rip_frame(true, 0);
if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to broadcast rip frame");
}
}
vTaskDelete(nullptr);
}
[[noreturn]] void DataLinkManager::rip_ttl_decrement_task(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, "RIP Broadacst task failed to start due to invalid pointer");
vTaskDelete(nullptr);
}
ESP_LOGI(DEBUG_LINK_TAG, "Starting RIP ttl decrement task");
bool broadcast = false;
bool dummy = true;
xQueueSend(link_layer_obj->manual_broadcasts, &dummy, 0);
while(!link_layer_obj->stop_tasks){
vTaskDelay(pdMS_TO_TICKS(RIP_MS_TO_SEC)); //run every second
for (size_t i = 1; i < RIP_MAX_ROUTES; i++){
// ESP_LOGI(DEBUG_LINK_TAG, "Decrementing ttl on entry %d", i);
if (xSemaphoreTake(link_layer_obj->rip_table[i].row_sem, (TickType_t)RIP_MAX_SEM_WAIT) !=pdTRUE){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to get sem from entry %d", i);
continue;
}
if (link_layer_obj->rip_table[i].valid == RIP_INVALID_ROW){
xSemaphoreGive(link_layer_obj->rip_table[i].row_sem);
continue;
}
if (link_layer_obj->rip_table[i].ttl != 0){
// link_layer_obj->rip_table[i].valid = RIP_INVALID_ROW;
// ESP_LOGI(DEBUG_LINK_TAG, "Entry %d now has ttl %d", i, link_layer_obj->rip_table[i].ttl);
// } else {
link_layer_obj->rip_table[i].ttl--;
if (link_layer_obj->rip_table[i].ttl == 0){
link_layer_obj->rip_table[i].info.hops = RIP_MAX_HOPS + 1;
link_layer_obj->rip_table[i].ttl_flush = RIP_FLUSH_COUNT;
broadcast = true;
}
}
xSemaphoreGive(link_layer_obj->rip_table[i].row_sem);
}
if (broadcast && uxQueueMessagesWaiting(link_layer_obj->manual_broadcasts) == 0){
broadcast = false;
xQueueSend(link_layer_obj->manual_broadcasts, &dummy, 0);
}
}
vTaskDelete(nullptr);
}
/**
* @brief This function will start the tasks required for RIP to function.
* Currently, this function will:
* - start the task to periodically broadcast the board's current copy of the RIP table to all other boards via the 4 RMT channels
* - start a task to periodically decrement the ttl values of each row in the RIP table (WIP) - this will require some sort of mutex on the table itself
*/
void DataLinkManager::start_rip_tasks(){
manual_broadcasts = xQueueCreate(2, sizeof(bool));
ESP_LOGI(DEBUG_LINK_TAG, "Starting RIP Broadcast task");
xTaskCreate(DataLinkManager::rip_broadcast_timer_function, "RIPBroadcast", 4096, static_cast<void*>(this), 5, &rip_broadcast_task);
ESP_LOGI(DEBUG_LINK_TAG, "Starting RIP TTL task");
xTaskCreate(DataLinkManager::rip_ttl_decrement_task, "RIPTTL", 2048, static_cast<void*>(this), 5, &rip_ttl_task);
}

223
components/dataLink/DataLinkScheduler.cpp Normal file
View File

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

23
components/dataLink/README.md
View File

@@ -1,3 +1,5 @@
WIP
# Data Link Layer
This component represents the data link layer. It will handle board to board communication (abstracting away the RMT specific details of transmitting physical raw bits over wires).
@@ -8,14 +10,27 @@ See `./include/Frames.h` for frame definitions.
There will be two types of frames: Control and Generic.
Control frames will contain all control information (eg. Spinning a DC motor, moving a servo to a particular angle, sensor information). These frames will have a limit of 32B of data size with a total packet size of 41B. These frames will not be fragmented.
Control frames will contain all control information (eg. Spinning a DC motor, moving a servo to a particular angle, sensor information). These frames will have a limit of 256B of data size with a total packet size of 41B. These frames will not be fragmented.
Generic frames will contain all other information. They will have a max data size of 8KiB with a total packet size of 8206B (8.013672 KiB). These frames can be fragmented.
Generic frames will contain all other information. They will have a max data size of 16MiB if fragmented (max 2**16 fragments). Each fragment will still maintain a max 256B data size.
To differentiate between the two frames, the `type` field in both frames will be compared against. If the MSB is set to 1, it will be determined to be a control packet.
## Routing Information Protocol (RIP) Table
See `./include/Tables.h` for table definitions.
See `./include/Tables.h` for table definitions. See `DataLinkRIP.cpp` for function definitions.
It handles all known (advertised) boards on the same network of ESP32-S3 boards. It will broadcast its stored routing table to its neighbours roughly every 30 seconds or upon receiving a routing table from its neighbours. All routes on a routing table will expire after 180 seconds, unless refreshed/updated/re-advertised by that respective board. The routing table itself will be used to route frames using the path with the least number of hops (known locally).
## Frame Management
See `DataLinkFrames.cpp` for more information.
It handles generic frame fragmentation, user handling of receiving received frames (stored in a queue, per channel), and a thread to handle polling the available channels for receiving.
### Send Frames Scheduler
See `DataLinkScheduler.cpp` for more information.
It handles all TX frames passed from the user and schedules them to be sent. The frames are stored in a priority queue (per channel), where Control frames has a higher priority than Generic frames (generally). The actual scheduler algorithm can be found in `Scheduler.h`.
WIP

124
components/dataLink/include/DataLinkManager.h
View File

@@ -11,11 +11,14 @@
#include "Frames.h"
#include "Tables.h"
#include "RMTManager.h"
#include <unordered_map>
#include "Scheduler.h"
#define DEBUG_LINK_TAG "LinkLayer"
#define CRC_POLYNOMIAL 0x1021
//look up table for crc
static const uint16_t crc16_table[256] = {
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
@@ -33,8 +36,15 @@ static const uint16_t crc16_table[256] = {
0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
}; //look up table for crc
};
#define ASYNC_QUEUE_WAIT_TICKS 100
/**
* @brief Class to represent the Data Link Layer
*
* @author Justin Chow
*/
class DataLinkManager{
public:
DataLinkManager(uint8_t board_id, uint8_t num_channels);
@@ -42,37 +52,36 @@ class DataLinkManager{
esp_err_t send(uint8_t dest_board, uint8_t* data, uint16_t data_len, FrameType type, uint8_t flag);
esp_err_t start_receive_frames(uint8_t curr_channel);
esp_err_t receive(uint8_t* data, size_t data_len, size_t* recv_len, uint8_t curr_channel);
esp_err_t print_frame_info(uint8_t* data, size_t data_len, uint8_t* message);
esp_err_t get_network_toplogy(RIPRow_public_matrix* matrix, size_t* matrix_size);
esp_err_t print_frame_info(uint8_t* data, size_t data_len, uint8_t* message, size_t message_len);
esp_err_t get_routing_table(RIPRow_public* table, size_t* table_size);
esp_err_t async_receive_info(uint16_t* frame_size, FrameHeader* header, uint8_t channel);
esp_err_t async_receive(uint8_t* data, uint16_t data_len, FrameHeader* header, uint8_t channel);
private:
uint8_t this_board_id = 0;
uint8_t num_channels = MAX_CHANNELS;
//std::priority_queue<Frame, std::vector<Frame>, FrameCompare> frame_queue; //create a priority queue - not in use
std::unique_ptr<RMTManager> phys_comms;
std::unordered_map<uint8_t, uint16_t> sequence_num_map;
volatile bool stop_tasks = false; //used by the tasks to know when to stop (set true when DataLinkManager is destroyed)
TaskHandle_t rip_broadcast_task = NULL;
TaskHandle_t rip_ttl_task = NULL;
esp_err_t set_board_id(uint8_t board_id);
esp_err_t get_board_id(uint8_t& board_id);
void print_binary(uint8_t byte);
void print_buffer_binary(const uint8_t* buffer, size_t length);
esp_err_t get_data_from_frame(uint8_t* data, size_t data_len, uint8_t* message, size_t* message_size, frame_header* header);
esp_err_t get_data_from_frame(uint8_t* data, size_t data_len, uint8_t* message, size_t* message_size, FrameHeader* header);
esp_err_t geneate_crc_16(uint8_t* data, size_t data_len, uint16_t* crc);
esp_err_t create_control_frame(uint8_t* data, uint16_t data_len, ControlFrame control_frame, uint8_t* send_data, size_t* send_data_len);
esp_err_t create_generic_frame(uint8_t* data, uint16_t data_len, GenericFrame generic_frame, uint16_t offset, uint8_t* send_data, size_t* send_data_len);
//==== RIP related functions ====
/**
* TODO for RIP:
* Periodic Routing Updates via timer (broadcast routing table every 30 seconds)
* Handle RIP table updates when a RIP table arrives (ensure there are no loops between boards of sending tables back and forth)
* TTL handling and route expiration
* TTLs in all rows should decrement at once via timer
*/
void init_rip();
esp_err_t rip_find_entry(uint8_t board_id, RIPRow** entry, bool reserve_row);
esp_err_t rip_update_entry(uint8_t new_hop, uint8_t channel, RIPRow** entry);
esp_err_t rip_add_entry(uint8_t board_id, uint8_t hops, uint8_t channel, RIPRow** entry);
esp_err_t rip_reset_entry_ttl(uint8_t board_id);
esp_err_t rip_get_row(RIPRow** entry, uint8_t row_num);
//this is stored locally with metadata `ttl`
// std::unordered_map<uint8_t, RIPRow> rip_table; //using a hash map to store the routes to other boards - will be used as we scale up
@@ -87,6 +96,95 @@ class DataLinkManager{
QueueHandle_t discovery_tables;
esp_err_t route_frame(uint8_t dest_id, uint8_t* channel_to_send);
//==== Frame Scheduling related functions ====
/**
* @brief Priority queue for each channel to schedule when to send frames
*
*/
std::priority_queue<SchedulerMetadata, std::vector<SchedulerMetadata>, FrameCompare> frame_queue[MAX_CHANNELS];
SemaphoreHandle_t sq_handle[MAX_CHANNELS];
void init_scheduler();
esp_err_t push_frame_to_scheduler(SchedulerMetadata frame, uint8_t channel);
TaskHandle_t scheduler_task = NULL;
/**
* @brief Schedules which frame to send
*
* Scheduler:
* - All frames will be pushed to the back onto a queue
* - When a generic frame sends a chunk, it will be pushed back to the queue for the next chunk to be sent
*
* Scheduling may change (above scheduler will lead to starvation of control frames depending on the number of generic frames/fragments to send)
*/
[[noreturn]] static void frame_scheduler(void* args);
/**
* @brief Scheduler sending the actual frame at the top of the heap on a channel
*
* @return esp_err_t
*/
esp_err_t scheduler_send(uint8_t channel);
//Generic Frame Receive Fragments
/**
* @brief Store a fragment that has been received
*
* @param fragment
* @param channel
* @return esp_err_t
*/
esp_err_t store_fragment(GenericFrame* fragment, uint8_t channel);
/**
* @brief Stores generic frame fragments
*
* Mapping:
* Board ID -> Sequence number -> Array of Generic Frame Fragments, with size of the number of expected fragments
*
* TODO:
* - When receiving a fragment, insert it into the map
* - When all fragments have been received in a sequence, remove the entire entry (sequence) from the map, and push final data for async receive
* - Sliding window + ACKs
*
*/
std::unordered_map<uint16_t, std::unordered_map<uint16_t, FragmentMetadata>> fragment_map;
esp_err_t complete_fragment(uint16_t board_id, uint16_t sequence_num, uint8_t channel);
SemaphoreHandle_t async_rx_queue_mutex[MAX_CHANNELS];
//Async receive
/**
* @brief Queue to store complete received frame data
*
* TODO:
* - Replace the public `receive()` with the `async_receive()`.
*
*/
std::queue<Rx_Metadata> async_receive_queue[MAX_CHANNELS];
esp_err_t start_receive_frames_rmt(uint8_t curr_channel);
/**
* @brief Receive thread entry point
*
* @param args
*/
[[noreturn]] static void receive_thread_main(void* args);
/**
* @brief Receive bytes from Physical Layer (RMT)
*
* @note This replaces the deprecated `receive` function
*
* @param channel Physical channel pair to look at
* @return esp_err_t
*/
esp_err_t receive_rmt(uint8_t channel);
TaskHandle_t receive_task = NULL;
};
#endif //DATA_LINK

43
components/dataLink/include/Frames.h
View File

@@ -1,13 +1,21 @@
#ifdef DATA_LINK
#pragma once
#include "freertos/FreeRTOS.h"
#include <variant>
#include <cstdint>
#include <vector>
#define BROADCAST_ADDR 0xFF //used for discovery (finding the board's neighbours). this will mean the board ids will have 2^8-2 = 254 unique IDs that could be assigned
#define PC_ADDR 0x0 //setting 0 to be the PC
#define START_OF_FRAME 0xAB //0b1010_1011 - denotes the start of frame
#define MAX_GENERIC_DATA_LEN (1 << 16) //Max 65.5KiB
#define MAX_CONTROL_DATA_LEN (1 << 8) // Max 256B
#define MAX_GENERIC_NUM_FRAG (1 << 16) // Max 2**16 Fragments can be made with a generic frame (total 2**16 *MAX_CONTROL_DATA_LEN B of data can be sent ~ 16 MiB)
#define MAX_FRAME_QUEUE_SIZE 15 //Size of the queue for the frame scheduler (per channel)
//Flags
#define FLAG_FRAG 0x8 //0b1000 //this fragmented frame is part of a larger frame
#define FLAG_DISCOVERY 0x4 //0b0100
@@ -20,7 +28,7 @@
#define IS_CONTROL_FRAME(x) (((x) & 0x80) != 0)
#define CONTROL_FRAME_OVERHEAD 9
#define GENERIC_FRAME_OVERHEAD 12
#define GENERIC_FRAME_OVERHEAD 14
#define CONTROL_FRAME_TYPE 0x80 //if the frame type MSB is set to 1, use the control frame
//Types (total 2^4 = 16 different types)
@@ -45,7 +53,7 @@ typedef struct _control_frame{
uint16_t data_len; //Data Length (max 256B)
uint8_t data[MAX_CONTROL_DATA_LEN]; //Variable Length of Data
uint16_t crc_16; //CRC-16
} control_frame; //this will have a max size of 9 + 256B = 265B
} ControlFrame; //this will have a max size of 9 + 256B = 265B
typedef struct _data_link_frame{
uint8_t preamble; //Start of Frame
@@ -53,11 +61,12 @@ typedef struct _data_link_frame{
uint8_t receiver_id; //receiver board id
uint16_t seq_num; //sequence number to differentiate frames being sent from sender to receiver
uint8_t type_flag; //(type << 4) | flag - both are 4 bits
uint16_t frag_info; //(total_frag_num << 8) | frag_num - total_frag_num denotes the total number of fragmented frames to expect for this sequence number(?) and frag_num denotes the fragment frame num
uint16_t total_frag; //total number of fragments for this sequence
uint16_t frag_num; //current fragment number
uint16_t data_len; //Data Length (max 178B)
uint8_t data[MAX_GENERIC_DATA_LEN]; //Variable Length of Data
uint8_t data[MAX_CONTROL_DATA_LEN]; //Variable Length of Data
uint16_t crc_16; //CRC-16
} data_link_frame; //this will have a max size of ~65.5KiB
} GenericFrame; //this will have a max size of 14 + 2^8 B = 270 B
#pragma pack(pop)
typedef struct _header{
@@ -66,8 +75,26 @@ typedef struct _header{
uint8_t receiver_id; //receiver board id
uint16_t seq_num; //sequence number to differentiate frames being sent from sender to receiver
uint8_t type_flag; //(type << 4) | flag - both are 4 bits
uint16_t frag_info; //(total_frag_num << 8) | frag_num - total_frag_num denotes the total number of fragmented frames to expect for this sequence number(?) and frag_num denotes the fragment frame num
uint32_t frag_info; //(total_frag_num << 16) | frag_num - total_frag_num denotes the total number of fragmented frames to expect for this sequence number(?) and frag_num denotes the fragment frame num
uint16_t data_len; //Data Length (max 178B)
uint16_t crc_16; //CRC-16
} frame_header;
} FrameHeader;
using Frame = std::variant<ControlFrame, GenericFrame>;
ControlFrame make_control_frame_from_header(const FrameHeader& header);
GenericFrame make_generic_frame_from_header(const FrameHeader& header);
typedef struct _fragment_metadata {
std::vector<GenericFrame> fragments;
uint16_t num_fragments_rx;
} FragmentMetadata;
typedef struct _receive_metadata{
uint8_t* data;
uint16_t data_len;
FrameHeader header;
} Rx_Metadata;
#endif //DATA_LINK

67
components/dataLink/include/Scheduler.h Normal file
View File

@@ -0,0 +1,67 @@
#ifdef DATA_LINK
#include "Frames.h"
#include "esp_timer.h"
#include <cstdint>
//TODO: need to make a private send() to accept a Frame type.
//the public send() is fine to keep but needs to be modified to simply create the Frame type based on the flags, data size, and args, and then push to the priority queue based on routing
//TODO: receive also needs to be updated, when receiving a frame not destined for that board; need to route that frame towards the actual destination (requires pushing that frame onto the priority queue)
//TODO: generic frames receive needs to be created (handle fragmenting, resend on corruption). ethe resend on corruption will be a lot of work as it requires ACK frames to be send and currently there is 0 support for ACK since we don't save any recently sent frames (can't resend)
#define SCHEDULER_MUTEX_WAIT 10 //max time duration to wait
#define SCHEDULER_PERIOD_MS 25
//Metadata representing the frame to be sent but is currently scheduled
typedef struct _frame_scheduler_metadata {
FrameHeader header; //header of the frame
uint16_t generic_frame_data_offset; //For data greater than MAX_CONTROL_DATA_LEN to keep track of fragment positions
int64_t enqueue_time_ns; //when the frame has been first enqueued into the priority queue
uint8_t* data; //dyanmically allocated memory - contains the actual data
uint16_t len; // length of the actual data
} SchedulerMetadata;
typedef struct _frame_compare {
/**
* @brief Uses aging based priority scheduling (linearly increasing priority with time)
*
* $P_f = B_f - A_f\alpha$
*
* - $P_f$ is the effective priority value (lower comes first)
*
* - $B_f$ is the base priority
*
* - $A_f$ is the age (amount of time the frame has waited in the queue)
*
* - $\alpha$ is the aging factor (rate at which a frame increases priority)
*
* @param a
* @param b
* @return true
* @return false
*/
bool operator()(const SchedulerMetadata& a, const SchedulerMetadata& b) const {
int64_t now = esp_timer_get_time();
double age_a = (now - a.enqueue_time_ns) / 1e6;
double age_b = (now - a.enqueue_time_ns) / 1e6;
// Base priorities: lower is higher priority
double base_a = (IS_CONTROL_FRAME(a.header.type_flag)) ? 0.0 : 10.0;
double base_b = (IS_CONTROL_FRAME(b.header.type_flag)) ? 0.0 : 10.0;
// Aging coefficient (tune this)
constexpr double aging_factor = 0.1;
double effective_a = base_a - age_a * aging_factor;
double effective_b = base_b - age_b * aging_factor;
// If effective priority equal, fall back to enqueue time (FIFO)
if (effective_a == effective_b) {
return a.enqueue_time_ns > b.enqueue_time_ns;
}
// Return true if a has *lower* priority (so b stays on top)
return effective_a < effective_b;
}
} FrameCompare;
#endif //DATA_LINK

3
components/dataLink/test/CMakeLists.txt Normal file
View File

@@ -0,0 +1,3 @@
idf_component_register(SRC_DIRS "."
INCLUDE_DIRS "."
REQUIRES unity dataLink rmt esp_event)

29
components/dataLink/test/test_dataLink.cpp Normal file
View File

@@ -0,0 +1,29 @@
#include "unity.h"
#include "DataLinkManager.h"
#include <memory>
#define TEST_BOARD_ID 69
std::unique_ptr<DataLinkManager> createObj(){
std::unique_ptr<DataLinkManager> obj = std::make_unique<DataLinkManager>(TEST_BOARD_ID, 4);
TEST_ASSERT_NOT_NULL(obj.get());
return obj;
}
TEST_CASE("should instantiate an DataLinkManager object with 4 channels", "[dataLink]"){
createObj();
}
// void board_a(){
// std::unique_ptr<DataLinkManager> obj = createObj();
// unity_send_signal("board a");
// unity_wait_for_signal("board b");
// }
// void board_b(){
// std::unique_ptr<DataLinkManager> obj = createObj();
// unity_send_signal("board b");
// unity_wait_for_signal("board a");
// }
// TEST_CASE_MULTIPLE_DEVICES("should be able to send tables to another board and receive it", "[dataLink]", board_a, board_b);