From c2417ee696dc19c608b5844097da31032d024185 Mon Sep 17 00:00:00 2001 From: Ilia Date: Mon, 15 Jun 2026 10:25:56 +0300 Subject: [PATCH 1/5] feat: add can driver on stm32h7 --- include/libdcnode/can_driver.h | 12 + platform_specific/fdcan/config.cmake | 9 +- .../fdcan/{can_driver.c => stm32g0b1.cpp} | 25 +- platform_specific/fdcan/stm32h753xx.cpp | 215 ++++++++++++++++++ src/dronecan.cpp | 107 +++++++-- 5 files changed, 342 insertions(+), 26 deletions(-) rename platform_specific/fdcan/{can_driver.c => stm32g0b1.cpp} (85%) create mode 100644 platform_specific/fdcan/stm32h753xx.cpp diff --git a/include/libdcnode/can_driver.h b/include/libdcnode/can_driver.h index 66706f8..2913381 100644 --- a/include/libdcnode/can_driver.h +++ b/include/libdcnode/can_driver.h @@ -58,6 +58,18 @@ extern "C" uint64_t canDriverGetRxOverflowCount(); uint64_t canDriverGetErrorCount(); +#ifdef USE_PLATFORM_NODE_V4 + uint64_t canDriverGetTxCount(uint8_t can_driver_idx); + uint64_t canDriverGetRxCount(uint8_t can_driver_idx); + int32_t canDriverGetLastTxStatus(uint8_t can_driver_idx); + int32_t canDriverGetLastInitStatus(uint8_t can_driver_idx); + uint32_t canDriverGetTxErrorCount(uint8_t can_driver_idx); + uint32_t canDriverGetBusOff(uint8_t can_driver_idx); + uint32_t canDriverGetTxFifoFreeLevel(uint8_t can_driver_idx); + uint32_t canDriverGetLastErrorCode(uint8_t can_driver_idx); + uint32_t canDriverGetActivity(uint8_t can_driver_idx); +#endif + #ifdef __cplusplus } #endif diff --git a/platform_specific/fdcan/config.cmake b/platform_specific/fdcan/config.cmake index 1dba486..6860050 100644 --- a/platform_specific/fdcan/config.cmake +++ b/platform_specific/fdcan/config.cmake @@ -1,3 +1,10 @@ set(DRONECAN_PLATFORM_SOURCES - ${CMAKE_CURRENT_LIST_DIR}/can_driver.c ) + +if(APP_PLATFORM STREQUAL "stm32g0b1") + list(APPEND DRONECAN_PLATFORM_SOURCES ${CMAKE_CURRENT_LIST_DIR}/stm32g0b1.cpp) +elseif(APP_PLATFORM STREQUAL "stm32h753xx") + list(APPEND DRONECAN_PLATFORM_SOURCES ${CMAKE_CURRENT_LIST_DIR}/stm32h753xx.cpp) +else() + message(FATAL_ERROR "FDCAN DroneCAN platform is unsupported for APP_PLATFORM='${APP_PLATFORM}'.") +endif() diff --git a/platform_specific/fdcan/can_driver.c b/platform_specific/fdcan/stm32g0b1.cpp similarity index 85% rename from platform_specific/fdcan/can_driver.c rename to platform_specific/fdcan/stm32g0b1.cpp index b0042f1..c4b92be 100644 --- a/platform_specific/fdcan/can_driver.c +++ b/platform_specific/fdcan/stm32g0b1.cpp @@ -6,9 +6,10 @@ */ #include "libdcnode/can_driver.h" + #include -#include "main.h" +#include "main.h" #ifndef NUM_OF_CAN_BUSES #define NUM_OF_CAN_BUSES 1 @@ -17,7 +18,7 @@ extern FDCAN_HandleTypeDef hfdcan1; extern FDCAN_HandleTypeDef hfdcan2; -typedef struct{ +typedef struct { FDCAN_HandleTypeDef* handler; FDCAN_TxHeaderTypeDef tx_header; uint8_t rx_buf[8]; @@ -27,9 +28,9 @@ typedef struct{ } CanDriver; static CanDriver driver[NUM_OF_CAN_BUSES] = { - {.handler = &hfdcan1}, + {.handler = &hfdcan1, .tx_header = {}, .rx_buf = {}, .err_counter = 0, .tx_counter = 0, .rx_counter = 0}, #if NUM_OF_CAN_BUSES >= 2 - {.handler = &hfdcan2} + {.handler = &hfdcan2, .tx_header = {}, .rx_buf = {}, .err_counter = 0, .tx_counter = 0, .rx_counter = 0} #endif }; @@ -39,6 +40,10 @@ void canDriverSetInterfaceName(const char* interface_name) { int16_t canDriverInit(uint32_t can_speed, uint8_t can_driver_idx) { (void)can_speed; + if (can_driver_idx >= NUM_OF_CAN_BUSES) { + return -1; + } + driver[can_driver_idx].tx_header.IdType = FDCAN_EXTENDED_ID; driver[can_driver_idx].tx_header.TxFrameType = FDCAN_DATA_FRAME; driver[can_driver_idx].tx_header.ErrorStateIndicator = FDCAN_ESI_ACTIVE; @@ -63,7 +68,7 @@ int16_t canDriverInit(uint32_t can_speed, uint8_t can_driver_idx) { } int16_t canDriverReceive(CanardCANFrame* const rx_frame, uint8_t can_driver_idx) { - if (rx_frame == NULL) { + if (rx_frame == NULL || can_driver_idx >= NUM_OF_CAN_BUSES) { return 0; } @@ -79,15 +84,19 @@ int16_t canDriverReceive(CanardCANFrame* const rx_frame, uint8_t can_driver_idx) driver[can_driver_idx].rx_counter++; rx_frame->id = (CANARD_CAN_EXT_ID_MASK & (rx_header.Identifier)) | CANARD_CAN_FRAME_EFF; - rx_frame->data_len = rx_header.DataLength >> 4*4; - rx_frame->iface_id = 0; + rx_frame->data_len = rx_header.DataLength >> 16U; + rx_frame->iface_id = can_driver_idx; memcpy(rx_frame->data, driver[can_driver_idx].rx_buf, rx_frame->data_len); return 1; } int16_t canDriverTransmit(const CanardCANFrame* const tx_frame, uint8_t can_driver_idx) { + if (tx_frame == NULL || can_driver_idx >= NUM_OF_CAN_BUSES) { + return 0; + } + driver[can_driver_idx].tx_header.Identifier = tx_frame->id; - driver[can_driver_idx].tx_header.DataLength = tx_frame->data_len << 4*4; + driver[can_driver_idx].tx_header.DataLength = tx_frame->data_len << 16U; HAL_StatusTypeDef res = HAL_FDCAN_AddMessageToTxFifoQ(driver[can_driver_idx].handler, &driver[can_driver_idx].tx_header, diff --git a/platform_specific/fdcan/stm32h753xx.cpp b/platform_specific/fdcan/stm32h753xx.cpp new file mode 100644 index 0000000..3dffc45 --- /dev/null +++ b/platform_specific/fdcan/stm32h753xx.cpp @@ -0,0 +1,215 @@ +/* + * Copyright (C) 2026 Dmitry Ponomarev + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. + */ + +#include "libdcnode/can_driver.h" + +#include + +#include "main.h" + +#ifndef NUM_OF_CAN_BUSES + #define NUM_OF_CAN_BUSES 1 +#endif + +#ifndef DRONECAN_FDCAN_PRIMARY + #define DRONECAN_FDCAN_PRIMARY 1 +#endif + +extern FDCAN_HandleTypeDef hfdcan1; +extern FDCAN_HandleTypeDef hfdcan2; + +typedef struct { + FDCAN_HandleTypeDef* handler; + FDCAN_TxHeaderTypeDef tx_header; + uint8_t rx_buf[8]; + size_t err_counter; + size_t tx_counter; + size_t rx_counter; + HAL_StatusTypeDef last_tx_status; + HAL_StatusTypeDef last_init_status; +} CanDriver; + +static CanDriver driver[NUM_OF_CAN_BUSES] = { +#if DRONECAN_FDCAN_PRIMARY == 2 + {.handler = &hfdcan2, .tx_header = {}, .rx_buf = {}, .err_counter = 0, .tx_counter = 0, .rx_counter = 0, .last_tx_status = HAL_OK, .last_init_status = HAL_OK}, +#if NUM_OF_CAN_BUSES >= 2 + {.handler = &hfdcan1, .tx_header = {}, .rx_buf = {}, .err_counter = 0, .tx_counter = 0, .rx_counter = 0, .last_tx_status = HAL_OK, .last_init_status = HAL_OK} +#endif +#else + {.handler = &hfdcan1, .tx_header = {}, .rx_buf = {}, .err_counter = 0, .tx_counter = 0, .rx_counter = 0, .last_tx_status = HAL_OK, .last_init_status = HAL_OK}, +#if NUM_OF_CAN_BUSES >= 2 + {.handler = &hfdcan2, .tx_header = {}, .rx_buf = {}, .err_counter = 0, .tx_counter = 0, .rx_counter = 0, .last_tx_status = HAL_OK, .last_init_status = HAL_OK} +#endif +#endif +}; + +void canDriverSetInterfaceName(const char* interface_name) { + (void)interface_name; +} + +int16_t canDriverInit(uint32_t can_speed, uint8_t can_driver_idx) { + (void)can_speed; + if (can_driver_idx >= NUM_OF_CAN_BUSES) { + return -1; + } + + driver[can_driver_idx].tx_header.IdType = FDCAN_EXTENDED_ID; + driver[can_driver_idx].tx_header.TxFrameType = FDCAN_DATA_FRAME; + driver[can_driver_idx].tx_header.ErrorStateIndicator = FDCAN_ESI_ACTIVE; + driver[can_driver_idx].tx_header.BitRateSwitch = FDCAN_BRS_OFF; + driver[can_driver_idx].tx_header.FDFormat = FDCAN_CLASSIC_CAN; + driver[can_driver_idx].tx_header.TxEventFifoControl = FDCAN_NO_TX_EVENTS; + driver[can_driver_idx].tx_header.MessageMarker = 0; + + driver[can_driver_idx].last_init_status = HAL_FDCAN_ConfigGlobalFilter( + driver[can_driver_idx].handler, + FDCAN_ACCEPT_IN_RX_FIFO0, + FDCAN_ACCEPT_IN_RX_FIFO0, + FDCAN_REJECT_REMOTE, + FDCAN_REJECT_REMOTE); + if (driver[can_driver_idx].last_init_status != HAL_OK) { + driver[can_driver_idx].err_counter++; + return -1; + } + + driver[can_driver_idx].last_init_status = HAL_FDCAN_Start(driver[can_driver_idx].handler); + if (driver[can_driver_idx].last_init_status != HAL_OK) { + driver[can_driver_idx].err_counter++; + return -1; + } + + return 0; +} + +int16_t canDriverReceive(CanardCANFrame* const rx_frame, uint8_t can_driver_idx) { + if (rx_frame == NULL || can_driver_idx >= NUM_OF_CAN_BUSES) { + return 0; + } + + FDCAN_RxHeaderTypeDef rx_header; + + HAL_StatusTypeDef res = HAL_FDCAN_GetRxMessage(driver[can_driver_idx].handler, + FDCAN_RX_FIFO0, + &rx_header, + driver[can_driver_idx].rx_buf); + if (res != HAL_OK) { + return 0; + } + + driver[can_driver_idx].rx_counter++; + rx_frame->id = (CANARD_CAN_EXT_ID_MASK & (rx_header.Identifier)) | CANARD_CAN_FRAME_EFF; + rx_frame->data_len = static_cast(rx_header.DataLength); + rx_frame->iface_id = can_driver_idx; + memcpy(rx_frame->data, driver[can_driver_idx].rx_buf, rx_frame->data_len); + return 1; +} + +int16_t canDriverTransmit(const CanardCANFrame* const tx_frame, uint8_t can_driver_idx) { + if (tx_frame == NULL || can_driver_idx >= NUM_OF_CAN_BUSES) { + return 0; + } + + driver[can_driver_idx].tx_header.Identifier = tx_frame->id & CANARD_CAN_EXT_ID_MASK; + driver[can_driver_idx].tx_header.DataLength = tx_frame->data_len; + + driver[can_driver_idx].last_tx_status = HAL_FDCAN_AddMessageToTxFifoQ( + driver[can_driver_idx].handler, + &driver[can_driver_idx].tx_header, + (uint8_t*)tx_frame->data); + if (driver[can_driver_idx].last_tx_status == HAL_OK) { + driver[can_driver_idx].tx_counter++; + return 1; + } else { + driver[can_driver_idx].err_counter++; + return 0; + } +} + +uint64_t canDriverGetErrorCount() { + uint64_t errors = 0; + for (uint8_t idx = 0; idx < NUM_OF_CAN_BUSES; idx++) { + FDCAN_ProtocolStatusTypeDef protocol_status = {}; + FDCAN_ErrorCountersTypeDef error_counters = {}; + (void)HAL_FDCAN_GetProtocolStatus(driver[idx].handler, &protocol_status); + (void)HAL_FDCAN_GetErrorCounters(driver[idx].handler, &error_counters); + errors += driver[idx].err_counter; + errors += protocol_status.BusOff; + errors += protocol_status.Warning; + errors += protocol_status.ErrorPassive; + errors += error_counters.TxErrorCnt; + errors += error_counters.RxErrorCnt; + } + return errors; +} + +uint64_t canDriverGetRxOverflowCount() { + return 0; +} + +extern "C" uint64_t canDriverGetTxCount(uint8_t can_driver_idx) { + return (can_driver_idx < NUM_OF_CAN_BUSES) ? driver[can_driver_idx].tx_counter : 0; +} + +extern "C" uint64_t canDriverGetRxCount(uint8_t can_driver_idx) { + return (can_driver_idx < NUM_OF_CAN_BUSES) ? driver[can_driver_idx].rx_counter : 0; +} + +extern "C" int32_t canDriverGetLastTxStatus(uint8_t can_driver_idx) { + return (can_driver_idx < NUM_OF_CAN_BUSES) ? driver[can_driver_idx].last_tx_status : -1; +} + +extern "C" int32_t canDriverGetLastInitStatus(uint8_t can_driver_idx) { + return (can_driver_idx < NUM_OF_CAN_BUSES) ? driver[can_driver_idx].last_init_status : -1; +} + +extern "C" uint32_t canDriverGetTxErrorCount(uint8_t can_driver_idx) { + if (can_driver_idx >= NUM_OF_CAN_BUSES) { + return 0; + } + + FDCAN_ErrorCountersTypeDef error_counters = {}; + (void)HAL_FDCAN_GetErrorCounters(driver[can_driver_idx].handler, &error_counters); + return error_counters.TxErrorCnt; +} + +extern "C" uint32_t canDriverGetBusOff(uint8_t can_driver_idx) { + if (can_driver_idx >= NUM_OF_CAN_BUSES) { + return 0; + } + + FDCAN_ProtocolStatusTypeDef protocol_status = {}; + (void)HAL_FDCAN_GetProtocolStatus(driver[can_driver_idx].handler, &protocol_status); + return protocol_status.BusOff; +} + +extern "C" uint32_t canDriverGetTxFifoFreeLevel(uint8_t can_driver_idx) { + if (can_driver_idx >= NUM_OF_CAN_BUSES) { + return 0; + } + + return HAL_FDCAN_GetTxFifoFreeLevel(driver[can_driver_idx].handler); +} + +extern "C" uint32_t canDriverGetLastErrorCode(uint8_t can_driver_idx) { + if (can_driver_idx >= NUM_OF_CAN_BUSES) { + return 0; + } + + FDCAN_ProtocolStatusTypeDef protocol_status = {}; + (void)HAL_FDCAN_GetProtocolStatus(driver[can_driver_idx].handler, &protocol_status); + return protocol_status.LastErrorCode; +} + +extern "C" uint32_t canDriverGetActivity(uint8_t can_driver_idx) { + if (can_driver_idx >= NUM_OF_CAN_BUSES) { + return 0; + } + + FDCAN_ProtocolStatusTypeDef protocol_status = {}; + (void)HAL_FDCAN_GetProtocolStatus(driver[can_driver_idx].handler, &protocol_status); + return protocol_status.Activity; +} diff --git a/src/dronecan.cpp b/src/dronecan.cpp index 2adbd5c..344af56 100644 --- a/src/dronecan.cpp +++ b/src/dronecan.cpp @@ -24,6 +24,10 @@ #define CANARD_BUFFER_SIZE 1024 #endif +#ifndef NUM_OF_CAN_BUSES +#define NUM_OF_CAN_BUSES 1 +#endif + /** * @brief Encapsulate everything required for a subscriber */ @@ -77,6 +81,64 @@ static DronecanNodeInstance node = {}; static ParamsApi params = {}; static PlatformApi platform = {}; +namespace { + +struct BridgedFrameCacheEntry { + CanardCANFrame frame{}; + uint32_t timestamp_ms{}; + uint8_t iface_id{}; + bool valid{false}; +}; + +static constexpr uint32_t BRIDGE_CACHE_TTL_MS = 50; +static BridgedFrameCacheEntry bridge_cache[32] = {}; +static uint8_t bridge_cache_next_idx = 0; + +bool isSameFrame(const CanardCANFrame& lhs, const CanardCANFrame& rhs) { + return lhs.id == rhs.id && + lhs.data_len == rhs.data_len && + memcmp(lhs.data, rhs.data, lhs.data_len) == 0; +} + +bool wasRecentlyBridged(const CanardCANFrame& frame, uint8_t iface_id, uint32_t now_ms) { + for (const auto& entry : bridge_cache) { + if (entry.valid && + entry.iface_id == iface_id && + now_ms - entry.timestamp_ms <= BRIDGE_CACHE_TTL_MS && + isSameFrame(entry.frame, frame)) { + return true; + } + } + return false; +} + +void markRecentlyBridged(const CanardCANFrame& frame, uint8_t iface_id, uint32_t now_ms) { + bridge_cache[bridge_cache_next_idx] = { + .frame = frame, + .timestamp_ms = now_ms, + .iface_id = iface_id, + .valid = true + }; + bridge_cache_next_idx = (bridge_cache_next_idx + 1U) % (sizeof(bridge_cache) / sizeof(bridge_cache[0])); +} + +void bridgeFrame(const CanardCANFrame& rx_frame, uint8_t rx_iface_idx, uint32_t now_ms) { + if (NUM_OF_CAN_BUSES < 2 || + wasRecentlyBridged(rx_frame, rx_iface_idx, now_ms)) { + return; + } + + for (uint8_t tx_iface_idx = 0; tx_iface_idx < NUM_OF_CAN_BUSES; tx_iface_idx++) { + if (tx_iface_idx == rx_iface_idx) { + continue; + } + (void)platform.can.send(&rx_frame, tx_iface_idx); + markRecentlyBridged(rx_frame, tx_iface_idx, now_ms); + } +} + +} // namespace + static bool shouldAcceptTransfer(const CanardInstance *ins, uint64_t *out_data_type_signature, uint16_t data_type_id, @@ -273,13 +335,19 @@ static uint8_t uavcanProcessSending() uint8_t tx_frames_counter = 0; while (txf) { - const int tx_res = platform.can.send(txf, CAN_DRIVER_FIRST); - if (tx_res > 0) - { + bool sent = false; + bool failed = false; + for (uint8_t iface_idx = 0; iface_idx < NUM_OF_CAN_BUSES; iface_idx++) { + const int tx_res = platform.can.send(txf, iface_idx); + sent = sent || tx_res > 0; + failed = failed || tx_res < 0; + } + + if (sent) { canardPopTxQueue(&node.g_canard); txf = canardPeekTxQueue(&node.g_canard); tx_frames_counter++; - } else if (tx_res < 0) { + } else if (failed) { break; } @@ -294,15 +362,18 @@ static uint8_t uavcanProcessSending() static bool uavcanProcessReceiving(uint32_t crnt_time_ms) { CanardCANFrame rx_frame; - for (size_t idx = 0; idx < 10; idx++) - { - int16_t res = platform.can.recv(&rx_frame, CAN_DRIVER_FIRST); - if (res) + for (uint8_t iface_idx = 0; iface_idx < NUM_OF_CAN_BUSES; iface_idx++) { + for (size_t idx = 0; idx < 10; idx++) { - uint64_t crnt_time_us = crnt_time_ms * 1000UL; - canardHandleRxFrame(&node.g_canard, &rx_frame, crnt_time_us); - } else { - break; + int16_t res = platform.can.recv(&rx_frame, iface_idx); + if (res) + { + uint64_t crnt_time_us = crnt_time_ms * 1000UL; + bridgeFrame(rx_frame, iface_idx, crnt_time_ms); + canardHandleRxFrame(&node.g_canard, &rx_frame, crnt_time_us); + } else { + break; + } } } @@ -419,7 +490,7 @@ static void uavcanParamExecuteOpcodeHandle(CanardRxTransfer *transfer) switch (opcode) { case 0: - ok = (params.save() == -1) ? 0 : 1; + ok = (params.save() < 0) ? 0 : 1; break; case 1: ok = (params.resetToDefault() < 0) ? 0 : 1; @@ -484,10 +555,12 @@ int16_t uavcanInitApplication(ParamsApi params_api, PlatformApi platform_api, co params = params_api; - int16_t res = platform.can.init(1000000, CAN_DRIVER_FIRST); - if (res < 0) - { - return res; + for (uint8_t iface_idx = 0; iface_idx < NUM_OF_CAN_BUSES; iface_idx++) { + int16_t res = platform.can.init(1000000, iface_idx); + if (res < 0) + { + return res; + } } canardInit(&node.g_canard, From 9efc75e0794ef09938ca1e6aeab8dd156997b6e0 Mon Sep 17 00:00:00 2001 From: Ilia Date: Mon, 15 Jun 2026 11:05:32 +0300 Subject: [PATCH 2/5] feat: can2 works - cleanup --- include/libdcnode/can_driver.h | 12 ---- platform_specific/fdcan/stm32h753xx.cpp | 86 +++---------------------- 2 files changed, 10 insertions(+), 88 deletions(-) diff --git a/include/libdcnode/can_driver.h b/include/libdcnode/can_driver.h index 2913381..66706f8 100644 --- a/include/libdcnode/can_driver.h +++ b/include/libdcnode/can_driver.h @@ -58,18 +58,6 @@ extern "C" uint64_t canDriverGetRxOverflowCount(); uint64_t canDriverGetErrorCount(); -#ifdef USE_PLATFORM_NODE_V4 - uint64_t canDriverGetTxCount(uint8_t can_driver_idx); - uint64_t canDriverGetRxCount(uint8_t can_driver_idx); - int32_t canDriverGetLastTxStatus(uint8_t can_driver_idx); - int32_t canDriverGetLastInitStatus(uint8_t can_driver_idx); - uint32_t canDriverGetTxErrorCount(uint8_t can_driver_idx); - uint32_t canDriverGetBusOff(uint8_t can_driver_idx); - uint32_t canDriverGetTxFifoFreeLevel(uint8_t can_driver_idx); - uint32_t canDriverGetLastErrorCode(uint8_t can_driver_idx); - uint32_t canDriverGetActivity(uint8_t can_driver_idx); -#endif - #ifdef __cplusplus } #endif diff --git a/platform_specific/fdcan/stm32h753xx.cpp b/platform_specific/fdcan/stm32h753xx.cpp index 3dffc45..1e66cb8 100644 --- a/platform_specific/fdcan/stm32h753xx.cpp +++ b/platform_specific/fdcan/stm32h753xx.cpp @@ -29,20 +29,18 @@ typedef struct { size_t err_counter; size_t tx_counter; size_t rx_counter; - HAL_StatusTypeDef last_tx_status; - HAL_StatusTypeDef last_init_status; } CanDriver; static CanDriver driver[NUM_OF_CAN_BUSES] = { #if DRONECAN_FDCAN_PRIMARY == 2 - {.handler = &hfdcan2, .tx_header = {}, .rx_buf = {}, .err_counter = 0, .tx_counter = 0, .rx_counter = 0, .last_tx_status = HAL_OK, .last_init_status = HAL_OK}, + {.handler = &hfdcan2, .tx_header = {}, .rx_buf = {}, .err_counter = 0, .tx_counter = 0, .rx_counter = 0}, #if NUM_OF_CAN_BUSES >= 2 - {.handler = &hfdcan1, .tx_header = {}, .rx_buf = {}, .err_counter = 0, .tx_counter = 0, .rx_counter = 0, .last_tx_status = HAL_OK, .last_init_status = HAL_OK} + {.handler = &hfdcan1, .tx_header = {}, .rx_buf = {}, .err_counter = 0, .tx_counter = 0, .rx_counter = 0} #endif #else - {.handler = &hfdcan1, .tx_header = {}, .rx_buf = {}, .err_counter = 0, .tx_counter = 0, .rx_counter = 0, .last_tx_status = HAL_OK, .last_init_status = HAL_OK}, + {.handler = &hfdcan1, .tx_header = {}, .rx_buf = {}, .err_counter = 0, .tx_counter = 0, .rx_counter = 0}, #if NUM_OF_CAN_BUSES >= 2 - {.handler = &hfdcan2, .tx_header = {}, .rx_buf = {}, .err_counter = 0, .tx_counter = 0, .rx_counter = 0, .last_tx_status = HAL_OK, .last_init_status = HAL_OK} + {.handler = &hfdcan2, .tx_header = {}, .rx_buf = {}, .err_counter = 0, .tx_counter = 0, .rx_counter = 0} #endif #endif }; @@ -65,19 +63,19 @@ int16_t canDriverInit(uint32_t can_speed, uint8_t can_driver_idx) { driver[can_driver_idx].tx_header.TxEventFifoControl = FDCAN_NO_TX_EVENTS; driver[can_driver_idx].tx_header.MessageMarker = 0; - driver[can_driver_idx].last_init_status = HAL_FDCAN_ConfigGlobalFilter( + HAL_StatusTypeDef res = HAL_FDCAN_ConfigGlobalFilter( driver[can_driver_idx].handler, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_REJECT_REMOTE, FDCAN_REJECT_REMOTE); - if (driver[can_driver_idx].last_init_status != HAL_OK) { + if (res != HAL_OK) { driver[can_driver_idx].err_counter++; return -1; } - driver[can_driver_idx].last_init_status = HAL_FDCAN_Start(driver[can_driver_idx].handler); - if (driver[can_driver_idx].last_init_status != HAL_OK) { + res = HAL_FDCAN_Start(driver[can_driver_idx].handler); + if (res != HAL_OK) { driver[can_driver_idx].err_counter++; return -1; } @@ -116,11 +114,11 @@ int16_t canDriverTransmit(const CanardCANFrame* const tx_frame, uint8_t can_driv driver[can_driver_idx].tx_header.Identifier = tx_frame->id & CANARD_CAN_EXT_ID_MASK; driver[can_driver_idx].tx_header.DataLength = tx_frame->data_len; - driver[can_driver_idx].last_tx_status = HAL_FDCAN_AddMessageToTxFifoQ( + HAL_StatusTypeDef res = HAL_FDCAN_AddMessageToTxFifoQ( driver[can_driver_idx].handler, &driver[can_driver_idx].tx_header, (uint8_t*)tx_frame->data); - if (driver[can_driver_idx].last_tx_status == HAL_OK) { + if (res == HAL_OK) { driver[can_driver_idx].tx_counter++; return 1; } else { @@ -149,67 +147,3 @@ uint64_t canDriverGetErrorCount() { uint64_t canDriverGetRxOverflowCount() { return 0; } - -extern "C" uint64_t canDriverGetTxCount(uint8_t can_driver_idx) { - return (can_driver_idx < NUM_OF_CAN_BUSES) ? driver[can_driver_idx].tx_counter : 0; -} - -extern "C" uint64_t canDriverGetRxCount(uint8_t can_driver_idx) { - return (can_driver_idx < NUM_OF_CAN_BUSES) ? driver[can_driver_idx].rx_counter : 0; -} - -extern "C" int32_t canDriverGetLastTxStatus(uint8_t can_driver_idx) { - return (can_driver_idx < NUM_OF_CAN_BUSES) ? driver[can_driver_idx].last_tx_status : -1; -} - -extern "C" int32_t canDriverGetLastInitStatus(uint8_t can_driver_idx) { - return (can_driver_idx < NUM_OF_CAN_BUSES) ? driver[can_driver_idx].last_init_status : -1; -} - -extern "C" uint32_t canDriverGetTxErrorCount(uint8_t can_driver_idx) { - if (can_driver_idx >= NUM_OF_CAN_BUSES) { - return 0; - } - - FDCAN_ErrorCountersTypeDef error_counters = {}; - (void)HAL_FDCAN_GetErrorCounters(driver[can_driver_idx].handler, &error_counters); - return error_counters.TxErrorCnt; -} - -extern "C" uint32_t canDriverGetBusOff(uint8_t can_driver_idx) { - if (can_driver_idx >= NUM_OF_CAN_BUSES) { - return 0; - } - - FDCAN_ProtocolStatusTypeDef protocol_status = {}; - (void)HAL_FDCAN_GetProtocolStatus(driver[can_driver_idx].handler, &protocol_status); - return protocol_status.BusOff; -} - -extern "C" uint32_t canDriverGetTxFifoFreeLevel(uint8_t can_driver_idx) { - if (can_driver_idx >= NUM_OF_CAN_BUSES) { - return 0; - } - - return HAL_FDCAN_GetTxFifoFreeLevel(driver[can_driver_idx].handler); -} - -extern "C" uint32_t canDriverGetLastErrorCode(uint8_t can_driver_idx) { - if (can_driver_idx >= NUM_OF_CAN_BUSES) { - return 0; - } - - FDCAN_ProtocolStatusTypeDef protocol_status = {}; - (void)HAL_FDCAN_GetProtocolStatus(driver[can_driver_idx].handler, &protocol_status); - return protocol_status.LastErrorCode; -} - -extern "C" uint32_t canDriverGetActivity(uint8_t can_driver_idx) { - if (can_driver_idx >= NUM_OF_CAN_BUSES) { - return 0; - } - - FDCAN_ProtocolStatusTypeDef protocol_status = {}; - (void)HAL_FDCAN_GetProtocolStatus(driver[can_driver_idx].handler, &protocol_status); - return protocol_status.Activity; -} From 9c0f9d1b566249b0056fc429ecca1d35db6bc53b Mon Sep 17 00:00:00 2001 From: Ilia Date: Mon, 15 Jun 2026 15:12:03 +0300 Subject: [PATCH 3/5] chore: author edit --- platform_specific/fdcan/stm32h753xx.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platform_specific/fdcan/stm32h753xx.cpp b/platform_specific/fdcan/stm32h753xx.cpp index 1e66cb8..830c758 100644 --- a/platform_specific/fdcan/stm32h753xx.cpp +++ b/platform_specific/fdcan/stm32h753xx.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2026 Dmitry Ponomarev + * Copyright (C) 2026 Ilia Kliantsevich * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at https://mozilla.org/MPL/2.0/. From 5759da5fff414b15ed9c76ea15f126713eb8f224 Mon Sep 17 00:00:00 2001 From: Ilia Date: Tue, 16 Jun 2026 11:30:37 +0300 Subject: [PATCH 4/5] fix: remove automatic CAN synchronisation. FDCAN1 and FDCAN2 are independent --- src/dronecan.cpp | 59 ------------------------------------------------ 1 file changed, 59 deletions(-) diff --git a/src/dronecan.cpp b/src/dronecan.cpp index 344af56..8ffb7b8 100644 --- a/src/dronecan.cpp +++ b/src/dronecan.cpp @@ -81,64 +81,6 @@ static DronecanNodeInstance node = {}; static ParamsApi params = {}; static PlatformApi platform = {}; -namespace { - -struct BridgedFrameCacheEntry { - CanardCANFrame frame{}; - uint32_t timestamp_ms{}; - uint8_t iface_id{}; - bool valid{false}; -}; - -static constexpr uint32_t BRIDGE_CACHE_TTL_MS = 50; -static BridgedFrameCacheEntry bridge_cache[32] = {}; -static uint8_t bridge_cache_next_idx = 0; - -bool isSameFrame(const CanardCANFrame& lhs, const CanardCANFrame& rhs) { - return lhs.id == rhs.id && - lhs.data_len == rhs.data_len && - memcmp(lhs.data, rhs.data, lhs.data_len) == 0; -} - -bool wasRecentlyBridged(const CanardCANFrame& frame, uint8_t iface_id, uint32_t now_ms) { - for (const auto& entry : bridge_cache) { - if (entry.valid && - entry.iface_id == iface_id && - now_ms - entry.timestamp_ms <= BRIDGE_CACHE_TTL_MS && - isSameFrame(entry.frame, frame)) { - return true; - } - } - return false; -} - -void markRecentlyBridged(const CanardCANFrame& frame, uint8_t iface_id, uint32_t now_ms) { - bridge_cache[bridge_cache_next_idx] = { - .frame = frame, - .timestamp_ms = now_ms, - .iface_id = iface_id, - .valid = true - }; - bridge_cache_next_idx = (bridge_cache_next_idx + 1U) % (sizeof(bridge_cache) / sizeof(bridge_cache[0])); -} - -void bridgeFrame(const CanardCANFrame& rx_frame, uint8_t rx_iface_idx, uint32_t now_ms) { - if (NUM_OF_CAN_BUSES < 2 || - wasRecentlyBridged(rx_frame, rx_iface_idx, now_ms)) { - return; - } - - for (uint8_t tx_iface_idx = 0; tx_iface_idx < NUM_OF_CAN_BUSES; tx_iface_idx++) { - if (tx_iface_idx == rx_iface_idx) { - continue; - } - (void)platform.can.send(&rx_frame, tx_iface_idx); - markRecentlyBridged(rx_frame, tx_iface_idx, now_ms); - } -} - -} // namespace - static bool shouldAcceptTransfer(const CanardInstance *ins, uint64_t *out_data_type_signature, uint16_t data_type_id, @@ -369,7 +311,6 @@ static bool uavcanProcessReceiving(uint32_t crnt_time_ms) if (res) { uint64_t crnt_time_us = crnt_time_ms * 1000UL; - bridgeFrame(rx_frame, iface_idx, crnt_time_ms); canardHandleRxFrame(&node.g_canard, &rx_frame, crnt_time_us); } else { break; From d5c6274cf2527b6e75cc3381f66850b1b833f8de Mon Sep 17 00:00:00 2001 From: Ilia Date: Wed, 17 Jun 2026 10:57:48 +0300 Subject: [PATCH 5/5] fix: stabilise after regression tests on v2, v3 --- platform_specific/fdcan/stm32h753xx.cpp | 112 +++++++++++++++--------- src/dronecan.cpp | 48 ++++------ 2 files changed, 86 insertions(+), 74 deletions(-) diff --git a/platform_specific/fdcan/stm32h753xx.cpp b/platform_specific/fdcan/stm32h753xx.cpp index 830c758..07dbc23 100644 --- a/platform_specific/fdcan/stm32h753xx.cpp +++ b/platform_specific/fdcan/stm32h753xx.cpp @@ -49,82 +49,108 @@ void canDriverSetInterfaceName(const char* interface_name) { (void)interface_name; } -int16_t canDriverInit(uint32_t can_speed, uint8_t can_driver_idx) { - (void)can_speed; - if (can_driver_idx >= NUM_OF_CAN_BUSES) { +static int16_t canDriverInitPhysical(uint8_t physical_idx) { + if (physical_idx >= NUM_OF_CAN_BUSES) { return -1; } - driver[can_driver_idx].tx_header.IdType = FDCAN_EXTENDED_ID; - driver[can_driver_idx].tx_header.TxFrameType = FDCAN_DATA_FRAME; - driver[can_driver_idx].tx_header.ErrorStateIndicator = FDCAN_ESI_ACTIVE; - driver[can_driver_idx].tx_header.BitRateSwitch = FDCAN_BRS_OFF; - driver[can_driver_idx].tx_header.FDFormat = FDCAN_CLASSIC_CAN; - driver[can_driver_idx].tx_header.TxEventFifoControl = FDCAN_NO_TX_EVENTS; - driver[can_driver_idx].tx_header.MessageMarker = 0; + driver[physical_idx].tx_header.IdType = FDCAN_EXTENDED_ID; + driver[physical_idx].tx_header.TxFrameType = FDCAN_DATA_FRAME; + driver[physical_idx].tx_header.ErrorStateIndicator = FDCAN_ESI_ACTIVE; + driver[physical_idx].tx_header.BitRateSwitch = FDCAN_BRS_OFF; + driver[physical_idx].tx_header.FDFormat = FDCAN_CLASSIC_CAN; + driver[physical_idx].tx_header.TxEventFifoControl = FDCAN_NO_TX_EVENTS; + driver[physical_idx].tx_header.MessageMarker = 0; HAL_StatusTypeDef res = HAL_FDCAN_ConfigGlobalFilter( - driver[can_driver_idx].handler, + driver[physical_idx].handler, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_REJECT_REMOTE, FDCAN_REJECT_REMOTE); if (res != HAL_OK) { - driver[can_driver_idx].err_counter++; + driver[physical_idx].err_counter++; return -1; } - res = HAL_FDCAN_Start(driver[can_driver_idx].handler); + res = HAL_FDCAN_Start(driver[physical_idx].handler); if (res != HAL_OK) { - driver[can_driver_idx].err_counter++; + driver[physical_idx].err_counter++; return -1; } return 0; } -int16_t canDriverReceive(CanardCANFrame* const rx_frame, uint8_t can_driver_idx) { - if (rx_frame == NULL || can_driver_idx >= NUM_OF_CAN_BUSES) { - return 0; +int16_t canDriverInit(uint32_t can_speed, uint8_t can_driver_idx) { + (void)can_speed; + if (can_driver_idx != CAN_DRIVER_FIRST) { + return -1; } - FDCAN_RxHeaderTypeDef rx_header; + for (uint8_t physical_idx = 0; physical_idx < NUM_OF_CAN_BUSES; physical_idx++) { + const int16_t res = canDriverInitPhysical(physical_idx); + if (res < 0) { + return res; + } + } - HAL_StatusTypeDef res = HAL_FDCAN_GetRxMessage(driver[can_driver_idx].handler, - FDCAN_RX_FIFO0, - &rx_header, - driver[can_driver_idx].rx_buf); - if (res != HAL_OK) { + return 0; +} + +int16_t canDriverReceive(CanardCANFrame* const rx_frame, uint8_t can_driver_idx) { + if (rx_frame == NULL || can_driver_idx != CAN_DRIVER_FIRST) { return 0; } - driver[can_driver_idx].rx_counter++; - rx_frame->id = (CANARD_CAN_EXT_ID_MASK & (rx_header.Identifier)) | CANARD_CAN_FRAME_EFF; - rx_frame->data_len = static_cast(rx_header.DataLength); - rx_frame->iface_id = can_driver_idx; - memcpy(rx_frame->data, driver[can_driver_idx].rx_buf, rx_frame->data_len); - return 1; + static uint8_t next_physical_idx = 0; + for (uint8_t attempt = 0; attempt < NUM_OF_CAN_BUSES; attempt++) { + const uint8_t physical_idx = static_cast((next_physical_idx + attempt) % NUM_OF_CAN_BUSES); + FDCAN_RxHeaderTypeDef rx_header; + + HAL_StatusTypeDef res = HAL_FDCAN_GetRxMessage(driver[physical_idx].handler, + FDCAN_RX_FIFO0, + &rx_header, + driver[physical_idx].rx_buf); + if (res != HAL_OK) { + continue; + } + + driver[physical_idx].rx_counter++; + rx_frame->id = (CANARD_CAN_EXT_ID_MASK & (rx_header.Identifier)) | CANARD_CAN_FRAME_EFF; + rx_frame->data_len = static_cast(rx_header.DataLength); + rx_frame->iface_id = physical_idx; + memcpy(rx_frame->data, driver[physical_idx].rx_buf, rx_frame->data_len); + next_physical_idx = static_cast((physical_idx + 1) % NUM_OF_CAN_BUSES); + return 1; + } + + return 0; } int16_t canDriverTransmit(const CanardCANFrame* const tx_frame, uint8_t can_driver_idx) { - if (tx_frame == NULL || can_driver_idx >= NUM_OF_CAN_BUSES) { + if (tx_frame == NULL || can_driver_idx != CAN_DRIVER_FIRST) { return 0; } - driver[can_driver_idx].tx_header.Identifier = tx_frame->id & CANARD_CAN_EXT_ID_MASK; - driver[can_driver_idx].tx_header.DataLength = tx_frame->data_len; - - HAL_StatusTypeDef res = HAL_FDCAN_AddMessageToTxFifoQ( - driver[can_driver_idx].handler, - &driver[can_driver_idx].tx_header, - (uint8_t*)tx_frame->data); - if (res == HAL_OK) { - driver[can_driver_idx].tx_counter++; - return 1; - } else { - driver[can_driver_idx].err_counter++; - return 0; + bool sent = false; + for (uint8_t physical_idx = 0; physical_idx < NUM_OF_CAN_BUSES; physical_idx++) { + driver[physical_idx].tx_header.Identifier = tx_frame->id & CANARD_CAN_EXT_ID_MASK; + driver[physical_idx].tx_header.DataLength = tx_frame->data_len; + + HAL_StatusTypeDef res = HAL_FDCAN_AddMessageToTxFifoQ( + driver[physical_idx].handler, + &driver[physical_idx].tx_header, + (uint8_t*)tx_frame->data); + if (res == HAL_OK) { + driver[physical_idx].tx_counter++; + sent = true; + } else { + driver[physical_idx].err_counter++; + } } + + return sent ? 1 : 0; } uint64_t canDriverGetErrorCount() { diff --git a/src/dronecan.cpp b/src/dronecan.cpp index 8ffb7b8..2adbd5c 100644 --- a/src/dronecan.cpp +++ b/src/dronecan.cpp @@ -24,10 +24,6 @@ #define CANARD_BUFFER_SIZE 1024 #endif -#ifndef NUM_OF_CAN_BUSES -#define NUM_OF_CAN_BUSES 1 -#endif - /** * @brief Encapsulate everything required for a subscriber */ @@ -277,19 +273,13 @@ static uint8_t uavcanProcessSending() uint8_t tx_frames_counter = 0; while (txf) { - bool sent = false; - bool failed = false; - for (uint8_t iface_idx = 0; iface_idx < NUM_OF_CAN_BUSES; iface_idx++) { - const int tx_res = platform.can.send(txf, iface_idx); - sent = sent || tx_res > 0; - failed = failed || tx_res < 0; - } - - if (sent) { + const int tx_res = platform.can.send(txf, CAN_DRIVER_FIRST); + if (tx_res > 0) + { canardPopTxQueue(&node.g_canard); txf = canardPeekTxQueue(&node.g_canard); tx_frames_counter++; - } else if (failed) { + } else if (tx_res < 0) { break; } @@ -304,17 +294,15 @@ static uint8_t uavcanProcessSending() static bool uavcanProcessReceiving(uint32_t crnt_time_ms) { CanardCANFrame rx_frame; - for (uint8_t iface_idx = 0; iface_idx < NUM_OF_CAN_BUSES; iface_idx++) { - for (size_t idx = 0; idx < 10; idx++) + for (size_t idx = 0; idx < 10; idx++) + { + int16_t res = platform.can.recv(&rx_frame, CAN_DRIVER_FIRST); + if (res) { - int16_t res = platform.can.recv(&rx_frame, iface_idx); - if (res) - { - uint64_t crnt_time_us = crnt_time_ms * 1000UL; - canardHandleRxFrame(&node.g_canard, &rx_frame, crnt_time_us); - } else { - break; - } + uint64_t crnt_time_us = crnt_time_ms * 1000UL; + canardHandleRxFrame(&node.g_canard, &rx_frame, crnt_time_us); + } else { + break; } } @@ -431,7 +419,7 @@ static void uavcanParamExecuteOpcodeHandle(CanardRxTransfer *transfer) switch (opcode) { case 0: - ok = (params.save() < 0) ? 0 : 1; + ok = (params.save() == -1) ? 0 : 1; break; case 1: ok = (params.resetToDefault() < 0) ? 0 : 1; @@ -496,12 +484,10 @@ int16_t uavcanInitApplication(ParamsApi params_api, PlatformApi platform_api, co params = params_api; - for (uint8_t iface_idx = 0; iface_idx < NUM_OF_CAN_BUSES; iface_idx++) { - int16_t res = platform.can.init(1000000, iface_idx); - if (res < 0) - { - return res; - } + int16_t res = platform.can.init(1000000, CAN_DRIVER_FIRST); + if (res < 0) + { + return res; } canardInit(&node.g_canard,