diff --git a/components/dataLink/DataLinkFrames.cpp b/components/dataLink/DataLinkFrames.cpp index 2e2777f..5f38b85 100644 --- a/components/dataLink/DataLinkFrames.cpp +++ b/components/dataLink/DataLinkFrames.cpp @@ -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>(); diff --git a/components/dataLink/DataLinkManager.cpp b/components/dataLink/DataLinkManager.cpp index ebd56e1..d163fa7 100644 --- a/components/dataLink/DataLinkManager.cpp +++ b/components/dataLink/DataLinkManager.cpp @@ -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_ptrsize() <= 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); diff --git a/components/dataLink/DataLinkScheduler.cpp b/components/dataLink/DataLinkScheduler.cpp index f05e8d1..aea1e9e 100644 --- a/components/dataLink/DataLinkScheduler.cpp +++ b/components/dataLink/DataLinkScheduler.cpp @@ -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(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); diff --git a/components/dataLink/include/Scheduler.h b/components/dataLink/include/Scheduler.h index 19b9750..1783037 100644 --- a/components/dataLink/include/Scheduler.h +++ b/components/dataLink/include/Scheduler.h @@ -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; diff --git a/components/rpc/CommunicationRouter.cpp b/components/rpc/CommunicationRouter.cpp index c5c4c51..412d289 100644 --- a/components/rpc/CommunicationRouter.cpp +++ b/components/rpc/CommunicationRouter.cpp @@ -4,6 +4,7 @@ #include "AngleControlMessageBuilder.h" #include "CommunicationRouter.h" +#include "Frames.h" #include "MPIMessageBuilder.h" #include "OrientationDetection.h" #include "PtrQueue.h" @@ -121,7 +122,7 @@ void CommunicationRouter::route(uint8_t *buffer, size_t size) const { 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); + 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>&& buffer) 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); + this->m_data_link_manager->send(this->m_leader, std::move(buffer), get_frame_type(mpi_message->is_durable(), mpi_message->length()), 0); } else { - this->m_data_link_manager->send(mpi_message->destination(), std::move(buffer), FrameType::MOTOR_TYPE, - 0); + this->m_data_link_manager->send(mpi_message->destination(), std::move(buffer), get_frame_type(mpi_message->is_durable(), mpi_message->length()), 0); } } @@ -185,3 +185,15 @@ CommunicationRouter::get_physically_connected_modules() const { [[nodiscard]] uint8_t CommunicationRouter::get_leader() const { 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; + } + } +} diff --git a/components/rpc/include/CommunicationRouter.h b/components/rpc/include/CommunicationRouter.h index 33ca54f..61f48b2 100644 --- a/components/rpc/include/CommunicationRouter.h +++ b/components/rpc/include/CommunicationRouter.h @@ -80,6 +80,8 @@ private: uint8_t m_module_id; std::chrono::time_point m_last_leader_updated; std::unique_ptr m_discovery_service; + + static FrameType get_frame_type(bool durable, size_t size); }; #endif // COMMUNICATIONROUTER_H