botchain/firmware
1
0
Fork 0
mirror of https://github.com/BotChain-Robots/firmware.git synced 2026-07-08 09:37: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]; 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]); xSemaphoreGive(rx_fragment_mutex[channel]);
return ESP_ERR_INVALID_STATE; 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()){ if (metadata.num_fragments_rx != metadata.fragments.size()){
return ESP_ERR_INVALID_STATE; 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]); xSemaphoreGive(rx_fragment_mutex[channel]);
auto combined_data = std::make_unique<std::vector<uint8_t>>(); 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 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); vTaskDelete(rip_broadcast_task);
rip_broadcast_task = NULL; rip_broadcast_task = NULL;
} }
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;
} }
@@ -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) //calculate number of fragments required (for generic frames only)
uint32_t frag_info = 0; uint32_t frag_info = 0;
if (!isControlFrame){ if (!isControlFrame){
if (buffer->size() <= MAX_CONTROL_DATA_LEN){ if (buffer->size() <= MAX_GENERIC_DATA_LEN){
frag_info = (1 << 16); //1 total fragment required frag_info = (1 << 16) | 1; //1 total fragment, frag_num=1 (1-indexed)
} else { } else {
uint32_t total_frags = (buffer->size() + 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; //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; *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); ESP_LOGE(DEBUG_LINK_TAG, "Invalid payload length: %u", *message_size);
return ESP_ERR_INVALID_SIZE; return ESP_ERR_INVALID_SIZE;
} }
memcpy(message, &data[12], *message_size); memcpy(message, &data[12], *message_size);
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 {
header->crc_16 = 0;
}
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){
//CRC mismatch //CRC mismatch
ESP_LOGE(DEBUG_LINK_TAG, "CRC Mismatch - Generic Frame"); 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); 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) //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)){ static_cast<FrameType>(GET_TYPE(frame.header.type_flag)) != FrameType::MISC_UDP_GENERIC_TYPE)){
// frame.generic_frame_data_offset += fragment_size; // 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); // 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 { bool operator()(const SchedulerMetadata& a, const SchedulerMetadata& b) const {
int64_t now = esp_timer_get_time(); int64_t now = esp_timer_get_time();
double age_a = (now - a.enqueue_time_ns) / 1e6; 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 // Base priorities: lower is higher priority
double base_a = (IS_CONTROL_FRAME(a.header.type_flag)) ? 0.0 : 10.0; double base_a = (IS_CONTROL_FRAME(a.header.type_flag)) ? 0.0 : 10.0;

20
components/rpc/CommunicationRouter.cpp
View File

@@ -4,6 +4,7 @@
#include "AngleControlMessageBuilder.h" #include "AngleControlMessageBuilder.h"
#include "CommunicationRouter.h" #include "CommunicationRouter.h"
#include "Frames.h"
#include "MPIMessageBuilder.h" #include "MPIMessageBuilder.h"
#include "OrientationDetection.h" #include "OrientationDetection.h"
#include "PtrQueue.h" #include "PtrQueue.h"
@@ -121,7 +122,7 @@ void CommunicationRouter::route(uint8_t *buffer, size_t size) const {
u_buffer->resize(size); u_buffer->resize(size);
memcpy(u_buffer->data(), buffer, size); memcpy(u_buffer->data(), buffer, size);
this->m_data_link_manager->send(dest, std::move(u_buffer), FrameType::MOTOR_TYPE, 0); this->m_data_link_manager->send(dest, std::move(u_buffer), get_frame_type(mpi_message->is_durable(), mpi_message->length()), 0);
} }
} }
@@ -144,10 +145,9 @@ void CommunicationRouter::route(std::unique_ptr<std::vector<uint8_t>>&& buffer)
this->m_lossy_server->send_msg(buffer->data(), buffer->size()); this->m_lossy_server->send_msg(buffer->data(), buffer->size());
} }
} else if (mpi_message->destination() == PC_ADDR) { } else if (mpi_message->destination() == PC_ADDR) {
this->m_data_link_manager->send(this->m_leader, std::move(buffer), FrameType::MOTOR_TYPE, 0); this->m_data_link_manager->send(this->m_leader, std::move(buffer), get_frame_type(mpi_message->is_durable(), mpi_message->length()), 0);
} else { } else {
this->m_data_link_manager->send(mpi_message->destination(), std::move(buffer), FrameType::MOTOR_TYPE, this->m_data_link_manager->send(mpi_message->destination(), std::move(buffer), get_frame_type(mpi_message->is_durable(), mpi_message->length()), 0);
0);
} }
} }
@@ -185,3 +185,15 @@ CommunicationRouter::get_physically_connected_modules() const {
[[nodiscard]] uint8_t CommunicationRouter::get_leader() const { [[nodiscard]] uint8_t CommunicationRouter::get_leader() const {
return this->m_leader; return this->m_leader;
} }
FrameType CommunicationRouter::get_frame_type(bool durable, size_t size) {
if (durable) {
return FrameType::MISC_GENERIC_TYPE;
} else {
if (size > MAX_FRAME_SIZE) {
return FrameType::MISC_UDP_GENERIC_TYPE;
} else {
return FrameType::MISC_CONTROL_TYPE;
}
}
}

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

@@ -80,6 +80,8 @@ private:
uint8_t m_module_id; uint8_t m_module_id;
std::chrono::time_point<std::chrono::system_clock> m_last_leader_updated; std::chrono::time_point<std::chrono::system_clock> m_last_leader_updated;
std::unique_ptr<IDiscoveryService> m_discovery_service; std::unique_ptr<IDiscoveryService> m_discovery_service;
static FrameType get_frame_type(bool durable, size_t size);
}; };
#endif // COMMUNICATIONROUTER_H #endif // COMMUNICATIONROUTER_H