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 b9c6a99260
commit 2e9004ee34
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"
PRIV_REQUIRES driver esp_event nvs_flash esp_netif rmt
REQUIRES esp_timer
REQUIRES esp_timer ptrQueue
INCLUDE_DIRS "include")

137
components/dataLink/DataLinkFrames.cpp
View File

@@ -1,5 +1,7 @@
#include "DataLinkManager.h"
#include "esp_log.h"
#include <cstring>
#include <type_traits>
/**
* @brief Creates a Control Frame from `FrameHeader`
@@ -152,7 +154,8 @@ esp_err_t DataLinkManager::complete_fragment(uint16_t board_id, uint16_t sequenc
uint16_t total_data_len = metadata.num_fragments_rx*MAX_FRAME_SIZE; //max data size with n fragments
xSemaphoreGive(rx_fragment_mutex[channel]);
uint8_t* combined_data = (uint8_t*)pvPortMalloc(total_data_len);
auto combined_data = std::make_unique<std::vector<uint8_t>>();
combined_data->resize(total_data_len);
rx.data_len = total_data_len;
if (combined_data == nullptr){
return ESP_ERR_NO_MEM;
@@ -173,15 +176,15 @@ esp_err_t DataLinkManager::complete_fragment(uint16_t board_id, uint16_t sequenc
return ESP_ERR_NOT_FOUND;
}
rx.data = combined_data;
uint16_t prev_index = 0;
for (size_t i = 0; i < metadata.num_fragments_rx; i++){
memcpy(&combined_data[prev_index], metadata.fragments[i].data, metadata.fragments[i].data_len);
memcpy(&combined_data->data()[prev_index], metadata.fragments[i].data, metadata.fragments[i].data_len);
prev_index += metadata.fragments[i].data_len;
}
xSemaphoreGive(rx_fragment_mutex[channel]);
rx.data = std::move(combined_data);
rx.data_len = prev_index;
if (async_rx_queue_mutex[channel] == nullptr){
@@ -203,15 +206,10 @@ esp_err_t DataLinkManager::complete_fragment(uint16_t board_id, uint16_t sequenc
// ESP_LOGI(DEBUG_LINK_TAG, "pushing frame %d onto async rx queue", sequence_num);
if (xSemaphoreTake(async_rx_queue_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){
vPortFree(combined_data);
if (!async_receive_queue->enqueue(std::move(rx), std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS))) {
return ESP_ERR_TIMEOUT;
}
async_receive_queue[channel].push(rx);
xSemaphoreGive(async_rx_queue_mutex[channel]);
fragment_map[channel][board_id].erase(sequence_num);
if (fragment_map[channel][board_id].empty()) {
@@ -234,42 +232,11 @@ esp_err_t DataLinkManager::complete_fragment(uint16_t board_id, uint16_t sequenc
* @note This may be moved to a private function - Unsure if users should be able to manually send ACKs
*/
esp_err_t DataLinkManager::send_ack(uint8_t sender_id, uint8_t* data, uint16_t data_len){
return send(sender_id, data, data_len, FrameType::ACK_TYPE, 0x0);
}
/**
* @brief Checks the channel receive queue for any received frames. If there is, return the first frame's data size
*
* @param frame_size Size of the data
* @param header Header information of the combined generic frames
*
* @return esp_err_t
*/
esp_err_t DataLinkManager::async_receive_info(uint16_t* frame_size, FrameHeader* header, uint8_t channel){
if (frame_size == nullptr || header == nullptr){
return ESP_ERR_INVALID_ARG;
}
Rx_Metadata top;
if (xSemaphoreTake(async_rx_queue_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){
return ESP_ERR_TIMEOUT;
}
if (async_receive_queue[channel].size() == 0){
xSemaphoreGive(async_rx_queue_mutex[channel]);
*frame_size = 0;
return ESP_OK;
}
top = async_receive_queue[channel].front();
xSemaphoreGive(async_rx_queue_mutex[channel]);
*frame_size = top.data_len;
*header = top.header;
return ESP_OK;
// todo: change this to take in a unique_ptr
auto buffer = std::make_unique<std::vector<uint8_t>>();
buffer->resize(data_len);
memcpy(buffer->data(), data, data_len);
return send(sender_id, std::move(buffer), FrameType::ACK_TYPE, 0x0);
}
/**
@@ -280,49 +247,20 @@ esp_err_t DataLinkManager::async_receive_info(uint16_t* frame_size, FrameHeader*
* @param header Header information of returning frame
*
*/
esp_err_t DataLinkManager::async_receive(uint8_t* data, uint16_t data_len, FrameHeader* header, uint8_t channel){
if (data == nullptr || header == nullptr){
return ESP_ERR_INVALID_ARG;
std::optional<std::unique_ptr<std::vector<uint8_t>>> DataLinkManager::async_receive(){
auto maybe_top = async_receive_queue->dequeue(std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS));
if (!maybe_top) {
return std::nullopt;
}
Rx_Metadata top = std::move(*maybe_top);
if (data_len == 0){
return ESP_ERR_INVALID_ARG;
}
Rx_Metadata top;
if (xSemaphoreTake(async_rx_queue_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){
return ESP_ERR_TIMEOUT;
}
if (async_receive_queue[channel].size() == 0){
xSemaphoreGive(async_rx_queue_mutex[channel]);
return ESP_ERR_NOT_FOUND;
}
top = async_receive_queue[channel].front();
async_receive_queue[channel].pop();
xSemaphoreGive(async_rx_queue_mutex[channel]);
if (data_len < top.data_len){
vPortFree(top.data);
return ESP_ERR_INVALID_ARG;
}
*header = top.header;
memcpy(data, top.data, top.data_len);
vPortFree(top.data);
return std::make_optional<std::unique_ptr<std::vector<uint8_t>>>(std::move(top.data));
// ESP_LOGI(DEBUG_LINK_TAG, "pushed frame %d onto async queue", header->seq_num);
return ESP_OK;
}
esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
uint16_t data_len = MAX_FRAME_SIZE; //max possible data len
uint8_t data[data_len];
memset(data, 0, data_len);
size_t recv_len = 0;
@@ -343,16 +281,18 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
return ESP_ERR_INVALID_RESPONSE;
}
uint8_t message[MAX_FRAME_SIZE];
memset(message, 0, sizeof(message));
auto message = std::make_unique<std::vector<uint8_t>>();
message->resize(MAX_FRAME_SIZE);
size_t message_size = 0;
FrameHeader header;
res = get_data_from_frame(data, recv_len, message, &message_size, &header);
res = get_data_from_frame(data, recv_len, message->data(), &message_size, &header);
if (res != ESP_OK){
// print_buffer_binary(message, message_size);
return res;
}
message->resize(message_size);
// print_buffer_binary(message, message_size);
@@ -362,14 +302,14 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
return ESP_OK;
}
if (message[0] != GENERIC_FRAG_ACK_PREAMBLE){
if (message->data()[0] != GENERIC_FRAG_ACK_PREAMBLE){
return ESP_OK;
}
FrameAckRecord record = {
.last_ack = static_cast<uint16_t>((message[1] << 8) | (message[2])),
.total_frags = static_cast<uint16_t>((message[3] << 8) | (message[4])),
.seq_num = static_cast<uint16_t>((message[5] << 8) | (message[6]))
.last_ack = static_cast<uint16_t>((message->data()[1] << 8) | (message->data()[2])),
.total_frags = static_cast<uint16_t>((message->data()[3] << 8) | (message->data()[4])),
.seq_num = static_cast<uint16_t>((message->data()[5] << 8) | (message->data()[6]))
};
res = inc_head_sliding_window(channel, header.sender_id, record.seq_num, &record);
@@ -390,13 +330,12 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
return ESP_FAIL;
}
memcpy(frame.data, message, message_size);
memcpy(frame.data, message->data(), message_size);
esp_err_t res = store_fragment(&frame, channel);
return res;
}
//control frame handling: - TODO: clean up :)
memcpy(data, message, message_size);
// ESP_LOGI(DEBUG_LINK_TAG, "Received frame of type 0x%X destined for board %d", GET_TYPE(header.type_flag), header.receiver_id);
//check for a rip frame
@@ -404,8 +343,8 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
ESP_LOGI(DEBUG_LINK_TAG, "Got a RIP frame");
for (size_t i = 0; i < message_size-1; i+=2){
uint8_t board_id = message[i];
uint8_t hops = message[i+1];
uint8_t board_id = message->data()[i];
uint8_t hops = message->data()[i+1];
// ESP_LOGI(DEBUG_LINK_TAG, "Received: board_id %d and number of hops %d on channel %d", board_id, hops, channel);
RIPRow* entry = nullptr;
@@ -462,27 +401,17 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
//got frame but not destined for this board
if (header.receiver_id != this_board_id && header.receiver_id != BROADCAST_ADDR && header.seq_num > seq_num){
// ESP_LOGI(DEBUG_LINK_TAG, "Sending message to board %d with message %s", header.receiver_id, message);
res = send(header.receiver_id, message, message_size, FrameType::MISC_CONTROL_TYPE, 0);
res = send(header.receiver_id, std::move(message), FrameType::MISC_CONTROL_TYPE, 0);
return res;
}
uint8_t* metadata_message = (uint8_t*)pvPortMalloc(message_size);
if (metadata_message == nullptr){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to malloc for receive");
return ESP_ERR_NO_MEM;
}
memcpy(metadata_message, message, message_size);
Rx_Metadata metadata = {
.data = metadata_message,
.data = std::move(message),
.data_len = (uint16_t)message_size,
.header = header
};
if (xSemaphoreTake(async_rx_queue_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) == pdTRUE){
async_receive_queue[channel].push(metadata);
xSemaphoreGive(async_rx_queue_mutex[channel]);
} else {
if (!async_receive_queue->enqueue(std::move(metadata), std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS))){
return ESP_ERR_TIMEOUT;
}
return ESP_OK;

38
components/dataLink/DataLinkManager.cpp
View File

@@ -1,7 +1,12 @@
#include "DataLinkManager.h"
#include "BlockingQueue.h"
#include "Frames.h"
#include "RMTManager.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include <memory>
#define SCHEDULE_QUEUE_SIZE 25
/**
* @brief Constructs a new Data Link Manager object
@@ -28,6 +33,12 @@ DataLinkManager::DataLinkManager(uint8_t board_id, uint8_t num_channels = MAX_CH
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_rip();
}
@@ -346,15 +357,15 @@ esp_err_t DataLinkManager::create_generic_frame(uint8_t* data, uint16_t data_len
* @param type
* @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);
if (isControlFrame && data_len > MAX_FRAME_SIZE){
if (isControlFrame && buffer->size() > MAX_FRAME_SIZE){
//Control frames has max data size of MAX_FRAME_SIZE
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)
return ESP_ERR_INVALID_ARG;
}
@@ -364,24 +375,14 @@ esp_err_t DataLinkManager::send(uint8_t dest_board, uint8_t* data, uint16_t data
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)
uint32_t frag_info = 0;
if (!isControlFrame){
if (data_len <= MAX_CONTROL_DATA_LEN){
if (buffer->size() <= MAX_CONTROL_DATA_LEN){
frag_info = (1 << 16); //1 total fragment required
} 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;
}
}
@@ -401,13 +402,12 @@ esp_err_t DataLinkManager::send(uint8_t dest_board, uint8_t* data, uint16_t data
.seq_num = seq_num,
.type_flag = (uint8_t)((static_cast<uint8_t>(type) & 0xF0) | (flag & 0xF)),
.frag_info = frag_info,
.data_len = data_len,
.data_len = (uint16_t)buffer->size(),
.crc_16 = 0,
},
.generic_frame_data_offset = 0,
.enqueue_time_ns = 0,
.data = saved_data,
.len = data_len,
.data = std::move(buffer),
.last_ack = 0,
.curr_fragment = 0,
.timeout = 0,
@@ -418,7 +418,6 @@ esp_err_t DataLinkManager::send(uint8_t dest_board, uint8_t* data, uint16_t data
if (res != ESP_OK){
// ESP_LOGE(DEBUG_LINK_TAG, "Failed to route message to board %d", dest_board);
vPortFree(saved_data);
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){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to push frame to scheduler queue");
vPortFree(saved_data);
}
return res;
}

34
components/dataLink/DataLinkRIP.cpp
View File

@@ -225,9 +225,7 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
//use the control frame for the demo (as the number of rows increase, we will need to use the generic frame)
//data will be [board_id (1), hops (1), board_id (2), hops (2), ...]
uint8_t rip_message[RIP_MAX_ROUTES*2] = {};
uint16_t message_idx = 0;
esp_err_t res;
RIPRow* entry = nullptr;
@@ -235,6 +233,9 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
if(broadcast){
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++){
res = rip_get_row(&entry, i);
@@ -250,21 +251,15 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
if (entry->channel == channel){
//poisoned reverse
rip_message[message_idx++] = entry->info.board_id;
rip_message[message_idx++] = RIP_MAX_HOPS + 1;
rip_message->at(message_idx++) = entry->info.board_id;
rip_message->at(message_idx++) = RIP_MAX_HOPS + 1;
} else {
rip_message[message_idx++] = entry->info.board_id;
rip_message[message_idx++] = entry->info.hops;
rip_message->at(message_idx++) = entry->info.board_id;
rip_message->at(message_idx++) = entry->info.hops;
}
}
uint8_t* send_data = (uint8_t*)pvPortMalloc(message_idx);
if (send_data == nullptr){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to malloc when trying to send rip frame broadcast on channel %d", channel);
continue;
}
memset(send_data, 0, message_idx);
memcpy(send_data, rip_message, message_idx);
rip_message->resize(message_idx);
res = get_inc_sequence_num(BROADCAST_ADDR, &seq_num);
if (res != ESP_OK){
@@ -284,8 +279,7 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){
},
.generic_frame_data_offset = 0,
.enqueue_time_ns = 0,
.data = send_data,
.len = message_idx,
.data = std::move(rip_message),
.last_ack = 0,
.curr_fragment = 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);
}
message_idx = 0;
memset(rip_message, 0, sizeof(rip_message));
}
} 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++){
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){
continue;
}
rip_message[message_idx++] = entry->info.board_id;
rip_message[message_idx++] = entry->info.hops;
rip_message->data()[message_idx++] = entry->info.board_id;
rip_message->data()[message_idx++] = entry->info.hops;
}
ESP_LOGI(DEBUG_LINK_TAG, "replying to discovery request to board %d", dest_id);
res = send(dest_id, rip_message, message_idx, FrameType::RIP_TABLE_CONTROL, FLAG_DISCOVERY);
res = send(dest_id, std::move(rip_message), FrameType::RIP_TABLE_CONTROL, FLAG_DISCOVERY);
if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to send rip frame from send_rip_frame");
}

85
components/dataLink/DataLinkScheduler.cpp
View File

@@ -1,20 +1,30 @@
#include <chrono>
#include "DataLinkManager.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "freertos/projdefs.h"
#include "freertos/semphr.h"
#include "esp_random.h"
#include "portmacro.h"
#define FRAME_DEQUEUE_TIMEOUT_MS 2000
#define FRAME_ENQUEUE_TIMEOUT_MS 50
void DataLinkManager::init_scheduler(){
for (int i = 0; i < num_channels; i++){
sq_handle[i] = xSemaphoreCreateMutex();
async_rx_queue_mutex[i] = xSemaphoreCreateMutex();
rx_fragment_mutex[i] = xSemaphoreCreateMutex();
sliding_window_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::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)
*/
[[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){
ESP_LOGE(DEBUG_LINK_TAG, "Frame Scheduler failed to start due to invalid pointer");
vTaskDelete(nullptr);
@@ -37,12 +50,9 @@ void DataLinkManager::init_scheduler(){
ESP_LOGI(DEBUG_LINK_TAG, "Starting Frame Scheduler task");
while(!link_layer_obj->stop_tasks){
for (uint8_t i = 0; i < link_layer_obj->num_channels; i++){
link_layer_obj->scheduler_send(i);
}
vTaskDelay(pdMS_TO_TICKS(SCHEDULER_PERIOD_MS));
link_layer_obj->scheduler_send(channel);
}
free(args);
vTaskDelete(nullptr);
}
@@ -64,7 +74,7 @@ esp_err_t DataLinkManager::push_frame_to_scheduler(SchedulerMetadata frame, uint
return ESP_ERR_INVALID_ARG;
}
if (frame.len == 0){
if (frame.data->size() == 0){
ESP_LOGE(DEBUG_LINK_TAG, "Invalid Frame Length");
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();
frame.enqueue_time_ns = now;
if (sq_handle[channel] == nullptr){
ESP_LOGE(DEBUG_LINK_TAG, "Invalid scheduler queue handle");
return ESP_FAIL;
}
if (xSemaphoreTake(sq_handle[channel], pdMS_TO_TICKS(SCHEDULER_MUTEX_WAIT)) == pdTRUE){
frame_queue[channel].push(frame);
xSemaphoreGive(sq_handle[channel]);
} else {
//Failed to obtain mutex
ESP_LOGE(DEBUG_LINK_TAG, "Failed to get mutex");
return ESP_ERR_TIMEOUT;
}
frame_queue[channel]->enqueue(std::move(frame), std::chrono::milliseconds(FRAME_ENQUEUE_TIMEOUT_MS));
// 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;
}
if (sq_handle[channel] == nullptr){
return ESP_FAIL;
}
vTaskDelay(pdMS_TO_TICKS(10)); // the messages cannot be too close together
SchedulerMetadata frame;
if (xSemaphoreTake(sq_handle[channel], pdMS_TO_TICKS(SCHEDULER_MUTEX_WAIT)) == pdTRUE){
if (frame_queue[channel].empty()){
xSemaphoreGive(sq_handle[channel]);
// ESP_LOGI(DEBUG_LINK_TAG, "Scheduler queue for channel %d is empty", channel);
return ESP_OK;
}
frame = frame_queue[channel].top();
frame_queue[channel].pop();
xSemaphoreGive(sq_handle[channel]);
if (auto maybe_frame = frame_queue[channel]->dequeue(std::chrono::milliseconds(FRAME_DEQUEUE_TIMEOUT_MS))) {
frame = *maybe_frame;
} else {
ESP_LOGE(DEBUG_LINK_TAG, "Failed to get mutex when trying to send");
//Failed to obtain mutex
return ESP_ERR_TIMEOUT;
// ESP_LOGI(DEBUG_LINK_TAG, "Scheduler queue for channel %d is empty", channel);
return ESP_OK;
}
if (frame.data == nullptr){
@@ -131,7 +119,6 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
if (this_board_id == PC_ADDR){
ESP_LOGE(DEBUG_LINK_TAG, "This board is not assigned a board id");
vPortFree(frame.data);
return ESP_ERR_INVALID_ARG;
}
@@ -144,11 +131,9 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
if (isControlFrame){
//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);
vPortFree(frame.data);
if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to create control frame");
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);
} else {
//generic frame
if (frame.len > (MAX_GENERIC_DATA_LEN)){
if (frame.data->size() > (MAX_GENERIC_DATA_LEN)){
//fragment here
if (frame.timeout == 0){
@@ -168,7 +153,6 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
res = push_frame_to_scheduler(frame, channel);
if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment");
vPortFree(frame.data);
return res;
}
return ESP_OK;
@@ -184,7 +168,6 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
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);
vPortFree(frame.data);
return res;
}
@@ -206,7 +189,6 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
//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);
complete_record_sliding_window(channel, frame.header.receiver_id, frame.header.seq_num);
vPortFree(frame.data);
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)) {
fragment_size = MAX_GENERIC_DATA_LEN;
} 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);
@@ -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
//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);
if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to create generic frame fragment");
vPortFree(frame.data);
return res;
}
@@ -254,7 +235,6 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
res = push_frame_to_scheduler(frame, channel);
if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment");
vPortFree(frame.data);
}
return res;
}
@@ -267,20 +247,17 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
res = push_frame_to_scheduler(frame, channel);
if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment");
vPortFree(frame.data);
return res;
}
} else {
//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);
vPortFree(frame.data);
}
} else {
//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);
vPortFree(frame.data);
if (res != ESP_OK){
ESP_LOGE(DEBUG_LINK_TAG, "Failed to create generic frame");

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

@@ -11,6 +11,8 @@
#include "Frames.h"
#include "Tables.h"
#include "RMTManager.h"
#include "BlockingQueue.h"
#include "BlockingPriorityQueue.h"
#include <unordered_map>
#include "Scheduler.h"
@@ -43,6 +45,7 @@ static const uint16_t crc16_table[256] = {
#define ASYNC_QUEUE_WAIT_TICKS 100
#define SEQUENCE_NUM_MAP_MUTEX_MAX_WAIT_MS 50
#define MAX_RX_QUEUE_SIZE 100
/**
* @brief Class to represent the Data Link Layer
@@ -53,13 +56,12 @@ class DataLinkManager{
public:
DataLinkManager(uint8_t board_id, uint8_t num_channels);
~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 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 get_routing_table(RIPRow_public* table, size_t* table_size);
esp_err_t async_receive_info(uint16_t* frame_size, FrameHeader* header, uint8_t channel);
esp_err_t async_receive(uint8_t* data, uint16_t data_len, FrameHeader* header, uint8_t channel);
std::optional<std::unique_ptr<std::vector<uint8_t>>> async_receive();
esp_err_t ready();
esp_err_t send_ack(uint8_t sender_id, uint8_t* data, uint16_t data_len);
private:
@@ -114,8 +116,7 @@ class DataLinkManager{
* @brief Priority queue for each channel to schedule when to send frames
*
*/
std::priority_queue<SchedulerMetadata, std::vector<SchedulerMetadata>, FrameCompare> frame_queue[MAX_CHANNELS];
SemaphoreHandle_t sq_handle[MAX_CHANNELS];
std::unique_ptr<BlockingPriorityQueue<SchedulerMetadata, std::vector<SchedulerMetadata>, FrameCompare>> frame_queue[MAX_CHANNELS];
void init_scheduler();
esp_err_t push_frame_to_scheduler(SchedulerMetadata frame, uint8_t channel);
TaskHandle_t scheduler_task = NULL;
@@ -152,7 +153,7 @@ class DataLinkManager{
* @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);
@@ -204,4 +205,9 @@ class DataLinkManager{
std::queue<SendAckMetaData> send_ack_queue[MAX_CHANNELS];
};
struct frame_scheduler_args {
uint8_t channel_id;
DataLinkManager* that;
};
#endif //DATA_LINK

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

@@ -108,7 +108,7 @@ typedef struct _fragment_metadata {
} FragmentMetadata;
typedef struct _receive_metadata{
uint8_t* data;
std::unique_ptr<std::vector<uint8_t>> data;
uint16_t data_len;
FrameHeader header;
} Rx_Metadata;

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

@@ -4,7 +4,7 @@
#include <cstdint>
#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 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
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
uint8_t* data; //dyanmically allocated memory - contains the actual data
uint16_t len; // length of the actual data
std::shared_ptr<std::vector<uint8_t>> data; // the actual data, and length of data
//sliding window
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
uint32_t timeout;
} SchedulerMetadata;
typedef struct _frame_ack_record {

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

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

@@ -151,5 +151,6 @@ static const GPIO_Channel_Pair gpio_channel_pairs[MAX_CHANNELS] = {
.rx_pin = GPIO_NUM_15
}
}; //todo: use these pairs directly instead of the two arrays in the class definition above
// but ensure to update them first!!!
#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) {
case Wireless:
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) {
case Wireless:
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 "AngleControlMessageBuilder.h"
@@ -14,157 +16,172 @@
#include "include/wireless/mDNSDiscoveryService.h"
#define TAG "CommunicationRouter"
#define MAX_RX_BUFFER_SIZE 1024
#define WIRELESS_DEQUEUE_TIMEOUT_MS 3000
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
CommunicationRouter::~CommunicationRouter() {
vTaskDelete(m_router_thread);
}
[[noreturn]] void CommunicationRouter::router_thread(void *args) {
const auto that = static_cast<CommunicationRouter *>(args);
const auto that = static_cast<CommunicationRouter *>(args);
while (true) {
const auto buffer = that->m_tcp_rx_queue->dequeue();
ESP_LOGD(TAG, "Got message from TCP");
that->route(buffer->data(), buffer->size());
}
while (true) {
if( auto maybe_buffer = that->m_tcp_rx_queue->dequeue(std::chrono::milliseconds(WIRELESS_DEQUEUE_TIMEOUT_MS))) {
ESP_LOGD(TAG, "Got message from TCP");
that->route(std::move(*maybe_buffer));
}
}
}
[[noreturn]] void CommunicationRouter::link_layer_thread(void *args) {
const auto that = static_cast<CommunicationRouter *>(args);
const auto that = static_cast<CommunicationRouter *>(args);
while (true) {
if (std::chrono::system_clock::now() - that->m_last_leader_updated >
std::chrono::seconds(2)) {
that->m_last_leader_updated = std::chrono::system_clock::now();
that->update_leader();
while (true) {
if (std::chrono::system_clock::now() - that->m_last_leader_updated >
std::chrono::seconds(2)) {
that->m_last_leader_updated = std::chrono::system_clock::now();
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 {
ESP_LOGD(TAG, "Got message from application");
route(reinterpret_cast<uint8_t *>(buffer), length);
return 0;
route(reinterpret_cast<uint8_t *>(buffer), length);
return 0;
}
void CommunicationRouter::update_leader() {
RIPRow_public table[RIP_MAX_ROUTES];
size_t table_size = RIP_MAX_ROUTES;
this->m_data_link_manager->get_routing_table(table, &table_size);
RIPRow_public table[RIP_MAX_ROUTES];
size_t table_size = RIP_MAX_ROUTES;
this->m_data_link_manager->get_routing_table(table, &table_size);
// Leader election (just get the highest id in rip)
std::vector<int> connected_module_ids;
uint8_t max = m_module_id;
for (int i = 0; i < table_size; i++) {
const auto id = table[i].info.board_id;
connected_module_ids.emplace_back(id);
if (id > max) { // todo: change this to be correct
max = id;
// Leader election (just get the highest id in rip)
std::vector<int> connected_module_ids;
uint8_t max = m_module_id;
for (int i = 0; i < table_size; i++) {
const auto id = table[i].info.board_id;
connected_module_ids.emplace_back(id);
if (id > max) { // todo: change this to be correct
max = id;
}
}
}
// Leader has changed, we may need to change PC connection state
if (this->m_leader != max) {
ESP_LOGI(TAG, "Leader has changed from %d to %d", this->m_leader, max);
if (max == m_module_id) {
m_pc_connection->connect();
m_lossless_server->startup();
m_lossy_server->startup();
} else if (this->m_leader == m_module_id) {
m_pc_connection->disconnect();
m_lossless_server->shutdown();
m_lossy_server->shutdown();
// Leader has changed, we may need to change PC connection state
if (this->m_leader != max) {
ESP_LOGI(TAG, "Leader has changed from %d to %d", this->m_leader, max);
if (max == m_module_id) {
m_pc_connection->connect();
m_lossless_server->startup();
m_lossy_server->startup();
} else if (this->m_leader == m_module_id) {
m_pc_connection->disconnect();
m_lossless_server->shutdown();
m_lossy_server->shutdown();
}
}
}
this->m_leader = max;
this->m_leader = max;
if (this->m_leader == m_module_id) {
this->m_discovery_service->set_connected_boards(connected_module_ids);
}
if (this->m_leader == m_module_id) {
this->m_discovery_service->set_connected_boards(connected_module_ids);
}
}
void CommunicationRouter::route(uint8_t *buffer, const size_t length) const {
flatbuffers::Verifier verifier(buffer, length);
// 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);
mpi_message->destination() == m_module_id) {
this->m_rx_callback(reinterpret_cast<char *>(buffer), 512);
} else if (mpi_message->destination() == PC_ADDR &&
this->m_leader == m_module_id) {
if (mpi_message->is_durable()) {
this->m_lossless_server->send_msg(reinterpret_cast<char *>(buffer), 512);
} else {
this->m_lossy_server->send_msg(reinterpret_cast<char *>(buffer), 512);
// Route without trying to copy to heap. Only call if you do not have a unique_ptr.
// To handle the case of writing directly from control -> TCP/UDP, nothing has to touch the heap.
void CommunicationRouter::route(uint8_t *buffer, size_t size) const {
flatbuffers::Verifier verifier(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);
mpi_message->destination() == m_module_id) {
auto ubuffer = std::make_unique<std::vector<uint8_t>>();
ubuffer->resize(size);
memcpy(ubuffer->data(), buffer, size);
this->m_rx_callback(std::move(ubuffer));
} 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, 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>>
CommunicationRouter::get_physically_connected_modules() const {
std::vector<RIPRow_public> table;
table.resize(RIP_MAX_ROUTES);
size_t table_size = RIP_MAX_ROUTES * sizeof(RIPRow_public);
m_data_link_manager->get_routing_table(table.data(), &table_size);
std::vector<RIPRow_public> table;
table.resize(RIP_MAX_ROUTES);
size_t table_size = RIP_MAX_ROUTES * sizeof(RIPRow_public);
m_data_link_manager->get_routing_table(table.data(), &table_size);
std::vector<uint8_t> connected_module_ids;
std::vector<Orientation> connected_module_orientations;
connected_module_ids.resize(MAX_WIRED_CONNECTIONS);
connected_module_orientations.resize(MAX_WIRED_CONNECTIONS);
std::vector<uint8_t> connected_module_ids;
std::vector<Orientation> connected_module_orientations;
connected_module_ids.resize(MAX_WIRED_CONNECTIONS);
connected_module_orientations.resize(MAX_WIRED_CONNECTIONS);
for (int i = 0; i < MAX_WIRED_CONNECTIONS; i++) {
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;
for (int i = 0; i < MAX_WIRED_CONNECTIONS; i++) {
connected_module_ids[i] = 0; // this is not the PC ID here, marking as nc.
}
}
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);
}
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;
}
}
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 {
return this->m_leader;
return this->m_leader;
}

5
components/rpc/MessagingInterface.cpp
View File

@@ -13,7 +13,6 @@
#include "MPIMessageBuilder.h"
MessagingInterface::~MessagingInterface() {
vQueueDelete(m_mpi_rx_queue);
vSemaphoreDelete(m_map_semaphore);
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)
void MessagingInterface::handleRecv(const char* recv_buffer, int recv_size) {
const auto mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(reinterpret_cast<const uint8_t *>(recv_buffer));
void MessagingInterface::handleRecv(std::unique_ptr<std::vector<uint8_t>>&& buffer) {
const auto mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer->data());
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) {
if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_90_DEG_MAP[channel]))) {
ESP_LOGD(TAG, "90deg");
return Orientation_Deg90;
} else if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_180_DEG_MAP[channel]))) {
ESP_LOGD(TAG, "180deg");
return Orientation_Deg180;
} else if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_270_DEG_MAP[channel]))) {
ESP_LOGD(TAG, "270deg");
return Orientation_Deg270;
} else {
ESP_LOGD(TAG, "No orientation detected");
return Orientation_Deg0;
}
}

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

@@ -13,15 +13,15 @@
#include "IConnectionManager.h"
#include "IDiscoveryService.h"
#include "IRPCServer.h"
#include "PtrQueue.h"
#include "BlockingQueue.h"
#include "enums.h"
class CommunicationFactory {
public:
static std::unique_ptr<IConnectionManager> create_connection_manager(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_lossless_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<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>& rx_queue);
};
#endif //COMMUNICATIONFACTORY_H

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

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

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

@@ -5,12 +5,15 @@
#ifndef IRPCSERVER_H
#define IRPCSERVER_H
#include <memory>
#include <vector>
class IRPCServer {
public:
public:
virtual ~IRPCServer() = default;
virtual void startup() = 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

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

@@ -9,6 +9,7 @@
#include <unordered_map>
#include <flatbuffers_generated/TopologyMessage_generated.h>
#include "BlockingQueue.h"
#include "constants/app_comms.h"
#include "CommunicationRouter.h"
@@ -16,8 +17,8 @@ class MessagingInterface {
public:
explicit MessagingInterface()
: m_config_manager(ConfigManager::get_instance()),
m_mpi_rx_queue(xQueueCreate(MAX_RX_BUFFER_SIZE, RX_QUEUE_SIZE)),
m_router(std::make_unique<CommunicationRouter>([this](const char* buffer, const int size) { handleRecv(buffer, 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](std::unique_ptr<std::vector<uint8_t>>&& buffer) { handleRecv(std::move(buffer)); })),
m_map_semaphore(xSemaphoreCreateMutex()) {};
~MessagingInterface();
@@ -31,13 +32,13 @@ public:
uint8_t get_leader() const;
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);
ConfigManager& m_config_manager;
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;
SemaphoreHandle_t m_map_semaphore;
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
#include <memory>
#include <vector>
#include <unordered_set>
#include <vector>
#include "freertos/FreeRTOS.h"
#include "IRPCServer.h"
#include "PtrQueue.h"
#include "BlockingQueue.h"
#include "freertos/FreeRTOS.h"
class TCPServer final : public IRPCServer {
public:
TCPServer(int port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue);
public:
TCPServer(int port, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue);
~TCPServer() override;
void startup() 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);
static bool is_network_connected();
@@ -34,7 +34,7 @@ private:
TaskHandle_t m_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;
std::unordered_set<int> m_clients;

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

@@ -6,36 +6,37 @@
#define UDPSERVER_H
#include <memory>
#include <vector>
#include <unordered_set>
#include <vector>
#include "freertos/FreeRTOS.h"
#include "IRPCServer.h"
#include "PtrQueue.h"
#include "BlockingQueue.h"
#include "freertos/FreeRTOS.h"
class UDPServer final : public IRPCServer {
public:
UDPServer(int rx_port, int tx_port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue);
public:
UDPServer(int rx_port, int tx_port,
const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue);
~UDPServer() override;
void startup() 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);
static bool is_network_connected();
[[noreturn]] static void socket_monitor_thread(void *args);
int m_tx_port;
int m_tx_port;
int m_rx_port;
int m_tx_server_sock;
int m_rx_server_sock;
int m_rx_server_sock;
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

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

@@ -1,3 +1,4 @@
#include <chrono>
#include <memory>
#include "bits/shared_ptr_base.h"
@@ -15,6 +16,8 @@
#include "sys/param.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 MIN(X, Y) (((X) < (Y)) ? (X) : (Y))
@@ -24,7 +27,7 @@
// - tx from board
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_mutex = xSemaphoreCreateMutex();
this->m_clients = std::unordered_set<int>();
@@ -215,7 +218,7 @@ void TCPServer::shutdown() {
} else {
ESP_LOGD(TAG, "TCP Server Received %d bytes\n", 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;
}
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()) {
return -1;
}
const auto length = (uint32_t)size;
for (const auto client_sock : m_clients) {
send(client_sock, &length, 4, 0);
send(client_sock, buffer, length, 0);

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

@@ -1,3 +1,4 @@
#include <chrono>
#include <cstring>
#include <memory>
@@ -17,205 +18,207 @@
#include "wireless/UDPServer.h"
#define TAG "UDPServer"
#define MAX_RX_QUEUE_ENQUEUE_TIMEOUT_MS 50
#define MIN(X, Y) (((X) < (Y)) ? (X) : (Y))
// todo: - authenticate
UDPServer::UDPServer(
const int rx_port, const int tx_port,
const std::shared_ptr<PtrQueue<std::vector<uint8_t>>> &rx_queue) {
this->m_rx_port = rx_port;
this->m_tx_port = tx_port;
this->m_rx_task = nullptr;
this->m_rx_queue = rx_queue;
this->m_rx_server_sock = 0;
this->m_tx_server_sock = 0;
UDPServer::UDPServer(const int rx_port, const int tx_port,
const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue) {
this->m_rx_port = rx_port;
this->m_tx_port = tx_port;
this->m_rx_task = nullptr;
this->m_rx_queue = rx_queue;
this->m_rx_server_sock = 0;
this->m_tx_server_sock = 0;
}
UDPServer::~UDPServer() { this->shutdown(); }
UDPServer::~UDPServer() {
this->shutdown();
}
void UDPServer::startup() {
ESP_LOGI(TAG, "Starting UDP server on port %d", this->m_rx_port);
if (nullptr != this->m_rx_task) {
ESP_LOGW(TAG, "Attempted to start UDP server when already started, "
"ignoring start request");
return;
}
ESP_LOGI(TAG, "Starting UDP server on port %d", this->m_rx_port);
if (nullptr != this->m_rx_task) {
ESP_LOGW(TAG, "Attempted to start UDP server when already started, "
"ignoring start request");
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() {
ESP_LOGI(TAG, "Shutting down UDP server");
if (nullptr != this->m_rx_task) {
vTaskDelete(this->m_rx_task);
close(this->m_rx_server_sock);
close(this->m_tx_server_sock);
this->m_rx_task = nullptr;
this->m_rx_server_sock = -1;
this->m_tx_server_sock = -1;
}
ESP_LOGI(TAG, "Shutting down UDP server");
if (nullptr != this->m_rx_task) {
vTaskDelete(this->m_rx_task);
close(this->m_rx_server_sock);
close(this->m_tx_server_sock);
this->m_rx_task = nullptr;
this->m_rx_server_sock = -1;
this->m_tx_server_sock = -1;
}
}
[[noreturn]] void UDPServer::socket_monitor_thread(void *args) {
const auto that = static_cast<UDPServer *>(args);
const auto that = static_cast<UDPServer *>(args);
while (true) {
ESP_LOGI(TAG, "Attempting to start UDP Server on %d", that->m_rx_port);
while (true) {
ESP_LOGI(TAG, "Attempting to start UDP Server on %d", that->m_rx_port);
if (!is_network_connected()) {
ESP_LOGW(TAG, "Network is disconnected");
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
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;
if (!is_network_connected()) {
ESP_LOGW(TAG, "Network is disconnected");
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
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");
close(that->m_tx_server_sock);
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
}
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), 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() {
esp_netif_ip_info_t ip_info;
esp_netif_t *netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
esp_netif_ip_info_t ip_info;
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) {
return true;
}
if (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) {
return true;
}
if (0 != ip_info.ip.addr) {
return true;
}
if (0 != ip_info.ip.addr) {
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) {
return true;
}
if (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) {
return true;
}
if (0 != ip_info.ip.addr) {
return true;
}
if (0 != ip_info.ip.addr) {
return true;
}
return false;
return false;
}
bool UDPServer::authenticate_client(int sock) {
// todo: authentication (key?)
return 0;
// todo: authentication (key?)
return 0;
}
int UDPServer::send_msg(char *buffer, const uint32_t length) const {
if (!is_network_connected() || m_tx_server_sock == -1) {
return -1;
}
int UDPServer::send_msg(uint8_t *buffer, size_t len) const {
if (!is_network_connected() || m_tx_server_sock == -1) {
return -1;
}
sockaddr_in mcast_dest = {
.sin_family = AF_INET,
.sin_port = htons(m_tx_port),
.sin_addr = {.s_addr = inet_addr(SEND_MCAST)},
};
sockaddr_in mcast_dest = {
.sin_family = AF_INET,
.sin_port = htons(m_tx_port),
.sin_addr = {.s_addr = inet_addr(SEND_MCAST)},
};
uint32_t size = length;
uint32_t size = (uint32_t)len;
iovec iov[2];
iov[0].iov_base = &size;
iov[0].iov_len = 4;
iov[1].iov_base = buffer;
iov[1].iov_len = length;
iovec iov[2];
iov[0].iov_base = &size;
iov[0].iov_len = 4;
iov[1].iov_base = buffer;
iov[1].iov_len = size;
msghdr msg = {};
msg.msg_iov = iov;
msg.msg_iovlen = 2;
msg.msg_name = &mcast_dest;
msg.msg_namelen = sizeof(mcast_dest);
msghdr msg = {};
msg.msg_iov = iov;
msg.msg_iovlen = 2;
msg.msg_name = &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 {
Flatbuffers::SensorMessageBuilder smb{};
// todo: get data from sensor
auto data = m_actuator->get_sensor_data();
const auto [ptr, size] = smb.build_sensor_message(data);
m_messaging_interface->send(reinterpret_cast<char *>(ptr), size, PC_ADDR, SENSOR_TAG, durable);
if (m_actuator) {
auto data = m_actuator->get_sensor_data();
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 "flatbuffers_generated/SensorMessage_generated.h"
#include "util/number_utils.h"
#include "esp_log.h"
#define LOW_DUTY 200
#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);
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_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0));