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

Fix generic UDP frame type

Browse Source
This commit is contained in:
Johnathon Slightham
2026-02-28 12:09:35 -05:00
committed by Johnathon Slightham
parent f8b43036b3
commit 71ff7b63ac
6 changed files with 33 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
components/dataLink/DataLinkFrames.cpp
View File

@@ -82,7 +82,7 @@ esp_err_t DataLinkManager::store_fragment(GenericFrame* fragment, uint8_t channe
}
FragmentMetadata& metadata = fragment_map[channel][fragment->receiver_id][fragment->seq_num];
if ((fragment->frag_num-1) > metadata.fragments.size()){
if ((fragment->frag_num == 0) || (fragment->frag_num - 1) >= metadata.fragments.size()){
xSemaphoreGive(rx_fragment_mutex[channel]);
return ESP_ERR_INVALID_STATE;
}
@@ -151,7 +151,7 @@ esp_err_t DataLinkManager::complete_fragment(uint16_t board_id, uint16_t sequenc
if (metadata.num_fragments_rx != metadata.fragments.size()){
return ESP_ERR_INVALID_STATE;
}
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_GENERIC_DATA_LEN; //max data size with n fragments
xSemaphoreGive(rx_fragment_mutex[channel]);
auto combined_data = std::make_unique<std::vector<uint8_t>>();

26
components/dataLink/DataLinkManager.cpp
View File

@@ -106,23 +106,23 @@ DataLinkManager::~DataLinkManager(){
vTaskDelay(pdMS_TO_TICKS(100)); //delay to allow tasks to be killed
if (rip_broadcast_task == NULL){
if (rip_broadcast_task != NULL){
vTaskDelete(rip_broadcast_task);
rip_broadcast_task = NULL;
}
if (rip_ttl_task == NULL){
if (rip_ttl_task != NULL){
vTaskDelete(rip_ttl_task);
rip_ttl_task = NULL;
}
if (scheduler_task == NULL){
if (scheduler_task != NULL){
vTaskDelete(scheduler_task);
scheduler_task = NULL;
}
if (receive_task == NULL){
if (receive_task != NULL){
vTaskDelete(receive_task);
receive_task = NULL;
}
if (send_ack_task == NULL){
if (send_ack_task != NULL){
vTaskDelete(send_ack_task);
send_ack_task = NULL;
}
@@ -378,11 +378,11 @@ esp_err_t DataLinkManager::send(uint8_t dest_board, std::unique_ptr<std::vector<
//calculate number of fragments required (for generic frames only)
uint32_t frag_info = 0;
if (!isControlFrame){
if (buffer->size() <= MAX_CONTROL_DATA_LEN){
frag_info = (1 << 16); //1 total fragment required
if (buffer->size() <= MAX_GENERIC_DATA_LEN){
frag_info = (1 << 16) | 1; //1 total fragment, frag_num=1 (1-indexed)
} else {
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; //frag_num starts at 0 and is incremented to 1 before first send in scheduler
}
}
@@ -570,22 +570,18 @@ esp_err_t DataLinkManager::get_data_from_frame(uint8_t* data, size_t data_len, u
*message_size = header->data_len;
if ((*message_size > MAX_GENERIC_DATA_LEN && total_frag != 1) || (14 + *message_size > data_len)){
if (*message_size > MAX_GENERIC_DATA_LEN || (14 + *message_size > data_len)){
ESP_LOGE(DEBUG_LINK_TAG, "Invalid payload length: %u", *message_size);
return ESP_ERR_INVALID_SIZE;
}
memcpy(message, &data[12], *message_size);
if (total_frag != 1){
geneate_crc_16(data, 12*sizeof(uint8_t) + *message_size, &header->crc_16);
} else {
header->crc_16 = 0;
}
geneate_crc_16(data, 12*sizeof(uint8_t) + *message_size, &header->crc_16);
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){
//CRC mismatch
ESP_LOGE(DEBUG_LINK_TAG, "CRC Mismatch - Generic Frame");
ESP_LOGE(DEBUG_LINK_TAG, "Got 0x%04X but calculated 0x%04X\n", crc_calc, header->crc_16);

2
components/dataLink/DataLinkScheduler.cpp
View File

@@ -240,7 +240,7 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){
}
//need to schedule the next fragment (if total_frags != frag_num)
if ((frame.header.frag_info >> 16) > (frame.header.frag_info & 0xFF) || (frame.last_ack != (frame.header.frag_info >> 16) &&
if ((frame.header.frag_info >> 16) > (frame.header.frag_info & 0xFFFF) || (frame.last_ack != (frame.header.frag_info >> 16) &&
static_cast<FrameType>(GET_TYPE(frame.header.type_flag)) != FrameType::MISC_UDP_GENERIC_TYPE)){
// frame.generic_frame_data_offset += fragment_size;
// ESP_LOGI(DEBUG_LINK_TAG, "scheduling frame %d with frag_info 0x%X", frame.header.seq_num, frame.header.frag_info);

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

@@ -62,7 +62,7 @@ typedef struct _frame_compare {
bool operator()(const SchedulerMetadata& a, const SchedulerMetadata& b) const {
int64_t now = esp_timer_get_time();
double age_a = (now - a.enqueue_time_ns) / 1e6;
double age_b = (now - a.enqueue_time_ns) / 1e6;
double age_b = (now - b.enqueue_time_ns) / 1e6;
// Base priorities: lower is higher priority
double base_a = (IS_CONTROL_FRAME(a.header.type_flag)) ? 0.0 : 10.0;