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

Reduce DataLink latency

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

2
components/dataLink/CMakeLists.txt
View File

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

217
components/dataLink/DataLinkFrames.cpp
View File

@@ -1,11 +1,13 @@
#include "DataLinkManager.h" #include "DataLinkManager.h"
#include "esp_log.h" #include "esp_log.h"
#include <cstring>
#include <type_traits>
/** /**
* @brief Creates a Control Frame from `FrameHeader` * @brief Creates a Control Frame from `FrameHeader`
* *
* @param header * @param header
* @return ControlFrame * @return ControlFrame
*/ */
ControlFrame make_control_frame_from_header(const FrameHeader& header) { ControlFrame make_control_frame_from_header(const FrameHeader& header) {
ControlFrame frame{}; ControlFrame frame{};
@@ -21,9 +23,9 @@ ControlFrame make_control_frame_from_header(const FrameHeader& header) {
/** /**
* @brief Creates a Generic Frame from `FrameHeader` * @brief Creates a Generic Frame from `FrameHeader`
* *
* @param header * @param header
* @return GenericFrame * @return GenericFrame
*/ */
GenericFrame make_generic_frame_from_header(const FrameHeader& header) { GenericFrame make_generic_frame_from_header(const FrameHeader& header) {
GenericFrame frame{}; GenericFrame frame{};
@@ -40,11 +42,11 @@ GenericFrame make_generic_frame_from_header(const FrameHeader& header) {
} }
/** /**
* @brief Store a fragment that has been received * @brief Store a fragment that has been received
* *
* @param fragment * @param fragment
* @param channel * @param channel
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t DataLinkManager::store_fragment(GenericFrame* fragment, uint8_t channel){ esp_err_t DataLinkManager::store_fragment(GenericFrame* fragment, uint8_t channel){
if (fragment == nullptr){ if (fragment == nullptr){
@@ -64,7 +66,7 @@ esp_err_t DataLinkManager::store_fragment(GenericFrame* fragment, uint8_t channe
if (rx_fragment_mutex[channel] == NULL){ if (rx_fragment_mutex[channel] == NULL){
return ESP_FAIL; return ESP_FAIL;
} }
if (xSemaphoreTake(rx_fragment_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){ if (xSemaphoreTake(rx_fragment_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){
return ESP_ERR_TIMEOUT; 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){ if (static_cast<FrameType>(GET_TYPE(fragment->type_flag)) != FrameType::MISC_UDP_GENERIC_TYPE){
SendAckMetaData data = { 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), .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->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)}, static_cast<uint8_t>((fragment->seq_num & 0xFF00) >> 8), static_cast<uint8_t>(fragment->seq_num & 0xFF)},
.sender_id = fragment->sender_id, .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` * @brief Removes the corresponding entry from `fragment_map` and pushes the data onto `async_receive_queue`
* *
* @param board_id * @param board_id
* @param sequence_num * @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){ esp_err_t DataLinkManager::complete_fragment(uint16_t board_id, uint16_t sequence_num, uint8_t channel){
Rx_Metadata rx; Rx_Metadata rx;
if (xSemaphoreTake(rx_fragment_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){ if (xSemaphoreTake(rx_fragment_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){
return ESP_ERR_TIMEOUT; 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 uint16_t total_data_len = metadata.num_fragments_rx*MAX_FRAME_SIZE; //max data size with n fragments
xSemaphoreGive(rx_fragment_mutex[channel]); 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; rx.data_len = total_data_len;
if (combined_data == nullptr){ if (combined_data == nullptr){
return ESP_ERR_NO_MEM; 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){ if (xSemaphoreTake(rx_fragment_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){
return ESP_ERR_TIMEOUT; return ESP_ERR_TIMEOUT;
} }
if (fragment_map[channel][board_id].find(sequence_num) == fragment_map[channel][board_id].end()){ if (fragment_map[channel][board_id].find(sequence_num) == fragment_map[channel][board_id].end()){
xSemaphoreGive(rx_fragment_mutex[channel]); xSemaphoreGive(rx_fragment_mutex[channel]);
return ESP_ERR_NOT_FOUND; return ESP_ERR_NOT_FOUND;
} }
rx.data = combined_data;
uint16_t prev_index = 0; uint16_t prev_index = 0;
for (size_t i = 0; i < metadata.num_fragments_rx; i++){ 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; prev_index += metadata.fragments[i].data_len;
} }
xSemaphoreGive(rx_fragment_mutex[channel]); xSemaphoreGive(rx_fragment_mutex[channel]);
rx.data = std::move(combined_data);
rx.data_len = prev_index; rx.data_len = prev_index;
if (async_rx_queue_mutex[channel] == nullptr){ 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); // 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){ if (!async_receive_queue->enqueue(std::move(rx), std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS))) {
vPortFree(combined_data);
return ESP_ERR_TIMEOUT; return ESP_ERR_TIMEOUT;
} }
async_receive_queue[channel].push(rx);
xSemaphoreGive(async_rx_queue_mutex[channel]);
fragment_map[channel][board_id].erase(sequence_num); fragment_map[channel][board_id].erase(sequence_num);
if (fragment_map[channel][board_id].empty()) { if (fragment_map[channel][board_id].empty()) {
fragment_map[channel].erase(board_id); 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 * @brief Sends an ACK
* *
* @param sender_id This is the board id that is receiving the ACK (the original sender board id) * @param sender_id This is the board id that is receiving the ACK (the original sender board id)
* @param data * @param data
* @param data_len * @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 * @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){ 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); // 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);
* @brief Checks the channel receive queue for any received frames. If there is, return the first frame's data size return send(sender_id, std::move(buffer), FrameType::ACK_TYPE, 0x0);
*
* @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 * @brief Get the first frame's data
* *
* @param data Char array of the actual combined data * @param data Char array of the actual combined data
* @param data_len Combined data length * @param data_len Combined data length
* @param header Header information of returning frame * @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){ std::optional<std::unique_ptr<std::vector<uint8_t>>> DataLinkManager::async_receive(){
if (data == nullptr || header == nullptr){ auto maybe_top = async_receive_queue->dequeue(std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS));
return ESP_ERR_INVALID_ARG; if (!maybe_top) {
return std::nullopt;
} }
Rx_Metadata top = std::move(*maybe_top);
if (data_len == 0){ return std::make_optional<std::unique_ptr<std::vector<uint8_t>>>(std::move(top.data));
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);
// ESP_LOGI(DEBUG_LINK_TAG, "pushed frame %d onto async queue", header->seq_num); // 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){ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
uint16_t data_len = MAX_FRAME_SIZE; //max possible data len uint16_t data_len = MAX_FRAME_SIZE; //max possible data len
uint8_t data[data_len]; uint8_t data[data_len];
memset(data, 0, data_len);
size_t recv_len = 0; 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"); // ESP_LOGE(DEBUG_LINK_TAG, "RMT Failed to receive - recieve_rmt");
return ESP_ERR_TIMEOUT; return ESP_ERR_TIMEOUT;
} }
if (recv_len > MAX_FRAME_SIZE){ if (recv_len > MAX_FRAME_SIZE){
ESP_LOGE(DEBUG_LINK_TAG, "Received frame is too large to be control or generic"); ESP_LOGE(DEBUG_LINK_TAG, "Received frame is too large to be control or generic");
return ESP_ERR_INVALID_RESPONSE; return ESP_ERR_INVALID_RESPONSE;
@@ -343,16 +281,18 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
return ESP_ERR_INVALID_RESPONSE; return ESP_ERR_INVALID_RESPONSE;
} }
uint8_t message[MAX_FRAME_SIZE]; auto message = std::make_unique<std::vector<uint8_t>>();
memset(message, 0, sizeof(message)); message->resize(MAX_FRAME_SIZE);
size_t message_size = 0; size_t message_size = 0;
FrameHeader header; 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){ if (res != ESP_OK){
// print_buffer_binary(message, message_size); // print_buffer_binary(message, message_size);
return res; return res;
} }
message->resize(message_size);
// print_buffer_binary(message, 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){ if (message_size != GENERIC_FRAG_ACK_DATA_SIZE || message_size == 0){
return ESP_OK; return ESP_OK;
} }
if (message[0] != GENERIC_FRAG_ACK_PREAMBLE){ if (message->data()[0] != GENERIC_FRAG_ACK_PREAMBLE){
return ESP_OK; return ESP_OK;
} }
FrameAckRecord record = { FrameAckRecord record = {
.last_ack = static_cast<uint16_t>((message[1] << 8) | (message[2])), .last_ack = static_cast<uint16_t>((message->data()[1] << 8) | (message->data()[2])),
.total_frags = static_cast<uint16_t>((message[3] << 8) | (message[4])), .total_frags = static_cast<uint16_t>((message->data()[3] << 8) | (message->data()[4])),
.seq_num = static_cast<uint16_t>((message[5] << 8) | (message[6])) .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); res = inc_head_sliding_window(channel, header.sender_id, record.seq_num, &record);
// if (res == ESP_OK){ // if (res == ESP_OK){
@@ -379,7 +319,7 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
// } else { // } 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]); // 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; return ESP_OK;
} }
@@ -390,36 +330,35 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
return ESP_FAIL; return ESP_FAIL;
} }
memcpy(frame.data, message, message_size); memcpy(frame.data, message->data(), message_size);
esp_err_t res = store_fragment(&frame, channel); esp_err_t res = store_fragment(&frame, channel);
return res; return res;
} }
//control frame handling: - TODO: clean up :) //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); // 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 //check for a rip frame
if (static_cast<FrameType>(GET_TYPE(header.type_flag)) == FrameType::RIP_TABLE_CONTROL){ if (static_cast<FrameType>(GET_TYPE(header.type_flag)) == FrameType::RIP_TABLE_CONTROL){
ESP_LOGI(DEBUG_LINK_TAG, "Got a RIP frame"); ESP_LOGI(DEBUG_LINK_TAG, "Got a RIP frame");
for (size_t i = 0; i < message_size-1; i+=2){ for (size_t i = 0; i < message_size-1; i+=2){
uint8_t board_id = message[i]; uint8_t board_id = message->data()[i];
uint8_t hops = message[i+1]; 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); // ESP_LOGI(DEBUG_LINK_TAG, "Received: board_id %d and number of hops %d on channel %d", board_id, hops, channel);
RIPRow* entry = nullptr; RIPRow* entry = nullptr;
res = rip_find_entry(board_id, &entry, true); res = rip_find_entry(board_id, &entry, true);
if (res != ESP_OK){ if (res != ESP_OK){
return ESP_FAIL; return ESP_FAIL;
} }
if (entry == nullptr){ if (entry == nullptr){
printf("rip pointer\n"); printf("rip pointer\n");
return ESP_FAIL; //no room for more entries in the table return ESP_FAIL; //no room for more entries in the table
} }
if (entry->valid == RIP_NEW_ROW){ if (entry->valid == RIP_NEW_ROW){
//adding a new entry //adding a new entry
rip_add_entry(board_id, hops + 1, channel, &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 //updating an entry
rip_update_entry(hops + 1, channel, &entry); rip_update_entry(hops + 1, channel, &entry);
} }
if (GET_FLAG(header.type_flag) == FLAG_DISCOVERY){ if (GET_FLAG(header.type_flag) == FLAG_DISCOVERY){
//discovery -> send routing table //discovery -> send routing table
// ESP_LOGI(DEBUG_LINK_TAG, "got discovery reply"); // ESP_LOGI(DEBUG_LINK_TAG, "got discovery reply");
@@ -435,10 +374,10 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
.info = entry->info, .info = entry->info,
.channel = entry->channel .channel = entry->channel
}; };
xQueueSendToBack(discovery_tables, &row_queue, (TickType_t)10); xQueueSendToBack(discovery_tables, &row_queue, (TickType_t)10);
} }
} }
if (message_size == RIP_DISCOVERY_MESSAGE_SIZE){ if (message_size == RIP_DISCOVERY_MESSAGE_SIZE){
res = send_rip_frame(false, header.sender_id); 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 //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){ 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); // 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; 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 = { Rx_Metadata metadata = {
.data = metadata_message, .data = std::move(message),
.data_len = (uint16_t)message_size, .data_len = (uint16_t)message_size,
.header = header .header = header
}; };
if (xSemaphoreTake(async_rx_queue_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) == pdTRUE){ if (!async_receive_queue->enqueue(std::move(metadata), std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS))){
async_receive_queue[channel].push(metadata);
xSemaphoreGive(async_rx_queue_mutex[channel]);
} else {
return ESP_ERR_TIMEOUT; return ESP_ERR_TIMEOUT;
} }
return ESP_OK; 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->receive_rmt(i);
res = link_layer_obj->start_receive_frames_rmt(i); res = link_layer_obj->start_receive_frames_rmt(i);
} }
vTaskDelay(pdMS_TO_TICKS(RECEIVE_TASK_PERIOD_MS)); 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){ if (xSemaphoreTake(link_layer_obj->send_ack_queue_mutex[channel], pdMS_TO_TICKS(SEND_ACK_MUTEX_WAIT)) != pdTRUE){
continue; continue;
} }
if (link_layer_obj->send_ack_queue[channel].empty()){ if (link_layer_obj->send_ack_queue[channel].empty()){
xSemaphoreGive(link_layer_obj->send_ack_queue_mutex[channel]); xSemaphoreGive(link_layer_obj->send_ack_queue_mutex[channel]);
continue; continue;
} }
SendAckMetaData data = link_layer_obj->send_ack_queue[channel].front(); SendAckMetaData data = link_layer_obj->send_ack_queue[channel].front();
link_layer_obj->send_ack_queue[channel].pop(); link_layer_obj->send_ack_queue[channel].pop();
link_layer_obj->send_ack(data.sender_id, data.data, GENERIC_FRAG_ACK_DATA_SIZE); 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); vTaskDelete(nullptr);
} }

214
components/dataLink/DataLinkManager.cpp
View File

@@ -1,11 +1,16 @@
#include "DataLinkManager.h" #include "DataLinkManager.h"
#include "BlockingQueue.h"
#include "Frames.h"
#include "RMTManager.h" #include "RMTManager.h"
#include "esp_log.h" #include "esp_log.h"
#include "nvs_flash.h" #include "nvs_flash.h"
#include <memory>
#define SCHEDULE_QUEUE_SIZE 25
/** /**
* @brief Constructs a new Data Link Manager object * @brief Constructs a new Data Link Manager object
* *
* @param board_id Board ID of the current board. Will be written to the NVM under key "board" if not already written. * @param board_id Board ID of the current board. Will be written to the NVM under key "board" if not already written.
*/ */
DataLinkManager::DataLinkManager(uint8_t board_id, uint8_t num_channels = MAX_CHANNELS){ DataLinkManager::DataLinkManager(uint8_t board_id, uint8_t num_channels = MAX_CHANNELS){
@@ -28,14 +33,20 @@ DataLinkManager::DataLinkManager(uint8_t board_id, uint8_t num_channels = MAX_CH
sequence_num_map_mutex = xSemaphoreCreateMutex(); sequence_num_map_mutex = xSemaphoreCreateMutex();
for (int i = 0; i < MAX_CHANNELS; i++) {
frame_queue[i] = std::make_unique<BlockingPriorityQueue<SchedulerMetadata, std::vector<SchedulerMetadata>, FrameCompare>>(SCHEDULE_QUEUE_SIZE);
}
async_receive_queue = std::make_unique<BlockingQueue<Rx_Metadata>>(MAX_RX_QUEUE_SIZE);
init_scheduler(); init_scheduler();
init_rip(); init_rip();
} }
/** /**
* @brief Returns if the link layer is ready to receive frames * @brief Returns if the link layer is ready to receive frames
* *
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t DataLinkManager::ready(){ esp_err_t DataLinkManager::ready(){
return (phys_comms == nullptr || rip_broadcast_task == NULL || rip_ttl_task == NULL || scheduler_task == NULL || receive_task == NULL) ? ESP_FAIL : ESP_OK; return (phys_comms == nullptr || rip_broadcast_task == NULL || rip_ttl_task == NULL || scheduler_task == NULL || receive_task == NULL) ? ESP_FAIL : ESP_OK;
@@ -43,10 +54,10 @@ esp_err_t DataLinkManager::ready(){
/** /**
* @brief Atomic function to get and post increment sequence number map * @brief Atomic function to get and post increment sequence number map
* *
* @param board_id * @param board_id
* @param seq_num * @param seq_num
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t DataLinkManager::get_inc_sequence_num(uint8_t board_id, uint16_t* seq_num){ esp_err_t DataLinkManager::get_inc_sequence_num(uint8_t board_id, uint16_t* seq_num){
if (seq_num == NULL){ if (seq_num == NULL){
@@ -60,16 +71,16 @@ esp_err_t DataLinkManager::get_inc_sequence_num(uint8_t board_id, uint16_t* seq_
*seq_num = sequence_num_map[board_id]++; *seq_num = sequence_num_map[board_id]++;
xSemaphoreGive(sequence_num_map_mutex); xSemaphoreGive(sequence_num_map_mutex);
return ESP_OK; return ESP_OK;
} }
/** /**
* @brief Atomic function to get sequence number map * @brief Atomic function to get sequence number map
* *
* @param board_id * @param board_id
* @param seq_num * @param seq_num
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t DataLinkManager::get_sequence_num(uint8_t board_id, uint16_t* seq_num){ esp_err_t DataLinkManager::get_sequence_num(uint8_t board_id, uint16_t* seq_num){
if (seq_num == NULL){ if (seq_num == NULL){
@@ -83,7 +94,7 @@ esp_err_t DataLinkManager::get_sequence_num(uint8_t board_id, uint16_t* seq_num)
*seq_num = sequence_num_map[board_id]; *seq_num = sequence_num_map[board_id];
xSemaphoreGive(sequence_num_map_mutex); xSemaphoreGive(sequence_num_map_mutex);
return ESP_OK; return ESP_OK;
} }
@@ -102,15 +113,15 @@ DataLinkManager::~DataLinkManager(){
if (rip_ttl_task == NULL){ if (rip_ttl_task == NULL){
vTaskDelete(rip_ttl_task); vTaskDelete(rip_ttl_task);
rip_ttl_task = NULL; rip_ttl_task = NULL;
} }
if (scheduler_task == NULL){ if (scheduler_task == NULL){
vTaskDelete(scheduler_task); vTaskDelete(scheduler_task);
scheduler_task = NULL; scheduler_task = NULL;
} }
if (receive_task == NULL){ if (receive_task == NULL){
vTaskDelete(receive_task); vTaskDelete(receive_task);
receive_task = NULL; receive_task = NULL;
} }
if (send_ack_task == NULL){ if (send_ack_task == NULL){
vTaskDelete(send_ack_task); vTaskDelete(send_ack_task);
send_ack_task = NULL; send_ack_task = NULL;
@@ -136,14 +147,14 @@ esp_err_t DataLinkManager::set_board_id(uint8_t board_id){
nvs_close(handle); nvs_close(handle);
return res; return res;
} }
res = nvs_commit(handle); res = nvs_commit(handle);
if (res != ESP_OK){ if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to commit write"); ESP_LOGE(DEBUG_LINK_TAG, "Failed to commit write");
nvs_close(handle); nvs_close(handle);
return res; return res;
} }
this_board_id = board_id; this_board_id = board_id;
ESP_LOGI(DEBUG_LINK_TAG, "Successfully wrote %d to NVM", board_id); ESP_LOGI(DEBUG_LINK_TAG, "Successfully wrote %d to NVM", board_id);
@@ -170,19 +181,19 @@ esp_err_t DataLinkManager::get_board_id(uint8_t& board_id){
ESP_LOGI(DEBUG_LINK_TAG, "Successfully got board id %d from NVM", board_id); ESP_LOGI(DEBUG_LINK_TAG, "Successfully got board id %d from NVM", board_id);
nvs_close(handle); nvs_close(handle);
return ESP_OK; return ESP_OK;
} }
/** /**
* @brief Helper function to create a control frame * @brief Helper function to create a control frame
* *
* @param dest_board * @param dest_board
* @param data * @param data
* @param data_len * @param data_len
* @param type * @param type
* @param flag * @param flag
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t DataLinkManager::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 DataLinkManager::create_control_frame(uint8_t* data, uint16_t data_len, ControlFrame control_frame, uint8_t* send_data, size_t* send_data_len){
if (data == nullptr){ if (data == nullptr){
@@ -198,7 +209,7 @@ esp_err_t DataLinkManager::create_control_frame(uint8_t* data, uint16_t data_len
if (data_len > MAX_FRAME_SIZE){ if (data_len > MAX_FRAME_SIZE){
ESP_LOGE(DEBUG_LINK_TAG, "Data for control frame is too large. Maximum size is %d. Current data length is %d", MAX_FRAME_SIZE, data_len); ESP_LOGE(DEBUG_LINK_TAG, "Data for control frame is too large. Maximum size is %d. Current data length is %d", MAX_FRAME_SIZE, data_len);
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
if (send_data == nullptr){ if (send_data == nullptr){
ESP_LOGE(DEBUG_LINK_TAG, "Invalid pointer for send_data"); ESP_LOGE(DEBUG_LINK_TAG, "Invalid pointer for send_data");
@@ -229,7 +240,7 @@ esp_err_t DataLinkManager::create_control_frame(uint8_t* data, uint16_t data_len
send_data[offset++] = control_frame.type_flag; send_data[offset++] = control_frame.type_flag;
send_data[offset++] = data_len; send_data[offset++] = data_len;
send_data[offset++] = (data_len >> 8) & 0xFF; send_data[offset++] = (data_len >> 8) & 0xFF;
memcpy(&send_data[offset], data, data_len); memcpy(&send_data[offset], data, data_len);
offset += control_frame.data_len; offset += control_frame.data_len;
@@ -253,14 +264,14 @@ esp_err_t DataLinkManager::create_control_frame(uint8_t* data, uint16_t data_len
/** /**
* @brief Helper function to create a generic frame * @brief Helper function to create a generic frame
* *
* @param data * @param data
* @param data_len * @param data_len
* @param generic_frame * @param generic_frame
* @param offset * @param offset
* @param send_data * @param send_data
* @param send_data_len * @param send_data_len
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t DataLinkManager::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){ esp_err_t DataLinkManager::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){
if (data == nullptr){ if (data == nullptr){
@@ -276,7 +287,7 @@ esp_err_t DataLinkManager::create_generic_frame(uint8_t* data, uint16_t data_len
if (data_len > MAX_FRAME_SIZE){ if (data_len > MAX_FRAME_SIZE){
ESP_LOGE(DEBUG_LINK_TAG, "Data for generic frame is too large. Maximum size is %d. Current data length is %d", MAX_FRAME_SIZE, data_len); ESP_LOGE(DEBUG_LINK_TAG, "Data for generic frame is too large. Maximum size is %d. Current data length is %d", MAX_FRAME_SIZE, data_len);
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
if (send_data == nullptr){ if (send_data == nullptr){
ESP_LOGE(DEBUG_LINK_TAG, "Invalid pointer for send_data"); ESP_LOGE(DEBUG_LINK_TAG, "Invalid pointer for send_data");
@@ -306,16 +317,16 @@ esp_err_t DataLinkManager::create_generic_frame(uint8_t* data, uint16_t data_len
send_data[send_data_offset++] = (generic_frame.seq_num >> 8) & 0xFF; send_data[send_data_offset++] = (generic_frame.seq_num >> 8) & 0xFF;
send_data[send_data_offset++] = generic_frame.type_flag; send_data[send_data_offset++] = generic_frame.type_flag;
send_data[send_data_offset++] = generic_frame.total_frag & 0xFF; send_data[send_data_offset++] = generic_frame.total_frag & 0xFF;
send_data[send_data_offset++] = (generic_frame.total_frag >> 8) & 0xFF; send_data[send_data_offset++] = (generic_frame.total_frag >> 8) & 0xFF;
send_data[send_data_offset++] = generic_frame.frag_num & 0xFF; send_data[send_data_offset++] = generic_frame.frag_num & 0xFF;
send_data[send_data_offset++] = (generic_frame.frag_num >> 8) & 0xFF; send_data[send_data_offset++] = (generic_frame.frag_num >> 8) & 0xFF;
send_data[send_data_offset++] = data_len; send_data[send_data_offset++] = data_len;
send_data[send_data_offset++] = (data_len >> 8) & 0xFF; send_data[send_data_offset++] = (data_len >> 8) & 0xFF;
memcpy(&send_data[send_data_offset], &data[offset], data_len); memcpy(&send_data[send_data_offset], &data[offset], data_len);
send_data_offset += data_len; send_data_offset += data_len;
@@ -339,22 +350,22 @@ esp_err_t DataLinkManager::create_generic_frame(uint8_t* data, uint16_t data_len
/** /**
* @brief Schedules a frame to be sent via RMT * @brief Schedules a frame to be sent via RMT
* *
* @param dest_board 8 bit ID of the destination board * @param dest_board 8 bit ID of the destination board
* @param data * @param data
* @param data_len Length of the data in bytes * @param data_len Length of the data in bytes
* @param type * @param type
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t DataLinkManager::send(uint8_t dest_board, uint8_t* data, uint16_t data_len, FrameType type, uint8_t flag){ esp_err_t DataLinkManager::send(uint8_t dest_board, std::unique_ptr<std::vector<uint8_t>>&& buffer, FrameType type, uint8_t flag){
bool isControlFrame = IS_CONTROL_FRAME((uint8_t)type); bool isControlFrame = IS_CONTROL_FRAME((uint8_t)type);
if (isControlFrame && data_len > MAX_FRAME_SIZE){ if (isControlFrame && buffer->size() > MAX_FRAME_SIZE){
//Control frames has max data size of MAX_FRAME_SIZE //Control frames has max data size of MAX_FRAME_SIZE
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
if (!isControlFrame && data_len > MAX_GENERIC_NUM_FRAG * MAX_GENERIC_DATA_LEN){ if (!isControlFrame && buffer->size() > MAX_GENERIC_NUM_FRAG * MAX_GENERIC_DATA_LEN){
//Generic frames has max MAX_GENERIC_NUM_FRAG fragments, each max size of MAX_GENERIC_DATA_LEN (data size) //Generic frames has max MAX_GENERIC_NUM_FRAG fragments, each max size of MAX_GENERIC_DATA_LEN (data size)
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
@@ -363,25 +374,15 @@ esp_err_t DataLinkManager::send(uint8_t dest_board, uint8_t* data, uint16_t data
//If broadcasting generic frames, we don't to spam acks to this board //If broadcasting generic frames, we don't to spam acks to this board
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
//save data onto heap
uint8_t* saved_data = (uint8_t*)pvPortMalloc(data_len);
if (saved_data == nullptr){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to malloc in send()");
return ESP_ERR_NO_MEM;
}
memset(saved_data, 0, data_len);
memcpy(saved_data, data, data_len); //copy the contents to the heap
//calculate number of fragments required (for generic frames only) //calculate number of fragments required (for generic frames only)
uint32_t frag_info = 0; uint32_t frag_info = 0;
if (!isControlFrame){ if (!isControlFrame){
if (data_len <= MAX_CONTROL_DATA_LEN){ if (buffer->size() <= MAX_CONTROL_DATA_LEN){
frag_info = (1 << 16); //1 total fragment required frag_info = (1 << 16); //1 total fragment required
} else { } else {
uint32_t total_frags = (data_len + MAX_GENERIC_DATA_LEN - 1) / MAX_GENERIC_DATA_LEN; uint32_t total_frags = (buffer->size() + MAX_GENERIC_DATA_LEN - 1) / MAX_GENERIC_DATA_LEN;
frag_info = (total_frags) << 16; frag_info = (total_frags) << 16;
} }
} }
@@ -401,13 +402,12 @@ esp_err_t DataLinkManager::send(uint8_t dest_board, uint8_t* data, uint16_t data
.seq_num = seq_num, .seq_num = seq_num,
.type_flag = (uint8_t)((static_cast<uint8_t>(type) & 0xF0) | (flag & 0xF)), .type_flag = (uint8_t)((static_cast<uint8_t>(type) & 0xF0) | (flag & 0xF)),
.frag_info = frag_info, .frag_info = frag_info,
.data_len = data_len, .data_len = (uint16_t)buffer->size(),
.crc_16 = 0, .crc_16 = 0,
}, },
.generic_frame_data_offset = 0, .generic_frame_data_offset = 0,
.enqueue_time_ns = 0, .enqueue_time_ns = 0,
.data = saved_data, .data = std::move(buffer),
.len = data_len,
.last_ack = 0, .last_ack = 0,
.curr_fragment = 0, .curr_fragment = 0,
.timeout = 0, .timeout = 0,
@@ -415,10 +415,9 @@ esp_err_t DataLinkManager::send(uint8_t dest_board, uint8_t* data, uint16_t data
uint8_t channel = 0; uint8_t channel = 0;
res = route_frame(dest_board, &channel); res = route_frame(dest_board, &channel);
if (res != ESP_OK){ if (res != ESP_OK){
// ESP_LOGE(DEBUG_LINK_TAG, "Failed to route message to board %d", dest_board); // ESP_LOGE(DEBUG_LINK_TAG, "Failed to route message to board %d", dest_board);
vPortFree(saved_data);
return res; return res;
} }
@@ -426,7 +425,6 @@ esp_err_t DataLinkManager::send(uint8_t dest_board, uint8_t* data, uint16_t data
if (res != ESP_OK){ if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to push frame to scheduler queue"); ESP_LOGE(DEBUG_LINK_TAG, "Failed to push frame to scheduler queue");
vPortFree(saved_data);
} }
return res; return res;
} }
@@ -447,11 +445,11 @@ void DataLinkManager::print_buffer_binary(const uint8_t* buffer, size_t length)
/** /**
* @deprecated This function is deprecated. This is replaced by `async_receive_info` and `async_receive`. This function returns `ESP_FAIL` * @deprecated This function is deprecated. This is replaced by `async_receive_info` and `async_receive`. This function returns `ESP_FAIL`
* *
* @brief Starts the RMT async receive job to start listening for a new frame over a given channel * @brief Starts the RMT async receive job to start listening for a new frame over a given channel
* *
* @param curr_channel * @param curr_channel
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t DataLinkManager::start_receive_frames(uint8_t curr_channel){ esp_err_t DataLinkManager::start_receive_frames(uint8_t curr_channel){
return ESP_FAIL; return ESP_FAIL;
@@ -459,9 +457,9 @@ esp_err_t DataLinkManager::start_receive_frames(uint8_t curr_channel){
/** /**
* @brief Starts the RMT async receive job to start listening for a new frame over a given channel * @brief Starts the RMT async receive job to start listening for a new frame over a given channel
* *
* @param curr_channel * @param curr_channel
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t DataLinkManager::start_receive_frames_rmt(uint8_t curr_channel){ esp_err_t DataLinkManager::start_receive_frames_rmt(uint8_t curr_channel){
if (curr_channel >= num_channels){ if (curr_channel >= num_channels){
@@ -472,29 +470,29 @@ esp_err_t DataLinkManager::start_receive_frames_rmt(uint8_t curr_channel){
/** /**
* @deprecated This function is deprecated. This is replaced by `async_receive_info` and `async_receive`. This function returns `ESP_FAIL` * @deprecated This function is deprecated. This is replaced by `async_receive_info` and `async_receive`. This function returns `ESP_FAIL`
* *
* @brief Receive Control Frame from RMT Physical Layer * @brief Receive Control Frame from RMT Physical Layer
* *
* @param data Byte array * @param data Byte array
* @param data_len Length of the byte array * @param data_len Length of the byte array
* @param recv_len Length of the received data * @param recv_len Length of the received data
* @param curr_channel Physical channel pair to look at * @param curr_channel Physical channel pair to look at
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t DataLinkManager::receive(uint8_t* data, size_t data_len, size_t* recv_len, uint8_t curr_channel){ esp_err_t DataLinkManager::receive(uint8_t* data, size_t data_len, size_t* recv_len, uint8_t curr_channel){
return ESP_FAIL; return ESP_FAIL;
} }
/** /**
* @brief * @brief
* *
* @param data * @param data
* @param data_len * @param data_len
* @param message * @param message
* @param message_size * @param message_size
* @param header * @param header
* @return esp_err_t * @return esp_err_t
* *
* @deprecated * @deprecated
* Will be moved to private function * Will be moved to private function
*/ */
@@ -527,7 +525,7 @@ esp_err_t DataLinkManager::get_data_from_frame(uint8_t* data, size_t data_len, u
} }
header->data_len = (uint16_t)data[6] | ((uint16_t)data[7] << 8); header->data_len = (uint16_t)data[6] | ((uint16_t)data[7] << 8);
if (header->data_len > data_len){ if (header->data_len > data_len){
ESP_LOGE(DEBUG_LINK_TAG, "Mismatch data length in control frame"); ESP_LOGE(DEBUG_LINK_TAG, "Mismatch data length in control frame");
return ESP_ERR_INVALID_RESPONSE; return ESP_ERR_INVALID_RESPONSE;
@@ -546,18 +544,18 @@ esp_err_t DataLinkManager::get_data_from_frame(uint8_t* data, size_t data_len, u
} }
memcpy(message, &data[8], header->data_len); memcpy(message, &data[8], header->data_len);
geneate_crc_16(data, 8*sizeof(uint8_t) + header->data_len, &header->crc_16); geneate_crc_16(data, 8*sizeof(uint8_t) + header->data_len, &header->crc_16);
uint16_t crc_calc = ((uint16_t)data[8 + header->data_len] | ((uint16_t)data[9 + header->data_len] << 8)); uint16_t crc_calc = ((uint16_t)data[8 + header->data_len] | ((uint16_t)data[9 + header->data_len] << 8));
if (crc_calc != header->crc_16){ if (crc_calc != header->crc_16){
//CRC mismatch //CRC mismatch
ESP_LOGE(DEBUG_LINK_TAG, "CRC Mismatch - Control Frame"); ESP_LOGE(DEBUG_LINK_TAG, "CRC Mismatch - Control Frame");
ESP_LOGE(DEBUG_LINK_TAG, "Got 0x%04X but calculated 0x%04X\n", crc_calc, header->crc_16); ESP_LOGE(DEBUG_LINK_TAG, "Got 0x%04X but calculated 0x%04X\n", crc_calc, header->crc_16);
return ESP_ERR_INVALID_CRC; return ESP_ERR_INVALID_CRC;
} }
} else { } else {
//generic frame //generic frame
@@ -578,7 +576,7 @@ esp_err_t DataLinkManager::get_data_from_frame(uint8_t* data, size_t data_len, u
} }
memcpy(message, &data[12], *message_size); memcpy(message, &data[12], *message_size);
if (total_frag != 1){ if (total_frag != 1){
geneate_crc_16(data, 12*sizeof(uint8_t) + *message_size, &header->crc_16); geneate_crc_16(data, 12*sizeof(uint8_t) + *message_size, &header->crc_16);
} else { } else {
@@ -586,7 +584,7 @@ esp_err_t DataLinkManager::get_data_from_frame(uint8_t* data, size_t data_len, u
} }
uint16_t crc_calc = ((uint16_t)data[12 + *message_size] | ((uint16_t)data[13 + *message_size] << 8)); uint16_t crc_calc = ((uint16_t)data[12 + *message_size] | ((uint16_t)data[13 + *message_size] << 8));
if (crc_calc != header->crc_16 && total_frag != 1){ if (crc_calc != header->crc_16 && total_frag != 1){
//CRC mismatch //CRC mismatch
ESP_LOGE(DEBUG_LINK_TAG, "CRC Mismatch - Generic Frame"); ESP_LOGE(DEBUG_LINK_TAG, "CRC Mismatch - Generic Frame");
@@ -594,7 +592,7 @@ esp_err_t DataLinkManager::get_data_from_frame(uint8_t* data, size_t data_len, u
return ESP_ERR_INVALID_CRC; return ESP_ERR_INVALID_CRC;
} }
} }
// printf("Received Frame Information:\n"); // printf("Received Frame Information:\n");
// printf("%-10s %-12s %-13s %-15s %-12s %-10s %-6s\n", // printf("%-10s %-12s %-13s %-15s %-12s %-10s %-6s\n",
// "Preamble", "Sender ID", "Receiver ID", "Sequence Num", "Type+Flag", "Data Len", "CRC"); // "Preamble", "Sender ID", "Receiver ID", "Sequence Num", "Type+Flag", "Data Len", "CRC");
@@ -609,11 +607,11 @@ esp_err_t DataLinkManager::get_data_from_frame(uint8_t* data, size_t data_len, u
/** /**
* @brief This function implements the CRC-16/CCITT algorithm * @brief This function implements the CRC-16/CCITT algorithm
* *
* @param data * @param data
* @param data_len * @param data_len
* @param crc * @param crc
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t DataLinkManager::geneate_crc_16(uint8_t* data, size_t data_len, uint16_t* crc){ esp_err_t DataLinkManager::geneate_crc_16(uint8_t* data, size_t data_len, uint16_t* crc){
if (data == nullptr){ if (data == nullptr){
@@ -625,27 +623,27 @@ esp_err_t DataLinkManager::geneate_crc_16(uint8_t* data, size_t data_len, uint16
} }
*crc = 0x0; *crc = 0x0;
for (size_t i = 0; i < data_len; i++){ for (size_t i = 0; i < data_len; i++){
uint8_t tbl_idx = (*crc >> 8) ^ data[i]; uint8_t tbl_idx = (*crc >> 8) ^ data[i];
*crc = (*crc << 8) ^ crc16_table[tbl_idx]; *crc = (*crc << 8) ^ crc16_table[tbl_idx];
} }
return ESP_OK; return ESP_OK;
} }
/** /**
* @brief Prints to console the encoded frame information from a byte array recevied from RMT * @brief Prints to console the encoded frame information from a byte array recevied from RMT
* *
* @note Should only be used for debug purposes * @note Should only be used for debug purposes
* *
* @warning This function may not be reliable/buggy * @warning This function may not be reliable/buggy
* *
* @param data * @param data
* @param data_len * @param data_len
* @param message * @param message
* @param message_len * @param message_len
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t DataLinkManager::print_frame_info(uint8_t* data, size_t data_len, uint8_t* message, size_t message_len){ esp_err_t DataLinkManager::print_frame_info(uint8_t* data, size_t data_len, uint8_t* message, size_t message_len){
// printf("Received frame of size %d:\n", data_len); // printf("Received frame of size %d:\n", data_len);

100
components/dataLink/DataLinkRIP.cpp
View File

@@ -4,7 +4,7 @@
/** /**
* @brief Initializes the RIP table * @brief Initializes the RIP table
* *
*/ */
void DataLinkManager::init_rip(){ void DataLinkManager::init_rip(){
for (size_t i = 0; i < RIP_MAX_ROUTES; i++){ for (size_t i = 0; i < RIP_MAX_ROUTES; i++){
@@ -54,11 +54,11 @@ esp_err_t DataLinkManager::rip_add_entry(uint8_t board_id, uint8_t hops, uint8_t
(*entry)->ttl = RIP_TTL_START; (*entry)->ttl = RIP_TTL_START;
(*entry)->valid = 1; (*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); // 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); xSemaphoreGive((*entry)->row_sem);
if (uxQueueMessagesWaiting(manual_broadcasts) == 0){ if (uxQueueMessagesWaiting(manual_broadcasts) == 0){
bool dummy = true; bool dummy = true;
xQueueSend(manual_broadcasts, &dummy, 0); //new row - send broadcast xQueueSend(manual_broadcasts, &dummy, 0); //new row - send broadcast
@@ -108,9 +108,9 @@ esp_err_t DataLinkManager::rip_update_entry(uint8_t new_hop, uint8_t channel, RI
(*entry)->channel = channel; (*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); // 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)->ttl = RIP_TTL_START;
(*entry)->valid = 1; (*entry)->valid = 1;
// ESP_LOGI(DEBUG_LINK_TAG, "refreshed board_id %d ttl", (*entry)->info.board_id); // ESP_LOGI(DEBUG_LINK_TAG, "refreshed board_id %d ttl", (*entry)->info.board_id);
@@ -128,10 +128,10 @@ esp_err_t DataLinkManager::rip_update_entry(uint8_t new_hop, uint8_t channel, RI
/** /**
* @brief Finds the board_id in the table if it exists and stores that row in `entry` * @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? * TODO: use an unordered map instead of an array?
* *
* @param board_id * @param board_id
* @param entry * @param entry
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t DataLinkManager::rip_find_entry(uint8_t board_id, RIPRow** entry, bool reserve_row = false){ esp_err_t DataLinkManager::rip_find_entry(uint8_t board_id, RIPRow** entry, bool reserve_row = false){
RIPRow* free_slot = nullptr; RIPRow* free_slot = nullptr;
@@ -144,7 +144,7 @@ esp_err_t DataLinkManager::rip_find_entry(uint8_t board_id, RIPRow** entry, bool
xSemaphoreGive(rip_table[i].row_sem); xSemaphoreGive(rip_table[i].row_sem);
// ESP_LOGI(DEBUG_LINK_TAG, "Found %d in table at row %d", board_id, i); // ESP_LOGI(DEBUG_LINK_TAG, "Found %d in table at row %d", board_id, i);
return ESP_OK; return ESP_OK;
} }
if (rip_table[i].valid == RIP_INVALID_ROW && free_slot == nullptr){ if (rip_table[i].valid == RIP_INVALID_ROW && free_slot == nullptr){
free_slot = &rip_table[i]; free_slot = &rip_table[i];
} }
@@ -176,10 +176,10 @@ esp_err_t DataLinkManager::rip_find_entry(uint8_t board_id, RIPRow** entry, bool
/** /**
* @brief Returns the associated RIP Table row by row number. Information returned is read only. * @brief Returns the associated RIP Table row by row number. Information returned is read only.
* *
* @param entry * @param entry
* @param row_num * @param row_num
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t DataLinkManager::rip_get_row(RIPRow** entry, uint8_t row_num){ esp_err_t DataLinkManager::rip_get_row(RIPRow** entry, uint8_t row_num){
if (entry == nullptr){ if (entry == nullptr){
@@ -216,18 +216,16 @@ esp_err_t DataLinkManager::rip_get_row(RIPRow** entry, uint8_t row_num){
/** /**
* @brief Sends RIP frame * @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 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`) * @param dest_id Destination board (requesting board) to send the rip table to (ignored if `broadcast is true`)
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){ 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) //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), ...] //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; uint16_t message_idx = 0;
esp_err_t res; esp_err_t res;
RIPRow* entry = nullptr; RIPRow* entry = nullptr;
@@ -235,43 +233,40 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
if(broadcast){ if(broadcast){
for (size_t channel = 0; channel < num_channels; channel++){ for (size_t channel = 0; channel < num_channels; channel++){
auto rip_message = std::make_unique<std::vector<uint8_t>>();
rip_message->resize(RIP_MAX_ROUTES * 2);
for (size_t i = 0; i < RIP_MAX_ROUTES; i++){ for (size_t i = 0; i < RIP_MAX_ROUTES; i++){
res = rip_get_row(&entry, i); res = rip_get_row(&entry, i);
if (res != ESP_OK){ if (res != ESP_OK){
continue; continue;
} }
if (entry == nullptr){ if (entry == nullptr){
continue; continue;
} }
// ESP_LOGI(DEBUG_LINK_TAG, "Found entry for board %d with hops %d", entry->info.board_id, entry->info.hops); // ESP_LOGI(DEBUG_LINK_TAG, "Found entry for board %d with hops %d", entry->info.board_id, entry->info.hops);
if (entry->channel == channel){ if (entry->channel == channel){
//poisoned reverse //poisoned reverse
rip_message[message_idx++] = entry->info.board_id; rip_message->at(message_idx++) = entry->info.board_id;
rip_message[message_idx++] = RIP_MAX_HOPS + 1; rip_message->at(message_idx++) = RIP_MAX_HOPS + 1;
} else { } else {
rip_message[message_idx++] = entry->info.board_id; rip_message->at(message_idx++) = entry->info.board_id;
rip_message[message_idx++] = entry->info.hops; rip_message->at(message_idx++) = entry->info.hops;
} }
} }
uint8_t* send_data = (uint8_t*)pvPortMalloc(message_idx); rip_message->resize(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);
res = get_inc_sequence_num(BROADCAST_ADDR, &seq_num); res = get_inc_sequence_num(BROADCAST_ADDR, &seq_num);
if (res != ESP_OK){ if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed atomic get increment sequence number map"); ESP_LOGE(DEBUG_LINK_TAG, "Failed atomic get increment sequence number map");
return res; return res;
} }
SchedulerMetadata metadata = { SchedulerMetadata metadata = {
.header = { .header = {
.preamble = START_OF_FRAME, .preamble = START_OF_FRAME,
@@ -284,8 +279,7 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
}, },
.generic_frame_data_offset = 0, .generic_frame_data_offset = 0,
.enqueue_time_ns = 0, .enqueue_time_ns = 0,
.data = send_data, .data = std::move(rip_message),
.len = message_idx,
.last_ack = 0, .last_ack = 0,
.curr_fragment = 0, .curr_fragment = 0,
.timeout = 0, .timeout = 0,
@@ -296,9 +290,11 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule rip frame from send_rip_frame for channel %d", channel); ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule rip frame from send_rip_frame for channel %d", channel);
} }
message_idx = 0; message_idx = 0;
memset(rip_message, 0, sizeof(rip_message));
} }
} else { } else {
auto rip_message = std::make_unique<std::vector<uint8_t>>();
rip_message->resize(RIP_MAX_ROUTES * 2);
for (size_t i = 0; i < RIP_MAX_ROUTES; i++){ for (size_t i = 0; i < RIP_MAX_ROUTES; i++){
res = rip_get_row(&entry, i); res = rip_get_row(&entry, i);
@@ -309,11 +305,11 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
if (entry == nullptr){ if (entry == nullptr){
continue; continue;
} }
rip_message[message_idx++] = entry->info.board_id; rip_message->data()[message_idx++] = entry->info.board_id;
rip_message[message_idx++] = entry->info.hops; rip_message->data()[message_idx++] = entry->info.hops;
} }
ESP_LOGI(DEBUG_LINK_TAG, "replying to discovery request to board %d", dest_id); 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); res = send(dest_id, std::move(rip_message), FrameType::RIP_TABLE_CONTROL, FLAG_DISCOVERY);
if (res != ESP_OK){ if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to send rip frame from send_rip_frame"); ESP_LOGE(DEBUG_LINK_TAG, "Failed to send rip frame from send_rip_frame");
} }
@@ -324,10 +320,10 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
/** /**
* @brief Determines which channel to route the frame to, depending on the dest (board) id * @brief Determines which channel to route the frame to, depending on the dest (board) id
* *
* @param dest_id * @param dest_id
* @param channel_to_send * @param channel_to_send
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t DataLinkManager::route_frame(uint8_t dest_id, uint8_t* channel_to_send){ esp_err_t DataLinkManager::route_frame(uint8_t dest_id, uint8_t* channel_to_send){
RIPRow* entry = nullptr; RIPRow* entry = nullptr;
@@ -350,12 +346,12 @@ esp_err_t DataLinkManager::route_frame(uint8_t dest_id, uint8_t* channel_to_send
/** /**
* @brief Fetches the current routing table at the perspective of the host board * @brief Fetches the current routing table at the perspective of the host board
* *
* @details The routing table is based off of RIP * @details The routing table is based off of RIP
* *
* @param table * @param table
* @param table_size * @param table_size
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t DataLinkManager::get_routing_table(RIPRow_public* table, size_t* table_size){ esp_err_t DataLinkManager::get_routing_table(RIPRow_public* table, size_t* table_size){
if (table == nullptr){ if (table == nullptr){
@@ -383,7 +379,7 @@ esp_err_t DataLinkManager::get_routing_table(RIPRow_public* table, size_t* table
table[i].info = rip_table[i].info; table[i].info = rip_table[i].info;
table[i].channel = rip_table[i].channel; table[i].channel = rip_table[i].channel;
curr_size++; curr_size++;
} }
xSemaphoreGive(rip_table[i].row_sem); xSemaphoreGive(rip_table[i].row_sem);
} }
@@ -446,7 +442,7 @@ esp_err_t DataLinkManager::get_routing_table(RIPRow_public* table, size_t* table
link_layer_obj->rip_table[i].ttl_flush = RIP_FLUSH_COUNT; link_layer_obj->rip_table[i].ttl_flush = RIP_FLUSH_COUNT;
broadcast = true; broadcast = true;
} }
} }
xSemaphoreGive(link_layer_obj->rip_table[i].row_sem); xSemaphoreGive(link_layer_obj->rip_table[i].row_sem);
} }
@@ -472,4 +468,4 @@ void DataLinkManager::start_rip_tasks(){
xTaskCreate(DataLinkManager::rip_broadcast_timer_function, "RIPBroadcast", 4096, static_cast<void*>(this), 5, &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"); ESP_LOGI(DEBUG_LINK_TAG, "Starting RIP TTL task");
xTaskCreate(DataLinkManager::rip_ttl_decrement_task, "RIPTTL", 4096, static_cast<void*>(this), 5, &rip_ttl_task); xTaskCreate(DataLinkManager::rip_ttl_decrement_task, "RIPTTL", 4096, static_cast<void*>(this), 5, &rip_ttl_task);
} }

85
components/dataLink/DataLinkScheduler.cpp
View File

@@ -1,20 +1,30 @@
#include <chrono>
#include "DataLinkManager.h" #include "DataLinkManager.h"
#include "esp_log.h" #include "esp_log.h"
#include "esp_timer.h" #include "esp_timer.h"
#include "freertos/projdefs.h"
#include "freertos/semphr.h" #include "freertos/semphr.h"
#include "esp_random.h" #include "esp_random.h"
#include "portmacro.h"
#define FRAME_DEQUEUE_TIMEOUT_MS 2000
#define FRAME_ENQUEUE_TIMEOUT_MS 50
void DataLinkManager::init_scheduler(){ void DataLinkManager::init_scheduler(){
for (int i = 0; i < num_channels; i++){ for (int i = 0; i < num_channels; i++){
sq_handle[i] = xSemaphoreCreateMutex();
async_rx_queue_mutex[i] = xSemaphoreCreateMutex(); async_rx_queue_mutex[i] = xSemaphoreCreateMutex();
rx_fragment_mutex[i] = xSemaphoreCreateMutex(); rx_fragment_mutex[i] = xSemaphoreCreateMutex();
sliding_window_mutex[i] = xSemaphoreCreateMutex(); sliding_window_mutex[i] = xSemaphoreCreateMutex();
send_ack_queue_mutex[i] = xSemaphoreCreateMutex(); send_ack_queue_mutex[i] = xSemaphoreCreateMutex();
ESP_LOGI(DEBUG_LINK_TAG, "Starting Frame Scheduler task for channel %d", i);
auto args = (frame_scheduler_args*)malloc(sizeof(frame_scheduler_args));
args->channel_id = i;
args->that = this;
xTaskCreate(DataLinkManager::frame_scheduler, "Scheduler", 4096, static_cast<void*>(args), 4, &scheduler_task);
} }
ESP_LOGI(DEBUG_LINK_TAG, "Starting Frame Scheduler task");
xTaskCreate(DataLinkManager::frame_scheduler, "Scheduler", 4096, static_cast<void*>(this), 4, &scheduler_task);
xTaskCreate(DataLinkManager::receive_thread_main, "Receiver", 8192, static_cast<void*>(this), 5, &receive_task); xTaskCreate(DataLinkManager::receive_thread_main, "Receiver", 8192, static_cast<void*>(this), 5, &receive_task);
xTaskCreate(DataLinkManager::send_ack_thread_main, "Send ACKs", 8192, static_cast<void*>(this), 5, &send_ack_task); xTaskCreate(DataLinkManager::send_ack_thread_main, "Send ACKs", 8192, static_cast<void*>(this), 5, &send_ack_task);
} }
@@ -29,7 +39,10 @@ void DataLinkManager::init_scheduler(){
* Scheduling may change (above scheduler will lead to starvation of control frames depending on the number of generic frames/fragments to send) * Scheduling may change (above scheduler will lead to starvation of control frames depending on the number of generic frames/fragments to send)
*/ */
[[noreturn]] void DataLinkManager::frame_scheduler(void* args){ [[noreturn]] void DataLinkManager::frame_scheduler(void* args){
DataLinkManager* link_layer_obj = static_cast<DataLinkManager*>(args); const auto parsed_args = static_cast<frame_scheduler_args*>(args);
uint8_t channel = parsed_args->channel_id;
DataLinkManager* link_layer_obj = parsed_args->that;
if (link_layer_obj == nullptr){ if (link_layer_obj == nullptr){
ESP_LOGE(DEBUG_LINK_TAG, "Frame Scheduler failed to start due to invalid pointer"); ESP_LOGE(DEBUG_LINK_TAG, "Frame Scheduler failed to start due to invalid pointer");
vTaskDelete(nullptr); vTaskDelete(nullptr);
@@ -37,12 +50,9 @@ void DataLinkManager::init_scheduler(){
ESP_LOGI(DEBUG_LINK_TAG, "Starting Frame Scheduler task"); ESP_LOGI(DEBUG_LINK_TAG, "Starting Frame Scheduler task");
while(!link_layer_obj->stop_tasks){ while(!link_layer_obj->stop_tasks){
for (uint8_t i = 0; i < link_layer_obj->num_channels; i++){ link_layer_obj->scheduler_send(channel);
link_layer_obj->scheduler_send(i);
}
vTaskDelay(pdMS_TO_TICKS(SCHEDULER_PERIOD_MS));
} }
free(args);
vTaskDelete(nullptr); vTaskDelete(nullptr);
} }
@@ -64,7 +74,7 @@ esp_err_t DataLinkManager::push_frame_to_scheduler(SchedulerMetadata frame, uint
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
if (frame.len == 0){ if (frame.data->size() == 0){
ESP_LOGE(DEBUG_LINK_TAG, "Invalid Frame Length"); ESP_LOGE(DEBUG_LINK_TAG, "Invalid Frame Length");
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
@@ -73,19 +83,7 @@ esp_err_t DataLinkManager::push_frame_to_scheduler(SchedulerMetadata frame, uint
int64_t now = esp_timer_get_time(); int64_t now = esp_timer_get_time();
frame.enqueue_time_ns = now; frame.enqueue_time_ns = now;
if (sq_handle[channel] == nullptr){ frame_queue[channel]->enqueue(std::move(frame), std::chrono::milliseconds(FRAME_ENQUEUE_TIMEOUT_MS));
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); // ESP_LOGI(DEBUG_LINK_TAG, "Pushed frame to queue on channel %d", channel);
@@ -103,25 +101,15 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
if (sq_handle[channel] == nullptr){ vTaskDelay(pdMS_TO_TICKS(10)); // the messages cannot be too close together
return ESP_FAIL;
}
SchedulerMetadata frame; SchedulerMetadata frame;
if (xSemaphoreTake(sq_handle[channel], pdMS_TO_TICKS(SCHEDULER_MUTEX_WAIT)) == pdTRUE){ if (auto maybe_frame = frame_queue[channel]->dequeue(std::chrono::milliseconds(FRAME_DEQUEUE_TIMEOUT_MS))) {
if (frame_queue[channel].empty()){ frame = *maybe_frame;
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 { } else {
ESP_LOGE(DEBUG_LINK_TAG, "Failed to get mutex when trying to send"); // ESP_LOGI(DEBUG_LINK_TAG, "Scheduler queue for channel %d is empty", channel);
//Failed to obtain mutex return ESP_OK;
return ESP_ERR_TIMEOUT;
} }
if (frame.data == nullptr){ if (frame.data == nullptr){
@@ -131,7 +119,6 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
if (this_board_id == PC_ADDR){ if (this_board_id == PC_ADDR){
ESP_LOGE(DEBUG_LINK_TAG, "This board is not assigned a board id"); ESP_LOGE(DEBUG_LINK_TAG, "This board is not assigned a board id");
vPortFree(frame.data);
return ESP_ERR_INVALID_ARG; return ESP_ERR_INVALID_ARG;
} }
@@ -144,11 +131,9 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
if (isControlFrame){ if (isControlFrame){
//control frame //control frame
res = create_control_frame(frame.data, frame.len, res = create_control_frame(frame.data->data(), frame.data->size(),
make_control_frame_from_header(frame.header), send_data, &frame_size); make_control_frame_from_header(frame.header), send_data, &frame_size);
vPortFree(frame.data);
if (res != ESP_OK){ if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to create control frame"); ESP_LOGE(DEBUG_LINK_TAG, "Failed to create control frame");
return res; return res;
@@ -158,7 +143,7 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
return scheduler_send_rmt(channel, frame, send_data, frame_size, false); return scheduler_send_rmt(channel, frame, send_data, frame_size, false);
} else { } else {
//generic frame //generic frame
if (frame.len > (MAX_GENERIC_DATA_LEN)){ if (frame.data->size() > (MAX_GENERIC_DATA_LEN)){
//fragment here //fragment here
if (frame.timeout == 0){ if (frame.timeout == 0){
@@ -168,7 +153,6 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
res = push_frame_to_scheduler(frame, channel); res = push_frame_to_scheduler(frame, channel);
if (res != ESP_OK){ if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment"); ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment");
vPortFree(frame.data);
return res; return res;
} }
return ESP_OK; return ESP_OK;
@@ -184,7 +168,6 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
if (res != ESP_OK){ if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to get sliding window ack record for board id %d seq num %d", frame.header.receiver_id, frame.header.seq_num); ESP_LOGE(DEBUG_LINK_TAG, "Failed to get sliding window ack record for board id %d seq num %d", frame.header.receiver_id, frame.header.seq_num);
vPortFree(frame.data);
return res; return res;
} }
@@ -206,7 +189,6 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
//all acks received, can simply exit //all acks received, can simply exit
// ESP_LOGI(DEBUG_LINK_TAG, "All acks recevied for board id %d seq num %d", frame.header.receiver_id, frame.header.seq_num); // ESP_LOGI(DEBUG_LINK_TAG, "All acks recevied for board id %d seq num %d", frame.header.receiver_id, frame.header.seq_num);
complete_record_sliding_window(channel, frame.header.receiver_id, frame.header.seq_num); complete_record_sliding_window(channel, frame.header.receiver_id, frame.header.seq_num);
vPortFree(frame.data);
return ESP_OK; return ESP_OK;
} }
@@ -228,7 +210,7 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
if (frame.curr_fragment != (frame.header.frag_info >> 16)) { if (frame.curr_fragment != (frame.header.frag_info >> 16)) {
fragment_size = MAX_GENERIC_DATA_LEN; fragment_size = MAX_GENERIC_DATA_LEN;
} else { } else {
fragment_size = frame.len - (MAX_GENERIC_DATA_LEN * (frame.curr_fragment-1)); fragment_size = frame.data->size() - (MAX_GENERIC_DATA_LEN * (frame.curr_fragment-1));
} }
uint16_t curr_offset = MAX_GENERIC_DATA_LEN * (frame.curr_fragment - 1); uint16_t curr_offset = MAX_GENERIC_DATA_LEN * (frame.curr_fragment - 1);
@@ -238,12 +220,11 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
frame.header.frag_info = (frame.header.frag_info & 0xFFFF0000) | frame.curr_fragment; //increment frag_num frame.header.frag_info = (frame.header.frag_info & 0xFFFF0000) | frame.curr_fragment; //increment frag_num
//create fragment //create fragment
res = create_generic_frame(frame.data, fragment_size, res = create_generic_frame(frame.data->data(), fragment_size,
make_generic_frame_from_header(frame.header), curr_offset, send_data, &frame_size); make_generic_frame_from_header(frame.header), curr_offset, send_data, &frame_size);
if (res != ESP_OK){ if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to create generic frame fragment"); ESP_LOGE(DEBUG_LINK_TAG, "Failed to create generic frame fragment");
vPortFree(frame.data);
return res; return res;
} }
@@ -254,7 +235,6 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
res = push_frame_to_scheduler(frame, channel); res = push_frame_to_scheduler(frame, channel);
if (res != ESP_OK){ if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment"); ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment");
vPortFree(frame.data);
} }
return res; return res;
} }
@@ -267,20 +247,17 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
res = push_frame_to_scheduler(frame, channel); res = push_frame_to_scheduler(frame, channel);
if (res != ESP_OK){ if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment"); ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment");
vPortFree(frame.data);
return res; return res;
} }
} else { } else {
//Done fragmenting, can free data array //Done fragmenting, can free data array
// ESP_LOGI(DEBUG_LINK_TAG, "finished fragmenting seq num %d frag_info 0x%X", frame.header.seq_num, frame.header.frag_info); // ESP_LOGI(DEBUG_LINK_TAG, "finished fragmenting seq num %d frag_info 0x%X", frame.header.seq_num, frame.header.frag_info);
vPortFree(frame.data);
} }
} else { } else {
//no fragmenting //no fragmenting
res = create_generic_frame(frame.data, frame.len, res = create_generic_frame(frame.data->data(), frame.data->size(),
make_generic_frame_from_header(frame.header), 0, send_data, &frame_size); make_generic_frame_from_header(frame.header), 0, send_data, &frame_size);
vPortFree(frame.data);
if (res != ESP_OK){ if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to create generic frame"); ESP_LOGE(DEBUG_LINK_TAG, "Failed to create generic frame");

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

@@ -11,6 +11,8 @@
#include "Frames.h" #include "Frames.h"
#include "Tables.h" #include "Tables.h"
#include "RMTManager.h" #include "RMTManager.h"
#include "BlockingQueue.h"
#include "BlockingPriorityQueue.h"
#include <unordered_map> #include <unordered_map>
#include "Scheduler.h" #include "Scheduler.h"
@@ -23,43 +25,43 @@ static const char* NVS_BOARD_NAMESPACE = "board";
//look up table for crc //look up table for crc
static const uint16_t crc16_table[256] = { static const uint16_t crc16_table[256] = {
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, 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, 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, 0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49, 0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78, 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067, 0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, 0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3, 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, 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, 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 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
}; };
#define ASYNC_QUEUE_WAIT_TICKS 100 #define ASYNC_QUEUE_WAIT_TICKS 100
#define SEQUENCE_NUM_MAP_MUTEX_MAX_WAIT_MS 50 #define SEQUENCE_NUM_MAP_MUTEX_MAX_WAIT_MS 50
#define MAX_RX_QUEUE_SIZE 100
/** /**
* @brief Class to represent the Data Link Layer * @brief Class to represent the Data Link Layer
* *
* @author Justin Chow * @author Justin Chow
*/ */
class DataLinkManager{ class DataLinkManager{
public: public:
DataLinkManager(uint8_t board_id, uint8_t num_channels); DataLinkManager(uint8_t board_id, uint8_t num_channels);
~DataLinkManager(); ~DataLinkManager();
esp_err_t send(uint8_t dest_board, uint8_t* data, uint16_t data_len, FrameType type, uint8_t flag); esp_err_t send(uint8_t dest_board, std::unique_ptr<std::vector<uint8_t>>&& buffer, FrameType type, uint8_t flag);
esp_err_t start_receive_frames(uint8_t curr_channel); 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 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, size_t message_len); 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 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); std::optional<std::unique_ptr<std::vector<uint8_t>>> async_receive();
esp_err_t async_receive(uint8_t* data, uint16_t data_len, FrameHeader* header, uint8_t channel);
esp_err_t ready(); esp_err_t ready();
esp_err_t send_ack(uint8_t sender_id, uint8_t* data, uint16_t data_len); esp_err_t send_ack(uint8_t sender_id, uint8_t* data, uint16_t data_len);
private: private:
@@ -75,7 +77,7 @@ class DataLinkManager{
volatile bool stop_tasks = false; //used by the tasks to know when to stop (set true when DataLinkManager is destroyed) 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_broadcast_task = NULL;
TaskHandle_t rip_ttl_task = NULL; TaskHandle_t rip_ttl_task = NULL;
esp_err_t set_board_id(uint8_t board_id); esp_err_t set_board_id(uint8_t board_id);
esp_err_t get_board_id(uint8_t& board_id); esp_err_t get_board_id(uint8_t& board_id);
void print_binary(uint8_t byte); void print_binary(uint8_t byte);
@@ -84,7 +86,7 @@ class DataLinkManager{
esp_err_t geneate_crc_16(uint8_t* data, size_t data_len, uint16_t* crc); 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_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); 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 ==== //==== RIP related functions ====
void init_rip(); void init_rip();
@@ -93,29 +95,28 @@ class DataLinkManager{
esp_err_t rip_add_entry(uint8_t board_id, uint8_t hops, 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_reset_entry_ttl(uint8_t board_id);
esp_err_t rip_get_row(RIPRow** entry, uint8_t row_num); esp_err_t rip_get_row(RIPRow** entry, uint8_t row_num);
//this is stored locally with metadata `ttl` //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 // 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
RIPRow rip_table[RIP_MAX_ROUTES]; //temp using a static array RIPRow rip_table[RIP_MAX_ROUTES]; //temp using a static array
void start_rip_tasks(); void start_rip_tasks();
esp_err_t send_rip_frame(bool broadcast, uint8_t dest_id); esp_err_t send_rip_frame(bool broadcast, uint8_t dest_id);
[[noreturn]] static void rip_broadcast_timer_function(void* args); [[noreturn]] static void rip_broadcast_timer_function(void* args);
[[noreturn]] static void rip_ttl_decrement_task(void* args); [[noreturn]] static void rip_ttl_decrement_task(void* args);
QueueHandle_t manual_broadcasts; QueueHandle_t manual_broadcasts;
QueueHandle_t discovery_tables; QueueHandle_t discovery_tables;
esp_err_t route_frame(uint8_t dest_id, uint8_t* channel_to_send); esp_err_t route_frame(uint8_t dest_id, uint8_t* channel_to_send);
//==== Frame Scheduling related functions ==== //==== Frame Scheduling related functions ====
/** /**
* @brief Priority queue for each channel to schedule when to send frames * @brief Priority queue for each channel to schedule when to send frames
* *
*/ */
std::priority_queue<SchedulerMetadata, std::vector<SchedulerMetadata>, FrameCompare> frame_queue[MAX_CHANNELS]; std::unique_ptr<BlockingPriorityQueue<SchedulerMetadata, std::vector<SchedulerMetadata>, FrameCompare>> frame_queue[MAX_CHANNELS];
SemaphoreHandle_t sq_handle[MAX_CHANNELS];
void init_scheduler(); void init_scheduler();
esp_err_t push_frame_to_scheduler(SchedulerMetadata frame, uint8_t channel); esp_err_t push_frame_to_scheduler(SchedulerMetadata frame, uint8_t channel);
TaskHandle_t scheduler_task = NULL; TaskHandle_t scheduler_task = NULL;
@@ -132,13 +133,13 @@ class DataLinkManager{
/** /**
* @brief Stores generic frame fragments * @brief Stores generic frame fragments
* *
* Mapping: * Mapping:
* Board ID (of the receiver) -> Sequence number -> Array of Generic Frame Fragments, with size of the number of expected fragments * Board ID (of the receiver) -> Sequence number -> Array of Generic Frame Fragments, with size of the number of expected fragments
* *
* TODO: * TODO:
* - Sliding window + ACKs * - Sliding window + ACKs
* *
*/ */
std::unordered_map<uint16_t, std::unordered_map<uint16_t, FragmentMetadata>> fragment_map[MAX_CHANNELS]; std::unordered_map<uint16_t, std::unordered_map<uint16_t, FragmentMetadata>> fragment_map[MAX_CHANNELS];
@@ -150,26 +151,26 @@ class DataLinkManager{
//Async receive //Async receive
/** /**
* @brief Queue to store complete received frame data * @brief Queue to store complete received frame data
* *
*/ */
std::queue<Rx_Metadata> async_receive_queue[MAX_CHANNELS]; std::unique_ptr<BlockingQueue<Rx_Metadata>> async_receive_queue;
esp_err_t start_receive_frames_rmt(uint8_t curr_channel); esp_err_t start_receive_frames_rmt(uint8_t curr_channel);
/** /**
* @brief Receive thread entry point * @brief Receive thread entry point
* *
* @param args * @param args
*/ */
[[noreturn]] static void receive_thread_main(void* args); [[noreturn]] static void receive_thread_main(void* args);
/** /**
* @brief Receive bytes from Physical Layer (RMT) * @brief Receive bytes from Physical Layer (RMT)
* *
* @note This replaces the deprecated `receive` function * @note This replaces the deprecated `receive` function
* *
* @param channel Physical channel pair to look at * @param channel Physical channel pair to look at
* @return esp_err_t * @return esp_err_t
*/ */
esp_err_t receive_rmt(uint8_t channel); esp_err_t receive_rmt(uint8_t channel);
@@ -177,10 +178,10 @@ class DataLinkManager{
/** /**
* @brief Generic Frame Sliding Window * @brief Generic Frame Sliding Window
* *
* Mapping: * Mapping:
* Board Id (of the receiver) -> Sequence Number -> FrameAckRecord * Board Id (of the receiver) -> Sequence Number -> FrameAckRecord
* *
*/ */
std::unordered_map<uint16_t, std::unordered_map<uint16_t, FrameAckRecord>> sliding_window[MAX_CHANNELS]; std::unordered_map<uint16_t, std::unordered_map<uint16_t, FrameAckRecord>> sliding_window[MAX_CHANNELS];
@@ -194,8 +195,8 @@ class DataLinkManager{
/** /**
* @brief Thread for sending acks - Send ACKs on a separate thread to not hold up the receive thread (missing other frames) * @brief Thread for sending acks - Send ACKs on a separate thread to not hold up the receive thread (missing other frames)
* *
* @param args * @param args
*/ */
[[noreturn]] static void send_ack_thread_main(void* args); [[noreturn]] static void send_ack_thread_main(void* args);
TaskHandle_t send_ack_task = NULL; TaskHandle_t send_ack_task = NULL;
@@ -204,4 +205,9 @@ class DataLinkManager{
std::queue<SendAckMetaData> send_ack_queue[MAX_CHANNELS]; std::queue<SendAckMetaData> send_ack_queue[MAX_CHANNELS];
}; };
struct frame_scheduler_args {
uint8_t channel_id;
DataLinkManager* that;
};
#endif //DATA_LINK #endif //DATA_LINK

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

@@ -46,7 +46,7 @@ enum class FrameType : uint8_t {
DISTANCE_SENSOR_TYPE = 0xA0, //0b1010_0000 DISTANCE_SENSOR_TYPE = 0xA0, //0b1010_0000
SERVO_TYPE = 0xC0, //0b1100_0000 SERVO_TYPE = 0xC0, //0b1100_0000
MISC_CONTROL_TYPE = 0xD0, //0b1101_0000 MISC_CONTROL_TYPE = 0xD0, //0b1101_0000
//Generic Frames //Generic Frames
MISC_GENERIC_TYPE = 0x00, //0b0000_0000 MISC_GENERIC_TYPE = 0x00, //0b0000_0000
MISC_UDP_GENERIC_TYPE = 0x10, // 0b0001_0000 - Same as MISC_GENERIC_TYPE except no ACK frames will be expected MISC_UDP_GENERIC_TYPE = 0x10, // 0b0001_0000 - Same as MISC_GENERIC_TYPE except no ACK frames will be expected
@@ -98,7 +98,7 @@ typedef struct _header{
using Frame = std::variant<ControlFrame, GenericFrame>; using Frame = std::variant<ControlFrame, GenericFrame>;
ControlFrame make_control_frame_from_header(const FrameHeader& header); ControlFrame make_control_frame_from_header(const FrameHeader& header);
GenericFrame make_generic_frame_from_header(const FrameHeader& header); GenericFrame make_generic_frame_from_header(const FrameHeader& header);
@@ -108,9 +108,9 @@ typedef struct _fragment_metadata {
} FragmentMetadata; } FragmentMetadata;
typedef struct _receive_metadata{ typedef struct _receive_metadata{
uint8_t* data; std::unique_ptr<std::vector<uint8_t>> data;
uint16_t data_len; uint16_t data_len;
FrameHeader header; FrameHeader header;
} Rx_Metadata; } Rx_Metadata;
#endif //DATA_LINK #endif //DATA_LINK

28
components/dataLink/include/Scheduler.h
View File

@@ -4,7 +4,7 @@
#include <cstdint> #include <cstdint>
#define SCHEDULER_MUTEX_WAIT 10 //max time duration to wait #define SCHEDULER_MUTEX_WAIT 10 //max time duration to wait
#define SCHEDULER_PERIOD_MS 140 #define SCHEDULER_PERIOD_MS 10
#define RECEIVE_TASK_PERIOD_MS 2 #define RECEIVE_TASK_PERIOD_MS 2
#define GENERIC_FRAME_SLIDING_WINDOW_SIZE 5 //defines the maximum size of the sliding window before resending previously un-ack'd fragments #define GENERIC_FRAME_SLIDING_WINDOW_SIZE 5 //defines the maximum size of the sliding window before resending previously un-ack'd fragments
@@ -20,13 +20,13 @@ typedef struct _frame_scheduler_metadata {
FrameHeader header; //header of the frame FrameHeader header; //header of the frame
uint16_t generic_frame_data_offset; //For data greater than MAX_GENERIC_DATA_LEN to keep track of fragment positions uint16_t generic_frame_data_offset; //For data greater than MAX_GENERIC_DATA_LEN to keep track of fragment positions
int64_t enqueue_time_ns; //when the frame has been first enqueued into the priority queue 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 std::shared_ptr<std::vector<uint8_t>> data; // the actual data, and length of data
uint16_t len; // length of the actual data
//sliding window //sliding window
uint16_t last_ack; //fragment number represnting the last ack'd fragment (from rx) - head uint16_t last_ack; //fragment number represnting the last ack'd fragment (from rx) - head
uint16_t curr_fragment; //fragment number of the current fragment being sent uint16_t curr_fragment; //fragment number of the current fragment being sent
uint32_t timeout; uint32_t timeout;
} SchedulerMetadata; } SchedulerMetadata;
typedef struct _frame_ack_record { typedef struct _frame_ack_record {
@@ -43,21 +43,21 @@ typedef struct _send_ack_metadata{
typedef struct _frame_compare { typedef struct _frame_compare {
/** /**
* @brief Uses aging based priority scheduling (linearly increasing priority with time) * @brief Uses aging based priority scheduling (linearly increasing priority with time)
* *
* $P_f = B_f - A_f\alpha$ * $P_f = B_f - A_f\alpha$
* *
* - $P_f$ is the effective priority value (lower comes first) * - $P_f$ is the effective priority value (lower comes first)
* *
* - $B_f$ is the base priority * - $B_f$ is the base priority
* *
* - $A_f$ is the age (amount of time the frame has waited in the queue) * - $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) * - $\alpha$ is the aging factor (rate at which a frame increases priority)
* *
* @param a * @param a
* @param b * @param b
* @return true * @return true
* @return false * @return false
*/ */
bool operator()(const SchedulerMetadata& a, const SchedulerMetadata& b) const { bool operator()(const SchedulerMetadata& a, const SchedulerMetadata& b) const {
int64_t now = esp_timer_get_time(); int64_t now = esp_timer_get_time();
@@ -84,4 +84,4 @@ typedef struct _frame_compare {
} }
} FrameCompare; } FrameCompare;
#endif //DATA_LINK #endif //DATA_LINK

54
components/ptrQueue/include/BlockingPriorityQueue.h Normal file
View File

@@ -0,0 +1,54 @@
#ifndef BLOCKINGPRIORITYQUEUE_H
#define BLOCKINGPRIORITYQUEUE_H
#include <chrono>
#include <condition_variable>
#include <mutex>
#include <optional>
#include <queue>
#include <vector>
#include <functional>
template <typename T,
typename Container = std::vector<T>,
typename Compare = std::less<typename Container::value_type>>
class BlockingPriorityQueue {
public:
explicit BlockingPriorityQueue(const size_t capacity) : m_capacity(capacity) {
}
// Enqueue with timeout. Returns true on success, false on timeout.
bool enqueue(T &&item, std::chrono::milliseconds max_wait) {
std::unique_lock lock(m_mutex);
if (!m_cond_not_full.wait_for(lock, max_wait,
[this]() { return m_queue.size() < m_capacity; })) {
return false;
}
m_queue.push(std::move(item));
m_cond_not_empty.notify_one();
return true;
}
// Dequeue with timeout. Returns optional<T> (empty on timeout).
std::optional<T> dequeue(std::chrono::milliseconds max_wait) {
std::unique_lock lock(m_mutex);
if (!m_cond_not_empty.wait_for(lock, max_wait, [this]() { return !m_queue.empty(); })) {
return std::nullopt;
}
T item = std::move(m_queue.top());
m_queue.pop();
m_cond_not_full.notify_one();
return item;
}
private:
std::priority_queue<T, Container, Compare> m_queue;
size_t m_capacity;
std::mutex m_mutex;
std::condition_variable m_cond_not_empty;
std::condition_variable m_cond_not_full;
};
#endif // BLOCKINGPRIORITYQUEUE_H

53
components/ptrQueue/include/BlockingQueue.h Normal file
View File

@@ -0,0 +1,53 @@
//
// Created by Johnathon Slightham on 2025-07-10.
//
#ifndef BLOCKINGQUEUE_H
#define BLOCKINGQUEUE_H
#include <chrono>
#include <condition_variable>
#include <mutex>
#include <optional>
#include <queue>
template <typename T> class BlockingQueue {
public:
explicit BlockingQueue(const size_t capacity) : m_capacity(capacity) {
}
// Enqueue with timeout. Returns true on success, false on timeout.
bool enqueue(T &&item, std::chrono::milliseconds max_wait) {
std::unique_lock lock(m_mutex);
if (!m_cond_not_full.wait_for(lock, max_wait,
[this]() { return m_queue.size() < m_capacity; })) {
return false;
}
m_queue.push(std::move(item));
m_cond_not_empty.notify_one();
return true;
}
// Dequeue with timeout. Returns optional<T> (empty on timeout).
std::optional<T> dequeue(std::chrono::milliseconds max_wait) {
std::unique_lock lock(m_mutex);
if (!m_cond_not_empty.wait_for(lock, max_wait, [this]() { return !m_queue.empty(); })) {
return std::nullopt;
}
T item = std::move(m_queue.front());
m_queue.pop();
m_cond_not_full.notify_one();
return item;
}
private:
std::queue<T> m_queue;
size_t m_capacity;
std::mutex m_mutex;
std::condition_variable m_cond_not_empty;
std::condition_variable m_cond_not_full;
};
#endif // BLOCKINGQUEUE_H

17
components/rmt/include/RMTManager.h
View File

@@ -26,7 +26,7 @@
/** /**
* @brief This struct keeps track of the current byte and bit index of the user data being transmmitted via RMT * @brief This struct keeps track of the current byte and bit index of the user data being transmmitted via RMT
* *
*/ */
typedef struct { typedef struct {
size_t byte_index; //which byte is currently being encoded when transmitting size_t byte_index; //which byte is currently being encoded when transmitting
@@ -63,16 +63,16 @@ typedef struct _rmt_channel{
QueueHandle_t rx_queue; QueueHandle_t rx_queue;
rmt_symbol_word_t raw_symbols[RECEIVE_BUFFER_SIZE]; //buffer to store the symbols on receive rmt_symbol_word_t raw_symbols[RECEIVE_BUFFER_SIZE]; //buffer to store the symbols on receive
rmt_symbol_word_t decoded_recv_symbols[RECEIVE_BUFFER_SIZE]; //allocating some dummy size buffer for decoded string rmt_symbol_word_t decoded_recv_symbols[RECEIVE_BUFFER_SIZE]; //allocating some dummy size buffer for decoded string
//General //General
uint8_t status; uint8_t status;
} rmt_channel; } rmt_channel;
/** /**
* @brief Class representing the RMT/Physical Layer * @brief Class representing the RMT/Physical Layer
* *
* @author Justin Chow * @author Justin Chow
* *
*/ */
class RMTManager{ class RMTManager{
public: public:
@@ -81,7 +81,7 @@ class RMTManager{
esp_err_t send(uint8_t* data, size_t size, rmt_transmit_config_t* config, uint8_t channel_num); //temp function to send some string data esp_err_t send(uint8_t* data, size_t size, rmt_transmit_config_t* config, uint8_t channel_num); //temp function to send some string data
esp_err_t receive(uint8_t* recv_buf, size_t size, size_t* output_size, uint8_t channel_num); esp_err_t receive(uint8_t* recv_buf, size_t size, size_t* output_size, uint8_t channel_num);
static size_t encoder_callback(const void* data, size_t data_size, size_t symbols_written, static size_t encoder_callback(const void* data, size_t data_size, size_t symbols_written,
size_t symbols_free, rmt_symbol_word_t* symbols, bool* done, void* arg); size_t symbols_free, rmt_symbol_word_t* symbols, bool* done, void* arg);
static bool rmt_rx_done_callback(rmt_channel_handle_t channel, const rmt_rx_done_event_data_t *edata, void *user_data); static bool rmt_rx_done_callback(rmt_channel_handle_t channel, const rmt_rx_done_event_data_t *edata, void *user_data);
@@ -90,7 +90,7 @@ class RMTManager{
esp_err_t start_receiving(uint8_t channel_num); esp_err_t start_receiving(uint8_t channel_num);
esp_err_t wait_until_send_complete(uint8_t channel_num); esp_err_t wait_until_send_complete(uint8_t channel_num);
private: private:
uint8_t num_channels; //number of channels initalized uint8_t num_channels; //number of channels initalized
esp_err_t init(); esp_err_t init();
@@ -101,7 +101,7 @@ class RMTManager{
int convert_symbols_to_char(rmt_symbol_word_t* symbols, size_t num, uint8_t* string, size_t output_num); int convert_symbols_to_char(rmt_symbol_word_t* symbols, size_t num, uint8_t* string, size_t output_num);
[[noreturn]] static void freeMemory(void* args); [[noreturn]] static void freeMemory(void* args);
rmt_channel channels[MAX_CHANNELS] = {0}; rmt_channel channels[MAX_CHANNELS] = {0};
//=====================TX===================== //=====================TX=====================
@@ -119,7 +119,7 @@ class RMTManager{
//rx_receive_config //rx_receive_config
rmt_receive_config_t receive_config = { rmt_receive_config_t receive_config = {
.signal_range_min_ns = 200, .signal_range_min_ns = 200,
.signal_range_max_ns = 200 * 1000, .signal_range_max_ns = 200 * 1000,
.flags = { .flags = {
.en_partial_rx = true .en_partial_rx = true
} }
@@ -151,5 +151,6 @@ static const GPIO_Channel_Pair gpio_channel_pairs[MAX_CHANNELS] = {
.rx_pin = GPIO_NUM_15 .rx_pin = GPIO_NUM_15
} }
}; //todo: use these pairs directly instead of the two arrays in the class definition above }; //todo: use these pairs directly instead of the two arrays in the class definition above
// but ensure to update them first!!!
#endif //RMT_COMMUNICATIONS #endif //RMT_COMMUNICATIONS

4
components/rpc/CommunicationFactory.cpp
View File

@@ -30,7 +30,7 @@ std::unique_ptr<IDiscoveryService> CommunicationFactory::create_discovery_servic
} }
} }
std::unique_ptr<IRPCServer> CommunicationFactory::create_lossy_server(const CommunicationMethod type, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue) { std::unique_ptr<IRPCServer> CommunicationFactory::create_lossy_server(const CommunicationMethod type, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>& rx_queue) {
switch (type) { switch (type) {
case Wireless: case Wireless:
return std::make_unique<UDPServer>(RECV_PORT, SEND_PORT, rx_queue); return std::make_unique<UDPServer>(RECV_PORT, SEND_PORT, rx_queue);
@@ -39,7 +39,7 @@ std::unique_ptr<IRPCServer> CommunicationFactory::create_lossy_server(const Comm
} }
} }
std::unique_ptr<IRPCServer> CommunicationFactory::create_lossless_server(const CommunicationMethod type, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue) { std::unique_ptr<IRPCServer> CommunicationFactory::create_lossless_server(const CommunicationMethod type, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>& rx_queue) {
switch (type) { switch (type) {
case Wireless: case Wireless:
return std::make_unique<TCPServer>(TCP_PORT, rx_queue); return std::make_unique<TCPServer>(TCP_PORT, rx_queue);

251
components/rpc/CommunicationRouter.cpp
View File

@@ -1,3 +1,5 @@
#include <chrono>
#include <cstring>
#include <iostream> #include <iostream>
#include "AngleControlMessageBuilder.h" #include "AngleControlMessageBuilder.h"
@@ -14,157 +16,172 @@
#include "include/wireless/mDNSDiscoveryService.h" #include "include/wireless/mDNSDiscoveryService.h"
#define TAG "CommunicationRouter" #define TAG "CommunicationRouter"
#define MAX_RX_BUFFER_SIZE 1024
#define WIRELESS_DEQUEUE_TIMEOUT_MS 3000
CommunicationRouter::~CommunicationRouter() { vTaskDelete(m_router_thread); } CommunicationRouter::~CommunicationRouter() {
vTaskDelete(m_router_thread);
// todo: we really need to change all char to uint8_t everywhere }
// todo: get rid of copying going on, need to pass around sharedptrs/uniqueptrs
[[noreturn]] void CommunicationRouter::router_thread(void *args) { [[noreturn]] void CommunicationRouter::router_thread(void *args) {
const auto that = static_cast<CommunicationRouter *>(args); const auto that = static_cast<CommunicationRouter *>(args);
while (true) { while (true) {
const auto buffer = that->m_tcp_rx_queue->dequeue(); if( auto maybe_buffer = that->m_tcp_rx_queue->dequeue(std::chrono::milliseconds(WIRELESS_DEQUEUE_TIMEOUT_MS))) {
ESP_LOGD(TAG, "Got message from TCP"); ESP_LOGD(TAG, "Got message from TCP");
that->route(buffer->data(), buffer->size()); that->route(std::move(*maybe_buffer));
} }
}
} }
[[noreturn]] void CommunicationRouter::link_layer_thread(void *args) { [[noreturn]] void CommunicationRouter::link_layer_thread(void *args) {
const auto that = static_cast<CommunicationRouter *>(args); const auto that = static_cast<CommunicationRouter *>(args);
while (true) { while (true) {
if (std::chrono::system_clock::now() - that->m_last_leader_updated > if (std::chrono::system_clock::now() - that->m_last_leader_updated >
std::chrono::seconds(2)) { std::chrono::seconds(2)) {
that->m_last_leader_updated = std::chrono::system_clock::now(); that->m_last_leader_updated = std::chrono::system_clock::now();
that->update_leader(); that->update_leader();
}
if (auto ptr = that->m_data_link_manager->async_receive()) {
that->route(std::move(*ptr));
}
} }
for (uint8_t channel = 0;
channel <
MODULE_TO_NUM_CHANNELS_MAP[that->m_config_manager.get_module_type()];
channel++) {
uint16_t frame_size = 0;
FrameHeader frame_header{};
if (ESP_OK != that->m_data_link_manager->async_receive_info(
&frame_size, &frame_header, channel) ||
0 == frame_size) {
continue;
}
std::vector<uint8_t> data{};
data.resize(frame_size);
that->m_data_link_manager->async_receive(data.data(), frame_size,
&frame_header, channel);
that->route(data.data(), frame_size);
}
vTaskDelay(pdMS_TO_TICKS(50));
}
} }
int CommunicationRouter::send_msg(char *buffer, const size_t length) const { int CommunicationRouter::send_msg(char *buffer, const size_t length) const {
ESP_LOGD(TAG, "Got message from application"); route(reinterpret_cast<uint8_t *>(buffer), length);
route(reinterpret_cast<uint8_t *>(buffer), length); return 0;
return 0;
} }
void CommunicationRouter::update_leader() { void CommunicationRouter::update_leader() {
RIPRow_public table[RIP_MAX_ROUTES]; RIPRow_public table[RIP_MAX_ROUTES];
size_t table_size = RIP_MAX_ROUTES; size_t table_size = RIP_MAX_ROUTES;
this->m_data_link_manager->get_routing_table(table, &table_size); this->m_data_link_manager->get_routing_table(table, &table_size);
// Leader election (just get the highest id in rip) // Leader election (just get the highest id in rip)
std::vector<int> connected_module_ids; std::vector<int> connected_module_ids;
uint8_t max = m_module_id; uint8_t max = m_module_id;
for (int i = 0; i < table_size; i++) { for (int i = 0; i < table_size; i++) {
const auto id = table[i].info.board_id; const auto id = table[i].info.board_id;
connected_module_ids.emplace_back(id); connected_module_ids.emplace_back(id);
if (id > max) { // todo: change this to be correct if (id > max) { // todo: change this to be correct
max = id; max = id;
}
} }
}
// Leader has changed, we may need to change PC connection state // Leader has changed, we may need to change PC connection state
if (this->m_leader != max) { if (this->m_leader != max) {
ESP_LOGI(TAG, "Leader has changed from %d to %d", this->m_leader, max); ESP_LOGI(TAG, "Leader has changed from %d to %d", this->m_leader, max);
if (max == m_module_id) { if (max == m_module_id) {
m_pc_connection->connect(); m_pc_connection->connect();
m_lossless_server->startup(); m_lossless_server->startup();
m_lossy_server->startup(); m_lossy_server->startup();
} else if (this->m_leader == m_module_id) { } else if (this->m_leader == m_module_id) {
m_pc_connection->disconnect(); m_pc_connection->disconnect();
m_lossless_server->shutdown(); m_lossless_server->shutdown();
m_lossy_server->shutdown(); m_lossy_server->shutdown();
}
} }
}
this->m_leader = max; this->m_leader = max;
if (this->m_leader == m_module_id) { if (this->m_leader == m_module_id) {
this->m_discovery_service->set_connected_boards(connected_module_ids); this->m_discovery_service->set_connected_boards(connected_module_ids);
} }
} }
void CommunicationRouter::route(uint8_t *buffer, const size_t length) const { // Route without trying to copy to heap. Only call if you do not have a unique_ptr.
flatbuffers::Verifier verifier(buffer, length); // To handle the case of writing directly from control -> TCP/UDP, nothing has to touch the heap.
// This could be moved to just be called on wireline data to save cpu cycles. void CommunicationRouter::route(uint8_t *buffer, size_t size) const {
if (bool ok = Messaging::VerifyMPIMessageBuffer(verifier); !ok) { flatbuffers::Verifier verifier(buffer, size);
ESP_LOGW(TAG, "route: got an invalid MPI message, disregarding"); // This could be moved to just be called on wireline data to save cpu cycles.
return; if (bool ok = Messaging::VerifyMPIMessageBuffer(verifier); !ok) {
} ESP_LOGW(TAG, "route: got an invalid MPI message, disregarding");
return;
if (const auto &mpi_message = }
Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer);
mpi_message->destination() == m_module_id) { if (const auto &mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer);
this->m_rx_callback(reinterpret_cast<char *>(buffer), 512); mpi_message->destination() == m_module_id) {
} else if (mpi_message->destination() == PC_ADDR && auto ubuffer = std::make_unique<std::vector<uint8_t>>();
this->m_leader == m_module_id) { ubuffer->resize(size);
if (mpi_message->is_durable()) { memcpy(ubuffer->data(), buffer, size);
this->m_lossless_server->send_msg(reinterpret_cast<char *>(buffer), 512); this->m_rx_callback(std::move(ubuffer));
} else { } else if (mpi_message->destination() == PC_ADDR && this->m_leader == m_module_id) {
this->m_lossy_server->send_msg(reinterpret_cast<char *>(buffer), 512); if (mpi_message->is_durable()) {
this->m_lossless_server->send_msg(buffer, size);
} else {
this->m_lossy_server->send_msg(buffer, size);
}
} else {
const auto dest = mpi_message->destination() == PC_ADDR ? this->m_leader : mpi_message->destination();
auto u_buffer = std::make_unique<std::vector<uint8_t>>();
u_buffer->resize(size);
memcpy(u_buffer->data(), buffer, size);
this->m_data_link_manager->send(dest, std::move(u_buffer), FrameType::MOTOR_TYPE, 0);
}
}
// Route heap messages
void CommunicationRouter::route(std::unique_ptr<std::vector<uint8_t>>&& buffer) const {
flatbuffers::Verifier verifier(buffer->data(), buffer->size());
// This could be moved to just be called on wireline data to save cpu cycles.
if (bool ok = Messaging::VerifyMPIMessageBuffer(verifier); !ok) {
ESP_LOGW(TAG, "route: got an invalid MPI message, disregarding");
return;
}
if (const auto &mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer->data());
mpi_message->destination() == m_module_id) {
this->m_rx_callback(std::move(buffer));
} else if (mpi_message->destination() == PC_ADDR && this->m_leader == m_module_id) {
if (mpi_message->is_durable()) {
this->m_lossless_server->send_msg(buffer->data(), buffer->size());
} else {
this->m_lossy_server->send_msg(buffer->data(), buffer->size());
}
} else if (mpi_message->destination() == PC_ADDR) {
this->m_data_link_manager->send(this->m_leader, std::move(buffer), FrameType::MOTOR_TYPE, 0);
} else {
this->m_data_link_manager->send(mpi_message->destination(), std::move(buffer), FrameType::MOTOR_TYPE,
0);
} }
} else if (mpi_message->destination() == PC_ADDR) {
this->m_data_link_manager->send(this->m_leader, buffer, length,
FrameType::MOTOR_TYPE, 0);
} else {
this->m_data_link_manager->send(mpi_message->destination(), buffer, length,
FrameType::MOTOR_TYPE, 0);
}
} }
std::pair<std::vector<uint8_t>, std::vector<Orientation>> std::pair<std::vector<uint8_t>, std::vector<Orientation>>
CommunicationRouter::get_physically_connected_modules() const { CommunicationRouter::get_physically_connected_modules() const {
std::vector<RIPRow_public> table; std::vector<RIPRow_public> table;
table.resize(RIP_MAX_ROUTES); table.resize(RIP_MAX_ROUTES);
size_t table_size = RIP_MAX_ROUTES * sizeof(RIPRow_public); size_t table_size = RIP_MAX_ROUTES * sizeof(RIPRow_public);
m_data_link_manager->get_routing_table(table.data(), &table_size); m_data_link_manager->get_routing_table(table.data(), &table_size);
std::vector<uint8_t> connected_module_ids; std::vector<uint8_t> connected_module_ids;
std::vector<Orientation> connected_module_orientations; std::vector<Orientation> connected_module_orientations;
connected_module_ids.resize(MAX_WIRED_CONNECTIONS); connected_module_ids.resize(MAX_WIRED_CONNECTIONS);
connected_module_orientations.resize(MAX_WIRED_CONNECTIONS); connected_module_orientations.resize(MAX_WIRED_CONNECTIONS);
for (int i = 0; i < MAX_WIRED_CONNECTIONS; i++) { for (int i = 0; i < MAX_WIRED_CONNECTIONS; i++) {
connected_module_ids[i] = 0; // this is not the PC ID here, marking as nc. connected_module_ids[i] = 0; // this is not the PC ID here, marking as nc.
}
for (int i = 0; i < table_size; i++) {
if (table[i].info.hops == 1 && table[i].channel < MAX_WIRED_CONNECTIONS) {
connected_module_ids[table[i].channel] = table[i].info.board_id;
} }
}
if (const auto id = connected_module_ids[0]; 0 == id) { for (int i = 0; i < table_size; i++) {
connected_module_orientations[0] = Orientation_Deg0; if (table[i].info.hops == 1 && table[i].channel < MAX_WIRED_CONNECTIONS) {
} else { connected_module_ids[table[i].channel] = table[i].info.board_id;
connected_module_orientations[0] = OrientationDetection::get_orientation(0); }
} }
return {connected_module_ids, connected_module_orientations}; if (const auto id = connected_module_ids[0]; 0 == id) {
connected_module_orientations[0] = Orientation_Deg0;
} else {
connected_module_orientations[0] = OrientationDetection::get_orientation(0);
}
return {connected_module_ids, connected_module_orientations};
} }
[[nodiscard]] uint8_t CommunicationRouter::get_leader() const { [[nodiscard]] uint8_t CommunicationRouter::get_leader() const {
return this->m_leader; return this->m_leader;
} }

5
components/rpc/MessagingInterface.cpp
View File

@@ -13,7 +13,6 @@
#include "MPIMessageBuilder.h" #include "MPIMessageBuilder.h"
MessagingInterface::~MessagingInterface() { MessagingInterface::~MessagingInterface() {
vQueueDelete(m_mpi_rx_queue);
vSemaphoreDelete(m_map_semaphore); vSemaphoreDelete(m_map_semaphore);
for (const auto queue: m_tag_to_queue | std::views::values) { for (const auto queue: m_tag_to_queue | std::views::values) {
@@ -52,8 +51,8 @@ int MessagingInterface::sendrecv(char* send_buffer, const int send_size, const i
} }
// todo: when handleRecv returns, remove from queue (from router) // todo: when handleRecv returns, remove from queue (from router)
void MessagingInterface::handleRecv(const char* recv_buffer, int recv_size) { void MessagingInterface::handleRecv(std::unique_ptr<std::vector<uint8_t>>&& buffer) {
const auto mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(reinterpret_cast<const uint8_t *>(recv_buffer)); const auto mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer->data());
checkOrInsertTag(mpi_message->tag()); checkOrInsertTag(mpi_message->tag());

4
components/rpc/OrientationDetection.cpp
View File

@@ -21,16 +21,12 @@ void OrientationDetection::init() {
Orientation OrientationDetection::get_orientation(const uint8_t channel) { Orientation OrientationDetection::get_orientation(const uint8_t channel) {
if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_90_DEG_MAP[channel]))) { if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_90_DEG_MAP[channel]))) {
ESP_LOGD(TAG, "90deg");
return Orientation_Deg90; return Orientation_Deg90;
} else if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_180_DEG_MAP[channel]))) { } else if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_180_DEG_MAP[channel]))) {
ESP_LOGD(TAG, "180deg");
return Orientation_Deg180; return Orientation_Deg180;
} else if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_270_DEG_MAP[channel]))) { } else if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_270_DEG_MAP[channel]))) {
ESP_LOGD(TAG, "270deg");
return Orientation_Deg270; return Orientation_Deg270;
} else { } else {
ESP_LOGD(TAG, "No orientation detected");
return Orientation_Deg0; return Orientation_Deg0;
} }
} }

6
components/rpc/include/CommunicationFactory.h
View File

@@ -13,15 +13,15 @@
#include "IConnectionManager.h" #include "IConnectionManager.h"
#include "IDiscoveryService.h" #include "IDiscoveryService.h"
#include "IRPCServer.h" #include "IRPCServer.h"
#include "PtrQueue.h" #include "BlockingQueue.h"
#include "enums.h" #include "enums.h"
class CommunicationFactory { class CommunicationFactory {
public: public:
static std::unique_ptr<IConnectionManager> create_connection_manager(CommunicationMethod type); static std::unique_ptr<IConnectionManager> create_connection_manager(CommunicationMethod type);
static std::unique_ptr<IDiscoveryService> create_discovery_service(CommunicationMethod type); static std::unique_ptr<IDiscoveryService> create_discovery_service(CommunicationMethod type);
static std::unique_ptr<IRPCServer> create_lossy_server(CommunicationMethod type, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>> &rx_queue); static std::unique_ptr<IRPCServer> create_lossy_server(CommunicationMethod type, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue);
static std::unique_ptr<IRPCServer> create_lossless_server(CommunicationMethod type, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue); static std::unique_ptr<IRPCServer> create_lossless_server(CommunicationMethod type, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>& rx_queue);
}; };
#endif //COMMUNICATIONFACTORY_H #endif //COMMUNICATIONFACTORY_H

16
components/rpc/include/CommunicationRouter.h
View File

@@ -21,13 +21,15 @@
#include "wireless/TCPServer.h" #include "wireless/TCPServer.h"
#include "wireless/WifiManager.h" #include "wireless/WifiManager.h"
#define MAX_NETWORK_QUEUE_SIZE 10
class CommunicationRouter { class CommunicationRouter {
public: public:
explicit CommunicationRouter( explicit CommunicationRouter(
const std::function<void(char *, int)> &rx_callback) const std::function<void(std::unique_ptr<std::vector<uint8_t>>&&)> &rx_callback)
: m_tcp_rx_queue(std::make_shared<PtrQueue<std::vector<uint8_t>>>(10)), : m_tcp_rx_queue(std::make_shared<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>(MAX_NETWORK_QUEUE_SIZE)),
m_rx_callback(rx_callback), m_rx_callback(std::move(rx_callback)),
m_config_manager(ConfigManager::get_instance()), m_config_manager(ConfigManager::get_instance()),
m_pc_connection(CommunicationFactory::create_connection_manager( m_pc_connection(CommunicationFactory::create_connection_manager(
m_config_manager.get_communication_method())), m_config_manager.get_communication_method())),
@@ -56,15 +58,15 @@ public:
[[noreturn]] static void link_layer_thread(void *args); [[noreturn]] static void link_layer_thread(void *args);
int send_msg(char *buffer, size_t length) const; int send_msg(char *buffer, size_t length) const;
void update_leader(); void update_leader();
void route(uint8_t *buffer, size_t length) const; void route(std::unique_ptr<std::vector<uint8_t>>&& buffer) const;
void route(uint8_t* buffer, size_t size) const;
[[nodiscard]] std::pair<std::vector<uint8_t>, std::vector<Orientation>> [[nodiscard]] std::pair<std::vector<uint8_t>, std::vector<Orientation>>
get_physically_connected_modules() const; get_physically_connected_modules() const;
[[nodiscard]] uint8_t get_leader() const; [[nodiscard]] uint8_t get_leader() const;
// todo: does this really need to be here (so i can access from thread)? // todo: does this really need to be here (so i can access from thread)?
std::shared_ptr<PtrQueue<std::vector<uint8_t>>> std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_tcp_rx_queue;
m_tcp_rx_queue; // todo: this should probably be thread safe std::function<void(std::unique_ptr<std::vector<std::uint8_t>>)> m_rx_callback;
std::function<void(char *, int)> m_rx_callback;
private: private:
TaskHandle_t m_router_thread = nullptr; TaskHandle_t m_router_thread = nullptr;

7
components/rpc/include/IRPCServer.h
View File

@@ -5,12 +5,15 @@
#ifndef IRPCSERVER_H #ifndef IRPCSERVER_H
#define IRPCSERVER_H #define IRPCSERVER_H
#include <memory>
#include <vector>
class IRPCServer { class IRPCServer {
public: public:
virtual ~IRPCServer() = default; virtual ~IRPCServer() = default;
virtual void startup() = 0; virtual void startup() = 0;
virtual void shutdown() = 0; virtual void shutdown() = 0;
virtual int send_msg(char* buffer, uint32_t length) const = 0; virtual int send_msg(uint8_t *buffer, size_t size) const = 0;
}; };
#endif //IRPCSERVER_H #endif //IRPCSERVER_H

9
components/rpc/include/MessagingInterface.h
View File

@@ -9,6 +9,7 @@
#include <unordered_map> #include <unordered_map>
#include <flatbuffers_generated/TopologyMessage_generated.h> #include <flatbuffers_generated/TopologyMessage_generated.h>
#include "BlockingQueue.h"
#include "constants/app_comms.h" #include "constants/app_comms.h"
#include "CommunicationRouter.h" #include "CommunicationRouter.h"
@@ -16,8 +17,8 @@ class MessagingInterface {
public: public:
explicit MessagingInterface() explicit MessagingInterface()
: m_config_manager(ConfigManager::get_instance()), : m_config_manager(ConfigManager::get_instance()),
m_mpi_rx_queue(xQueueCreate(MAX_RX_BUFFER_SIZE, RX_QUEUE_SIZE)), m_mpi_rx_queue(std::make_unique<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>(RX_QUEUE_SIZE)),
m_router(std::make_unique<CommunicationRouter>([this](const char* buffer, const int size) { handleRecv(buffer, size); })), m_router(std::make_unique<CommunicationRouter>([this](std::unique_ptr<std::vector<uint8_t>>&& buffer) { handleRecv(std::move(buffer)); })),
m_map_semaphore(xSemaphoreCreateMutex()) {}; m_map_semaphore(xSemaphoreCreateMutex()) {};
~MessagingInterface(); ~MessagingInterface();
@@ -31,13 +32,13 @@ public:
uint8_t get_leader() const; uint8_t get_leader() const;
private: private:
void handleRecv(const char* recv_buffer, int recv_size); void handleRecv(std::unique_ptr<std::vector<uint8_t>>&& buffer);
void checkOrInsertTag(uint8_t tag); void checkOrInsertTag(uint8_t tag);
ConfigManager& m_config_manager; ConfigManager& m_config_manager;
uint16_t m_sequence_number = 0; uint16_t m_sequence_number = 0;
QueueHandle_t m_mpi_rx_queue; // todo: maybe move this down classes more std::unique_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_mpi_rx_queue;
std::unique_ptr<CommunicationRouter> m_router; std::unique_ptr<CommunicationRouter> m_router;
SemaphoreHandle_t m_map_semaphore; SemaphoreHandle_t m_map_semaphore;
std::unordered_map<uint8_t, QueueHandle_t> m_tag_to_queue; std::unordered_map<uint8_t, QueueHandle_t> m_tag_to_queue;

16
components/rpc/include/wireless/TCPServer.h
View File

@@ -6,22 +6,22 @@
#define TCPSERVER_H #define TCPSERVER_H
#include <memory> #include <memory>
#include <vector>
#include <unordered_set> #include <unordered_set>
#include <vector>
#include "freertos/FreeRTOS.h"
#include "IRPCServer.h" #include "IRPCServer.h"
#include "PtrQueue.h" #include "BlockingQueue.h"
#include "freertos/FreeRTOS.h"
class TCPServer final : public IRPCServer { class TCPServer final : public IRPCServer {
public: public:
TCPServer(int port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue); TCPServer(int port, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue);
~TCPServer() override; ~TCPServer() override;
void startup() override; void startup() override;
void shutdown() override; void shutdown() override;
int send_msg(char* buffer, uint32_t length) const override; int send_msg(uint8_t* buffer, size_t size) const override;
private: private:
bool authenticate_client(int client_sock); bool authenticate_client(int client_sock);
static bool is_network_connected(); static bool is_network_connected();
@@ -34,7 +34,7 @@ private:
TaskHandle_t m_task; TaskHandle_t m_task;
TaskHandle_t m_rx_task; TaskHandle_t m_rx_task;
std::shared_ptr<PtrQueue<std::vector<uint8_t>>> m_rx_queue; std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_rx_queue;
SemaphoreHandle_t m_mutex; SemaphoreHandle_t m_mutex;
std::unordered_set<int> m_clients; std::unordered_set<int> m_clients;

21
components/rpc/include/wireless/UDPServer.h
View File

@@ -6,36 +6,37 @@
#define UDPSERVER_H #define UDPSERVER_H
#include <memory> #include <memory>
#include <vector>
#include <unordered_set> #include <unordered_set>
#include <vector>
#include "freertos/FreeRTOS.h"
#include "IRPCServer.h" #include "IRPCServer.h"
#include "PtrQueue.h" #include "BlockingQueue.h"
#include "freertos/FreeRTOS.h"
class UDPServer final : public IRPCServer { class UDPServer final : public IRPCServer {
public: public:
UDPServer(int rx_port, int tx_port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue); UDPServer(int rx_port, int tx_port,
const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue);
~UDPServer() override; ~UDPServer() override;
void startup() override; void startup() override;
void shutdown() override; void shutdown() override;
int send_msg(char* buffer, uint32_t length) const override; int send_msg(uint8_t *buffer, size_t size) const override;
private: private:
bool authenticate_client(int client_sock); bool authenticate_client(int client_sock);
static bool is_network_connected(); static bool is_network_connected();
[[noreturn]] static void socket_monitor_thread(void *args); [[noreturn]] static void socket_monitor_thread(void *args);
int m_tx_port; int m_tx_port;
int m_rx_port; int m_rx_port;
int m_tx_server_sock; int m_tx_server_sock;
int m_rx_server_sock; int m_rx_server_sock;
TaskHandle_t m_rx_task; TaskHandle_t m_rx_task;
std::shared_ptr<PtrQueue<std::vector<uint8_t>>> m_rx_queue; std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_rx_queue;
}; };
#endif //UDPSERVER_H #endif //UDPSERVER_H

11
components/rpc/wireless/TCPServer.cpp
View File

@@ -1,3 +1,4 @@
#include <chrono>
#include <memory> #include <memory>
#include "bits/shared_ptr_base.h" #include "bits/shared_ptr_base.h"
@@ -15,6 +16,8 @@
#include "sys/param.h" #include "sys/param.h"
#include "wireless/TCPServer.h" #include "wireless/TCPServer.h"
#define RX_QUEUE_ENQUEUE_TIMEOUT_MS 50 // must be small to ensure we drain TCP buffer
#define TAG "TCPServer" #define TAG "TCPServer"
#define MIN(X, Y) (((X) < (Y)) ? (X) : (Y)) #define MIN(X, Y) (((X) < (Y)) ? (X) : (Y))
@@ -24,7 +27,7 @@
// - tx from board // - tx from board
TCPServer::TCPServer(const int port, TCPServer::TCPServer(const int port,
const std::shared_ptr<PtrQueue<std::vector<uint8_t>>> &rx_queue) { const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue) {
this->m_port = port; this->m_port = port;
this->m_mutex = xSemaphoreCreateMutex(); this->m_mutex = xSemaphoreCreateMutex();
this->m_clients = std::unordered_set<int>(); this->m_clients = std::unordered_set<int>();
@@ -215,7 +218,7 @@ void TCPServer::shutdown() {
} else { } else {
ESP_LOGD(TAG, "TCP Server Received %d bytes\n", len); ESP_LOGD(TAG, "TCP Server Received %d bytes\n", len);
buffer->resize(len); buffer->resize(len);
that->m_rx_queue->enqueue(std::move(buffer)); that->m_rx_queue->enqueue(std::move(buffer), std::chrono::milliseconds(RX_QUEUE_ENQUEUE_TIMEOUT_MS));
} }
} }
} }
@@ -260,11 +263,13 @@ bool TCPServer::authenticate_client(int sock) {
return 0; return 0;
} }
int TCPServer::send_msg(char *buffer, const uint32_t length) const { int TCPServer::send_msg(uint8_t *buffer, size_t size) const {
if (!is_network_connected()) { if (!is_network_connected()) {
return -1; return -1;
} }
const auto length = (uint32_t)size;
for (const auto client_sock : m_clients) { for (const auto client_sock : m_clients) {
send(client_sock, &length, 4, 0); send(client_sock, &length, 4, 0);
send(client_sock, buffer, length, 0); send(client_sock, buffer, length, 0);

325
components/rpc/wireless/UDPServer.cpp
View File

@@ -1,3 +1,4 @@
#include <chrono>
#include <cstring> #include <cstring>
#include <memory> #include <memory>
@@ -17,205 +18,207 @@
#include "wireless/UDPServer.h" #include "wireless/UDPServer.h"
#define TAG "UDPServer" #define TAG "UDPServer"
#define MAX_RX_QUEUE_ENQUEUE_TIMEOUT_MS 50
#define MIN(X, Y) (((X) < (Y)) ? (X) : (Y)) #define MIN(X, Y) (((X) < (Y)) ? (X) : (Y))
// todo: - authenticate // todo: - authenticate
UDPServer::UDPServer( UDPServer::UDPServer(const int rx_port, const int tx_port,
const int rx_port, const int tx_port, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue) {
const std::shared_ptr<PtrQueue<std::vector<uint8_t>>> &rx_queue) { this->m_rx_port = rx_port;
this->m_rx_port = rx_port; this->m_tx_port = tx_port;
this->m_tx_port = tx_port; this->m_rx_task = nullptr;
this->m_rx_task = nullptr; this->m_rx_queue = rx_queue;
this->m_rx_queue = rx_queue; this->m_rx_server_sock = 0;
this->m_rx_server_sock = 0; this->m_tx_server_sock = 0;
this->m_tx_server_sock = 0;
} }
UDPServer::~UDPServer() { this->shutdown(); } UDPServer::~UDPServer() {
this->shutdown();
}
void UDPServer::startup() { void UDPServer::startup() {
ESP_LOGI(TAG, "Starting UDP server on port %d", this->m_rx_port); ESP_LOGI(TAG, "Starting UDP server on port %d", this->m_rx_port);
if (nullptr != this->m_rx_task) { if (nullptr != this->m_rx_task) {
ESP_LOGW(TAG, "Attempted to start UDP server when already started, " ESP_LOGW(TAG, "Attempted to start UDP server when already started, "
"ignoring start request"); "ignoring start request");
return; return;
} }
xTaskCreate(socket_monitor_thread, "udp_rx", 4096, this, 5, &this->m_rx_task); xTaskCreate(socket_monitor_thread, "udp_rx", 4096, this, 5, &this->m_rx_task);
} }
void UDPServer::shutdown() { void UDPServer::shutdown() {
ESP_LOGI(TAG, "Shutting down UDP server"); ESP_LOGI(TAG, "Shutting down UDP server");
if (nullptr != this->m_rx_task) { if (nullptr != this->m_rx_task) {
vTaskDelete(this->m_rx_task); vTaskDelete(this->m_rx_task);
close(this->m_rx_server_sock); close(this->m_rx_server_sock);
close(this->m_tx_server_sock); close(this->m_tx_server_sock);
this->m_rx_task = nullptr; this->m_rx_task = nullptr;
this->m_rx_server_sock = -1; this->m_rx_server_sock = -1;
this->m_tx_server_sock = -1; this->m_tx_server_sock = -1;
} }
} }
[[noreturn]] void UDPServer::socket_monitor_thread(void *args) { [[noreturn]] void UDPServer::socket_monitor_thread(void *args) {
const auto that = static_cast<UDPServer *>(args); const auto that = static_cast<UDPServer *>(args);
while (true) { while (true) {
ESP_LOGI(TAG, "Attempting to start UDP Server on %d", that->m_rx_port); ESP_LOGI(TAG, "Attempting to start UDP Server on %d", that->m_rx_port);
if (!is_network_connected()) { if (!is_network_connected()) {
ESP_LOGW(TAG, "Network is disconnected"); ESP_LOGW(TAG, "Network is disconnected");
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS); vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue; continue;
}
sockaddr_in saddr = {0};
sockaddr_in from_addr = {0};
that->m_rx_server_sock = socket(AF_INET, SOCK_DGRAM, 0);
if (that->m_rx_server_sock == -1) {
ESP_LOGE(TAG, "Create UDP socket fail");
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
that->m_tx_server_sock = socket(AF_INET, SOCK_DGRAM, 0);
if (that->m_tx_server_sock < 0) {
ESP_LOGE(TAG, "Unable to create UDP tx socket: errno %d", errno);
close(that->m_rx_server_sock);
that->m_rx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
int reuse = 1;
if (setsockopt(that->m_rx_server_sock, SOL_SOCKET, SO_REUSEADDR, &reuse,
sizeof(reuse)) < 0) {
ESP_LOGE(TAG, "Failed to set SO_REUSEADDR. Error %d", errno);
close(that->m_rx_server_sock);
close(that->m_tx_server_sock);
that->m_rx_server_sock = -1;
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
saddr.sin_family = AF_INET;
saddr.sin_port = htons(that->m_rx_port);
saddr.sin_addr.s_addr = htonl(INADDR_ANY);
int ret = bind(that->m_rx_server_sock, (struct sockaddr *)&saddr,
sizeof(struct sockaddr_in));
if (ret < 0) {
ESP_LOGE(TAG, "Failed to bind socket. Error %d", errno);
close(that->m_rx_server_sock);
close(that->m_tx_server_sock);
that->m_rx_server_sock = -1;
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
struct ip_mreq imreq = {};
imreq.imr_multiaddr.s_addr = inet_addr(RECV_MCAST);
imreq.imr_interface.s_addr = htonl(INADDR_ANY);
if (setsockopt(that->m_rx_server_sock, IPPROTO_IP, IP_ADD_MEMBERSHIP,
&imreq, sizeof(struct ip_mreq)) < 0) {
ESP_LOGE(TAG, "Failed to set IP_ADD_MEMBERSHIP. Error %d", errno);
close(that->m_rx_server_sock);
close(that->m_tx_server_sock);
that->m_rx_server_sock = -1;
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
uint32_t msg_size;
while (is_network_connected()) {
auto buffer = std::make_unique<std::vector<uint8_t>>();
buffer->resize(MAX_RX_BUFFER_SIZE + 4);
if (int len = recvfrom(that->m_rx_server_sock, buffer->data(),
MAX_RX_BUFFER_SIZE, 0, nullptr, nullptr);
len < 0) {
ESP_LOGE(TAG, "Error occurred during receiving: errno %d", errno);
} else if (len < 4 || len > MAX_RX_BUFFER_SIZE) {
ESP_LOGE(TAG, "Got illegal message size");
} else {
msg_size = *reinterpret_cast<uint32_t *>(buffer->data());
if (msg_size > len - 4) {
ESP_LOGW(TAG, "Message size incorrect");
continue;
} }
buffer->erase(buffer->begin(), buffer->begin() + 4); // todo: copying
buffer->resize(msg_size);
that->m_rx_queue->enqueue(std::move(buffer));
}
}
ESP_LOGW(TAG, "Network disconnected"); sockaddr_in saddr = {0};
close(that->m_tx_server_sock); sockaddr_in from_addr = {0};
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS); that->m_rx_server_sock = socket(AF_INET, SOCK_DGRAM, 0);
} if (that->m_rx_server_sock == -1) {
ESP_LOGE(TAG, "Create UDP socket fail");
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
that->m_tx_server_sock = socket(AF_INET, SOCK_DGRAM, 0);
if (that->m_tx_server_sock < 0) {
ESP_LOGE(TAG, "Unable to create UDP tx socket: errno %d", errno);
close(that->m_rx_server_sock);
that->m_rx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
int reuse = 1;
if (setsockopt(that->m_rx_server_sock, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)) <
0) {
ESP_LOGE(TAG, "Failed to set SO_REUSEADDR. Error %d", errno);
close(that->m_rx_server_sock);
close(that->m_tx_server_sock);
that->m_rx_server_sock = -1;
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
saddr.sin_family = AF_INET;
saddr.sin_port = htons(that->m_rx_port);
saddr.sin_addr.s_addr = htonl(INADDR_ANY);
int ret =
bind(that->m_rx_server_sock, (struct sockaddr *)&saddr, sizeof(struct sockaddr_in));
if (ret < 0) {
ESP_LOGE(TAG, "Failed to bind socket. Error %d", errno);
close(that->m_rx_server_sock);
close(that->m_tx_server_sock);
that->m_rx_server_sock = -1;
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
struct ip_mreq imreq = {};
imreq.imr_multiaddr.s_addr = inet_addr(RECV_MCAST);
imreq.imr_interface.s_addr = htonl(INADDR_ANY);
if (setsockopt(that->m_rx_server_sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &imreq,
sizeof(struct ip_mreq)) < 0) {
ESP_LOGE(TAG, "Failed to set IP_ADD_MEMBERSHIP. Error %d", errno);
close(that->m_rx_server_sock);
close(that->m_tx_server_sock);
that->m_rx_server_sock = -1;
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
uint32_t msg_size;
while (is_network_connected()) {
auto buffer = std::make_unique<std::vector<uint8_t>>();
buffer->resize(MAX_RX_BUFFER_SIZE + 4);
if (int len = recvfrom(that->m_rx_server_sock, buffer->data(), MAX_RX_BUFFER_SIZE, 0,
nullptr, nullptr);
len < 0) {
ESP_LOGE(TAG, "Error occurred during receiving: errno %d", errno);
} else if (len < 4 || len > MAX_RX_BUFFER_SIZE) {
ESP_LOGE(TAG, "Got illegal message size");
} else {
msg_size = *reinterpret_cast<uint32_t *>(buffer->data());
if (msg_size > len - 4) {
ESP_LOGW(TAG, "Message size incorrect");
continue;
}
buffer->erase(buffer->begin(), buffer->begin() + 4); // todo: copying
buffer->resize(msg_size);
that->m_rx_queue->enqueue(std::move(buffer), std::chrono::milliseconds(MAX_RX_QUEUE_ENQUEUE_TIMEOUT_MS));
}
}
ESP_LOGW(TAG, "Network disconnected");
close(that->m_tx_server_sock);
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
}
} }
bool UDPServer::is_network_connected() { bool UDPServer::is_network_connected() {
esp_netif_ip_info_t ip_info; esp_netif_ip_info_t ip_info;
esp_netif_t *netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"); esp_netif_t *netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
if (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) { if (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) {
return true; return true;
} }
if (0 != ip_info.ip.addr) { if (0 != ip_info.ip.addr) {
return true; return true;
} }
netif = esp_netif_get_handle_from_ifkey("WIFI_AP_DEF"); netif = esp_netif_get_handle_from_ifkey("WIFI_AP_DEF");
if (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) { if (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) {
return true; return true;
} }
if (0 != ip_info.ip.addr) { if (0 != ip_info.ip.addr) {
return true; return true;
} }
return false; return false;
} }
bool UDPServer::authenticate_client(int sock) { bool UDPServer::authenticate_client(int sock) {
// todo: authentication (key?) // todo: authentication (key?)
return 0; return 0;
} }
int UDPServer::send_msg(char *buffer, const uint32_t length) const { int UDPServer::send_msg(uint8_t *buffer, size_t len) const {
if (!is_network_connected() || m_tx_server_sock == -1) { if (!is_network_connected() || m_tx_server_sock == -1) {
return -1; return -1;
} }
sockaddr_in mcast_dest = { sockaddr_in mcast_dest = {
.sin_family = AF_INET, .sin_family = AF_INET,
.sin_port = htons(m_tx_port), .sin_port = htons(m_tx_port),
.sin_addr = {.s_addr = inet_addr(SEND_MCAST)}, .sin_addr = {.s_addr = inet_addr(SEND_MCAST)},
}; };
uint32_t size = length; uint32_t size = (uint32_t)len;
iovec iov[2]; iovec iov[2];
iov[0].iov_base = &size; iov[0].iov_base = &size;
iov[0].iov_len = 4; iov[0].iov_len = 4;
iov[1].iov_base = buffer; iov[1].iov_base = buffer;
iov[1].iov_len = length; iov[1].iov_len = size;
msghdr msg = {}; msghdr msg = {};
msg.msg_iov = iov; msg.msg_iov = iov;
msg.msg_iovlen = 2; msg.msg_iovlen = 2;
msg.msg_name = &mcast_dest; msg.msg_name = &mcast_dest;
msg.msg_namelen = sizeof(mcast_dest); msg.msg_namelen = sizeof(mcast_dest);
sendmsg(this->m_tx_server_sock, &msg, 0); sendmsg(this->m_tx_server_sock, &msg, 0);
return 0; return 0;
} }

8
main/LoopManager.cpp
View File

@@ -71,7 +71,9 @@
void LoopManager::send_sensor_reading(bool durable) const { void LoopManager::send_sensor_reading(bool durable) const {
Flatbuffers::SensorMessageBuilder smb{}; Flatbuffers::SensorMessageBuilder smb{};
// todo: get data from sensor // todo: get data from sensor
auto data = m_actuator->get_sensor_data(); if (m_actuator) {
const auto [ptr, size] = smb.build_sensor_message(data); auto data = m_actuator->get_sensor_data();
m_messaging_interface->send(reinterpret_cast<char *>(ptr), size, PC_ADDR, SENSOR_TAG, durable); const auto [ptr, size] = smb.build_sensor_message(data);
m_messaging_interface->send(reinterpret_cast<char *>(ptr), size, PC_ADDR, SENSOR_TAG, durable);
}
} }

3
main/control/Servo1Actuator.cpp
View File

@@ -9,6 +9,7 @@
#include "driver/ledc.h" #include "driver/ledc.h"
#include "flatbuffers_generated/SensorMessage_generated.h" #include "flatbuffers_generated/SensorMessage_generated.h"
#include "util/number_utils.h" #include "util/number_utils.h"
#include "esp_log.h"
#define LOW_DUTY 200 #define LOW_DUTY 200
#define HIGH_DUTY 1000 #define HIGH_DUTY 1000
@@ -44,7 +45,7 @@ void Servo1Actuator::actuate(uint8_t *cmd) {
util::mapRange<int32_t>(angleControlCmd->angle(), 0, 180, LOW_DUTY, HIGH_DUTY); util::mapRange<int32_t>(angleControlCmd->angle(), 0, 180, LOW_DUTY, HIGH_DUTY);
m_target = angleControlCmd->angle(); m_target = angleControlCmd->angle();
std::cout << "actuating to " << angleControlCmd->angle() << std::endl; ESP_LOGI("TMP", "actuating to %d", angleControlCmd->angle());
ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, newDuty)); ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, newDuty));
ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0)); ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0));