From 2715b65f294afb1dcb5ec6a4391476a6d7010c35 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Sat, 2 May 2026 15:22:23 -0700 Subject: [PATCH 01/34] fix: remove LOG_DEBUG calls unsafe in ISR context and reduce CAN init verbosity MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Remove LOG_DEBUG from canardSTM32Transmit() failure path — this will run in ISR callback context after the TX ISR migration and calling printf from an ISR is undefined behaviour. Collapse 5 intermediate canardSTM32ComputeTimings() debug calls to the single summary line already present at the end of the function. Remove per-frame success-path LOG_DEBUG calls in dronecan.c transfer handlers. These fire on every received GPS fix, NodeStatus and battery update and are too noisy for upstream submission. --- src/main/drivers/dronecan/dronecan.c | 11 ----------- .../dronecan/libcanard/canard_stm32f7xx_driver.c | 11 +---------- 2 files changed, 1 insertion(+), 21 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 42e03212b38..a60eff54971 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -92,7 +92,6 @@ void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) { return; } dronecanGPSReceiveGNSSAuxiliary(&gnssAuxiliary); - LOG_DEBUG(CAN, "GNSS Auxiliary: Sats=%d HDOP=%.1f", gnssAuxiliary.sats_used, (double)gnssAuxiliary.hdop); } void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) { @@ -104,7 +103,6 @@ void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) { return; } dronecanGPSReceiveGNSSFix(&gnssFix); - LOG_DEBUG(CAN, "GNSS Fix received"); } void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) { @@ -116,7 +114,6 @@ void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) { return; } dronecanGPSReceiveGNSSFix2(&gnssFix2); - LOG_DEBUG(CAN, "GNSS Fix2 received"); } void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) { @@ -127,7 +124,6 @@ void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) { LOG_DEBUG(CAN, "RTCMStream decode failed"); return; } - LOG_DEBUG(CAN, "GNSS RTCM"); } void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) { @@ -139,7 +135,6 @@ void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) { return; } dronecanBatterySensorReceiveInfo(&batteryInfo); - LOG_DEBUG(CAN, "Battery Info"); } /* @@ -300,12 +295,6 @@ bool shouldAcceptTransfer(const CanardInstance *ins, */ void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { // switch on data type ID to pass to the right handler function - LOG_DEBUG(CAN, "Transfer type: %u, Transfer ID: %u ", transfer->transfer_type, transfer->data_type_id); - //LOG_DEBUG(CAN, "0x"); - //LOG_BUFFER_ERROR(SYSTEM, transfer->payload_head, transfer->payload_len); - // for (int i = 0; i < transfer->payload_len; i++) { - // LOG_DEBUG(CAN,"%02x", transfer->payload_head[i]); - // } if (transfer->transfer_type == CanardTransferTypeRequest) { // check if we want to handle a specific service request diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index dd7d3e1b357..a5d53c999ee 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -163,13 +163,10 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { memcpy(txData, tx_frame->data, tx_frame->data_len); returnCode = HAL_CAN_AddTxMessage(&hcan1, &txHeader, txData, &txMailbox); - if( returnCode == HAL_OK) { - // LOG_DEBUG(CAN, "Successfully sent message with id: %lu", tx_frame->id); + if (returnCode == HAL_OK) { return 1; } - LOG_DEBUG(CAN, "Failed at adding message with id: %lu to Tx Queue. Error: %lu", tx_frame->id, returnCode); - // TX failed (FIFO full or other error) - return 0 to signal retry needed return 0; } @@ -320,9 +317,6 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi * 125 kbps 16 17 */ const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 18; - LOG_DEBUG(CAN, "Baudrate: %lu", target_bitrate); - LOG_DEBUG(CAN, "Max Quanta per bit: %i", max_quanta_per_bit); - LOG_DEBUG(CAN, "Pclk1: %lu", pclk); static const int MaxSamplePointLocation = 900; /* @@ -336,7 +330,6 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi * PRESCALER_BS = PCLK / BITRATE */ const uint32_t prescaler_bs = pclk / target_bitrate; - LOG_DEBUG(CAN, "Prescaler BS product: %lu", prescaler_bs); /* * Searching for such prescaler value so that the number of quanta per bit is highest. */ @@ -353,8 +346,6 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi if ((prescaler < 1U) || (prescaler > 1024U)) { return false; // No solution } - LOG_DEBUG(CAN, "Prescaler: %lu", prescaler); - /* * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum. * We need to find the values so that the sample point is as close as possible to the optimal value. From 402990e7d83591c9663c2559bbbba03a78e8f8b8 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Sat, 2 May 2026 15:41:19 -0700 Subject: [PATCH 02/34] drivers: migrate STM32F7 CAN TX to ISR-driven transmission Replace blocking HAL_CAN_AddTxMessage() polling in canardSTM32Transmit() with a SPSC SW ring buffer (TX_QUEUE_SIZE=32) drained by CAN1_TX_IRQHandler via HAL_CAN_TxMailbox{0,1,2}CompleteCallback(). Bootstrapping: when all 3 mailboxes are idle RQCP=0 so TMEIE alone won't fire; seed the HW directly from the main-loop path when GetTxMailboxesFreeLevel()==3. ISR handles all subsequent frames. CAN_IT_TX_MAILBOX_EMPTY is activated on each enqueue and deactivated when the queue drains, keeping the interrupt silent at idle. --- .../libcanard/canard_stm32f7xx_driver.c | 136 ++++++++++++------ 1 file changed, 96 insertions(+), 40 deletions(-) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index a5d53c999ee..0c2320b223a 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -38,6 +38,14 @@ static struct RxBuffer_t { RxFrame_t rxMsg[RX_BUFFER_SIZE]; } RxBuffer; +#define TX_QUEUE_SIZE 32 + +static struct { + CanardCANFrame frames[TX_QUEUE_SIZE]; + volatile uint8_t head; + volatile uint8_t tail; +} canTxQueue; + static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings*out_timings); static void canardSTM32GPIO_Init(void); @@ -87,6 +95,29 @@ uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf) { return (rxBuf->writeIndex - rxBuf->readIndex); } +static bool canTxQueuePush(const CanardCANFrame *frame) { + uint8_t next = (canTxQueue.head + 1) % TX_QUEUE_SIZE; + if (next == canTxQueue.tail) { + return false; + } + canTxQueue.frames[canTxQueue.head] = *frame; + canTxQueue.head = next; + return true; +} + +static bool canTxQueuePop(CanardCANFrame *frame) { + if (canTxQueue.head == canTxQueue.tail) { + return false; + } + *frame = canTxQueue.frames[canTxQueue.tail]; + canTxQueue.tail = (canTxQueue.tail + 1) % TX_QUEUE_SIZE; + return true; +} + +static bool canTxQueueIsEmpty(void) { + return canTxQueue.head == canTxQueue.tail; +} + /** * @brief Process CAN message from RxLocation FIFO into rx_frame * @param rx_frame pointer to a CanardCANFrame structure where the received CAN message will be @@ -123,6 +154,20 @@ int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { return 0; } +static void buildTxHeader(const CanardCANFrame *frame, CAN_TxHeaderTypeDef *header, uint8_t *data) { + if (frame->id & CANARD_CAN_FRAME_EFF) { + header->IDE = CAN_ID_EXT; + header->ExtId = frame->id & CANARD_CAN_EXT_ID_MASK; + } else { + header->IDE = CAN_ID_STD; + header->StdId = frame->id & CANARD_CAN_STD_ID_MASK; + } + header->DLC = frame->data_len; + header->RTR = (frame->id & CANARD_CAN_FRAME_RTR) ? CAN_RTR_REMOTE : CAN_RTR_DATA; + header->TransmitGlobalTime = DISABLE; + memcpy(data, frame->data, frame->data_len); +} + /** * @brief Process tx_frame CAN message into Tx FIFO/Queue and transmit it * @param tx_frame pointer to a CanardCANFrame structure that contains the CAN message to @@ -130,45 +175,32 @@ int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode */ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { - CAN_TxHeaderTypeDef txHeader = {}; - uint8_t txData[8]; - uint32_t txMailbox; - uint32_t returnCode; - - if (tx_frame == NULL) { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - - if (tx_frame->id & CANARD_CAN_FRAME_ERR) { - return -CANARD_ERROR_INVALID_ARGUMENT; // unsupported frame format - } - - // Process canard id to STM FDCAN header format - if (tx_frame->id & CANARD_CAN_FRAME_EFF) { - txHeader.IDE = CAN_ID_EXT; - txHeader.ExtId = tx_frame->id & CANARD_CAN_EXT_ID_MASK; - } else { - txHeader.IDE = CAN_ID_STD; - txHeader.StdId = tx_frame->id & CANARD_CAN_STD_ID_MASK; - } - - txHeader.DLC = tx_frame->data_len; - - if (tx_frame->id & CANARD_CAN_FRAME_RTR) { - txHeader.RTR = CAN_RTR_REMOTE; - } else { - txHeader.RTR = CAN_RTR_DATA; - } - - memcpy(txData, tx_frame->data, tx_frame->data_len); - - returnCode = HAL_CAN_AddTxMessage(&hcan1, &txHeader, txData, &txMailbox); - if (returnCode == HAL_OK) { - return 1; - } - - // TX failed (FIFO full or other error) - return 0 to signal retry needed - return 0; + if (tx_frame == NULL) { + return -CANARD_ERROR_INVALID_ARGUMENT; + } + if (tx_frame->id & CANARD_CAN_FRAME_ERR) { + return -CANARD_ERROR_INVALID_ARGUMENT; + } + + if (!canTxQueuePush(tx_frame)) { + return 0; // SW queue full - caller retries next cycle + } + + // If all mailboxes are idle, RQCP will never fire to start the ISR chain. + // Seed the HW directly with the first frame. + if (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 3) { + CanardCANFrame frame; + if (canTxQueuePop(&frame)) { + CAN_TxHeaderTypeDef txHeader = {}; + uint8_t txData[8]; + uint32_t txMailbox; + buildTxHeader(&frame, &txHeader, txData); + HAL_CAN_AddTxMessage(&hcan1, &txHeader, txData, &txMailbox); + } + } + + HAL_CAN_ActivateNotification(&hcan1, CAN_IT_TX_MAILBOX_EMPTY); + return 1; } /** @@ -260,6 +292,8 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) // (if any previous step failed, we return early without enabling IRQ) HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 0, 0); HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); + HAL_NVIC_SetPriority(CAN1_TX_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(CAN1_TX_IRQn); return CANARD_OK; } @@ -445,4 +479,26 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &frame.header, frame.data) == HAL_OK) { rxBufferPushFrame(&RxBuffer, &frame); } -} \ No newline at end of file +} + +static void canTxDrainQueue(CAN_HandleTypeDef *hcan) { + CanardCANFrame frame; + if (canTxQueuePop(&frame)) { + CAN_TxHeaderTypeDef txHeader = {}; + uint8_t txData[8]; + uint32_t txMailbox; + buildTxHeader(&frame, &txHeader, txData); + HAL_CAN_AddTxMessage(hcan, &txHeader, txData, &txMailbox); + } + if (canTxQueueIsEmpty()) { + HAL_CAN_DeactivateNotification(hcan, CAN_IT_TX_MAILBOX_EMPTY); + } +} + +void CAN1_TX_IRQHandler(void) { + HAL_CAN_IRQHandler(&hcan1); +} + +void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } +void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } +void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } \ No newline at end of file From 0f5aa3327298a3f6240e391b98994dd73367e351 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 13 May 2026 17:06:30 -0700 Subject: [PATCH 03/34] feat(dronecan): add CAN TX ISR debug hooks for Phase 3 validation Adds four debug visibility hooks to the STM32F7 CAN TX ISR driver: - canTxDropped counter: incremented when SW TX queue (depth 32) is full and canardSTM32Transmit() silently drops a frame. Cumulative across the session; exposed via canardProtocolStatus_t.tx_dropped. - ESR register fields in canardProtocolStatus_t: tec (TX error counter), rec (RX error counter), lec (last error code) read from hcan1.Instance->ESR. Reads BusOff/ErrorPassive atomically with the ESR snapshot. - canardSTM32GetTxQueueFillLevel(): returns current SW queue depth as (head - tail + TX_QUEUE_SIZE) % TX_QUEUE_SIZE. - CLI 'dronecan' command (USE_DRONECAN guard): prints all of the above plus RX buffer fill level in a single readable block. Useful for polling during long-running hardware validation tests without needing the configurator or MSP. --- .../dronecan/libcanard/canard_stm32_driver.h | 5 ++++ .../libcanard/canard_stm32f7xx_driver.c | 19 ++++++++---- src/main/fc/cli.c | 30 +++++++++++++++++++ 3 files changed, 49 insertions(+), 5 deletions(-) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h b/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h index fe70cddf1d9..65b6891d6e8 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h +++ b/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h @@ -12,6 +12,10 @@ typedef struct { uint32_t BusOff; uint32_t ErrorPassive; + uint8_t tec; // Transmit Error Counter (ESR[23:16]) + uint8_t rec; // Receive Error Counter (ESR[7:0]) + uint8_t lec; // Last Error Code (ESR[6:4]) + uint16_t tx_dropped; // Frames dropped due to SW TX queue full } canardProtocolStatus_t; #ifdef USE_DRONECAN @@ -22,6 +26,7 @@ int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame); int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame); void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat); int32_t canardSTM32GetRxFifoFillLevel(void); +int32_t canardSTM32GetTxQueueFillLevel(void); void canardSTM32RecoverFromBusOff(void); void canardSTM32GetUniqueID(uint8_t id[16]); diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index 0c2320b223a..6e14c36a27c 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -95,9 +95,12 @@ uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf) { return (rxBuf->writeIndex - rxBuf->readIndex); } +static volatile uint16_t canTxDropped = 0; + static bool canTxQueuePush(const CanardCANFrame *frame) { uint8_t next = (canTxQueue.head + 1) % TX_QUEUE_SIZE; - if (next == canTxQueue.tail) { + if (next == canTxQueue.tail) { + canTxDropped++; return false; } canTxQueue.frames[canTxQueue.head] = *frame; @@ -441,11 +444,17 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi } void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){ - - pProtocolStat->BusOff = __HAL_CAN_GET_FLAG(&hcan1, CAN_FLAG_BOF); + uint32_t esr = hcan1.Instance->ESR; + pProtocolStat->BusOff = __HAL_CAN_GET_FLAG(&hcan1, CAN_FLAG_BOF); pProtocolStat->ErrorPassive = __HAL_CAN_GET_FLAG(&hcan1, CAN_FLAG_EPV); - // LOG_DEBUG(CAN, "BusOff: %lu", pProtocolStat->BusOff); - // LOG_DEBUG(CAN, "ErrorPassive: %lu", pProtocolStat->ErrorPassive); + pProtocolStat->tec = (uint8_t)((esr >> 16) & 0xFF); + pProtocolStat->rec = (uint8_t)(esr & 0xFF); + pProtocolStat->lec = (uint8_t)((esr >> 4) & 0x07); + pProtocolStat->tx_dropped = canTxDropped; +} + +int32_t canardSTM32GetTxQueueFillLevel(void) { + return (canTxQueue.head - canTxQueue.tail + TX_QUEUE_SIZE) % TX_QUEUE_SIZE; } int32_t canardSTM32GetRxFifoFillLevel(void){ diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 1976edaa8e0..6c2f9f32c8e 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -130,6 +130,10 @@ bool cliMode = false; #include "sensors/esc_sensor.h" #endif +#ifdef USE_DRONECAN +#include "drivers/dronecan/libcanard/canard_stm32_driver.h" +#endif + #include "telemetry/telemetry.h" #include "build/debug.h" @@ -4368,6 +4372,29 @@ static void cliStatus(char *cmdline) } } +#ifdef USE_DRONECAN +static void cliDronecan(char *cmdline) +{ + UNUSED(cmdline); + static const char * const lecNames[] = { + "None", "Stuff", "Form", "ACK", "BitR", "BitD", "CRC", "SW" + }; + canardProtocolStatus_t stat; + canardSTM32GetProtocolStatus(&stat); + int32_t txFill = canardSTM32GetTxQueueFillLevel(); + int32_t rxFill = canardSTM32GetRxFifoFillLevel(); + cliPrintLine("DroneCAN CAN peripheral status:"); + cliPrintLinef(" BusOff: %s", stat.BusOff ? "YES" : "no"); + cliPrintLinef(" ErrorPassive:%s", stat.ErrorPassive ? "YES" : "no"); + cliPrintLinef(" TEC: %u", (unsigned)stat.tec); + cliPrintLinef(" REC: %u", (unsigned)stat.rec); + cliPrintLinef(" LEC: %s (%u)", lecNames[stat.lec & 0x7], (unsigned)stat.lec); + cliPrintLinef(" TX queue: %ld / 32", (long)txFill); + cliPrintLinef(" RX buffer: %ld / 32", (long)rxFill); + cliPrintLinef(" TX dropped: %u", (unsigned)stat.tx_dropped); +} +#endif + static void cliTasks(char *cmdline) { UNUSED(cmdline); @@ -4936,6 +4963,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), CLI_COMMAND_DEF("diff", "list configuration changes from default", "[master|battery_profile|control_profile|mixer_profile|rates|all] {showdefaults}", cliDiff), +#ifdef USE_DRONECAN + CLI_COMMAND_DEF("dronecan", "show DroneCAN CAN peripheral debug status", NULL, cliDronecan), +#endif CLI_COMMAND_DEF("dump", "dump configuration", "[master|battery_profile|control_profile|mixer_profile|rates|all] {showdefaults}", cliDump), #ifdef USE_RX_ELERES From fe5467d286decf4b5b46f2385d73621a1b5b8754 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 14 May 2026 19:18:20 -0700 Subject: [PATCH 04/34] fix(dronecan): correct CAN IRQ priority and add queue high water marks - Add NVIC_PRIO_CAN = 4 to nvic.h; use it for CAN1_RX0 and CAN1_TX IRQs (was 0, violating NVIC_PRIO_MAX=1 and risking preemption of motor timers at priority 3) - Add canTxQueueHWM / canRxBufferHWM tracking; expose via canardProtocolStatus_t and dronecan CLI command as "N / 32 (hwm: N)" for both TX queue and RX buffer - Include drivers/nvic.h in canard_stm32f7xx_driver.c --- .../dronecan/libcanard/canard_stm32_driver.h | 4 ++- .../libcanard/canard_stm32f7xx_driver.c | 25 +++++++++++++------ src/main/drivers/nvic.h | 1 + src/main/fc/cli.c | 4 +-- 4 files changed, 23 insertions(+), 11 deletions(-) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h b/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h index 65b6891d6e8..f91b7edfecd 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h +++ b/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h @@ -15,7 +15,9 @@ typedef struct { uint8_t tec; // Transmit Error Counter (ESR[23:16]) uint8_t rec; // Receive Error Counter (ESR[7:0]) uint8_t lec; // Last Error Code (ESR[6:4]) - uint16_t tx_dropped; // Frames dropped due to SW TX queue full + uint16_t tx_dropped; // Frames dropped due to SW TX queue full + uint8_t tx_queue_hwm; // TX SW queue high water mark + uint8_t rx_buffer_hwm; // RX buffer high water mark } canardProtocolStatus_t; #ifdef USE_DRONECAN diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index 6e14c36a27c..1607bc83f72 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -8,6 +8,7 @@ #include "common/log.h" #include "common/time.h" #include "drivers/io.h" +#include "drivers/nvic.h" #include "canard.h" #include "canard_stm32_driver.h" @@ -52,6 +53,10 @@ static void canardSTM32GPIO_Init(void); static CAN_HandleTypeDef hcan1; RxFrame_t rxMsg; +static volatile uint16_t canTxDropped = 0; +static volatile uint8_t canTxQueueHWM = 0; +static volatile uint8_t canRxBufferHWM = 0; + uint8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { uint8_t next; RxFrame_t *pCurrentRxMsg; @@ -67,6 +72,8 @@ uint8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { pCurrentRxMsg = &rxBuf->rxMsg[rxBuf->writeIndex]; memcpy(pCurrentRxMsg, rxMsg, sizeof(RxFrame_t)); rxBuf->writeIndex = next; + uint8_t rxFill = (next >= rxBuf->readIndex) ? (next - rxBuf->readIndex) : (next + RX_BUFFER_SIZE - rxBuf->readIndex); + if (rxFill > canRxBufferHWM) canRxBufferHWM = rxFill; return 0; } @@ -95,16 +102,16 @@ uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf) { return (rxBuf->writeIndex - rxBuf->readIndex); } -static volatile uint16_t canTxDropped = 0; - static bool canTxQueuePush(const CanardCANFrame *frame) { uint8_t next = (canTxQueue.head + 1) % TX_QUEUE_SIZE; if (next == canTxQueue.tail) { canTxDropped++; return false; - } - canTxQueue.frames[canTxQueue.head] = *frame; - canTxQueue.head = next; + } + canTxQueue.frames[canTxQueue.head] = *frame; + canTxQueue.head = next; + uint8_t fill = (canTxQueue.head - canTxQueue.tail + TX_QUEUE_SIZE) % TX_QUEUE_SIZE; + if (fill > canTxQueueHWM) canTxQueueHWM = fill; return true; } @@ -293,9 +300,9 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) // Enable interrupt only after all initialization succeeds // (if any previous step failed, we return early without enabling IRQ) - HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 0, 0); + HAL_NVIC_SetPriority(CAN1_RX0_IRQn, NVIC_PRIO_CAN, 0); HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); - HAL_NVIC_SetPriority(CAN1_TX_IRQn, 0, 0); + HAL_NVIC_SetPriority(CAN1_TX_IRQn, NVIC_PRIO_CAN, 0); HAL_NVIC_EnableIRQ(CAN1_TX_IRQn); return CANARD_OK; @@ -450,7 +457,9 @@ void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){ pProtocolStat->tec = (uint8_t)((esr >> 16) & 0xFF); pProtocolStat->rec = (uint8_t)(esr & 0xFF); pProtocolStat->lec = (uint8_t)((esr >> 4) & 0x07); - pProtocolStat->tx_dropped = canTxDropped; + pProtocolStat->tx_dropped = canTxDropped; + pProtocolStat->tx_queue_hwm = canTxQueueHWM; + pProtocolStat->rx_buffer_hwm = canRxBufferHWM; } int32_t canardSTM32GetTxQueueFillLevel(void) { diff --git a/src/main/drivers/nvic.h b/src/main/drivers/nvic.h index cb484937d3d..8ff16cd2998 100644 --- a/src/main/drivers/nvic.h +++ b/src/main/drivers/nvic.h @@ -12,6 +12,7 @@ #define NVIC_PRIO_TIMER 3 #define NVIC_PRIO_TIMER_DMA 3 #define NVIC_PRIO_SDIO 3 +#define NVIC_PRIO_CAN 4 #define NVIC_PRIO_USB 5 #define NVIC_PRIO_SERIALUART 5 #define NVIC_PRIO_VCP 7 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 6c2f9f32c8e..b4439bcce27 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -4389,8 +4389,8 @@ static void cliDronecan(char *cmdline) cliPrintLinef(" TEC: %u", (unsigned)stat.tec); cliPrintLinef(" REC: %u", (unsigned)stat.rec); cliPrintLinef(" LEC: %s (%u)", lecNames[stat.lec & 0x7], (unsigned)stat.lec); - cliPrintLinef(" TX queue: %ld / 32", (long)txFill); - cliPrintLinef(" RX buffer: %ld / 32", (long)rxFill); + cliPrintLinef(" TX queue: %ld / 32 (hwm: %u)", (long)txFill, (unsigned)stat.tx_queue_hwm); + cliPrintLinef(" RX buffer: %ld / 32 (hwm: %u)", (long)rxFill, (unsigned)stat.rx_buffer_hwm); cliPrintLinef(" TX dropped: %u", (unsigned)stat.tx_dropped); } #endif From 7bfd46fc264ddc3fbaa4eb4530eddb8da89f927e Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 14 May 2026 19:41:48 -0700 Subject: [PATCH 05/34] fix(dronecan): restore SPSC contract in CAN TX queue and clean whitespace - Replace direct canTxQueuePop() in main-loop seed path with canTxDrainQueue() called under __disable_irq()/__enable_irq(), making the ISR the sole consumer of the SW TX queue at all times - Only call ActivateNotification if queue is non-empty after seeding, eliminating the spurious ISR invocation on single-frame bursts - Add forward declaration for canTxDrainQueue() - Strip trailing whitespace throughout file --- .../libcanard/canard_stm32f7xx_driver.c | 149 +++++++++--------- 1 file changed, 75 insertions(+), 74 deletions(-) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index 1607bc83f72..e90f72cd211 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -39,16 +39,17 @@ static struct RxBuffer_t { RxFrame_t rxMsg[RX_BUFFER_SIZE]; } RxBuffer; -#define TX_QUEUE_SIZE 32 - -static struct { +#define TX_QUEUE_SIZE 32 + +static struct { CanardCANFrame frames[TX_QUEUE_SIZE]; - volatile uint8_t head; + volatile uint8_t head; volatile uint8_t tail; -} canTxQueue; +} canTxQueue; static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings*out_timings); static void canardSTM32GPIO_Init(void); +static void canTxDrainQueue(CAN_HandleTypeDef *hcan); static CAN_HandleTypeDef hcan1; RxFrame_t rxMsg; @@ -98,7 +99,7 @@ uint8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf) { if(rxBuf->writeIndex < rxBuf->readIndex) return((rxBuf->writeIndex + RX_BUFFER_SIZE) - rxBuf->readIndex); - + return (rxBuf->writeIndex - rxBuf->readIndex); } @@ -113,18 +114,18 @@ static bool canTxQueuePush(const CanardCANFrame *frame) { uint8_t fill = (canTxQueue.head - canTxQueue.tail + TX_QUEUE_SIZE) % TX_QUEUE_SIZE; if (fill > canTxQueueHWM) canTxQueueHWM = fill; return true; -} +} static bool canTxQueuePop(CanardCANFrame *frame) { if (canTxQueue.head == canTxQueue.tail) { return false; - } + } *frame = canTxQueue.frames[canTxQueue.tail]; - canTxQueue.tail = (canTxQueue.tail + 1) % TX_QUEUE_SIZE; - return true; -} + canTxQueue.tail = (canTxQueue.tail + 1) % TX_QUEUE_SIZE; + return true; +} -static bool canTxQueueIsEmpty(void) { +static bool canTxQueueIsEmpty(void) { return canTxQueue.head == canTxQueue.tail; } @@ -164,18 +165,18 @@ int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { return 0; } -static void buildTxHeader(const CanardCANFrame *frame, CAN_TxHeaderTypeDef *header, uint8_t *data) { - if (frame->id & CANARD_CAN_FRAME_EFF) { - header->IDE = CAN_ID_EXT; - header->ExtId = frame->id & CANARD_CAN_EXT_ID_MASK; - } else { - header->IDE = CAN_ID_STD; - header->StdId = frame->id & CANARD_CAN_STD_ID_MASK; - } - header->DLC = frame->data_len; - header->RTR = (frame->id & CANARD_CAN_FRAME_RTR) ? CAN_RTR_REMOTE : CAN_RTR_DATA; - header->TransmitGlobalTime = DISABLE; - memcpy(data, frame->data, frame->data_len); +static void buildTxHeader(const CanardCANFrame *frame, CAN_TxHeaderTypeDef *header, uint8_t *data) { + if (frame->id & CANARD_CAN_FRAME_EFF) { + header->IDE = CAN_ID_EXT; + header->ExtId = frame->id & CANARD_CAN_EXT_ID_MASK; + } else { + header->IDE = CAN_ID_STD; + header->StdId = frame->id & CANARD_CAN_STD_ID_MASK; + } + header->DLC = frame->data_len; + header->RTR = (frame->id & CANARD_CAN_FRAME_RTR) ? CAN_RTR_REMOTE : CAN_RTR_DATA; + header->TransmitGlobalTime = DISABLE; + memcpy(data, frame->data, frame->data_len); } /** @@ -185,32 +186,32 @@ static void buildTxHeader(const CanardCANFrame *frame, CAN_TxHeaderTypeDef *head * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode */ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { - if (tx_frame == NULL) { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - if (tx_frame->id & CANARD_CAN_FRAME_ERR) { + if (tx_frame == NULL) { return -CANARD_ERROR_INVALID_ARGUMENT; } - + if (tx_frame->id & CANARD_CAN_FRAME_ERR) { + return -CANARD_ERROR_INVALID_ARGUMENT; + } + if (!canTxQueuePush(tx_frame)) { - return 0; // SW queue full - caller retries next cycle - } - - // If all mailboxes are idle, RQCP will never fire to start the ISR chain. - // Seed the HW directly with the first frame. - if (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 3) { - CanardCANFrame frame; - if (canTxQueuePop(&frame)) { - CAN_TxHeaderTypeDef txHeader = {}; - uint8_t txData[8]; - uint32_t txMailbox; - buildTxHeader(&frame, &txHeader, txData); - HAL_CAN_AddTxMessage(&hcan1, &txHeader, txData, &txMailbox); - } + return 0; // SW queue full - caller retries next cycle + } + + // If all mailboxes are idle, RQCP will never fire to start the ISR chain. + // Seed the HW via canTxDrainQueue with IRQs disabled to preserve the + // SPSC contract — ISR is the sole consumer; we become the consumer only + // while the ISR cannot run. + __disable_irq(); + if (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 3) { + canTxDrainQueue(&hcan1); + } + bool needs_notification = !canTxQueueIsEmpty(); + __enable_irq(); + + if (needs_notification) { + HAL_CAN_ActivateNotification(&hcan1, CAN_IT_TX_MAILBOX_EMPTY); } - - HAL_CAN_ActivateNotification(&hcan1, CAN_IT_TX_MAILBOX_EMPTY); - return 1; + return 1; } /** @@ -240,7 +241,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; sFilterConfig.FilterActivation = ENABLE; - + hcan1.Instance = CAN1; hcan1.Init.Mode = CAN_MODE_NORMAL; hcan1.Init.TimeTriggeredMode = DISABLE; @@ -249,7 +250,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) hcan1.Init.AutoRetransmission = ENABLE; hcan1.Init.ReceiveFifoLocked = DISABLE; hcan1.Init.TransmitFifoPriority = DISABLE; - + canardSTM32ComputeTimings(bitrate, &out_timings); hcan1.Init.Prescaler = out_timings.prescaler; @@ -273,7 +274,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) // } canardSTM32GPIO_Init(); // Set up the pins for CAN and optional listen only mode - + // LOG_DEBUG(CAN, "System Clock Speed: %lu", HAL_RCC_GetSysClockFreq()); // LOG_DEBUG(CAN, "PClk1 Clock Speed: %lu", HAL_RCC_GetPCLK1Freq()); if (HAL_CAN_Init(&hcan1) != HAL_OK) @@ -281,13 +282,13 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) LOG_ERROR(CAN, "Failed CAN Init"); return -CANARD_ERROR_INTERNAL; } - + /* USER CODE BEGIN FDCAN1_Init 2 */ if (HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig) != HAL_OK) { LOG_ERROR(CAN, "Failed Config Filter"); return -CANARD_ERROR_INTERNAL; } - + if (HAL_CAN_Start(&hcan1) != HAL_OK) { LOG_ERROR(CAN, "Failed to Start"); return -CANARD_ERROR_INTERNAL; @@ -297,13 +298,13 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) LOG_ERROR(CAN, "Failed to activate interrupt"); return -CANARD_ERROR_INTERNAL; } - + // Enable interrupt only after all initialization succeeds // (if any previous step failed, we return early without enabling IRQ) HAL_NVIC_SetPriority(CAN1_RX0_IRQn, NVIC_PRIO_CAN, 0); HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); HAL_NVIC_SetPriority(CAN1_TX_IRQn, NVIC_PRIO_CAN, 0); - HAL_NVIC_EnableIRQ(CAN1_TX_IRQn); + HAL_NVIC_EnableIRQ(CAN1_TX_IRQn); return CANARD_OK; } @@ -336,7 +337,7 @@ static void canardSTM32GPIO_Init(void) } static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings *out_timings) { - + if (target_bitrate < 1) { return false; } @@ -499,24 +500,24 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { } } -static void canTxDrainQueue(CAN_HandleTypeDef *hcan) { - CanardCANFrame frame; - if (canTxQueuePop(&frame)) { - CAN_TxHeaderTypeDef txHeader = {}; - uint8_t txData[8]; - uint32_t txMailbox; - buildTxHeader(&frame, &txHeader, txData); - HAL_CAN_AddTxMessage(hcan, &txHeader, txData, &txMailbox); - } - if (canTxQueueIsEmpty()) { - HAL_CAN_DeactivateNotification(hcan, CAN_IT_TX_MAILBOX_EMPTY); - } -} - -void CAN1_TX_IRQHandler(void) { - HAL_CAN_IRQHandler(&hcan1); -} - +static void canTxDrainQueue(CAN_HandleTypeDef *hcan) { + CanardCANFrame frame; + if (canTxQueuePop(&frame)) { + CAN_TxHeaderTypeDef txHeader = {}; + uint8_t txData[8]; + uint32_t txMailbox; + buildTxHeader(&frame, &txHeader, txData); + HAL_CAN_AddTxMessage(hcan, &txHeader, txData, &txMailbox); + } + if (canTxQueueIsEmpty()) { + HAL_CAN_DeactivateNotification(hcan, CAN_IT_TX_MAILBOX_EMPTY); + } +} + +void CAN1_TX_IRQHandler(void) { + HAL_CAN_IRQHandler(&hcan1); +} + void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } -void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } -void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } \ No newline at end of file +void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } +void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } \ No newline at end of file From 559ab37623d6528f06623b39e58164c78b334f93 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 14 May 2026 19:51:56 -0700 Subject: [PATCH 06/34] fix(dronecan): add Cortex-M7 memory barriers to SPSC TX queue Add __DMB() in canTxQueuePush() between writing frame data and advancing head, and in canTxQueuePop() between reading head and reading frame data. Prevents the M7 write buffer from reordering the head store before the frame data store is globally visible. --- src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index e90f72cd211..7484e427e73 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -110,6 +110,7 @@ static bool canTxQueuePush(const CanardCANFrame *frame) { return false; } canTxQueue.frames[canTxQueue.head] = *frame; + __DMB(); // ensure frame data is visible before head advances canTxQueue.head = next; uint8_t fill = (canTxQueue.head - canTxQueue.tail + TX_QUEUE_SIZE) % TX_QUEUE_SIZE; if (fill > canTxQueueHWM) canTxQueueHWM = fill; @@ -120,6 +121,7 @@ static bool canTxQueuePop(CanardCANFrame *frame) { if (canTxQueue.head == canTxQueue.tail) { return false; } + __DMB(); // observe all stores the producer made before advancing head *frame = canTxQueue.frames[canTxQueue.tail]; canTxQueue.tail = (canTxQueue.tail + 1) % TX_QUEUE_SIZE; return true; From abedc3b92534365b8a0a07a99e5f8c249398b90b Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 14 May 2026 20:37:29 -0700 Subject: [PATCH 07/34] fix(dronecan): use bxCAN TXFP mode to fix multi-frame transfer ordering Enable TransmitFifoPriority (TXFP) so the peripheral transmits mailboxes in request order rather than mailbox-number order. Without TXFP, loading the next frame into mailbox 0 after it completes causes it to transmit before frames still pending in mailboxes 1 and 2, corrupting the toggle bit sequence of multi-frame UAVCAN transfers. With TXFP enabled, restore the while loop in canTxDrainQueue to fill all free mailboxes per ISR callback for full pipeline utilization. Also fix remaining code review findings: - Check HAL_CAN_AddTxMessage return value; count failures as dropped frames - Add CanTxQueue_t struct tag to match RxBuffer_t convention - Remove blank line artifact at top of onTransferReceived body --- src/main/drivers/dronecan/dronecan.c | 1 - .../libcanard/canard_stm32f7xx_driver.c | 24 ++++++++++++------- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index a60eff54971..98a6f230d7c 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -295,7 +295,6 @@ bool shouldAcceptTransfer(const CanardInstance *ins, */ void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { // switch on data type ID to pass to the right handler function - if (transfer->transfer_type == CanardTransferTypeRequest) { // check if we want to handle a specific service request switch (transfer->data_type_id) { diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index 7484e427e73..8ea2378b015 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -41,7 +41,7 @@ static struct RxBuffer_t { #define TX_QUEUE_SIZE 32 -static struct { +static struct CanTxQueue_t { CanardCANFrame frames[TX_QUEUE_SIZE]; volatile uint8_t head; volatile uint8_t tail; @@ -251,7 +251,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) hcan1.Init.AutoWakeUp = DISABLE; hcan1.Init.AutoRetransmission = ENABLE; hcan1.Init.ReceiveFifoLocked = DISABLE; - hcan1.Init.TransmitFifoPriority = DISABLE; + hcan1.Init.TransmitFifoPriority = ENABLE; // transmit in request order, not mailbox-number order canardSTM32ComputeTimings(bitrate, &out_timings); @@ -504,12 +504,20 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { static void canTxDrainQueue(CAN_HandleTypeDef *hcan) { CanardCANFrame frame; - if (canTxQueuePop(&frame)) { - CAN_TxHeaderTypeDef txHeader = {}; - uint8_t txData[8]; - uint32_t txMailbox; - buildTxHeader(&frame, &txHeader, txData); - HAL_CAN_AddTxMessage(hcan, &txHeader, txData, &txMailbox); + // With TXFP=ENABLE the hardware transmits mailboxes in request order + // (chronological), not mailbox-number order, so filling multiple mailboxes + // per callback is safe and preserves frame sequence. + while (!canTxQueueIsEmpty() && HAL_CAN_GetTxMailboxesFreeLevel(hcan) > 0) { + if (canTxQueuePop(&frame)) { + CAN_TxHeaderTypeDef txHeader = {}; + uint8_t txData[8]; + uint32_t txMailbox; + buildTxHeader(&frame, &txHeader, txData); + if (HAL_CAN_AddTxMessage(hcan, &txHeader, txData, &txMailbox) != HAL_OK) { + canTxDropped++; + break; + } + } } if (canTxQueueIsEmpty()) { HAL_CAN_DeactivateNotification(hcan, CAN_IT_TX_MAILBOX_EMPTY); From 632b5ae6f47435e83aaa01401767b4e98bfe429f Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 14 May 2026 20:53:52 -0700 Subject: [PATCH 08/34] fix(dronecan): address code review findings in CAN TX ISR driver - Add volatile to RxBuffer writeIndex/readIndex: written in ISR and read in main loop, missing volatile allows compiler to cache stale values causing silent receive stalls - Peek-then-pop in canTxDrainQueue: advance tail only after HAL_CAN_ AddTxMessage succeeds so a HAL rejection never silently loses a frame - Move ActivateNotification inside critical section: eliminates race where ISR could drain queue and deactivate between __enable_irq and the ActivateNotification call, leaving a spurious notification armed - Snapshot tail before advancing head in canTxQueuePush: prevents ISR advancing tail between the HWM read and head write, which understated the high water mark - Remove dead canTxQueuePop (inlined into peek-then-pop drain logic) - Remove dead RxFrame_t rxMsg global - Fix 3-space indent on first if in canardSTM32Transmit - Fix missing space in ErrorPassive CLI output - Add missing newline at EOF --- .../libcanard/canard_stm32f7xx_driver.c | 50 +++++++------------ src/main/fc/cli.c | 2 +- 2 files changed, 20 insertions(+), 32 deletions(-) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index 8ea2378b015..0b817cba2d4 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -34,8 +34,8 @@ typedef struct { } RxFrame_t; static struct RxBuffer_t { - uint8_t writeIndex; - uint8_t readIndex; + volatile uint8_t writeIndex; // written in ISR, read in main loop + volatile uint8_t readIndex; RxFrame_t rxMsg[RX_BUFFER_SIZE]; } RxBuffer; @@ -52,7 +52,6 @@ static void canardSTM32GPIO_Init(void); static void canTxDrainQueue(CAN_HandleTypeDef *hcan); static CAN_HandleTypeDef hcan1; -RxFrame_t rxMsg; static volatile uint16_t canTxDropped = 0; static volatile uint8_t canTxQueueHWM = 0; @@ -105,28 +104,19 @@ uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf) { static bool canTxQueuePush(const CanardCANFrame *frame) { uint8_t next = (canTxQueue.head + 1) % TX_QUEUE_SIZE; - if (next == canTxQueue.tail) { + uint8_t tail_snapshot = canTxQueue.tail; // snapshot before ISR can advance it + if (next == tail_snapshot) { canTxDropped++; return false; } canTxQueue.frames[canTxQueue.head] = *frame; __DMB(); // ensure frame data is visible before head advances canTxQueue.head = next; - uint8_t fill = (canTxQueue.head - canTxQueue.tail + TX_QUEUE_SIZE) % TX_QUEUE_SIZE; + uint8_t fill = (next - tail_snapshot + TX_QUEUE_SIZE) % TX_QUEUE_SIZE; if (fill > canTxQueueHWM) canTxQueueHWM = fill; return true; } -static bool canTxQueuePop(CanardCANFrame *frame) { - if (canTxQueue.head == canTxQueue.tail) { - return false; - } - __DMB(); // observe all stores the producer made before advancing head - *frame = canTxQueue.frames[canTxQueue.tail]; - canTxQueue.tail = (canTxQueue.tail + 1) % TX_QUEUE_SIZE; - return true; -} - static bool canTxQueueIsEmpty(void) { return canTxQueue.head == canTxQueue.tail; } @@ -188,7 +178,7 @@ static void buildTxHeader(const CanardCANFrame *frame, CAN_TxHeaderTypeDef *head * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode */ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { - if (tx_frame == NULL) { + if (tx_frame == NULL) { return -CANARD_ERROR_INVALID_ARGUMENT; } if (tx_frame->id & CANARD_CAN_FRAME_ERR) { @@ -207,12 +197,10 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { if (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 3) { canTxDrainQueue(&hcan1); } - bool needs_notification = !canTxQueueIsEmpty(); - __enable_irq(); - - if (needs_notification) { + if (!canTxQueueIsEmpty()) { HAL_CAN_ActivateNotification(&hcan1, CAN_IT_TX_MAILBOX_EMPTY); } + __enable_irq(); return 1; } @@ -503,21 +491,21 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { } static void canTxDrainQueue(CAN_HandleTypeDef *hcan) { - CanardCANFrame frame; // With TXFP=ENABLE the hardware transmits mailboxes in request order // (chronological), not mailbox-number order, so filling multiple mailboxes // per callback is safe and preserves frame sequence. while (!canTxQueueIsEmpty() && HAL_CAN_GetTxMailboxesFreeLevel(hcan) > 0) { - if (canTxQueuePop(&frame)) { - CAN_TxHeaderTypeDef txHeader = {}; - uint8_t txData[8]; - uint32_t txMailbox; - buildTxHeader(&frame, &txHeader, txData); - if (HAL_CAN_AddTxMessage(hcan, &txHeader, txData, &txMailbox) != HAL_OK) { - canTxDropped++; - break; - } + __DMB(); // observe producer stores before reading frame data + CAN_TxHeaderTypeDef txHeader = {}; + uint8_t txData[8]; + uint32_t txMailbox; + buildTxHeader(&canTxQueue.frames[canTxQueue.tail], &txHeader, txData); + if (HAL_CAN_AddTxMessage(hcan, &txHeader, txData, &txMailbox) != HAL_OK) { + canTxDropped++; + break; } + // Advance tail only after HAL confirms acceptance — frame is never lost + canTxQueue.tail = (canTxQueue.tail + 1) % TX_QUEUE_SIZE; } if (canTxQueueIsEmpty()) { HAL_CAN_DeactivateNotification(hcan, CAN_IT_TX_MAILBOX_EMPTY); @@ -530,4 +518,4 @@ void CAN1_TX_IRQHandler(void) { void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } -void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } \ No newline at end of file +void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index b4439bcce27..d5ec23fa42a 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -4385,7 +4385,7 @@ static void cliDronecan(char *cmdline) int32_t rxFill = canardSTM32GetRxFifoFillLevel(); cliPrintLine("DroneCAN CAN peripheral status:"); cliPrintLinef(" BusOff: %s", stat.BusOff ? "YES" : "no"); - cliPrintLinef(" ErrorPassive:%s", stat.ErrorPassive ? "YES" : "no"); + cliPrintLinef(" ErrorPassive: %s", stat.ErrorPassive ? "YES" : "no"); cliPrintLinef(" TEC: %u", (unsigned)stat.tec); cliPrintLinef(" REC: %u", (unsigned)stat.rec); cliPrintLinef(" LEC: %s (%u)", lecNames[stat.lec & 0x7], (unsigned)stat.lec); From ee213189766051eb970de819628da49459271150 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 14 May 2026 21:06:09 -0700 Subject: [PATCH 09/34] fix(dronecan): fix REC extraction, drain stall semantics, and ActivateNotification error handling MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Fix REC register extraction: ESR bits 7:0 are error flags, REC lives at bits 31:24 (CAN_ESR_REC_Pos=24). Previous code always read near- zero instead of the actual receive error counter. - Remove canTxDropped increment from ISR drain path: a HAL_CAN_ AddTxMessage failure leaves tail unadvanced so the frame is retried on the next ISR — it is not dropped. Incrementing the drop counter here gave wrong semantics and created a dual-writer race on the volatile uint16_t counter. - Check ActivateNotification return in seed path: failure means the TX ISR will never fire for in-flight completions, stalling the queue with no indication. Return 0 on failure so the DroneCAN task retries. --- .../drivers/dronecan/libcanard/canard_stm32f7xx_driver.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index 0b817cba2d4..fae4adcfc7e 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -198,7 +198,9 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { canTxDrainQueue(&hcan1); } if (!canTxQueueIsEmpty()) { - HAL_CAN_ActivateNotification(&hcan1, CAN_IT_TX_MAILBOX_EMPTY); + HAL_StatusTypeDef status = HAL_CAN_ActivateNotification(&hcan1, CAN_IT_TX_MAILBOX_EMPTY); + __enable_irq(); + return (status == HAL_OK) ? 1 : 0; // 0 signals caller to retry } __enable_irq(); return 1; @@ -446,7 +448,7 @@ void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){ pProtocolStat->BusOff = __HAL_CAN_GET_FLAG(&hcan1, CAN_FLAG_BOF); pProtocolStat->ErrorPassive = __HAL_CAN_GET_FLAG(&hcan1, CAN_FLAG_EPV); pProtocolStat->tec = (uint8_t)((esr >> 16) & 0xFF); - pProtocolStat->rec = (uint8_t)(esr & 0xFF); + pProtocolStat->rec = (uint8_t)((esr >> 24) & 0xFF); pProtocolStat->lec = (uint8_t)((esr >> 4) & 0x07); pProtocolStat->tx_dropped = canTxDropped; pProtocolStat->tx_queue_hwm = canTxQueueHWM; @@ -501,8 +503,7 @@ static void canTxDrainQueue(CAN_HandleTypeDef *hcan) { uint32_t txMailbox; buildTxHeader(&canTxQueue.frames[canTxQueue.tail], &txHeader, txData); if (HAL_CAN_AddTxMessage(hcan, &txHeader, txData, &txMailbox) != HAL_OK) { - canTxDropped++; - break; + break; // frame stays in queue (tail not advanced) and will retry next ISR } // Advance tail only after HAL confirms acceptance — frame is never lost canTxQueue.tail = (canTxQueue.tail + 1) % TX_QUEUE_SIZE; From 6dfc41c817a7bd3aa0041f2c64ffa60b4d9e7e74 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 14 May 2026 21:12:02 -0700 Subject: [PATCH 10/34] refactor(dronecan): introduce peek+consume API to encapsulate TX queue access MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replace direct struct field access in canTxDrainQueue with two new queue functions: canTxQueuePeek() returns a const pointer to the tail frame (including the DMB barrier) without consuming it, and canTxQueueConsume() advances the tail after the HW accepts the frame. This keeps queue invariants — barrier placement, index arithmetic, and the peek-then-consume ordering — inside the queue API rather than spread across canTxDrainQueue. --- .../libcanard/canard_stm32f7xx_driver.c | 27 ++++++++++++++----- 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index fae4adcfc7e..174e85e2398 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -121,6 +121,22 @@ static bool canTxQueueIsEmpty(void) { return canTxQueue.head == canTxQueue.tail; } +// Returns a pointer to the tail frame without consuming it, or NULL if empty. +// Includes the DMB so the caller sees all producer stores before reading data. +static const CanardCANFrame *canTxQueuePeek(void) { + if (canTxQueue.head == canTxQueue.tail) { + return NULL; + } + __DMB(); + return &canTxQueue.frames[canTxQueue.tail]; +} + +// Advances the tail to consume the peeked frame. Must be called only after +// canTxQueuePeek returned non-NULL and the frame has been accepted by the HW. +static void canTxQueueConsume(void) { + canTxQueue.tail = (canTxQueue.tail + 1) % TX_QUEUE_SIZE; +} + /** * @brief Process CAN message from RxLocation FIFO into rx_frame * @param rx_frame pointer to a CanardCANFrame structure where the received CAN message will be @@ -496,17 +512,16 @@ static void canTxDrainQueue(CAN_HandleTypeDef *hcan) { // With TXFP=ENABLE the hardware transmits mailboxes in request order // (chronological), not mailbox-number order, so filling multiple mailboxes // per callback is safe and preserves frame sequence. - while (!canTxQueueIsEmpty() && HAL_CAN_GetTxMailboxesFreeLevel(hcan) > 0) { - __DMB(); // observe producer stores before reading frame data + const CanardCANFrame *frame; + while ((frame = canTxQueuePeek()) != NULL && HAL_CAN_GetTxMailboxesFreeLevel(hcan) > 0) { CAN_TxHeaderTypeDef txHeader = {}; uint8_t txData[8]; uint32_t txMailbox; - buildTxHeader(&canTxQueue.frames[canTxQueue.tail], &txHeader, txData); + buildTxHeader(frame, &txHeader, txData); if (HAL_CAN_AddTxMessage(hcan, &txHeader, txData, &txMailbox) != HAL_OK) { - break; // frame stays in queue (tail not advanced) and will retry next ISR + break; // frame stays in queue and will retry on next ISR } - // Advance tail only after HAL confirms acceptance — frame is never lost - canTxQueue.tail = (canTxQueue.tail + 1) % TX_QUEUE_SIZE; + canTxQueueConsume(); } if (canTxQueueIsEmpty()) { HAL_CAN_DeactivateNotification(hcan, CAN_IT_TX_MAILBOX_EMPTY); From a941647b75c408f98319659e4056f1f076ace2ec Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 14 May 2026 21:47:21 -0700 Subject: [PATCH 11/34] fix(dronecan): correct REC comment, document abort callback rationale, flag CLI denominators - Fix ESR bit-range annotation on rec field in header: was ESR[7:0] (error flags), correct position is ESR[31:24] per CAN_ESR_REC_Pos=24. Extraction code was already correct; wrong comment risked a future maintainer reverting it. - Add comment explaining why TxMailboxAbortCallback is not wired: AutoRetransmission=ENABLE prevents ALST; TERR leaves the frame in the queue (tail not advanced) so the next Transmit call re-seeds the HW. - Add coupling comments on hardcoded /32 denominators in CLI output so they stay in sync if TX_QUEUE_SIZE or RX_BUFFER_SIZE ever changes. --- src/main/drivers/dronecan/libcanard/canard_stm32_driver.h | 2 +- src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c | 3 +++ src/main/fc/cli.c | 4 ++-- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h b/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h index f91b7edfecd..a9a7ecd3b0c 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h +++ b/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h @@ -13,7 +13,7 @@ typedef struct { uint32_t BusOff; uint32_t ErrorPassive; uint8_t tec; // Transmit Error Counter (ESR[23:16]) - uint8_t rec; // Receive Error Counter (ESR[7:0]) + uint8_t rec; // Receive Error Counter (ESR[31:24]) uint8_t lec; // Last Error Code (ESR[6:4]) uint16_t tx_dropped; // Frames dropped due to SW TX queue full uint8_t tx_queue_hwm; // TX SW queue high water mark diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index 174e85e2398..23be36beebe 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -535,3 +535,6 @@ void CAN1_TX_IRQHandler(void) { void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } +// AbortCallback is intentionally not wired: AutoRetransmission=ENABLE prevents +// ALST (arbitration lost), and TERR (bus fault) leaves the frame in the queue +// (tail not advanced) so the next canardSTM32Transmit call re-seeds the HW. diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index d5ec23fa42a..7ad5d8e1a1a 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -4389,8 +4389,8 @@ static void cliDronecan(char *cmdline) cliPrintLinef(" TEC: %u", (unsigned)stat.tec); cliPrintLinef(" REC: %u", (unsigned)stat.rec); cliPrintLinef(" LEC: %s (%u)", lecNames[stat.lec & 0x7], (unsigned)stat.lec); - cliPrintLinef(" TX queue: %ld / 32 (hwm: %u)", (long)txFill, (unsigned)stat.tx_queue_hwm); - cliPrintLinef(" RX buffer: %ld / 32 (hwm: %u)", (long)rxFill, (unsigned)stat.rx_buffer_hwm); + cliPrintLinef(" TX queue: %ld / 32 (hwm: %u)", (long)txFill, (unsigned)stat.tx_queue_hwm); // denominator must match TX_QUEUE_SIZE in canard_stm32f7xx_driver.c + cliPrintLinef(" RX buffer: %ld / 32 (hwm: %u)", (long)rxFill, (unsigned)stat.rx_buffer_hwm); // denominator must match RX_BUFFER_SIZE in canard_stm32f7xx_driver.c cliPrintLinef(" TX dropped: %u", (unsigned)stat.tx_dropped); } #endif From 3e14f07849db73c31e1ee6fa4802b75fce6a45f7 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 07:16:19 -0700 Subject: [PATCH 12/34] docs(dronecan): add Doxygen function documentation to STM32F7 CAN driver MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add @brief/@param/@retval doc blocks to all previously undocumented functions in canard_stm32f7xx_driver.c. Fix stale FDCAN1/hfdcan copy-paste in the canardSTM32CAN1_Init block — this is bxCAN, not FDCAN. --- .../libcanard/canard_stm32f7xx_driver.c | 117 ++++++++++++++++-- 1 file changed, 105 insertions(+), 12 deletions(-) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index 23be36beebe..f35aeec950c 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -57,6 +57,12 @@ static volatile uint16_t canTxDropped = 0; static volatile uint8_t canTxQueueHWM = 0; static volatile uint8_t canRxBufferHWM = 0; +/** + * @brief Push a received CAN frame into the ring buffer (called from ISR). + * @param rxBuf Pointer to the RxBuffer_t ring buffer. + * @param rxMsg Pointer to the RxFrame_t to copy into the buffer. + * @retval 0 on success, -1 if the buffer is full (frame dropped). + */ uint8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { uint8_t next; RxFrame_t *pCurrentRxMsg; @@ -77,6 +83,12 @@ uint8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { return 0; } +/** + * @brief Pop the oldest CAN frame from the ring buffer (called from main loop). + * @param rxBuf Pointer to the RxBuffer_t ring buffer. + * @param rxMsg Pointer to an RxFrame_t where the frame will be copied. + * @retval 0 on success, -1 if the buffer is empty. + */ uint8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { uint8_t next; RxFrame_t *pCurrentRxMsg; @@ -95,6 +107,11 @@ uint8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { return 0; } +/** + * @brief Return the number of frames currently held in the ring buffer. + * @param rxBuf Pointer to the RxBuffer_t ring buffer. + * @retval Number of frames available to read (0 to RX_BUFFER_SIZE-1). + */ uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf) { if(rxBuf->writeIndex < rxBuf->readIndex) return((rxBuf->writeIndex + RX_BUFFER_SIZE) - rxBuf->readIndex); @@ -102,6 +119,12 @@ uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf) { return (rxBuf->writeIndex - rxBuf->readIndex); } +/** + * @brief Push a frame onto the software TX queue (called from main loop). + * Updates the high-water mark and increments the drop counter if full. + * @param frame Pointer to the CanardCANFrame to enqueue. + * @retval true if the frame was enqueued, false if the queue was full (frame dropped). + */ static bool canTxQueuePush(const CanardCANFrame *frame) { uint8_t next = (canTxQueue.head + 1) % TX_QUEUE_SIZE; uint8_t tail_snapshot = canTxQueue.tail; // snapshot before ISR can advance it @@ -117,12 +140,20 @@ static bool canTxQueuePush(const CanardCANFrame *frame) { return true; } +/** + * @brief Check whether the software TX queue is empty. + * @retval true if the queue contains no frames, false otherwise. + */ static bool canTxQueueIsEmpty(void) { return canTxQueue.head == canTxQueue.tail; } -// Returns a pointer to the tail frame without consuming it, or NULL if empty. -// Includes the DMB so the caller sees all producer stores before reading data. +/** + * @brief Return a pointer to the next frame to transmit without consuming it. + * Issues a DMB to ensure all producer stores are visible before the data is read. + * Must be paired with canTxQueueConsume() after the frame is accepted by hardware. + * @retval Pointer to the tail CanardCANFrame, or NULL if the queue is empty. + */ static const CanardCANFrame *canTxQueuePeek(void) { if (canTxQueue.head == canTxQueue.tail) { return NULL; @@ -131,8 +162,11 @@ static const CanardCANFrame *canTxQueuePeek(void) { return &canTxQueue.frames[canTxQueue.tail]; } -// Advances the tail to consume the peeked frame. Must be called only after -// canTxQueuePeek returned non-NULL and the frame has been accepted by the HW. +/** + * @brief Advance the tail to consume the frame returned by canTxQueuePeek(). + * Must only be called after canTxQueuePeek() returned non-NULL and the + * frame has been successfully accepted by the hardware (HAL_OK). + */ static void canTxQueueConsume(void) { canTxQueue.tail = (canTxQueue.tail + 1) % TX_QUEUE_SIZE; } @@ -173,6 +207,12 @@ int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { return 0; } +/** + * @brief Populate a HAL CAN TX header and data buffer from a CanardCANFrame. + * @param frame Pointer to the source CanardCANFrame. + * @param header Pointer to the CAN_TxHeaderTypeDef to fill. + * @param data Buffer of at least 8 bytes to copy frame payload into. + */ static void buildTxHeader(const CanardCANFrame *frame, CAN_TxHeaderTypeDef *header, uint8_t *data) { if (frame->id & CANARD_CAN_FRAME_EFF) { header->IDE = CAN_ID_EXT; @@ -223,11 +263,11 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { } /** - * @brief FDCAN1 Initialization Function - * @param hfdcan pointer to an FDCAN_HandleTypeDef structure that contains - * the configuration information for the specified FDCAN. - * @param bitrate desired bitrate to run the CAN network at. - * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode + * @brief Initialize the bxCAN1 peripheral at the requested bitrate. + * Configures GPIO pins, bit timings, a pass-all acceptance filter, + * starts the peripheral, and enables RX and TX interrupts. + * @param bitrate Desired CAN bus bitrate in bits per second (e.g. 1000000). + * @retval CANARD_OK (1) on success, negative CANARD_ERROR code on failure. */ int16_t canardSTM32CAN1_Init(uint32_t bitrate) { @@ -343,6 +383,13 @@ static void canardSTM32GPIO_Init(void) IOLo(IOGetByTag(IO_TAG(CAN1_STANDBY))); #endif } +/** + * @brief Compute bxCAN bit-timing parameters (prescaler, SJW, BS1, BS2) for a + * given target bitrate, targeting an ~87.5% sample point location. + * @param target_bitrate Desired CAN bus bitrate in bits per second. + * @param out_timings Pointer to a Timings struct to receive the computed values. + * @retval true on success, false if no valid timing solution exists for the bitrate. + */ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings *out_timings) { @@ -459,6 +506,11 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi return true; } +/** + * @brief Read CAN bus error counters and queue statistics from hardware and + * software state into a canardProtocolStatus_t struct. + * @param pProtocolStat Pointer to the struct to populate with current status. + */ void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){ uint32_t esr = hcan1.Instance->ESR; pProtocolStat->BusOff = __HAL_CAN_GET_FLAG(&hcan1, CAN_FLAG_BOF); @@ -471,22 +523,37 @@ void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){ pProtocolStat->rx_buffer_hwm = canRxBufferHWM; } +/** + * @brief Return the current number of frames pending in the software TX queue. + * @retval Number of frames queued (0 to TX_QUEUE_SIZE-1). + */ int32_t canardSTM32GetTxQueueFillLevel(void) { return (canTxQueue.head - canTxQueue.tail + TX_QUEUE_SIZE) % TX_QUEUE_SIZE; } +/** + * @brief Return the current number of frames waiting in the software RX ring buffer. + * @retval Number of frames available to read (0 to RX_BUFFER_SIZE-1). + */ int32_t canardSTM32GetRxFifoFillLevel(void){ return rxBufferNumMessages(&RxBuffer); } +/** + * @brief Attempt to recover the CAN peripheral from a bus-off condition. + * AutoBusOff=ENABLE in hcan1.Init means the hardware recovers automatically + * after 128 * 11 recessive bits; this function is a no-op placeholder. + */ void canardSTM32RecoverFromBusOff(void){ // Auto recover from bus off is enabled // CLEAR_BIT(hcan1.Instance->CCCR, FDCAN_CCCR_INIT); // Clear INIT bit to recover from Bus-Off } -/* - get a 16 byte unique ID for this node, this should be based on the CPU unique ID or other unique ID - */ +/** + * @brief Populate a 16-byte unique node ID from the STM32 96-bit factory UID. + * The 12 UID bytes are copied into the first 12 bytes; the remainder is zero-padded. + * @param id Output buffer of exactly 16 bytes to receive the unique ID. + */ void canardSTM32GetUniqueID(uint8_t id[16]) { uint32_t HALUniqueIDs[3]; // Make Unique ID out of the 96-bit STM32 UID and fill the rest with 0s @@ -497,10 +564,18 @@ void canardSTM32GetUniqueID(uint8_t id[16]) { memcpy(id, HALUniqueIDs, 12); } +/** + * @brief CAN1 RX FIFO0 interrupt handler — delegates to the HAL IRQ handler. + */ void CAN1_RX0_IRQHandler(void) { HAL_CAN_IRQHandler(&hcan1); } +/** + * @brief HAL callback fired when a frame arrives in RX FIFO0 (runs in ISR context). + * Reads the frame from hardware and pushes it into the software RX ring buffer. + * @param hcan Pointer to the CAN_HandleTypeDef that triggered the callback. + */ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { RxFrame_t frame; if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &frame.header, frame.data) == HAL_OK) { @@ -508,6 +583,14 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { } } +/** + * @brief Drain the software TX queue into available hardware mailboxes. + * Called both from the TX-complete ISR callbacks and from canardSTM32Transmit() + * (with IRQs disabled) to seed the first transmission. With TXFP=ENABLE, + * hardware transmits mailboxes in request order, preserving frame sequence. + * Deactivates the TX empty notification when the queue is fully drained. + * @param hcan Pointer to the CAN_HandleTypeDef to load frames into. + */ static void canTxDrainQueue(CAN_HandleTypeDef *hcan) { // With TXFP=ENABLE the hardware transmits mailboxes in request order // (chronological), not mailbox-number order, so filling multiple mailboxes @@ -528,10 +611,20 @@ static void canTxDrainQueue(CAN_HandleTypeDef *hcan) { } } +/** + * @brief CAN1 TX interrupt handler — delegates to the HAL IRQ handler. + */ void CAN1_TX_IRQHandler(void) { HAL_CAN_IRQHandler(&hcan1); } +/** + * @brief HAL callbacks fired when a TX mailbox completes transmission (runs in ISR context). + * Each refills available mailboxes from the software TX queue via canTxDrainQueue(). + * AbortCallback is intentionally not wired: AutoRetransmission=ENABLE prevents + * ALST (arbitration lost), and TERR (bus fault) leaves the frame in the queue + * (tail not advanced) so the next canardSTM32Transmit call re-seeds the HW. + */ void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } From e2cc885d068460beb8572720aba7c00eb10ff08d Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 07:33:26 -0700 Subject: [PATCH 13/34] refactor(dronecan): reorganize driver files into logical sections MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Group functions by subsystem in all three files to aid navigation: F7 driver: Initialization → RX subsystem → TX subsystem → Diagnostics. Removes forward declarations (each function now defined before use). H7 driver: Initialization → RX → TX → Diagnostics. Same section layout as F7 for consistency. Also adds missing canardSTM32GetTxQueueFillLevel stub (declared in shared header, never implemented for H7 — caused undefined-reference link error on MATEKH743). dronecan.c: Message handlers → Libcanard callbacks → Message senders → TX/periodic processing → Public API. No logic changed. Both MATEKF765SE and MATEKH743 build clean. --- src/main/drivers/dronecan/dronecan.c | 119 +-- .../libcanard/canard_stm32f7xx_driver.c | 720 +++++++++--------- .../libcanard/canard_stm32h7xx_driver.c | 452 ++++++----- 3 files changed, 671 insertions(+), 620 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 98a6f230d7c..c9877bf575b 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -25,7 +25,7 @@ #include #include -/* Private variables ---------------------------------------------------------*/ +/* ── Private state ─────────────────────────────────────────────────────────── */ CanardInstance canard; uint8_t memory_pool[1024]; @@ -45,6 +45,8 @@ static dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES]; // NOTE: All canard handlers and senders are based on this reference: https://dronecan.github.io/Specification/7._List_of_standard_data_types/ // Alternatively, you can look at the corresponding generated header file in the dsdlc_generated folder +/* ── Message handlers (incoming) ───────────────────────────────────────────── */ + // Canard Handlers ( Many have code copied from libcanard esc_node example: https://github.com/dronecan/libcanard/blob/master/examples/ESCNode/esc_node.c ) void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { @@ -182,46 +184,7 @@ void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) { total_size); } -// Canard Senders - -/* - send the 1Hz NodeStatus message. This is what allows a node to show - up in the DroneCAN GUI tool and in the flight controller logs - */ -void send_NodeStatus(void) { - uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE]; - - // LOG_DEBUG(CAN, "Sending Node Status"); - node_status.uptime_sec = millis() / 1000UL; - if(isHardwareHealthy()){ - node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; - } - else { - node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL; - } - - node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; // Indicates that node is able to communicate over CAN, not that it is in flight. - node_status.sub_mode = 0; // Not currently used in dronecan - - // put whatever you like in here for display in GUI - node_status.vendor_specific_status_code = armingFlags; - - uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); - - // we need a static variable for the transfer ID. This is - // incremeneted on each transfer, allowing for detection of packet - // loss - static uint8_t transfer_id; - - canardBroadcast(&canard, - UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, - UAVCAN_PROTOCOL_NODESTATUS_ID, - &transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, - buffer, - len); - // PrintCanStatus(); -} +/* ── Libcanard protocol callbacks ───────────────────────────────────────────── */ // Canard Util /* @@ -312,27 +275,27 @@ void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { // check if we want to handle a specific broadcast message switch (transfer->data_type_id) { - case UAVCAN_PROTOCOL_NODESTATUS_ID: + case UAVCAN_PROTOCOL_NODESTATUS_ID: handle_NodeStatus(ins, transfer); break; - - case UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID: + + case UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID: handle_GNSSAuxiliary(ins, transfer); break; - - case UAVCAN_EQUIPMENT_GNSS_FIX_ID: + + case UAVCAN_EQUIPMENT_GNSS_FIX_ID: handle_GNSSFix(ins, transfer); break; - - case UAVCAN_EQUIPMENT_GNSS_FIX2_ID: + + case UAVCAN_EQUIPMENT_GNSS_FIX2_ID: handle_GNSSFix2(ins, transfer); break; - - case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID: + + case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID: handle_GNSSRCTMStream(ins, transfer); break; - + case UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID: LOG_DEBUG(CAN, "Battery Info"); handle_BatteryInfo(ins, transfer); @@ -341,6 +304,50 @@ void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { } } +/* ── Message senders (outgoing) ────────────────────────────────────────────── */ + +// Canard Senders + +/* + send the 1Hz NodeStatus message. This is what allows a node to show + up in the DroneCAN GUI tool and in the flight controller logs + */ +void send_NodeStatus(void) { + uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE]; + + // LOG_DEBUG(CAN, "Sending Node Status"); + node_status.uptime_sec = millis() / 1000UL; + if(isHardwareHealthy()){ + node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; + } + else { + node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL; + } + + node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; // Indicates that node is able to communicate over CAN, not that it is in flight. + node_status.sub_mode = 0; // Not currently used in dronecan + + // put whatever you like in here for display in GUI + node_status.vendor_specific_status_code = armingFlags; + + uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); + + // we need a static variable for the transfer ID. This is + // incremeneted on each transfer, allowing for detection of packet + // loss + static uint8_t transfer_id; + + canardBroadcast(&canard, + UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, + UAVCAN_PROTOCOL_NODESTATUS_ID, + &transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + len); + // PrintCanStatus(); +} + +/* ── TX and periodic task processing ───────────────────────────────────────── */ void processCanardTxQueue(void) { // Transmitting @@ -378,6 +385,8 @@ void process1HzTasks(timeUs_t timestamp_usec) send_NodeStatus(); } +/* ── Public API ────────────────────────────────────────────────────────────── */ + void dronecanInit(void) { LOG_DEBUG(CAN, "dronecan Init"); @@ -391,7 +400,7 @@ void dronecanInit(void) case DRONECAN_BITRATE_250KBPS: bitrate = 250000; break; - + case DRONECAN_BITRATE_500KBPS: bitrate = 500000; break; @@ -410,7 +419,7 @@ void dronecanInit(void) LOG_ERROR(CAN, "Unable to initialize the CAN peripheral"); // TODO: Notify the user that CAN does not work and disable the peripheral return; - } + } /* Initializing the Libcanard instance. */ @@ -492,9 +501,9 @@ void dronecanUpdate(timeUs_t currentTimeUs) dronecanState = STATE_DRONECAN_NORMAL; } break; - + } - + } dronecanState_e dronecanGetState(void) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index f35aeec950c..6c20e5c1a27 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -19,7 +19,10 @@ #include #include +/* ── Internal types and state ──────────────────────────────────────────────── */ + #define RX_BUFFER_SIZE 32 +#define TX_QUEUE_SIZE 32 struct Timings { uint16_t prescaler; @@ -39,24 +42,266 @@ static struct RxBuffer_t { RxFrame_t rxMsg[RX_BUFFER_SIZE]; } RxBuffer; -#define TX_QUEUE_SIZE 32 - static struct CanTxQueue_t { CanardCANFrame frames[TX_QUEUE_SIZE]; volatile uint8_t head; volatile uint8_t tail; } canTxQueue; -static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings*out_timings); -static void canardSTM32GPIO_Init(void); -static void canTxDrainQueue(CAN_HandleTypeDef *hcan); - static CAN_HandleTypeDef hcan1; static volatile uint16_t canTxDropped = 0; static volatile uint8_t canTxQueueHWM = 0; static volatile uint8_t canRxBufferHWM = 0; +/* ── Initialization ────────────────────────────────────────────────────────── */ + +/** + * @brief GPIO Initialization Function + * @param None + * @retval None + */ +static void canardSTM32GPIO_Init(void) +{ + // Set up the Rx and Tx pins for CAN1 and if present, the standby or listen only pin. +#if defined(CAN1_TX) && defined(CAN1_RX) + IOInit(IOGetByTag(IO_TAG(CAN1_TX)), OWNER_DRONECAN, RESOURCE_CAN_TX, 0); + IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_TX)), IOCFG_AF_PP, GPIO_AF9_CAN1); // How do I make the alternate function crossplatform? + IOInit(IOGetByTag(IO_TAG(CAN1_RX)), OWNER_DRONECAN, RESOURCE_CAN_RX, 0); + IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_RX)), IOCFG_AF_PP, GPIO_AF9_CAN1); // How do I make the alternate function crossplatform? +#endif + + + #ifdef CAN1_STANDBY + // Initialize the standby or listen only pin. Set default state to enable CAN. + // TODO: Tie the pin state to a configuration option so we can turn CAN on and off. + + IOInit(IOGetByTag(IO_TAG(CAN1_STANDBY)), OWNER_DRONECAN, RESOURCE_CAN_STANDBY, 0); + IOConfigGPIO(IOGetByTag(IO_TAG(CAN1_STANDBY)), IOCFG_OUT_PP); // Do any boards use pullups, external/internal? + IOLo(IOGetByTag(IO_TAG(CAN1_STANDBY))); +#endif +} + +/** + * @brief Compute bxCAN bit-timing parameters (prescaler, SJW, BS1, BS2) for a + * given target bitrate, targeting an ~87.5% sample point location. + * @param target_bitrate Desired CAN bus bitrate in bits per second. + * @param out_timings Pointer to a Timings struct to receive the computed values. + * @retval true on success, false if no valid timing solution exists for the bitrate. + */ +static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings *out_timings) +{ + + if (target_bitrate < 1) { + return false; + } + + /* + * Hardware configuration + */ + const uint32_t pclk = HAL_RCC_GetPCLK1Freq(); + + static const int MaxBS1 = 16; + static const int MaxBS2 = 8; + + /* + * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG + * CAN in Automation, 2003 + * + * According to the source, optimal quanta per bit are: + * Bitrate Optimal Maximum + * 1000 kbps 8 10 + * 500 kbps 16 17 + * 250 kbps 16 17 + * 125 kbps 16 17 + */ + const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 18; + static const int MaxSamplePointLocation = 900; + + /* + * Computing (prescaler * BS): + * BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2)) -- See the Reference Manual + * BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2)) -- Simplified + * let: + * BS = 1 + BS1 + BS2 -- Number of time quanta per bit + * PRESCALER_BS = PRESCALER * BS + * ==> + * PRESCALER_BS = PCLK / BITRATE + */ + const uint32_t prescaler_bs = pclk / target_bitrate; + /* + * Searching for such prescaler value so that the number of quanta per bit is highest. + */ + uint8_t bs1_bs2_sum = (uint8_t)(max_quanta_per_bit - 1); + + while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0) { + if (bs1_bs2_sum <= 2) { + return false; // No solution + } + bs1_bs2_sum--; + } + + const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum); + if ((prescaler < 1U) || (prescaler > 1024U)) { + return false; // No solution + } + /* + * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum. + * We need to find the values so that the sample point is as close as possible to the optimal value. + * + * Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2] (* Where 7/8 is 0.875, the recommended sample point location *) + * {{bs2 -> (1 + bs1)/7}} + * + * Hence: + * bs2 = (1 + bs1) / 7 + * bs1 = (7 * bs1_bs2_sum - 1) / 8 + * + * Sample point location can be computed as follows: + * Sample point location = (1 + bs1) / (1 + bs1 + bs2) + * + * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one: + * - With rounding to nearest + * - With rounding to zero + */ + struct BsPair { + uint8_t bs1; + uint8_t bs2; + uint16_t sample_point_permill; + } solution; + + // First attempt with rounding to nearest + solution.bs1 = (uint8_t)(((7 * bs1_bs2_sum - 1) + 4) / 8); + solution.bs2 = (uint8_t)(bs1_bs2_sum - solution.bs1); + solution.sample_point_permill = (uint16_t)(1000 * (1 + solution.bs1) / (1 + solution.bs1 + solution.bs2)); + + if (solution.sample_point_permill > MaxSamplePointLocation) { + // Second attempt with rounding to zero + solution.bs1 = (uint8_t)((7 * bs1_bs2_sum - 1) / 8); + solution.bs2 = (uint8_t)(bs1_bs2_sum - solution.bs1); + solution.sample_point_permill = (uint16_t)(1000 * (1 + solution.bs1) / (1 + solution.bs1 + solution.bs2)); + } + /* + * Final validation + * Helpful Python: + * def sample_point_from_btr(x): + * assert 0b0011110010000000111111000000000 & x == 0 + * ts2,ts1,brp = (x>>20)&7, (x>>16)&15, x&511 + * return (1+ts1+1)/(1+ts1+1+ts2+1) + * + */ + if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !((solution.bs1 >= 1) && (solution.bs1 <= MaxBS1) && (solution.bs2 >= 1) && (solution.bs2 <= MaxBS2))) + { + return false; + } + + LOG_DEBUG(CAN, "Timings: quanta/bit: %d, sample point location: %f%%", + (int)(1 + solution.bs1 + solution.bs2), (double)(solution.sample_point_permill) / (double)(10.0)); + + out_timings->prescaler = (uint16_t)(prescaler); + out_timings->sjw = 3; // Not happy with this value, but 1MBPs with unshielded cable? + out_timings->bs1 = (uint8_t)(solution.bs1)-1; // The HAL does not take care of the 1 bs offset in the register so remove it here like AP does. + out_timings->bs2 = (uint8_t)(solution.bs2)-1; // The HAL does not take care of the 1 bs offset in the register so remove it here like AP does. + + return true; +} + +/** + * @brief Initialize the bxCAN1 peripheral at the requested bitrate. + * Configures GPIO pins, bit timings, a pass-all acceptance filter, + * starts the peripheral, and enables RX and TX interrupts. + * @param bitrate Desired CAN bus bitrate in bits per second (e.g. 1000000). + * @retval CANARD_OK (1) on success, negative CANARD_ERROR code on failure. + */ +int16_t canardSTM32CAN1_Init(uint32_t bitrate) +{ +// RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0}; + struct Timings out_timings; + + /* CAN1 clock enable */ + __HAL_RCC_CAN1_CLK_ENABLE(); + + // /* USER CODE BEGIN CAN1_MspInit 1 */ + + CAN_FilterTypeDef sFilterConfig; + sFilterConfig.FilterIdHigh = 0; + sFilterConfig.FilterIdLow = 0; + sFilterConfig.FilterMaskIdHigh = 0; + sFilterConfig.FilterMaskIdLow = 0; + sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0; + sFilterConfig.FilterBank = 0; + sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; + sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; + sFilterConfig.FilterActivation = ENABLE; + + hcan1.Instance = CAN1; + hcan1.Init.Mode = CAN_MODE_NORMAL; + hcan1.Init.TimeTriggeredMode = DISABLE; + hcan1.Init.AutoBusOff = ENABLE; + hcan1.Init.AutoWakeUp = DISABLE; + hcan1.Init.AutoRetransmission = ENABLE; + hcan1.Init.ReceiveFifoLocked = DISABLE; + hcan1.Init.TransmitFifoPriority = ENABLE; // transmit in request order, not mailbox-number order + + canardSTM32ComputeTimings(bitrate, &out_timings); + + hcan1.Init.Prescaler = out_timings.prescaler; + hcan1.Init.SyncJumpWidth = (uint32_t)out_timings.sjw << CAN_BTR_SJW_Pos; + hcan1.Init.TimeSeg1 = (uint32_t)out_timings.bs1 << CAN_BTR_TS1_Pos; + hcan1.Init.TimeSeg2 = (uint32_t)out_timings.bs2 << CAN_BTR_TS2_Pos; + LOG_DEBUG(CAN, "Prescaler: %d, SJW: %d, BS1: %d, BS2: %d", out_timings.prescaler, out_timings.sjw, out_timings.bs1, out_timings.bs2); + + // hcan1.Init.StdFiltersNbr = 0; + // hcan1.Init.ExtFiltersNbr = 1; + // hcan1.Init.TxFifoQueueElmtsNbr = 32; + // LOG_DEBUG(CAN, "In CAN Init"); + + /** Initializes the peripherals clock + */ + // PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_FDCAN; + // PeriphClkInitStruct.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL; + // if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) + // { + // LOG_DEBUG(CAN, "Unable to configure peripheral clock"); + // } + + canardSTM32GPIO_Init(); // Set up the pins for CAN and optional listen only mode + + // LOG_DEBUG(CAN, "System Clock Speed: %lu", HAL_RCC_GetSysClockFreq()); + // LOG_DEBUG(CAN, "PClk1 Clock Speed: %lu", HAL_RCC_GetPCLK1Freq()); + if (HAL_CAN_Init(&hcan1) != HAL_OK) + { + LOG_ERROR(CAN, "Failed CAN Init"); + return -CANARD_ERROR_INTERNAL; + } + + /* USER CODE BEGIN FDCAN1_Init 2 */ + if (HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig) != HAL_OK) { + LOG_ERROR(CAN, "Failed Config Filter"); + return -CANARD_ERROR_INTERNAL; + } + + if (HAL_CAN_Start(&hcan1) != HAL_OK) { + LOG_ERROR(CAN, "Failed to Start"); + return -CANARD_ERROR_INTERNAL; + } + + if (HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK) { // persistent Rx notification + LOG_ERROR(CAN, "Failed to activate interrupt"); + return -CANARD_ERROR_INTERNAL; + } + + // Enable interrupt only after all initialization succeeds + // (if any previous step failed, we return early without enabling IRQ) + HAL_NVIC_SetPriority(CAN1_RX0_IRQn, NVIC_PRIO_CAN, 0); + HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); + HAL_NVIC_SetPriority(CAN1_TX_IRQn, NVIC_PRIO_CAN, 0); + HAL_NVIC_EnableIRQ(CAN1_TX_IRQn); + + return CANARD_OK; +} + +/* ── RX subsystem ──────────────────────────────────────────────────────────── */ + /** * @brief Push a received CAN frame into the ring buffer (called from ISR). * @param rxBuf Pointer to the RxBuffer_t ring buffer. @@ -120,13 +365,70 @@ uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf) { } /** - * @brief Push a frame onto the software TX queue (called from main loop). - * Updates the high-water mark and increments the drop counter if full. - * @param frame Pointer to the CanardCANFrame to enqueue. - * @retval true if the frame was enqueued, false if the queue was full (frame dropped). + * @brief CAN1 RX FIFO0 interrupt handler — delegates to the HAL IRQ handler. */ -static bool canTxQueuePush(const CanardCANFrame *frame) { - uint8_t next = (canTxQueue.head + 1) % TX_QUEUE_SIZE; +void CAN1_RX0_IRQHandler(void) { + HAL_CAN_IRQHandler(&hcan1); +} + +/** + * @brief HAL callback fired when a frame arrives in RX FIFO0 (runs in ISR context). + * Reads the frame from hardware and pushes it into the software RX ring buffer. + * @param hcan Pointer to the CAN_HandleTypeDef that triggered the callback. + */ +void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { + RxFrame_t frame; + if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &frame.header, frame.data) == HAL_OK) { + rxBufferPushFrame(&RxBuffer, &frame); + } +} + +/** + * @brief Process CAN message from RxLocation FIFO into rx_frame + * @param rx_frame pointer to a CanardCANFrame structure where the received CAN message will be + * stored. + * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode + */ +int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { + RxFrame_t canRxFrame; + + if (rx_frame == NULL) { + return -CANARD_ERROR_INVALID_ARGUMENT; + } + + if (rxBufferPopFrame(&RxBuffer, &canRxFrame) == 0) { // Wheres the data? + rx_frame->id = canRxFrame.header.ExtId; + + // Process ID to canard format + if (canRxFrame.header.IDE == CAN_ID_EXT) { // canard will only process the message if it is extended ID + rx_frame->id |= CANARD_CAN_FRAME_EFF; + } + + if (canRxFrame.header.RTR == CAN_RTR_REMOTE) { // canard won't process the message if it is a remote frame + rx_frame->id |= CANARD_CAN_FRAME_RTR; + } + + rx_frame->data_len = canRxFrame.header.DLC; + memcpy(rx_frame->data, canRxFrame.data, canRxFrame.header.DLC); + + // assume a single interface + rx_frame->iface_id = 0; + return 1; + } + // Either no CAN msg to be read, or an error that can be read from hfdcan->ErrorCode + return 0; +} + +/* ── TX subsystem ──────────────────────────────────────────────────────────── */ + +/** + * @brief Push a frame onto the software TX queue (called from main loop). + * Updates the high-water mark and increments the drop counter if full. + * @param frame Pointer to the CanardCANFrame to enqueue. + * @retval true if the frame was enqueued, false if the queue was full (frame dropped). + */ +static bool canTxQueuePush(const CanardCANFrame *frame) { + uint8_t next = (canTxQueue.head + 1) % TX_QUEUE_SIZE; uint8_t tail_snapshot = canTxQueue.tail; // snapshot before ISR can advance it if (next == tail_snapshot) { canTxDropped++; @@ -171,42 +473,6 @@ static void canTxQueueConsume(void) { canTxQueue.tail = (canTxQueue.tail + 1) % TX_QUEUE_SIZE; } -/** - * @brief Process CAN message from RxLocation FIFO into rx_frame - * @param rx_frame pointer to a CanardCANFrame structure where the received CAN message will be - * stored. - * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode - */ -int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { - RxFrame_t canRxFrame; - - if (rx_frame == NULL) { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - - if (rxBufferPopFrame(&RxBuffer, &canRxFrame) == 0) { // Wheres the data? - rx_frame->id = canRxFrame.header.ExtId; - - // Process ID to canard format - if (canRxFrame.header.IDE == CAN_ID_EXT) { // canard will only process the message if it is extended ID - rx_frame->id |= CANARD_CAN_FRAME_EFF; - } - - if (canRxFrame.header.RTR == CAN_RTR_REMOTE) { // canard won't process the message if it is a remote frame - rx_frame->id |= CANARD_CAN_FRAME_RTR; - } - - rx_frame->data_len = canRxFrame.header.DLC; - memcpy(rx_frame->data, canRxFrame.data, canRxFrame.header.DLC); - - // assume a single interface - rx_frame->iface_id = 0; - return 1; - } - // Either no CAN msg to be read, or an error that can be read from hfdcan->ErrorCode - return 0; -} - /** * @brief Populate a HAL CAN TX header and data buffer from a CanardCANFrame. * @param frame Pointer to the source CanardCANFrame. @@ -227,6 +493,52 @@ static void buildTxHeader(const CanardCANFrame *frame, CAN_TxHeaderTypeDef *head memcpy(data, frame->data, frame->data_len); } +/** + * @brief Drain the software TX queue into available hardware mailboxes. + * Called both from the TX-complete ISR callbacks and from canardSTM32Transmit() + * (with IRQs disabled) to seed the first transmission. With TXFP=ENABLE, + * hardware transmits mailboxes in request order, preserving frame sequence. + * Deactivates the TX empty notification when the queue is fully drained. + * @param hcan Pointer to the CAN_HandleTypeDef to load frames into. + */ +static void canTxDrainQueue(CAN_HandleTypeDef *hcan) { + // With TXFP=ENABLE the hardware transmits mailboxes in request order + // (chronological), not mailbox-number order, so filling multiple mailboxes + // per callback is safe and preserves frame sequence. + const CanardCANFrame *frame; + while ((frame = canTxQueuePeek()) != NULL && HAL_CAN_GetTxMailboxesFreeLevel(hcan) > 0) { + CAN_TxHeaderTypeDef txHeader = {}; + uint8_t txData[8]; + uint32_t txMailbox; + buildTxHeader(frame, &txHeader, txData); + if (HAL_CAN_AddTxMessage(hcan, &txHeader, txData, &txMailbox) != HAL_OK) { + break; // frame stays in queue and will retry on next ISR + } + canTxQueueConsume(); + } + if (canTxQueueIsEmpty()) { + HAL_CAN_DeactivateNotification(hcan, CAN_IT_TX_MAILBOX_EMPTY); + } +} + +/** + * @brief CAN1 TX interrupt handler — delegates to the HAL IRQ handler. + */ +void CAN1_TX_IRQHandler(void) { + HAL_CAN_IRQHandler(&hcan1); +} + +/** + * @brief HAL callbacks fired when a TX mailbox completes transmission (runs in ISR context). + * Each refills available mailboxes from the software TX queue via canTxDrainQueue(). + * AbortCallback is intentionally not wired: AutoRetransmission=ENABLE prevents + * ALST (arbitration lost), and TERR (bus fault) leaves the frame in the queue + * (tail not advanced) so the next canardSTM32Transmit call re-seeds the HW. + */ +void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } +void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } +void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } + /** * @brief Process tx_frame CAN message into Tx FIFO/Queue and transmit it * @param tx_frame pointer to a CanardCANFrame structure that contains the CAN message to @@ -262,249 +574,7 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { return 1; } -/** - * @brief Initialize the bxCAN1 peripheral at the requested bitrate. - * Configures GPIO pins, bit timings, a pass-all acceptance filter, - * starts the peripheral, and enables RX and TX interrupts. - * @param bitrate Desired CAN bus bitrate in bits per second (e.g. 1000000). - * @retval CANARD_OK (1) on success, negative CANARD_ERROR code on failure. - */ -int16_t canardSTM32CAN1_Init(uint32_t bitrate) -{ -// RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0}; - struct Timings out_timings; - - /* CAN1 clock enable */ - __HAL_RCC_CAN1_CLK_ENABLE(); - - // /* USER CODE BEGIN CAN1_MspInit 1 */ - - CAN_FilterTypeDef sFilterConfig; - sFilterConfig.FilterIdHigh = 0; - sFilterConfig.FilterIdLow = 0; - sFilterConfig.FilterMaskIdHigh = 0; - sFilterConfig.FilterMaskIdLow = 0; - sFilterConfig.FilterFIFOAssignment = CAN_FILTER_FIFO0; - sFilterConfig.FilterBank = 0; - sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; - sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; - sFilterConfig.FilterActivation = ENABLE; - - hcan1.Instance = CAN1; - hcan1.Init.Mode = CAN_MODE_NORMAL; - hcan1.Init.TimeTriggeredMode = DISABLE; - hcan1.Init.AutoBusOff = ENABLE; - hcan1.Init.AutoWakeUp = DISABLE; - hcan1.Init.AutoRetransmission = ENABLE; - hcan1.Init.ReceiveFifoLocked = DISABLE; - hcan1.Init.TransmitFifoPriority = ENABLE; // transmit in request order, not mailbox-number order - - canardSTM32ComputeTimings(bitrate, &out_timings); - - hcan1.Init.Prescaler = out_timings.prescaler; - hcan1.Init.SyncJumpWidth = (uint32_t)out_timings.sjw << CAN_BTR_SJW_Pos; - hcan1.Init.TimeSeg1 = (uint32_t)out_timings.bs1 << CAN_BTR_TS1_Pos; - hcan1.Init.TimeSeg2 = (uint32_t)out_timings.bs2 << CAN_BTR_TS2_Pos; - LOG_DEBUG(CAN, "Prescaler: %d, SJW: %d, BS1: %d, BS2: %d", out_timings.prescaler, out_timings.sjw, out_timings.bs1, out_timings.bs2); - - // hcan1.Init.StdFiltersNbr = 0; - // hcan1.Init.ExtFiltersNbr = 1; - // hcan1.Init.TxFifoQueueElmtsNbr = 32; - // LOG_DEBUG(CAN, "In CAN Init"); - - /** Initializes the peripherals clock - */ - // PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_FDCAN; - // PeriphClkInitStruct.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL; - // if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) - // { - // LOG_DEBUG(CAN, "Unable to configure peripheral clock"); - // } - - canardSTM32GPIO_Init(); // Set up the pins for CAN and optional listen only mode - - // LOG_DEBUG(CAN, "System Clock Speed: %lu", HAL_RCC_GetSysClockFreq()); - // LOG_DEBUG(CAN, "PClk1 Clock Speed: %lu", HAL_RCC_GetPCLK1Freq()); - if (HAL_CAN_Init(&hcan1) != HAL_OK) - { - LOG_ERROR(CAN, "Failed CAN Init"); - return -CANARD_ERROR_INTERNAL; - } - - /* USER CODE BEGIN FDCAN1_Init 2 */ - if (HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig) != HAL_OK) { - LOG_ERROR(CAN, "Failed Config Filter"); - return -CANARD_ERROR_INTERNAL; - } - - if (HAL_CAN_Start(&hcan1) != HAL_OK) { - LOG_ERROR(CAN, "Failed to Start"); - return -CANARD_ERROR_INTERNAL; - } - - if (HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK) { // persistent Rx notification - LOG_ERROR(CAN, "Failed to activate interrupt"); - return -CANARD_ERROR_INTERNAL; - } - - // Enable interrupt only after all initialization succeeds - // (if any previous step failed, we return early without enabling IRQ) - HAL_NVIC_SetPriority(CAN1_RX0_IRQn, NVIC_PRIO_CAN, 0); - HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); - HAL_NVIC_SetPriority(CAN1_TX_IRQn, NVIC_PRIO_CAN, 0); - HAL_NVIC_EnableIRQ(CAN1_TX_IRQn); - - return CANARD_OK; -} - -/** - * @brief GPIO Initialization Function - * @param None - * @retval None - */ -static void canardSTM32GPIO_Init(void) -{ - - // Set up the Rx and Tx pins for CAN1 and if present, the standby or listen only pin. -#if defined(CAN1_TX) && defined(CAN1_RX) - IOInit(IOGetByTag(IO_TAG(CAN1_TX)), OWNER_DRONECAN, RESOURCE_CAN_TX, 0); - IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_TX)), IOCFG_AF_PP, GPIO_AF9_CAN1); // How do I make the alternate function crossplatform? - IOInit(IOGetByTag(IO_TAG(CAN1_RX)), OWNER_DRONECAN, RESOURCE_CAN_RX, 0); - IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_RX)), IOCFG_AF_PP, GPIO_AF9_CAN1); // How do I make the alternate function crossplatform? -#endif - - - #ifdef CAN1_STANDBY - // Initialize the standby or listen only pin. Set default state to enable CAN. - // TODO: Tie the pin state to a configuration option so we can turn CAN on and off. - - IOInit(IOGetByTag(IO_TAG(CAN1_STANDBY)), OWNER_DRONECAN, RESOURCE_CAN_STANDBY, 0); - IOConfigGPIO(IOGetByTag(IO_TAG(CAN1_STANDBY)), IOCFG_OUT_PP); // Do any boards use pullups, external/internal? - IOLo(IOGetByTag(IO_TAG(CAN1_STANDBY))); -#endif -} -/** - * @brief Compute bxCAN bit-timing parameters (prescaler, SJW, BS1, BS2) for a - * given target bitrate, targeting an ~87.5% sample point location. - * @param target_bitrate Desired CAN bus bitrate in bits per second. - * @param out_timings Pointer to a Timings struct to receive the computed values. - * @retval true on success, false if no valid timing solution exists for the bitrate. - */ -static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings *out_timings) -{ - - if (target_bitrate < 1) { - return false; - } - - /* - * Hardware configuration - */ - const uint32_t pclk = HAL_RCC_GetPCLK1Freq(); - - static const int MaxBS1 = 16; - static const int MaxBS2 = 8; - - /* - * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG - * CAN in Automation, 2003 - * - * According to the source, optimal quanta per bit are: - * Bitrate Optimal Maximum - * 1000 kbps 8 10 - * 500 kbps 16 17 - * 250 kbps 16 17 - * 125 kbps 16 17 - */ - const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 18; - static const int MaxSamplePointLocation = 900; - - /* - * Computing (prescaler * BS): - * BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2)) -- See the Reference Manual - * BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2)) -- Simplified - * let: - * BS = 1 + BS1 + BS2 -- Number of time quanta per bit - * PRESCALER_BS = PRESCALER * BS - * ==> - * PRESCALER_BS = PCLK / BITRATE - */ - const uint32_t prescaler_bs = pclk / target_bitrate; - /* - * Searching for such prescaler value so that the number of quanta per bit is highest. - */ - uint8_t bs1_bs2_sum = (uint8_t)(max_quanta_per_bit - 1); - - while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0) { - if (bs1_bs2_sum <= 2) { - return false; // No solution - } - bs1_bs2_sum--; - } - - const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum); - if ((prescaler < 1U) || (prescaler > 1024U)) { - return false; // No solution - } - /* - * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum. - * We need to find the values so that the sample point is as close as possible to the optimal value. - * - * Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2] (* Where 7/8 is 0.875, the recommended sample point location *) - * {{bs2 -> (1 + bs1)/7}} - * - * Hence: - * bs2 = (1 + bs1) / 7 - * bs1 = (7 * bs1_bs2_sum - 1) / 8 - * - * Sample point location can be computed as follows: - * Sample point location = (1 + bs1) / (1 + bs1 + bs2) - * - * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one: - * - With rounding to nearest - * - With rounding to zero - */ - struct BsPair { - uint8_t bs1; - uint8_t bs2; - uint16_t sample_point_permill; - } solution; - - // First attempt with rounding to nearest - solution.bs1 = (uint8_t)(((7 * bs1_bs2_sum - 1) + 4) / 8); - solution.bs2 = (uint8_t)(bs1_bs2_sum - solution.bs1); - solution.sample_point_permill = (uint16_t)(1000 * (1 + solution.bs1) / (1 + solution.bs1 + solution.bs2)); - - if (solution.sample_point_permill > MaxSamplePointLocation) { - // Second attempt with rounding to zero - solution.bs1 = (uint8_t)((7 * bs1_bs2_sum - 1) / 8); - solution.bs2 = (uint8_t)(bs1_bs2_sum - solution.bs1); - solution.sample_point_permill = (uint16_t)(1000 * (1 + solution.bs1) / (1 + solution.bs1 + solution.bs2)); - } - /* - * Final validation - * Helpful Python: - * def sample_point_from_btr(x): - * assert 0b0011110010000000111111000000000 & x == 0 - * ts2,ts1,brp = (x>>20)&7, (x>>16)&15, x&511 - * return (1+ts1+1)/(1+ts1+1+ts2+1) - * - */ - if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !((solution.bs1 >= 1) && (solution.bs1 <= MaxBS1) && (solution.bs2 >= 1) && (solution.bs2 <= MaxBS2))) - { - return false; - } - - LOG_DEBUG(CAN, "Timings: quanta/bit: %d, sample point location: %f%%", - (int)(1 + solution.bs1 + solution.bs2), (double)(solution.sample_point_permill) / (double)(10.0)); - - out_timings->prescaler = (uint16_t)(prescaler); - out_timings->sjw = 3; // Not happy with this value, but 1MBPs with unshielded cable? - out_timings->bs1 = (uint8_t)(solution.bs1)-1; // The HAL does not take care of the 1 bs offset in the register so remove it here like AP does. - out_timings->bs2 = (uint8_t)(solution.bs2)-1; // The HAL does not take care of the 1 bs offset in the register so remove it here like AP does. - - return true; -} +/* ── Diagnostics ───────────────────────────────────────────────────────────── */ /** * @brief Read CAN bus error counters and queue statistics from hardware and @@ -563,71 +633,3 @@ void canardSTM32GetUniqueID(uint8_t id[16]) { HALUniqueIDs[2] = *(uint32_t *)(UID_BASE + 8); memcpy(id, HALUniqueIDs, 12); } - -/** - * @brief CAN1 RX FIFO0 interrupt handler — delegates to the HAL IRQ handler. - */ -void CAN1_RX0_IRQHandler(void) { - HAL_CAN_IRQHandler(&hcan1); -} - -/** - * @brief HAL callback fired when a frame arrives in RX FIFO0 (runs in ISR context). - * Reads the frame from hardware and pushes it into the software RX ring buffer. - * @param hcan Pointer to the CAN_HandleTypeDef that triggered the callback. - */ -void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { - RxFrame_t frame; - if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &frame.header, frame.data) == HAL_OK) { - rxBufferPushFrame(&RxBuffer, &frame); - } -} - -/** - * @brief Drain the software TX queue into available hardware mailboxes. - * Called both from the TX-complete ISR callbacks and from canardSTM32Transmit() - * (with IRQs disabled) to seed the first transmission. With TXFP=ENABLE, - * hardware transmits mailboxes in request order, preserving frame sequence. - * Deactivates the TX empty notification when the queue is fully drained. - * @param hcan Pointer to the CAN_HandleTypeDef to load frames into. - */ -static void canTxDrainQueue(CAN_HandleTypeDef *hcan) { - // With TXFP=ENABLE the hardware transmits mailboxes in request order - // (chronological), not mailbox-number order, so filling multiple mailboxes - // per callback is safe and preserves frame sequence. - const CanardCANFrame *frame; - while ((frame = canTxQueuePeek()) != NULL && HAL_CAN_GetTxMailboxesFreeLevel(hcan) > 0) { - CAN_TxHeaderTypeDef txHeader = {}; - uint8_t txData[8]; - uint32_t txMailbox; - buildTxHeader(frame, &txHeader, txData); - if (HAL_CAN_AddTxMessage(hcan, &txHeader, txData, &txMailbox) != HAL_OK) { - break; // frame stays in queue and will retry on next ISR - } - canTxQueueConsume(); - } - if (canTxQueueIsEmpty()) { - HAL_CAN_DeactivateNotification(hcan, CAN_IT_TX_MAILBOX_EMPTY); - } -} - -/** - * @brief CAN1 TX interrupt handler — delegates to the HAL IRQ handler. - */ -void CAN1_TX_IRQHandler(void) { - HAL_CAN_IRQHandler(&hcan1); -} - -/** - * @brief HAL callbacks fired when a TX mailbox completes transmission (runs in ISR context). - * Each refills available mailboxes from the software TX queue via canTxDrainQueue(). - * AbortCallback is intentionally not wired: AutoRetransmission=ENABLE prevents - * ALST (arbitration lost), and TERR (bus fault) leaves the frame in the queue - * (tail not advanced) so the next canardSTM32Transmit call re-seeds the HW. - */ -void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } -void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } -void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } -// AbortCallback is intentionally not wired: AutoRetransmission=ENABLE prevents -// ALST (arbitration lost), and TERR (bus fault) leaves the frame in the queue -// (tail not advanced) so the next canardSTM32Transmit call re-seeds the HW. diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c index 77c1f94921f..7d2ca18775e 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c @@ -18,6 +18,8 @@ #include #include +/* ── Internal types and state ──────────────────────────────────────────────── */ + struct Timings { uint16_t prescaler; uint8_t sjw; @@ -25,210 +27,9 @@ struct Timings { uint8_t bs2; }; -static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings*out_timings); -static void canardSTM32GPIO_Init(void); - static FDCAN_HandleTypeDef hfdcan1; -/** - * @brief Process CAN message from RxLocation FIFO into rx_frame - * @param hfdcan pointer to an FDCAN_HandleTypeDef structure that contains - * the configuration information for the specified FDCAN. - * @param RxLocation Location of the received message to be read. - * This parameter can be a value of @arg FDCAN_Rx_location. - * @param rx_frame pointer to a CanardCANFrame structure where the received CAN message will be - * stored. - * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode - */ -int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { - if (rx_frame == NULL) { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - - FDCAN_RxHeaderTypeDef RxHeader; - uint8_t RxData[8]; - - if (HAL_FDCAN_GetRxMessage(&hfdcan1, FDCAN_RX_FIFO0, &RxHeader, RxData) == HAL_OK) { - - // Process ID to canard format - rx_frame->id = RxHeader.Identifier; - - if (RxHeader.IdType == FDCAN_EXTENDED_ID) { // canard will only process the message if it is extended ID - rx_frame->id |= CANARD_CAN_FRAME_EFF; - } - - if (RxHeader.RxFrameType == FDCAN_REMOTE_FRAME) { // canard won't process the message if it is a remote frame - rx_frame->id |= CANARD_CAN_FRAME_RTR; - } - - rx_frame->data_len = RxHeader.DataLength; - memcpy(rx_frame->data, RxData, RxHeader.DataLength); - - // assume a single interface - rx_frame->iface_id = 0; - - return 1; - } - - // Either no CAN msg to be read, or an error that can be read from hfdcan->ErrorCode - return 0; -} - -/** - * @brief Process tx_frame CAN message into Tx FIFO/Queue and transmit it - * @param tx_frame pointer to a CanardCANFrame structure that contains the CAN message to - * transmit. - * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode - */ -int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { - if (tx_frame == NULL) { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - - if (tx_frame->id & CANARD_CAN_FRAME_ERR) { - return -CANARD_ERROR_INVALID_ARGUMENT; // unsupported frame format - } - - FDCAN_TxHeaderTypeDef TxHeader; - uint8_t TxData[8]; - - // Process canard id to STM FDCAN header format - if (tx_frame->id & CANARD_CAN_FRAME_EFF) { - TxHeader.IdType = FDCAN_EXTENDED_ID; - TxHeader.Identifier = tx_frame->id & CANARD_CAN_EXT_ID_MASK; - } else { - TxHeader.IdType = FDCAN_STANDARD_ID; - TxHeader.Identifier = tx_frame->id & CANARD_CAN_STD_ID_MASK; - } - - TxHeader.DataLength = tx_frame->data_len; - - if (tx_frame->id & CANARD_CAN_FRAME_RTR) { - TxHeader.TxFrameType = FDCAN_REMOTE_FRAME; - } else { - TxHeader.TxFrameType = FDCAN_DATA_FRAME; - } - - TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE; // unsure about this one - TxHeader.BitRateSwitch = FDCAN_BRS_OFF; // Disabling FDCAN (using CAN 2.0) - TxHeader.FDFormat = FDCAN_CLASSIC_CAN; // Disabling FDCAN (using CAN 2.0) - TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS; // unsure about this one - TxHeader.MessageMarker = 0; // unsure about this one - if (TxHeader.DataLength <= sizeof(TxData)) - { - memcpy(TxData, tx_frame->data, TxHeader.DataLength); - } - else - { - LOG_ERROR(CAN, "Data to transmit is larger than 8 byte frame size"); - return -CANARD_ERROR_INVALID_ARGUMENT; - } - - if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &TxHeader, TxData) == HAL_OK) { - // LOG_DEBUG(CAN, "Successfully sent message with id: %lu", TxHeader.Identifier); - return 1; - } - - LOG_DEBUG(CAN, "Failed at adding message with id: %lu to Tx Queue", TxHeader.Identifier); - - // This might be for many reasons including the Tx Fifo being full, the error can be read from hfdcan->ErrorCode - return 0; -} - -/** - * @brief CAN1 Initialization Function - * @param bitrate desired bitrate to run the CAN network at. - * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode - */ -int16_t canardSTM32CAN1_Init(uint32_t bitrate) -{ - RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0}; - struct Timings out_timings; - int16_t ErrorCode = 1; - - /* USER CODE BEGIN FDCAN1_Init 0 */ - - /* USER CODE END FDCAN1_Init 0 */ - - /* USER CODE BEGIN FDCAN1_Init 1 */ - - FDCAN_FilterTypeDef sFilterConfig; - sFilterConfig.IdType = FDCAN_EXTENDED_ID; - sFilterConfig.FilterIndex = 0; - sFilterConfig.FilterType = FDCAN_FILTER_DUAL; - sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0; - sFilterConfig.FilterID1 = 0x0; - sFilterConfig.FilterID2 = 0x1FFFFFFFU; - /* USER CODE END FDCAN1_Init 1 */ - hfdcan1.Instance = FDCAN1; - hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC; // Initialize in CAN2.0 mode not CAN_FD - hfdcan1.Init.Mode = FDCAN_MODE_NORMAL; - hfdcan1.Init.AutoRetransmission = DISABLE; - hfdcan1.Init.TransmitPause = DISABLE; - hfdcan1.Init.ProtocolException = DISABLE; - - ErrorCode = canardSTM32ComputeTimings(bitrate, &out_timings); - if (ErrorCode != 1) - { - LOG_ERROR(CAN, "Unable to calculate timings, Error Code:%d", ErrorCode); - return -CANARD_ERROR_INTERNAL; - } - - hfdcan1.Init.NominalPrescaler = out_timings.prescaler; - hfdcan1.Init.NominalSyncJumpWidth = out_timings.sjw; - hfdcan1.Init.NominalTimeSeg1 = out_timings.bs1; - hfdcan1.Init.NominalTimeSeg2 = out_timings.bs2; - LOG_DEBUG(CAN, "Prescaler: %d, SJW: %d, BS1: %d, BS2: %d", out_timings.prescaler, out_timings.sjw, out_timings.bs1, out_timings.bs2); - - hfdcan1.Init.RxFifo0ElmtsNbr = 30; - hfdcan1.Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_8; - hfdcan1.Init.RxBuffersNbr = 1; - hfdcan1.Init.RxBufferSize = FDCAN_DATA_BYTES_8; - hfdcan1.Init.StdFiltersNbr = 0; - hfdcan1.Init.ExtFiltersNbr = 1; - hfdcan1.Init.TxFifoQueueElmtsNbr = 32; - hfdcan1.Init.TxEventsNbr = 0; - hfdcan1.Init.TxBuffersNbr = 5; - hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION; - hfdcan1.Init.TxElmtSize = FDCAN_DATA_BYTES_8; - LOG_DEBUG(CAN, "In CAN Init"); - - /** Initializes the peripherals clock - */ - PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_FDCAN; - PeriphClkInitStruct.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL; - if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) - { - LOG_DEBUG(CAN, "Unable to configure peripheral clock"); - return -CANARD_ERROR_INTERNAL; - } - - /* FDCAN1 clock enable */ - __HAL_RCC_FDCAN_CLK_ENABLE(); - - canardSTM32GPIO_Init(); // Set up the pins for CAN and optional listen only mode - - if (HAL_FDCAN_Init(&hfdcan1) != HAL_OK) - { - LOG_ERROR(CAN, "Failed CAN Init"); - return -CANARD_ERROR_INTERNAL; - } - /* USER CODE BEGIN FDCAN1_Init 2 */ - if (HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig) != HAL_OK) { - LOG_ERROR(CAN, "Failed Config Filter"); - return -CANARD_ERROR_INTERNAL; - } - if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE) != HAL_OK) { - LOG_ERROR(CAN, "Failed to config FDCAN filter"); - return -CANARD_ERROR_INTERNAL; - } - - if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK) { - LOG_ERROR(CAN, "Failed to Start"); - return -CANARD_ERROR_INTERNAL; - } - return CANARD_OK; -} +/* ── Initialization ────────────────────────────────────────────────────────── */ /** * @brief GPIO Initialization Function @@ -255,6 +56,14 @@ static void canardSTM32GPIO_Init(void) IOLo(IOGetByTag(IO_TAG(CAN1_STANDBY))); #endif } + +/** + * @brief Compute FDCAN bit-timing parameters (prescaler, SJW, BS1, BS2) for a + * given target bitrate, targeting an ~87.5% sample point location. + * @param target_bitrate Desired CAN bus bitrate in bits per second. + * @param out_timings Pointer to a Timings struct to receive the computed values. + * @retval true on success, false if no valid timing solution exists for the bitrate. + */ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings *out_timings) { if (target_bitrate < 1) { @@ -376,6 +185,217 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi return true; } +/** + * @brief CAN1 Initialization Function + * @param bitrate desired bitrate to run the CAN network at. + * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode + */ +int16_t canardSTM32CAN1_Init(uint32_t bitrate) +{ + RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0}; + struct Timings out_timings; + int16_t ErrorCode = 1; + + /* USER CODE BEGIN FDCAN1_Init 0 */ + + /* USER CODE END FDCAN1_Init 0 */ + + /* USER CODE BEGIN FDCAN1_Init 1 */ + + FDCAN_FilterTypeDef sFilterConfig; + sFilterConfig.IdType = FDCAN_EXTENDED_ID; + sFilterConfig.FilterIndex = 0; + sFilterConfig.FilterType = FDCAN_FILTER_DUAL; + sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0; + sFilterConfig.FilterID1 = 0x0; + sFilterConfig.FilterID2 = 0x1FFFFFFFU; + /* USER CODE END FDCAN1_Init 1 */ + hfdcan1.Instance = FDCAN1; + hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC; // Initialize in CAN2.0 mode not CAN_FD + hfdcan1.Init.Mode = FDCAN_MODE_NORMAL; + hfdcan1.Init.AutoRetransmission = DISABLE; + hfdcan1.Init.TransmitPause = DISABLE; + hfdcan1.Init.ProtocolException = DISABLE; + + ErrorCode = canardSTM32ComputeTimings(bitrate, &out_timings); + if (ErrorCode != 1) + { + LOG_ERROR(CAN, "Unable to calculate timings, Error Code:%d", ErrorCode); + return -CANARD_ERROR_INTERNAL; + } + + hfdcan1.Init.NominalPrescaler = out_timings.prescaler; + hfdcan1.Init.NominalSyncJumpWidth = out_timings.sjw; + hfdcan1.Init.NominalTimeSeg1 = out_timings.bs1; + hfdcan1.Init.NominalTimeSeg2 = out_timings.bs2; + LOG_DEBUG(CAN, "Prescaler: %d, SJW: %d, BS1: %d, BS2: %d", out_timings.prescaler, out_timings.sjw, out_timings.bs1, out_timings.bs2); + + hfdcan1.Init.RxFifo0ElmtsNbr = 30; + hfdcan1.Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_8; + hfdcan1.Init.RxBuffersNbr = 1; + hfdcan1.Init.RxBufferSize = FDCAN_DATA_BYTES_8; + hfdcan1.Init.StdFiltersNbr = 0; + hfdcan1.Init.ExtFiltersNbr = 1; + hfdcan1.Init.TxFifoQueueElmtsNbr = 32; + hfdcan1.Init.TxEventsNbr = 0; + hfdcan1.Init.TxBuffersNbr = 5; + hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION; + hfdcan1.Init.TxElmtSize = FDCAN_DATA_BYTES_8; + LOG_DEBUG(CAN, "In CAN Init"); + + /** Initializes the peripherals clock + */ + PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_FDCAN; + PeriphClkInitStruct.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL; + if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) + { + LOG_DEBUG(CAN, "Unable to configure peripheral clock"); + return -CANARD_ERROR_INTERNAL; + } + + /* FDCAN1 clock enable */ + __HAL_RCC_FDCAN_CLK_ENABLE(); + + canardSTM32GPIO_Init(); // Set up the pins for CAN and optional listen only mode + + if (HAL_FDCAN_Init(&hfdcan1) != HAL_OK) + { + LOG_ERROR(CAN, "Failed CAN Init"); + return -CANARD_ERROR_INTERNAL; + } + /* USER CODE BEGIN FDCAN1_Init 2 */ + if (HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig) != HAL_OK) { + LOG_ERROR(CAN, "Failed Config Filter"); + return -CANARD_ERROR_INTERNAL; + } + if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE) != HAL_OK) { + LOG_ERROR(CAN, "Failed to config FDCAN filter"); + return -CANARD_ERROR_INTERNAL; + } + + if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK) { + LOG_ERROR(CAN, "Failed to Start"); + return -CANARD_ERROR_INTERNAL; + } + return CANARD_OK; +} + +/* ── RX ────────────────────────────────────────────────────────────────────── */ + +/** + * @brief Process CAN message from RxLocation FIFO into rx_frame + * @param hfdcan pointer to an FDCAN_HandleTypeDef structure that contains + * the configuration information for the specified FDCAN. + * @param RxLocation Location of the received message to be read. + * This parameter can be a value of @arg FDCAN_Rx_location. + * @param rx_frame pointer to a CanardCANFrame structure where the received CAN message will be + * stored. + * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode + */ +int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { + if (rx_frame == NULL) { + return -CANARD_ERROR_INVALID_ARGUMENT; + } + + FDCAN_RxHeaderTypeDef RxHeader; + uint8_t RxData[8]; + + if (HAL_FDCAN_GetRxMessage(&hfdcan1, FDCAN_RX_FIFO0, &RxHeader, RxData) == HAL_OK) { + + // Process ID to canard format + rx_frame->id = RxHeader.Identifier; + + if (RxHeader.IdType == FDCAN_EXTENDED_ID) { // canard will only process the message if it is extended ID + rx_frame->id |= CANARD_CAN_FRAME_EFF; + } + + if (RxHeader.RxFrameType == FDCAN_REMOTE_FRAME) { // canard won't process the message if it is a remote frame + rx_frame->id |= CANARD_CAN_FRAME_RTR; + } + + rx_frame->data_len = RxHeader.DataLength; + memcpy(rx_frame->data, RxData, RxHeader.DataLength); + + // assume a single interface + rx_frame->iface_id = 0; + + return 1; + } + + // Either no CAN msg to be read, or an error that can be read from hfdcan->ErrorCode + return 0; +} + +/* ── TX ────────────────────────────────────────────────────────────────────── */ + +/** + * @brief Process tx_frame CAN message into Tx FIFO/Queue and transmit it + * @param tx_frame pointer to a CanardCANFrame structure that contains the CAN message to + * transmit. + * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode + */ +int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { + if (tx_frame == NULL) { + return -CANARD_ERROR_INVALID_ARGUMENT; + } + + if (tx_frame->id & CANARD_CAN_FRAME_ERR) { + return -CANARD_ERROR_INVALID_ARGUMENT; // unsupported frame format + } + + FDCAN_TxHeaderTypeDef TxHeader; + uint8_t TxData[8]; + + // Process canard id to STM FDCAN header format + if (tx_frame->id & CANARD_CAN_FRAME_EFF) { + TxHeader.IdType = FDCAN_EXTENDED_ID; + TxHeader.Identifier = tx_frame->id & CANARD_CAN_EXT_ID_MASK; + } else { + TxHeader.IdType = FDCAN_STANDARD_ID; + TxHeader.Identifier = tx_frame->id & CANARD_CAN_STD_ID_MASK; + } + + TxHeader.DataLength = tx_frame->data_len; + + if (tx_frame->id & CANARD_CAN_FRAME_RTR) { + TxHeader.TxFrameType = FDCAN_REMOTE_FRAME; + } else { + TxHeader.TxFrameType = FDCAN_DATA_FRAME; + } + + TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE; // unsure about this one + TxHeader.BitRateSwitch = FDCAN_BRS_OFF; // Disabling FDCAN (using CAN 2.0) + TxHeader.FDFormat = FDCAN_CLASSIC_CAN; // Disabling FDCAN (using CAN 2.0) + TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS; // unsure about this one + TxHeader.MessageMarker = 0; // unsure about this one + if (TxHeader.DataLength <= sizeof(TxData)) + { + memcpy(TxData, tx_frame->data, TxHeader.DataLength); + } + else + { + LOG_ERROR(CAN, "Data to transmit is larger than 8 byte frame size"); + return -CANARD_ERROR_INVALID_ARGUMENT; + } + + if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &TxHeader, TxData) == HAL_OK) { + // LOG_DEBUG(CAN, "Successfully sent message with id: %lu", TxHeader.Identifier); + return 1; + } + + LOG_DEBUG(CAN, "Failed at adding message with id: %lu to Tx Queue", TxHeader.Identifier); + + // This might be for many reasons including the Tx Fifo being full, the error can be read from hfdcan->ErrorCode + return 0; +} + +/* ── Diagnostics ───────────────────────────────────────────────────────────── */ + +/** + * @brief Read FDCAN protocol status (bus-off, error passive) into a + * canardProtocolStatus_t struct. + * @param pProtocolStat Pointer to the struct to populate with current status. + */ void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){ FDCAN_ProtocolStatusTypeDef protocolStatus = {}; @@ -386,17 +406,37 @@ void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){ LOG_DEBUG(CAN, "ErrorPassive: %lu", protocolStatus.ErrorPassive); } +/** + * @brief Return the current number of frames pending in the software TX queue. + * The H7 FDCAN driver transmits directly into the hardware TX FIFO with no + * intermediate software queue, so this always returns 0. + * @retval 0 (no software TX queue on H7). + */ +int32_t canardSTM32GetTxQueueFillLevel(void) { + return 0; +} + +/** + * @brief Return the current number of frames waiting in the FDCAN RX FIFO0. + * @retval Number of frames available to read. + */ int32_t canardSTM32GetRxFifoFillLevel(void){ return (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1, FDCAN_RX_FIFO0)); } +/** + * @brief Recover the FDCAN peripheral from a bus-off condition by clearing + * the INIT bit in CCCR, which restarts the CAN participation. + */ void canardSTM32RecoverFromBusOff(void){ CLEAR_BIT(hfdcan1.Instance->CCCR, FDCAN_CCCR_INIT); // Clear INIT bit to recover from Bus-Off } -/* - get a 16 byte unique ID for this node, this should be based on the CPU unique ID or other unique ID - */ +/** + * @brief Populate a 16-byte unique node ID from the STM32 96-bit factory UID. + * The 12 UID bytes are copied into the first 12 bytes; the remainder is zero-padded. + * @param id Output buffer of exactly 16 bytes to receive the unique ID. + */ void canardSTM32GetUniqueID(uint8_t id[16]) { uint32_t HALUniqueIDs[3]; // Make Unique ID out of the 96-bit STM32 UID and fill the rest with 0s @@ -405,4 +445,4 @@ void canardSTM32GetUniqueID(uint8_t id[16]) { HALUniqueIDs[1] = HAL_GetUIDw1(); HALUniqueIDs[2] = HAL_GetUIDw2(); memcpy(id, HALUniqueIDs, 12); -} \ No newline at end of file +} From 327220e300bca0b3730ed2b71215c74915867fbe Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 07:39:10 -0700 Subject: [PATCH 14/34] refactor(dronecan): reorder dronecan.c top-down with forward declarations MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Public API (dronecanInit, dronecanUpdate) at the top, internals below: TX/periodic processing → message senders → libcanard callbacks → message handlers. Add forward declarations for all internal functions. Mark all internal functions static. --- src/main/drivers/dronecan/dronecan.c | 620 +++++++++++++-------------- 1 file changed, 308 insertions(+), 312 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index c9877bf575b..5aa6b22fa2d 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -45,143 +45,222 @@ static dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES]; // NOTE: All canard handlers and senders are based on this reference: https://dronecan.github.io/Specification/7._List_of_standard_data_types/ // Alternatively, you can look at the corresponding generated header file in the dsdlc_generated folder -/* ── Message handlers (incoming) ───────────────────────────────────────────── */ +/* ── Forward declarations ──────────────────────────────────────────────────── */ + +static void process1HzTasks(timeUs_t timestamp_usec); +static void processCanardTxQueue(void); +static void send_NodeStatus(void); +static bool shouldAcceptTransfer(const CanardInstance *ins, uint64_t *out_data_type_signature, + uint16_t data_type_id, CanardTransferType transfer_type, + uint8_t source_node_id); +static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer); +static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer); +static void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer); +static void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer); +static void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer); +static void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer); +static void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer); +static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer); -// Canard Handlers ( Many have code copied from libcanard esc_node example: https://github.com/dronecan/libcanard/blob/master/examples/ESCNode/esc_node.c ) +/* ── Public API ────────────────────────────────────────────────────────────── */ -void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_protocol_NodeStatus nodeStatus; +void dronecanInit(void) +{ + LOG_DEBUG(CAN, "dronecan Init"); + uint32_t bitrate = 500000; // At least define 500000 - if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) { - LOG_DEBUG(CAN, "NodeStatus decode failed"); - return; - } + switch (dronecanConfig()->bitRateKbps){ + case DRONECAN_BITRATE_125KBPS: + bitrate = 125000; + break; + + case DRONECAN_BITRATE_250KBPS: + bitrate = 250000; + break; + + case DRONECAN_BITRATE_500KBPS: + bitrate = 500000; + break; + + case DRONECAN_BITRATE_1000KBPS: + bitrate = 1000000; + break; - uint8_t nodeId = transfer->source_node_id; - for (uint8_t i = 0; i < activeNodeCount; i++) { - if (nodeTable[i].nodeID == nodeId) { - // update health, mode, uptime, vendor_status_code, last_seen_ms - nodeTable[i].health = nodeStatus.health; - nodeTable[i].mode = nodeStatus.mode; - nodeTable[i].uptime_sec = nodeStatus.uptime_sec; - nodeTable[i].vendor_status_code = nodeStatus.vendor_specific_status_code; - nodeTable[i].last_seen_ms = millis(); - return; - } - } - // new node - if (activeNodeCount < DRONECAN_MAX_NODES) { - nodeTable[activeNodeCount].nodeID = nodeId; - nodeTable[activeNodeCount].health = nodeStatus.health; - nodeTable[activeNodeCount].mode = nodeStatus.mode; - nodeTable[activeNodeCount].uptime_sec = nodeStatus.uptime_sec; - nodeTable[activeNodeCount].vendor_status_code = nodeStatus.vendor_specific_status_code; - nodeTable[activeNodeCount].name_len = 0; - nodeTable[activeNodeCount].name[0] = 0; - nodeTable[activeNodeCount].last_seen_ms = millis(); - activeNodeCount++; + case DRONECAN_BITRATE_COUNT: + LOG_ERROR(SYSTEM, "Undefined bitrate set in configuration. 500kbps selected"); + bitrate = 500000; + break; + } + if(canardSTM32CAN1_Init(bitrate) != CANARD_OK) + { + LOG_ERROR(CAN, "Unable to initialize the CAN peripheral"); + // TODO: Notify the user that CAN does not work and disable the peripheral + return; } + /* + Initializing the Libcanard instance. + */ + canardInit(&canard, + memory_pool, + sizeof(memory_pool), + onTransferReceived, + shouldAcceptTransfer, + NULL); + // Could use DNA (Dynamic Node Allocation) by following example in esc_node.c but that requires a lot of setup and I'm not too sure of what advantage it brings + // Instead, set a different NODE_ID for each device on the CAN bus by configuring node_settings + if (dronecanConfig()->nodeID > 0) { + canardSetLocalNodeID(&canard, dronecanConfig()->nodeID); + } else { + LOG_DEBUG(CAN, "Node ID is 0, this node is anonymous and can't transmit most messages. Please update this in config"); + } } -void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_equipment_gnss_Auxiliary gnssAuxiliary; +void dronecanUpdate(timeUs_t currentTimeUs) +{ + static timeUs_t next_1hz_service_at = 0; + static timeUs_t busoffTimeUs = 0; + CanardCANFrame rx_frame; + int numMessagesToProcess = 0; + canardProtocolStatus_t protocolStatus = {}; + uint64_t timestamp; + int16_t rx_res; - if (uavcan_equipment_gnss_Auxiliary_decode(transfer, &gnssAuxiliary)) { - LOG_DEBUG(CAN, "GNSSAuxiliary decode failed"); - return; - } - dronecanGPSReceiveGNSSAuxiliary(&gnssAuxiliary); -} + switch(dronecanState) { + case STATE_DRONECAN_INIT: + next_1hz_service_at = currentTimeUs + 1000000ULL; // First 1Hz tick in 1 second + dronecanState = STATE_DRONECAN_NORMAL; + break; -void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_equipment_gnss_Fix gnssFix; + case STATE_DRONECAN_NORMAL: + processCanardTxQueue(); - if (uavcan_equipment_gnss_Fix_decode(transfer, &gnssFix)) { - LOG_DEBUG(CAN, "GNSSFix decode failed"); - return; - } - dronecanGPSReceiveGNSSFix(&gnssFix); -} + for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--) + { + //LOG_DEBUG(CAN, "Received a message"); + //LOG_DEBUG(CAN, "Rx FIFO Fill Level: %lu", canardSTM32GetRxFifoFillLevel()); + timestamp = millis() * 1000ULL; + rx_res = canardSTM32Recieve(&rx_frame); -void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_equipment_gnss_Fix2 gnssFix2; + if (rx_res < 0) { + LOG_DEBUG(CAN, "Receive error %d", rx_res); + } + else if (rx_res > 0) // Success - process the frame + { + canardHandleRxFrame(&canard, &rx_frame, timestamp); + } + } + // Drain any TX frames queued by RX handlers (e.g. GetNodeInfo responses) + // in the same task cycle so multi-frame transfers complete before timeout. + processCanardTxQueue(); - if (uavcan_equipment_gnss_Fix2_decode(transfer, &gnssFix2)) { - LOG_DEBUG(CAN, "GNSSFix2 decode failed"); - return; - } - dronecanGPSReceiveGNSSFix2(&gnssFix2); -} + if (currentTimeUs >= next_1hz_service_at) + { + next_1hz_service_at += 1000000ULL; + process1HzTasks(currentTimeUs); + processCanardTxQueue(); + } -void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_equipment_gnss_RTCMStream gnssRTCMStream; + canardSTM32GetProtocolStatus(&protocolStatus); + if(protocolStatus.BusOff != 0) { + dronecanState = STATE_DRONECAN_BUS_OFF; + busoffTimeUs = currentTimeUs; + } + break; + + case STATE_DRONECAN_BUS_OFF: + if(currentTimeUs > (busoffTimeUs + 100000)) { // Wait 100 mS + canardSTM32RecoverFromBusOff(); + busoffTimeUs = currentTimeUs; + } + canardSTM32GetProtocolStatus(&protocolStatus); + if(protocolStatus.BusOff == 0) { + dronecanState = STATE_DRONECAN_NORMAL; + } + break; + + } - if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &gnssRTCMStream)) { - LOG_DEBUG(CAN, "RTCMStream decode failed"); - return; - } } -void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); - struct uavcan_equipment_power_BatteryInfo batteryInfo; +/* ── TX and periodic task processing ───────────────────────────────────────── */ - if (uavcan_equipment_power_BatteryInfo_decode(transfer, &batteryInfo)) { - LOG_DEBUG(CAN, "BatteryInfo decode failed"); - return; +static void processCanardTxQueue(void) { + // Transmitting + for (const CanardCANFrame *tx_frame ; (tx_frame = canardPeekTxQueue(&canard)) != NULL;) + { + const int16_t tx_res = canardSTM32Transmit(tx_frame); + + if (tx_res < 0) { + LOG_DEBUG(CAN, "Transmit error %d", tx_res); + canardPopTxQueue(&canard); // Error - discard frame + } else if (tx_res > 0) { + // LOG_DEBUG(CAN, "Successfully transmitted message"); + canardPopTxQueue(&canard); // Success - remove from queue + } else { + // tx_res == 0: TX FIFO full, retry later + break; + } } - dronecanBatterySensorReceiveInfo(&batteryInfo); } /* - handle a GetNodeInfo request + This function is called at 1 Hz rate from the main loop. */ +static void process1HzTasks(timeUs_t timestamp_usec) +{ + /* + Purge transfers that are no longer transmitted. This can free up some memory + */ + canardCleanupStaleTransfers(&canard, timestamp_usec); -// TODO: All the data in here is temporary for testing. If actually need to send valid data, edit accordingly. -void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) { - LOG_DEBUG(CAN, "GetNodeInfo request from %d", transfer->source_node_id); + /* + Transmit the node status message + */ + send_NodeStatus(); +} - uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; - struct uavcan_protocol_GetNodeInfoResponse pkt; +/* ── Message senders (outgoing) ────────────────────────────────────────────── */ - memset(&pkt, 0, sizeof(pkt)); +// Canard Senders - node_status.uptime_sec = millis() / 1000ULL; - pkt.status = node_status; +/* + send the 1Hz NodeStatus message. This is what allows a node to show + up in the DroneCAN GUI tool and in the flight controller logs + */ +static void send_NodeStatus(void) { + uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE]; - // fill in your major and minor firmware version - pkt.software_version.major = FC_VERSION_MAJOR; - pkt.software_version.minor = FC_VERSION_MINOR; - pkt.software_version.optional_field_flags = FC_VERSION_PATCH_LEVEL; - pkt.software_version.vcs_commit = strtoul(shortGitRevision, NULL, 16); // need to convert string to integer put git hash in here + // LOG_DEBUG(CAN, "Sending Node Status"); + node_status.uptime_sec = millis() / 1000UL; + if(isHardwareHealthy()){ + node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; + } + else { + node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL; + } - // should fill in hardware version - pkt.hardware_version.major = 1; - pkt.hardware_version.minor = 0; + node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; // Indicates that node is able to communicate over CAN, not that it is in flight. + node_status.sub_mode = 0; // Not currently used in dronecan - // just setting all 16 bytes to 1 for testing - canardSTM32GetUniqueID(pkt.hardware_version.unique_id); + // put whatever you like in here for display in GUI + node_status.vendor_specific_status_code = armingFlags; - strncpy((char*)pkt.name.data, FC_FIRMWARE_NAME, sizeof(pkt.name.data)); - pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data)); + uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); - uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); + // we need a static variable for the transfer ID. This is + // incremeneted on each transfer, allowing for detection of packet + // loss + static uint8_t transfer_id; - canardRequestOrRespond(ins, - transfer->source_node_id, - UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, - UAVCAN_PROTOCOL_GETNODEINFO_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &buffer[0], - total_size); + canardBroadcast(&canard, + UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, + UAVCAN_PROTOCOL_NODESTATUS_ID, + &transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + len); + // PrintCanStatus(); } /* ── Libcanard protocol callbacks ───────────────────────────────────────────── */ @@ -196,8 +275,7 @@ void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) { This function must fill in the out_data_type_signature to be the signature of the message. */ - -bool shouldAcceptTransfer(const CanardInstance *ins, +static bool shouldAcceptTransfer(const CanardInstance *ins, uint64_t *out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, @@ -256,7 +334,7 @@ bool shouldAcceptTransfer(const CanardInstance *ins, /* This callback is invoked by the library when a new message or request or response is received. */ -void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { +static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { // switch on data type ID to pass to the right handler function if (transfer->transfer_type == CanardTransferTypeRequest) { // check if we want to handle a specific service request @@ -279,7 +357,6 @@ void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { handle_NodeStatus(ins, transfer); break; - case UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID: handle_GNSSAuxiliary(ins, transfer); break; @@ -304,241 +381,160 @@ void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { } } -/* ── Message senders (outgoing) ────────────────────────────────────────────── */ - -// Canard Senders - -/* - send the 1Hz NodeStatus message. This is what allows a node to show - up in the DroneCAN GUI tool and in the flight controller logs - */ -void send_NodeStatus(void) { - uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE]; - - // LOG_DEBUG(CAN, "Sending Node Status"); - node_status.uptime_sec = millis() / 1000UL; - if(isHardwareHealthy()){ - node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; - } - else { - node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL; - } +/* ── Message handlers (incoming) ───────────────────────────────────────────── */ - node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; // Indicates that node is able to communicate over CAN, not that it is in flight. - node_status.sub_mode = 0; // Not currently used in dronecan +// Canard Handlers ( Many have code copied from libcanard esc_node example: https://github.com/dronecan/libcanard/blob/master/examples/ESCNode/esc_node.c ) - // put whatever you like in here for display in GUI - node_status.vendor_specific_status_code = armingFlags; +static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_protocol_NodeStatus nodeStatus; - uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); + if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) { + LOG_DEBUG(CAN, "NodeStatus decode failed"); + return; + } - // we need a static variable for the transfer ID. This is - // incremeneted on each transfer, allowing for detection of packet - // loss - static uint8_t transfer_id; + // LOG_DEBUG(CAN, "Node Health "); + + switch (nodeStatus.health) { + case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK: + // LOG_DEBUG(CAN, "OK"); + break; + case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING: + // LOG_DEBUG(CAN, "WARNING"); + break; + case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR: + // LOG_DEBUG(CAN, "ERROR"); + break; + case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL: + // LOG_DEBUG(CAN, "CRITICAL"); + break; + default: + // LOG_DEBUG(CAN, "UNKNOWN?"); + break; + } - canardBroadcast(&canard, - UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, - UAVCAN_PROTOCOL_NODESTATUS_ID, - &transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, - buffer, - len); - // PrintCanStatus(); + // LOG_DEBUG(CAN, "Node Mode "); + + switch(nodeStatus.mode) { + case UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL: + // LOG_DEBUG(CAN, "OPERATIONAL"); + break; + case UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION: + // LOG_DEBUG(CAN, "INITIALIZATION"); + break; + case UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE: + // LOG_DEBUG(CAN, "MAINTENANCE"); + break; + case UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE: + // LOG_DEBUG(CAN, "SOFTWARE UPDATE"); + break; + case UAVCAN_PROTOCOL_NODESTATUS_MODE_OFFLINE: + // LOG_DEBUG(CAN, "OFFLINE"); + break; + default: + // LOG_DEBUG(CAN, "UNKNOWN?"); + break; + } } -/* ── TX and periodic task processing ───────────────────────────────────────── */ - -void processCanardTxQueue(void) { - // Transmitting - for (const CanardCANFrame *tx_frame ; (tx_frame = canardPeekTxQueue(&canard)) != NULL;) - { - const int16_t tx_res = canardSTM32Transmit(tx_frame); +static void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_equipment_gnss_Auxiliary gnssAuxiliary; - if (tx_res < 0) { - LOG_DEBUG(CAN, "Transmit error %d", tx_res); - canardPopTxQueue(&canard); // Error - discard frame - } else if (tx_res > 0) { - // LOG_DEBUG(CAN, "Successfully transmitted message"); - canardPopTxQueue(&canard); // Success - remove from queue - } else { - // tx_res == 0: TX FIFO full, retry later - break; - } + if (uavcan_equipment_gnss_Auxiliary_decode(transfer, &gnssAuxiliary)) { + LOG_DEBUG(CAN, "GNSSAuxiliary decode failed"); + return; } - + dronecanGPSReceiveGNSSAuxiliary(&gnssAuxiliary); } -/* - This function is called at 1 Hz rate from the main loop. -*/ -void process1HzTasks(timeUs_t timestamp_usec) -{ - /* - Purge transfers that are no longer transmitted. This can free up some memory - */ - canardCleanupStaleTransfers(&canard, timestamp_usec); +static void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_equipment_gnss_Fix gnssFix; - /* - Transmit the node status message - */ - send_NodeStatus(); + if (uavcan_equipment_gnss_Fix_decode(transfer, &gnssFix)) { + LOG_DEBUG(CAN, "GNSSFix decode failed"); + return; + } + dronecanGPSReceiveGNSSFix(&gnssFix); } -/* ── Public API ────────────────────────────────────────────────────────────── */ - -void dronecanInit(void) -{ - LOG_DEBUG(CAN, "dronecan Init"); - uint32_t bitrate = 500000; // At least define 500000 - - switch (dronecanConfig()->bitRateKbps){ - case DRONECAN_BITRATE_125KBPS: - bitrate = 125000; - break; - - case DRONECAN_BITRATE_250KBPS: - bitrate = 250000; - break; - - case DRONECAN_BITRATE_500KBPS: - bitrate = 500000; - break; - - case DRONECAN_BITRATE_1000KBPS: - bitrate = 1000000; - break; - - case DRONECAN_BITRATE_COUNT: - LOG_ERROR(SYSTEM, "Undefined bitrate set in configuration. 500kbps selected"); - bitrate = 500000; - break; - } - if(canardSTM32CAN1_Init(bitrate) != CANARD_OK) - { - LOG_ERROR(CAN, "Unable to initialize the CAN peripheral"); - // TODO: Notify the user that CAN does not work and disable the peripheral - return; - } - /* - Initializing the Libcanard instance. - */ - canardInit(&canard, - memory_pool, - sizeof(memory_pool), - onTransferReceived, - shouldAcceptTransfer, - NULL); +static void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_equipment_gnss_Fix2 gnssFix2; - // Could use DNA (Dynamic Node Allocation) by following example in esc_node.c but that requires a lot of setup and I'm not too sure of what advantage it brings - // Instead, set a different NODE_ID for each device on the CAN bus by configuring node_settings - if (dronecanConfig()->nodeID > 0) { - canardSetLocalNodeID(&canard, dronecanConfig()->nodeID); - } else { - LOG_DEBUG(CAN, "Node ID is 0, this node is anonymous and can't transmit most messages. Please update this in config"); - } + if (uavcan_equipment_gnss_Fix2_decode(transfer, &gnssFix2)) { + LOG_DEBUG(CAN, "GNSSFix2 decode failed"); + return; + } + dronecanGPSReceiveGNSSFix2(&gnssFix2); } -void dronecanUpdate(timeUs_t currentTimeUs) -{ - static timeUs_t next_1hz_service_at = 0; - static timeUs_t busoffTimeUs = 0; - CanardCANFrame rx_frame; - int numMessagesToProcess = 0; - canardProtocolStatus_t protocolStatus = {}; - uint64_t timestamp; - int16_t rx_res; +static void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_equipment_gnss_RTCMStream gnssRTCMStream; - switch(dronecanState) { - case STATE_DRONECAN_INIT: - next_1hz_service_at = currentTimeUs + 1000000ULL; // First 1Hz tick in 1 second - dronecanState = STATE_DRONECAN_NORMAL; - break; + if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &gnssRTCMStream)) { + LOG_DEBUG(CAN, "RTCMStream decode failed"); + return; + } +} - case STATE_DRONECAN_NORMAL: - processCanardTxQueue(); +static void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_equipment_power_BatteryInfo batteryInfo; - for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--) - { - //LOG_DEBUG(CAN, "Received a message"); - //LOG_DEBUG(CAN, "Rx FIFO Fill Level: %lu", canardSTM32GetRxFifoFillLevel()); - timestamp = millis() * 1000ULL; - rx_res = canardSTM32Recieve(&rx_frame); + if (uavcan_equipment_power_BatteryInfo_decode(transfer, &batteryInfo)) { + LOG_DEBUG(CAN, "BatteryInfo decode failed"); + return; + } + dronecanBatterySensorReceiveInfo(&batteryInfo); +} - if (rx_res < 0) { - LOG_DEBUG(CAN, "Receive error %d", rx_res); - } - else if (rx_res > 0) // Success - process the frame - { - canardHandleRxFrame(&canard, &rx_frame, timestamp); - } - } - // Drain any TX frames queued by RX handlers (e.g. GetNodeInfo responses) - // in the same task cycle so multi-frame transfers complete before timeout. - processCanardTxQueue(); +/* + handle a GetNodeInfo request +*/ - if (currentTimeUs >= next_1hz_service_at) - { - next_1hz_service_at += 1000000ULL; - process1HzTasks(currentTimeUs); - processCanardTxQueue(); - } +// TODO: All the data in here is temporary for testing. If actually need to send valid data, edit accordingly. +static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) { + printf("GetNodeInfo request from %d", transfer->source_node_id); - canardSTM32GetProtocolStatus(&protocolStatus); - if(protocolStatus.BusOff != 0) { - dronecanState = STATE_DRONECAN_BUS_OFF; - busoffTimeUs = currentTimeUs; - } - break; + uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; + struct uavcan_protocol_GetNodeInfoResponse pkt; - case STATE_DRONECAN_BUS_OFF: - if(currentTimeUs > (busoffTimeUs + 100000)) { // Wait 100 mS - canardSTM32RecoverFromBusOff(); - busoffTimeUs = currentTimeUs; - } - canardSTM32GetProtocolStatus(&protocolStatus); - if(protocolStatus.BusOff == 0) { - dronecanState = STATE_DRONECAN_NORMAL; - } - break; + memset(&pkt, 0, sizeof(pkt)); - } + node_status.uptime_sec = millis() / 1000ULL; + pkt.status = node_status; -} + // fill in your major and minor firmware version + pkt.software_version.major = FC_VERSION_MAJOR; + pkt.software_version.minor = FC_VERSION_MINOR; + pkt.software_version.optional_field_flags = FC_VERSION_PATCH_LEVEL; + pkt.software_version.vcs_commit = strtoul(shortGitRevision, NULL, 16); // need to convert string to integer put git hash in here -dronecanState_e dronecanGetState(void) -{ - return dronecanState; -} + // should fill in hardware version + pkt.hardware_version.major = 1; + pkt.hardware_version.minor = 0; -uint8_t dronecanGetNodeCount(void) -{ - return activeNodeCount; -} + // just setting all 16 bytes to 1 for testing + canardSTM32GetUniqueID(pkt.hardware_version.unique_id); -uint32_t dronecanGetBitrateKbps(void) -{ - switch (dronecanConfig()->bitRateKbps){ - case DRONECAN_BITRATE_125KBPS: - return 125; + strncpy((char*)pkt.name.data, FC_FIRMWARE_NAME, sizeof(pkt.name.data)); + pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data)); - case DRONECAN_BITRATE_250KBPS: - return 250; - - case DRONECAN_BITRATE_500KBPS: - return 500; + uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); - case DRONECAN_BITRATE_1000KBPS: - return 1000; - - case DRONECAN_BITRATE_COUNT: - return 0; - } - return 0; + canardRequestOrRespond(ins, + transfer->source_node_id, + UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, + UAVCAN_PROTOCOL_GETNODEINFO_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + total_size); } -const dronecanNodeInfo_t *dronecanGetNode(uint8_t index) { - if (index < activeNodeCount) return &nodeTable[index]; - return NULL; -} #endif From 88b0b9caa9174f8cb67c636d6692646264dac292 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 07:57:25 -0700 Subject: [PATCH 15/34] fix(dronecan): address critical code review findings MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - canardSTM32Transmit: return 1 after successful canTxQueuePush regardless of ActivateNotification result — prevents libcanard from re-pushing the same frame and producing duplicate CAN transmissions that corrupt the UAVCAN transfer-ID toggle sequence - handle_GetNodeInfo: replace printf() with LOG_DEBUG() — printf has no output backend in firmware and risks a fault or linker error - rxBufferPushFrame/rxBufferPopFrame: change return type uint8_t → int8_t so the -1 error sentinel is type-correct --- src/main/drivers/dronecan/dronecan.c | 2 +- .../dronecan/libcanard/canard_stm32f7xx_driver.c | 11 +++++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 5aa6b22fa2d..ace776645b3 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -498,7 +498,7 @@ static void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) // TODO: All the data in here is temporary for testing. If actually need to send valid data, edit accordingly. static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) { - printf("GetNodeInfo request from %d", transfer->source_node_id); + LOG_DEBUG(CAN, "GetNodeInfo request from %d", transfer->source_node_id); uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; struct uavcan_protocol_GetNodeInfoResponse pkt; diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index 6c20e5c1a27..35cd95a2e2d 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -308,7 +308,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) * @param rxMsg Pointer to the RxFrame_t to copy into the buffer. * @retval 0 on success, -1 if the buffer is full (frame dropped). */ -uint8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { +int8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { uint8_t next; RxFrame_t *pCurrentRxMsg; @@ -334,7 +334,7 @@ uint8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { * @param rxMsg Pointer to an RxFrame_t where the frame will be copied. * @retval 0 on success, -1 if the buffer is empty. */ -uint8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { +int8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { uint8_t next; RxFrame_t *pCurrentRxMsg; @@ -566,9 +566,12 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { canTxDrainQueue(&hcan1); } if (!canTxQueueIsEmpty()) { - HAL_StatusTypeDef status = HAL_CAN_ActivateNotification(&hcan1, CAN_IT_TX_MAILBOX_EMPTY); + // If ActivateNotification fails (e.g. peripheral in error state) the frame is + // already in the SW queue. Return 1 so libcanard does not re-push a duplicate; + // the next canardSTM32Transmit call will re-seed the HW mailboxes. + HAL_CAN_ActivateNotification(&hcan1, CAN_IT_TX_MAILBOX_EMPTY); __enable_irq(); - return (status == HAL_OK) ? 1 : 0; // 0 signals caller to retry + return 1; } __enable_irq(); return 1; From 0242fb89848b17286d94c65b312e5c85909e5728 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 08:05:58 -0700 Subject: [PATCH 16/34] fix(dronecan): address important code review findings (issues 4 and 5) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Issue 4 — AbortCallback comment: previous text incorrectly described TERR as a synchronous AddTxMessage failure and misattributed the abort path suppression. Correct explanation: AutoRetransmission=ENABLE causes the hardware to retransmit automatically on arbitration loss (ALST), so the software abort path is never triggered; bus-off is handled by the state machine in dronecanUpdate(). Issue 5 — critical section: replace __disable_irq()/__enable_irq() with ATOMIC_BLOCK(NVIC_PRIO_CAN) from build/atomic.h. This raises BASEPRI to mask only the CAN TX ISR (priority 4 and below), allowing higher-priority IRQs such as the gyro timer (priority 3) to continue firing during the mailbox-seeding window. Also simplifies the two return-1 paths into one. --- .../libcanard/canard_stm32f7xx_driver.c | 34 +++++++++---------- 1 file changed, 16 insertions(+), 18 deletions(-) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index 35cd95a2e2d..ece7482b585 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -7,6 +7,7 @@ #include "common/log.h" #include "common/time.h" +#include "build/atomic.h" #include "drivers/io.h" #include "drivers/nvic.h" #include "canard.h" @@ -531,9 +532,9 @@ void CAN1_TX_IRQHandler(void) { /** * @brief HAL callbacks fired when a TX mailbox completes transmission (runs in ISR context). * Each refills available mailboxes from the software TX queue via canTxDrainQueue(). - * AbortCallback is intentionally not wired: AutoRetransmission=ENABLE prevents - * ALST (arbitration lost), and TERR (bus fault) leaves the frame in the queue - * (tail not advanced) so the next canardSTM32Transmit call re-seeds the HW. + * AbortCallback is intentionally not wired: with AutoRetransmission=ENABLE the hardware + * retransmits automatically on arbitration loss (ALST) and the software abort path is + * never triggered. Bus-off is handled separately by the state machine in dronecanUpdate(). */ void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { canTxDrainQueue(hcan); } @@ -558,22 +559,19 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { } // If all mailboxes are idle, RQCP will never fire to start the ISR chain. - // Seed the HW via canTxDrainQueue with IRQs disabled to preserve the - // SPSC contract — ISR is the sole consumer; we become the consumer only - // while the ISR cannot run. - __disable_irq(); - if (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 3) { - canTxDrainQueue(&hcan1); - } - if (!canTxQueueIsEmpty()) { - // If ActivateNotification fails (e.g. peripheral in error state) the frame is - // already in the SW queue. Return 1 so libcanard does not re-push a duplicate; - // the next canardSTM32Transmit call will re-seed the HW mailboxes. - HAL_CAN_ActivateNotification(&hcan1, CAN_IT_TX_MAILBOX_EMPTY); - __enable_irq(); - return 1; + // Seed the HW via canTxDrainQueue while blocking only the CAN TX ISR (not + // higher-priority IRQs like the gyro timer) to preserve the SPSC contract — + // ISR is the sole consumer; we become the consumer only during this window. + ATOMIC_BLOCK(NVIC_PRIO_CAN) { + if (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 3) { + canTxDrainQueue(&hcan1); + } + if (!canTxQueueIsEmpty()) { + // If ActivateNotification fails (e.g. peripheral in error state) the frame is + // already in the SW queue. The next canardSTM32Transmit call will re-seed the HW. + HAL_CAN_ActivateNotification(&hcan1, CAN_IT_TX_MAILBOX_EMPTY); + } } - __enable_irq(); return 1; } From 7f28dc34556ebb35c794e9f34129905b3bdd47c7 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 08:15:42 -0700 Subject: [PATCH 17/34] fix(dronecan): address minor code review findings (issues 7-11) - Fix 7: Clarify SJW=3 comment in canardSTM32ComputeTimings - Fix 8: Remove commented-out CubeMX scaffold from canardSTM32CAN1_Init - Fix 9: Convert tab indentation to 4-space in canardSTM32Recieve (F7) - Fix 10: Remove USER CODE BEGIN/END sentinel comments from H7 FDCAN1_Init - Fix 11: Replace Unicode section separators with ASCII dashes in all three files --- src/main/drivers/dronecan/dronecan.c | 14 ++-- .../libcanard/canard_stm32f7xx_driver.c | 70 +++++++------------ .../libcanard/canard_stm32h7xx_driver.c | 17 ++--- 3 files changed, 37 insertions(+), 64 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index ace776645b3..fc839d7a2dd 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -25,7 +25,7 @@ #include #include -/* ── Private state ─────────────────────────────────────────────────────────── */ +/* --- Private state --- */ CanardInstance canard; uint8_t memory_pool[1024]; @@ -45,7 +45,7 @@ static dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES]; // NOTE: All canard handlers and senders are based on this reference: https://dronecan.github.io/Specification/7._List_of_standard_data_types/ // Alternatively, you can look at the corresponding generated header file in the dsdlc_generated folder -/* ── Forward declarations ──────────────────────────────────────────────────── */ +/* --- Forward declarations --- */ static void process1HzTasks(timeUs_t timestamp_usec); static void processCanardTxQueue(void); @@ -62,7 +62,7 @@ static void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfe static void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer); static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer); -/* ── Public API ────────────────────────────────────────────────────────────── */ +/* --- Public API --- */ void dronecanInit(void) { @@ -183,7 +183,7 @@ void dronecanUpdate(timeUs_t currentTimeUs) } -/* ── TX and periodic task processing ───────────────────────────────────────── */ +/* --- TX and periodic task processing --- */ static void processCanardTxQueue(void) { // Transmitting @@ -220,7 +220,7 @@ static void process1HzTasks(timeUs_t timestamp_usec) send_NodeStatus(); } -/* ── Message senders (outgoing) ────────────────────────────────────────────── */ +/* --- Message senders (outgoing) --- */ // Canard Senders @@ -263,7 +263,7 @@ static void send_NodeStatus(void) { // PrintCanStatus(); } -/* ── Libcanard protocol callbacks ───────────────────────────────────────────── */ +/* --- Libcanard protocol callbacks --- */ // Canard Util /* @@ -381,7 +381,7 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) } } -/* ── Message handlers (incoming) ───────────────────────────────────────────── */ +/* --- Message handlers (incoming) --- */ // Canard Handlers ( Many have code copied from libcanard esc_node example: https://github.com/dronecan/libcanard/blob/master/examples/ESCNode/esc_node.c ) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index ece7482b585..7f734c6641e 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -20,7 +20,7 @@ #include #include -/* ── Internal types and state ──────────────────────────────────────────────── */ +/* --- Internal types and state --- */ #define RX_BUFFER_SIZE 32 #define TX_QUEUE_SIZE 32 @@ -55,7 +55,7 @@ static volatile uint16_t canTxDropped = 0; static volatile uint8_t canTxQueueHWM = 0; static volatile uint8_t canRxBufferHWM = 0; -/* ── Initialization ────────────────────────────────────────────────────────── */ +/* --- Initialization --- */ /** * @brief GPIO Initialization Function @@ -199,7 +199,7 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi (int)(1 + solution.bs1 + solution.bs2), (double)(solution.sample_point_permill) / (double)(10.0)); out_timings->prescaler = (uint16_t)(prescaler); - out_timings->sjw = 3; // Not happy with this value, but 1MBPs with unshielded cable? + out_timings->sjw = 3; // SJW=3: allows up to 3 tq of resynchronization per bit; sufficient for short bus runs at 1 Mbps out_timings->bs1 = (uint8_t)(solution.bs1)-1; // The HAL does not take care of the 1 bs offset in the register so remove it here like AP does. out_timings->bs2 = (uint8_t)(solution.bs2)-1; // The HAL does not take care of the 1 bs offset in the register so remove it here like AP does. @@ -215,14 +215,11 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi */ int16_t canardSTM32CAN1_Init(uint32_t bitrate) { -// RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0}; struct Timings out_timings; - /* CAN1 clock enable */ + /* CAN1 clock enable */ __HAL_RCC_CAN1_CLK_ENABLE(); - // /* USER CODE BEGIN CAN1_MspInit 1 */ - CAN_FilterTypeDef sFilterConfig; sFilterConfig.FilterIdHigh = 0; sFilterConfig.FilterIdLow = 0; @@ -251,31 +248,14 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) hcan1.Init.TimeSeg2 = (uint32_t)out_timings.bs2 << CAN_BTR_TS2_Pos; LOG_DEBUG(CAN, "Prescaler: %d, SJW: %d, BS1: %d, BS2: %d", out_timings.prescaler, out_timings.sjw, out_timings.bs1, out_timings.bs2); - // hcan1.Init.StdFiltersNbr = 0; - // hcan1.Init.ExtFiltersNbr = 1; - // hcan1.Init.TxFifoQueueElmtsNbr = 32; - // LOG_DEBUG(CAN, "In CAN Init"); - - /** Initializes the peripherals clock - */ - // PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_FDCAN; - // PeriphClkInitStruct.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL; - // if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) - // { - // LOG_DEBUG(CAN, "Unable to configure peripheral clock"); - // } - - canardSTM32GPIO_Init(); // Set up the pins for CAN and optional listen only mode + canardSTM32GPIO_Init(); // Set up the pins for CAN and optional standby control - // LOG_DEBUG(CAN, "System Clock Speed: %lu", HAL_RCC_GetSysClockFreq()); - // LOG_DEBUG(CAN, "PClk1 Clock Speed: %lu", HAL_RCC_GetPCLK1Freq()); if (HAL_CAN_Init(&hcan1) != HAL_OK) { LOG_ERROR(CAN, "Failed CAN Init"); return -CANARD_ERROR_INTERNAL; } - /* USER CODE BEGIN FDCAN1_Init 2 */ if (HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig) != HAL_OK) { LOG_ERROR(CAN, "Failed Config Filter"); return -CANARD_ERROR_INTERNAL; @@ -301,7 +281,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) return CANARD_OK; } -/* ── RX subsystem ──────────────────────────────────────────────────────────── */ +/* --- RX subsystem --- */ /** * @brief Push a received CAN frame into the ring buffer (called from ISR). @@ -394,33 +374,33 @@ int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { RxFrame_t canRxFrame; if (rx_frame == NULL) { - return -CANARD_ERROR_INVALID_ARGUMENT; - } + return -CANARD_ERROR_INVALID_ARGUMENT; + } - if (rxBufferPopFrame(&RxBuffer, &canRxFrame) == 0) { // Wheres the data? + if (rxBufferPopFrame(&RxBuffer, &canRxFrame) == 0) { // Wheres the data? rx_frame->id = canRxFrame.header.ExtId; - // Process ID to canard format - if (canRxFrame.header.IDE == CAN_ID_EXT) { // canard will only process the message if it is extended ID + // Process ID to canard format + if (canRxFrame.header.IDE == CAN_ID_EXT) { // canard will only process the message if it is extended ID rx_frame->id |= CANARD_CAN_FRAME_EFF; - } + } - if (canRxFrame.header.RTR == CAN_RTR_REMOTE) { // canard won't process the message if it is a remote frame - rx_frame->id |= CANARD_CAN_FRAME_RTR; - } + if (canRxFrame.header.RTR == CAN_RTR_REMOTE) { // canard won't process the message if it is a remote frame + rx_frame->id |= CANARD_CAN_FRAME_RTR; + } - rx_frame->data_len = canRxFrame.header.DLC; - memcpy(rx_frame->data, canRxFrame.data, canRxFrame.header.DLC); + rx_frame->data_len = canRxFrame.header.DLC; + memcpy(rx_frame->data, canRxFrame.data, canRxFrame.header.DLC); - // assume a single interface - rx_frame->iface_id = 0; - return 1; - } - // Either no CAN msg to be read, or an error that can be read from hfdcan->ErrorCode - return 0; + // assume a single interface + rx_frame->iface_id = 0; + return 1; + } + // Either no CAN msg to be read, or an error that can be read from hfdcan->ErrorCode + return 0; } -/* ── TX subsystem ──────────────────────────────────────────────────────────── */ +/* --- TX subsystem --- */ /** * @brief Push a frame onto the software TX queue (called from main loop). @@ -575,7 +555,7 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { return 1; } -/* ── Diagnostics ───────────────────────────────────────────────────────────── */ +/* --- Diagnostics --- */ /** * @brief Read CAN bus error counters and queue statistics from hardware and diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c index 7d2ca18775e..877dd954322 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c @@ -18,7 +18,7 @@ #include #include -/* ── Internal types and state ──────────────────────────────────────────────── */ +/* --- Internal types and state --- */ struct Timings { uint16_t prescaler; @@ -29,7 +29,7 @@ struct Timings { static FDCAN_HandleTypeDef hfdcan1; -/* ── Initialization ────────────────────────────────────────────────────────── */ +/* --- Initialization --- */ /** * @brief GPIO Initialization Function @@ -196,12 +196,6 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) struct Timings out_timings; int16_t ErrorCode = 1; - /* USER CODE BEGIN FDCAN1_Init 0 */ - - /* USER CODE END FDCAN1_Init 0 */ - - /* USER CODE BEGIN FDCAN1_Init 1 */ - FDCAN_FilterTypeDef sFilterConfig; sFilterConfig.IdType = FDCAN_EXTENDED_ID; sFilterConfig.FilterIndex = 0; @@ -209,7 +203,6 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0; sFilterConfig.FilterID1 = 0x0; sFilterConfig.FilterID2 = 0x1FFFFFFFU; - /* USER CODE END FDCAN1_Init 1 */ hfdcan1.Instance = FDCAN1; hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC; // Initialize in CAN2.0 mode not CAN_FD hfdcan1.Init.Mode = FDCAN_MODE_NORMAL; @@ -280,7 +273,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) return CANARD_OK; } -/* ── RX ────────────────────────────────────────────────────────────────────── */ +/* --- RX --- */ /** * @brief Process CAN message from RxLocation FIFO into rx_frame @@ -326,7 +319,7 @@ int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { return 0; } -/* ── TX ────────────────────────────────────────────────────────────────────── */ +/* --- TX --- */ /** * @brief Process tx_frame CAN message into Tx FIFO/Queue and transmit it @@ -389,7 +382,7 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { return 0; } -/* ── Diagnostics ───────────────────────────────────────────────────────────── */ +/* --- Diagnostics --- */ /** * @brief Read FDCAN protocol status (bus-off, error passive) into a From caf8a39a3175fae68871dffa4e883d532be92f0f Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 08:21:45 -0700 Subject: [PATCH 18/34] fix(dronecan): address remaining minor code review findings (issues 6 and 12) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Fix 6: Clarify hardcoded 32 denominators in CLI status output — add comments naming the matching TX_QUEUE_SIZE / RX_BUFFER_SIZE constants - Fix 12: Remove WHAT-comment "// Transmitting" from processCanardTxQueue --- src/main/drivers/dronecan/dronecan.c | 1 - src/main/fc/cli.c | 4 ++-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index fc839d7a2dd..801f073c4f9 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -186,7 +186,6 @@ void dronecanUpdate(timeUs_t currentTimeUs) /* --- TX and periodic task processing --- */ static void processCanardTxQueue(void) { - // Transmitting for (const CanardCANFrame *tx_frame ; (tx_frame = canardPeekTxQueue(&canard)) != NULL;) { const int16_t tx_res = canardSTM32Transmit(tx_frame); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 7ad5d8e1a1a..187ecdbb645 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -4389,8 +4389,8 @@ static void cliDronecan(char *cmdline) cliPrintLinef(" TEC: %u", (unsigned)stat.tec); cliPrintLinef(" REC: %u", (unsigned)stat.rec); cliPrintLinef(" LEC: %s (%u)", lecNames[stat.lec & 0x7], (unsigned)stat.lec); - cliPrintLinef(" TX queue: %ld / 32 (hwm: %u)", (long)txFill, (unsigned)stat.tx_queue_hwm); // denominator must match TX_QUEUE_SIZE in canard_stm32f7xx_driver.c - cliPrintLinef(" RX buffer: %ld / 32 (hwm: %u)", (long)rxFill, (unsigned)stat.rx_buffer_hwm); // denominator must match RX_BUFFER_SIZE in canard_stm32f7xx_driver.c + cliPrintLinef(" TX queue: %ld / 32 (hwm: %u)", (long)txFill, (unsigned)stat.tx_queue_hwm); // 32 = TX_QUEUE_SIZE; update if that constant changes + cliPrintLinef(" RX buffer: %ld / 32 (hwm: %u)", (long)rxFill, (unsigned)stat.rx_buffer_hwm); // 32 = RX_BUFFER_SIZE; update if that constant changes cliPrintLinef(" TX dropped: %u", (unsigned)stat.tx_dropped); } #endif From 553eaa0fd216b9669b8bc34d1d3ee59e79924df4 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 08:35:54 -0700 Subject: [PATCH 19/34] fix(dronecan): address critical and important code review findings MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit F7 driver: - Add __DMB() before writeIndex advance in rxBufferPushFrame (ISR→main memory ordering: ensures frame data visible before consumer sees new index) - Add __DMB() before memcpy in rxBufferPopFrame (acquire barrier: ensures ISR's writes are visible before main loop reads frame data) - Check canardSTM32ComputeTimings() return value; return error if no valid timing solution found for the requested bitrate - Mark rxBufferPushFrame, rxBufferPopFrame, rxBufferNumMessages static H7 driver: - Fix prescaler upper bound: 1024 → 512 (FDCAN NBTP.NBRP is 9-bit) - Zero-initialize canardProtocolStatus_t in GetProtocolStatus so TEC/REC/LEC/queue fields are not garbage on H7 builds - Remove always-true DataLength guard in canardSTM32Transmit; libcanard constrains data_len to 0-8 so the else branch was unreachable --- .../dronecan/libcanard/canard_stm32f7xx_driver.c | 13 +++++++++---- .../dronecan/libcanard/canard_stm32h7xx_driver.c | 13 +++---------- 2 files changed, 12 insertions(+), 14 deletions(-) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index 7f734c6641e..e537adf0021 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -240,7 +240,10 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) hcan1.Init.ReceiveFifoLocked = DISABLE; hcan1.Init.TransmitFifoPriority = ENABLE; // transmit in request order, not mailbox-number order - canardSTM32ComputeTimings(bitrate, &out_timings); + if (!canardSTM32ComputeTimings(bitrate, &out_timings)) { + LOG_ERROR(CAN, "Failed to compute CAN timings for bitrate %lu", (unsigned long)bitrate); + return -CANARD_ERROR_INTERNAL; + } hcan1.Init.Prescaler = out_timings.prescaler; hcan1.Init.SyncJumpWidth = (uint32_t)out_timings.sjw << CAN_BTR_SJW_Pos; @@ -289,7 +292,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) * @param rxMsg Pointer to the RxFrame_t to copy into the buffer. * @retval 0 on success, -1 if the buffer is full (frame dropped). */ -int8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { +static int8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { uint8_t next; RxFrame_t *pCurrentRxMsg; @@ -303,6 +306,7 @@ int8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { } pCurrentRxMsg = &rxBuf->rxMsg[rxBuf->writeIndex]; memcpy(pCurrentRxMsg, rxMsg, sizeof(RxFrame_t)); + __DMB(); // ensure frame data is visible to main loop before writeIndex advances rxBuf->writeIndex = next; uint8_t rxFill = (next >= rxBuf->readIndex) ? (next - rxBuf->readIndex) : (next + RX_BUFFER_SIZE - rxBuf->readIndex); if (rxFill > canRxBufferHWM) canRxBufferHWM = rxFill; @@ -315,7 +319,7 @@ int8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { * @param rxMsg Pointer to an RxFrame_t where the frame will be copied. * @retval 0 on success, -1 if the buffer is empty. */ -int8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { +static int8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { uint8_t next; RxFrame_t *pCurrentRxMsg; @@ -328,6 +332,7 @@ int8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { next = 0; } pCurrentRxMsg = &rxBuf->rxMsg[rxBuf->readIndex]; + __DMB(); // ensure ISR's frame data writes are visible before we read them memcpy(rxMsg, pCurrentRxMsg, sizeof(RxFrame_t)); rxBuf->readIndex = next; return 0; @@ -338,7 +343,7 @@ int8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { * @param rxBuf Pointer to the RxBuffer_t ring buffer. * @retval Number of frames available to read (0 to RX_BUFFER_SIZE-1). */ -uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf) { +static uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf) { if(rxBuf->writeIndex < rxBuf->readIndex) return((rxBuf->writeIndex + RX_BUFFER_SIZE) - rxBuf->readIndex); diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c index 877dd954322..72ece43d265 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c @@ -120,7 +120,7 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi } const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum); - if ((prescaler < 1U) || (prescaler > 1024U)) { + if ((prescaler < 1U) || (prescaler > 512U)) { // FDCAN NBTP.NBRP is 9-bit: valid range 1-512 return false; // No solution } LOG_DEBUG(CAN, "Prescaler: %lu", prescaler); @@ -361,15 +361,7 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { TxHeader.FDFormat = FDCAN_CLASSIC_CAN; // Disabling FDCAN (using CAN 2.0) TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS; // unsure about this one TxHeader.MessageMarker = 0; // unsure about this one - if (TxHeader.DataLength <= sizeof(TxData)) - { - memcpy(TxData, tx_frame->data, TxHeader.DataLength); - } - else - { - LOG_ERROR(CAN, "Data to transmit is larger than 8 byte frame size"); - return -CANARD_ERROR_INVALID_ARGUMENT; - } + memcpy(TxData, tx_frame->data, TxHeader.DataLength); if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &TxHeader, TxData) == HAL_OK) { // LOG_DEBUG(CAN, "Successfully sent message with id: %lu", TxHeader.Identifier); @@ -392,6 +384,7 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){ FDCAN_ProtocolStatusTypeDef protocolStatus = {}; + memset(pProtocolStat, 0, sizeof(*pProtocolStat)); HAL_FDCAN_GetProtocolStatus(&hfdcan1, &protocolStatus); pProtocolStat->BusOff = protocolStatus.BusOff; pProtocolStat->ErrorPassive = protocolStatus.ErrorPassive; From 309d32265db561e48eb59a2b10e4122d0cafbc41 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 08:39:50 -0700 Subject: [PATCH 20/34] fix(dronecan): address minor code review findings - Mark canard and memory_pool static in dronecan.c (internal state, no reason for external linkage) - Fix spelling: "incremeneted" -> "incremented" in dronecan.c - Clarify H7 FDCAN_FILTER_DUAL comment: the specific filter is a no-op because ConfigGlobalFilter accepts all non-matching frames --- src/main/drivers/dronecan/dronecan.c | 6 +++--- .../drivers/dronecan/libcanard/canard_stm32h7xx_driver.c | 3 ++- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 801f073c4f9..89129015e58 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -27,8 +27,8 @@ /* --- Private state --- */ -CanardInstance canard; -uint8_t memory_pool[1024]; +static CanardInstance canard; +static uint8_t memory_pool[1024]; static struct uavcan_protocol_NodeStatus node_status; PG_REGISTER_WITH_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig, PG_DRONECAN_CONFIG, 0); @@ -248,7 +248,7 @@ static void send_NodeStatus(void) { uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); // we need a static variable for the transfer ID. This is - // incremeneted on each transfer, allowing for detection of packet + // incremented on each transfer, allowing for detection of packet // loss static uint8_t transfer_id; diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c index 72ece43d265..dee5882242e 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c @@ -202,7 +202,8 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) sFilterConfig.FilterType = FDCAN_FILTER_DUAL; sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0; sFilterConfig.FilterID1 = 0x0; - sFilterConfig.FilterID2 = 0x1FFFFFFFU; + sFilterConfig.FilterID2 = 0x1FFFFFFFU; // FILTER_DUAL matches two IDs, but ConfigGlobalFilter below + // accepts all non-matching frames — this filter is a no-op hfdcan1.Instance = FDCAN1; hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC; // Initialize in CAN2.0 mode not CAN_FD hfdcan1.Init.Mode = FDCAN_MODE_NORMAL; From 30e774b716fb5cabca21f52e45e598f2fd0b87fd Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 08:47:49 -0700 Subject: [PATCH 21/34] docs(dronecan): add TODO comments for timing parameter review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Both F7 (max_quanta_per_bit=18) and H7 (max_quanta_per_bit=17) differ from the reference paper maximum of 17. Mark both for revisit alongside SJW tuning — SJW=3 was set conservatively and should be validated against actual crystal performance on the board. --- src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c | 2 +- src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index e537adf0021..ce8a9808650 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -116,7 +116,7 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi * 250 kbps 16 17 * 125 kbps 16 17 */ - const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 18; + const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 18; // TODO: reference paper max is 17; H7 uses 17 — revisit alongside SJW tuning static const int MaxSamplePointLocation = 900; /* diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c index dee5882242e..ffdbb1d5b4a 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c @@ -89,7 +89,7 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi * 250 kbps 16 17 * 125 kbps 16 17 */ - const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17; + const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17; // TODO: F7 uses 18 here — revisit both alongside SJW tuning LOG_DEBUG(CAN, "Baudrate: %lu", target_bitrate); LOG_DEBUG(CAN, "Max Quanta per bit: %i", max_quanta_per_bit); From f906487a51b070f453247bef9c03fd9fd306043b Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 08:49:12 -0700 Subject: [PATCH 22/34] style(dronecan): remove commented-out LOG_DEBUG calls MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Per INAV coding standards — dead debug logging should be deleted, not left commented out. --- src/main/drivers/dronecan/dronecan.c | 17 ----------------- .../libcanard/canard_stm32h7xx_driver.c | 1 - 2 files changed, 18 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 89129015e58..41c7a928966 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -137,8 +137,6 @@ void dronecanUpdate(timeUs_t currentTimeUs) for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--) { - //LOG_DEBUG(CAN, "Received a message"); - //LOG_DEBUG(CAN, "Rx FIFO Fill Level: %lu", canardSTM32GetRxFifoFillLevel()); timestamp = millis() * 1000ULL; rx_res = canardSTM32Recieve(&rx_frame); @@ -194,7 +192,6 @@ static void processCanardTxQueue(void) { LOG_DEBUG(CAN, "Transmit error %d", tx_res); canardPopTxQueue(&canard); // Error - discard frame } else if (tx_res > 0) { - // LOG_DEBUG(CAN, "Successfully transmitted message"); canardPopTxQueue(&canard); // Success - remove from queue } else { // tx_res == 0: TX FIFO full, retry later @@ -230,7 +227,6 @@ static void process1HzTasks(timeUs_t timestamp_usec) static void send_NodeStatus(void) { uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE]; - // LOG_DEBUG(CAN, "Sending Node Status"); node_status.uptime_sec = millis() / 1000UL; if(isHardwareHealthy()){ node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; @@ -393,46 +389,33 @@ static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { return; } - // LOG_DEBUG(CAN, "Node Health "); switch (nodeStatus.health) { case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK: - // LOG_DEBUG(CAN, "OK"); break; case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING: - // LOG_DEBUG(CAN, "WARNING"); break; case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR: - // LOG_DEBUG(CAN, "ERROR"); break; case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL: - // LOG_DEBUG(CAN, "CRITICAL"); break; default: - // LOG_DEBUG(CAN, "UNKNOWN?"); break; } - // LOG_DEBUG(CAN, "Node Mode "); switch(nodeStatus.mode) { case UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL: - // LOG_DEBUG(CAN, "OPERATIONAL"); break; case UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION: - // LOG_DEBUG(CAN, "INITIALIZATION"); break; case UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE: - // LOG_DEBUG(CAN, "MAINTENANCE"); break; case UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE: - // LOG_DEBUG(CAN, "SOFTWARE UPDATE"); break; case UAVCAN_PROTOCOL_NODESTATUS_MODE_OFFLINE: - // LOG_DEBUG(CAN, "OFFLINE"); break; default: - // LOG_DEBUG(CAN, "UNKNOWN?"); break; } } diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c index ffdbb1d5b4a..6f95e1a42b3 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c @@ -365,7 +365,6 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { memcpy(TxData, tx_frame->data, TxHeader.DataLength); if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &TxHeader, TxData) == HAL_OK) { - // LOG_DEBUG(CAN, "Successfully sent message with id: %lu", TxHeader.Identifier); return 1; } From aa9cc8749136b033d180098ddb3d9c3ae51500bd Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 09:24:58 -0700 Subject: [PATCH 23/34] docs(dronecan): clarify SJW register encoding discrepancy The raw sjw=3 value is written to the HAL without the N-1 offset that BS1/BS2 correctly apply, so the hardware actually runs 4TQ SJW not 3TQ. Mark with TODO for the planned SJW/timing tuning pass. --- src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index ce8a9808650..e1cb7c66bb2 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -199,7 +199,7 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi (int)(1 + solution.bs1 + solution.bs2), (double)(solution.sample_point_permill) / (double)(10.0)); out_timings->prescaler = (uint16_t)(prescaler); - out_timings->sjw = 3; // SJW=3: allows up to 3 tq of resynchronization per bit; sufficient for short bus runs at 1 Mbps + out_timings->sjw = 3; // TODO: review SJW value — stored raw (no N-1 offset unlike BS1/BS2), so register encodes 4TQ not 3TQ; verify this is intentional out_timings->bs1 = (uint8_t)(solution.bs1)-1; // The HAL does not take care of the 1 bs offset in the register so remove it here like AP does. out_timings->bs2 = (uint8_t)(solution.bs2)-1; // The HAL does not take care of the 1 bs offset in the register so remove it here like AP does. From a5e6c55e9bced2b5f318f967356374b369ddb54b Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 09:31:02 -0700 Subject: [PATCH 24/34] fix(dronecan): address important code review findings (round 3) F7 driver: - Fix false-full race in canTxQueuePush: read tail fresh at the full-check so a concurrent ISR drain cannot cause a spurious TX drop; move snapshot after the push for HWM calculation only H7 driver: - Replace ErrorCode/!= 1 timing check with if (!canardSTM32ComputeTimings()) to match F7 pattern and remove fragile bool-stored-as-int16_t - Fix @retval for canardSTM32CAN1_Init: documents CANARD_OK (0) on success, not 1 as previously stated - Remove stray USER CODE BEGIN FDCAN1_Init 2 CubeMX sentinel --- .../drivers/dronecan/libcanard/canard_stm32f7xx_driver.c | 6 ++++-- .../drivers/dronecan/libcanard/canard_stm32h7xx_driver.c | 9 +++------ 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index e1cb7c66bb2..687b9b93306 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -415,14 +415,16 @@ int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { */ static bool canTxQueuePush(const CanardCANFrame *frame) { uint8_t next = (canTxQueue.head + 1) % TX_QUEUE_SIZE; - uint8_t tail_snapshot = canTxQueue.tail; // snapshot before ISR can advance it - if (next == tail_snapshot) { + // Read tail fresh: ISR only advances tail, so a fresh read can only show more space, + // never less — eliminates false-full when ISR drains a slot concurrently. + if (next == canTxQueue.tail) { canTxDropped++; return false; } canTxQueue.frames[canTxQueue.head] = *frame; __DMB(); // ensure frame data is visible before head advances canTxQueue.head = next; + uint8_t tail_snapshot = canTxQueue.tail; // snapshot after push; may undercount fill if ISR drains concurrently, acceptable for HWM uint8_t fill = (next - tail_snapshot + TX_QUEUE_SIZE) % TX_QUEUE_SIZE; if (fill > canTxQueueHWM) canTxQueueHWM = fill; return true; diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c index 6f95e1a42b3..049ac2d7b13 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c @@ -188,13 +188,12 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi /** * @brief CAN1 Initialization Function * @param bitrate desired bitrate to run the CAN network at. - * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode + * @retval CANARD_OK (0) on success, negative CANARD_ERROR on failure. */ int16_t canardSTM32CAN1_Init(uint32_t bitrate) { RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0}; struct Timings out_timings; - int16_t ErrorCode = 1; FDCAN_FilterTypeDef sFilterConfig; sFilterConfig.IdType = FDCAN_EXTENDED_ID; @@ -211,10 +210,9 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) hfdcan1.Init.TransmitPause = DISABLE; hfdcan1.Init.ProtocolException = DISABLE; - ErrorCode = canardSTM32ComputeTimings(bitrate, &out_timings); - if (ErrorCode != 1) + if (!canardSTM32ComputeTimings(bitrate, &out_timings)) { - LOG_ERROR(CAN, "Unable to calculate timings, Error Code:%d", ErrorCode); + LOG_ERROR(CAN, "Failed to compute CAN timings for bitrate %lu", (unsigned long)bitrate); return -CANARD_ERROR_INTERNAL; } @@ -257,7 +255,6 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) LOG_ERROR(CAN, "Failed CAN Init"); return -CANARD_ERROR_INTERNAL; } - /* USER CODE BEGIN FDCAN1_Init 2 */ if (HAL_FDCAN_ConfigFilter(&hfdcan1, &sFilterConfig) != HAL_OK) { LOG_ERROR(CAN, "Failed Config Filter"); return -CANARD_ERROR_INTERNAL; From d434500b9db3596fac1445fef1581ff8a65eba8d Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 09:40:05 -0700 Subject: [PATCH 25/34] fix(dronecan): fix optional_field_flags misuse and clean up CLI output - Set optional_field_flags to OPTIONAL_FIELD_FLAG_VCS_COMMIT in handle_GetNodeInfo: the field is a bitmask indicating which optional fields are valid, not a storage field for patch version - Remove stale TODO comment from handle_GetNodeInfo - Drop '/ 32' denominator from CLI TX queue and RX buffer lines: value is misleading on H7 (no software queue exists); fill level alone is sufficient on both targets --- src/main/drivers/dronecan/dronecan.c | 3 +-- src/main/fc/cli.c | 4 ++-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 41c7a928966..0653a0d68e8 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -478,7 +478,6 @@ static void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) handle a GetNodeInfo request */ -// TODO: All the data in here is temporary for testing. If actually need to send valid data, edit accordingly. static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) { LOG_DEBUG(CAN, "GetNodeInfo request from %d", transfer->source_node_id); @@ -493,7 +492,7 @@ static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) // fill in your major and minor firmware version pkt.software_version.major = FC_VERSION_MAJOR; pkt.software_version.minor = FC_VERSION_MINOR; - pkt.software_version.optional_field_flags = FC_VERSION_PATCH_LEVEL; + pkt.software_version.optional_field_flags = UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT; pkt.software_version.vcs_commit = strtoul(shortGitRevision, NULL, 16); // need to convert string to integer put git hash in here // should fill in hardware version diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 187ecdbb645..0a9ee15c957 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -4389,8 +4389,8 @@ static void cliDronecan(char *cmdline) cliPrintLinef(" TEC: %u", (unsigned)stat.tec); cliPrintLinef(" REC: %u", (unsigned)stat.rec); cliPrintLinef(" LEC: %s (%u)", lecNames[stat.lec & 0x7], (unsigned)stat.lec); - cliPrintLinef(" TX queue: %ld / 32 (hwm: %u)", (long)txFill, (unsigned)stat.tx_queue_hwm); // 32 = TX_QUEUE_SIZE; update if that constant changes - cliPrintLinef(" RX buffer: %ld / 32 (hwm: %u)", (long)rxFill, (unsigned)stat.rx_buffer_hwm); // 32 = RX_BUFFER_SIZE; update if that constant changes + cliPrintLinef(" TX queue: %ld (hwm: %u)", (long)txFill, (unsigned)stat.tx_queue_hwm); + cliPrintLinef(" RX buffer: %ld (hwm: %u)", (long)rxFill, (unsigned)stat.rx_buffer_hwm); cliPrintLinef(" TX dropped: %u", (unsigned)stat.tx_dropped); } #endif From dec39ce48b377a780f1a139a9dcca5c555f5a4d3 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 09:46:20 -0700 Subject: [PATCH 26/34] style(dronecan): remove stale development comments and fix indentation - Remove rhetorical question comments from GPIO init in F7 and H7 drivers (AF selection is target-specific by design; cmake selects the right driver) - Remove "Wheres the data?" comment from canardSTM32Recieve in F7 driver - Replace "unsure about this one" on H7 TX header fields with factual notes: ESI_ACTIVE (not error-passive), NO_TX_EVENTS (no FIFO allocated), MessageMarker=0 (unused without TX events) - Normalize tab/space mixed indentation in dronecanInit in dronecan.c --- src/main/drivers/dronecan/dronecan.c | 14 +++++++------- .../dronecan/libcanard/canard_stm32f7xx_driver.c | 6 +++--- .../dronecan/libcanard/canard_stm32h7xx_driver.c | 10 +++++----- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 0653a0d68e8..d43bfc2d4ed 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -101,18 +101,18 @@ void dronecanInit(void) Initializing the Libcanard instance. */ canardInit(&canard, - memory_pool, - sizeof(memory_pool), - onTransferReceived, - shouldAcceptTransfer, - NULL); + memory_pool, + sizeof(memory_pool), + onTransferReceived, + shouldAcceptTransfer, + NULL); // Could use DNA (Dynamic Node Allocation) by following example in esc_node.c but that requires a lot of setup and I'm not too sure of what advantage it brings // Instead, set a different NODE_ID for each device on the CAN bus by configuring node_settings if (dronecanConfig()->nodeID > 0) { - canardSetLocalNodeID(&canard, dronecanConfig()->nodeID); + canardSetLocalNodeID(&canard, dronecanConfig()->nodeID); } else { - LOG_DEBUG(CAN, "Node ID is 0, this node is anonymous and can't transmit most messages. Please update this in config"); + LOG_DEBUG(CAN, "Node ID is 0, this node is anonymous and can't transmit most messages. Please update this in config"); } } diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index 687b9b93306..b2d6c739331 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -67,9 +67,9 @@ static void canardSTM32GPIO_Init(void) // Set up the Rx and Tx pins for CAN1 and if present, the standby or listen only pin. #if defined(CAN1_TX) && defined(CAN1_RX) IOInit(IOGetByTag(IO_TAG(CAN1_TX)), OWNER_DRONECAN, RESOURCE_CAN_TX, 0); - IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_TX)), IOCFG_AF_PP, GPIO_AF9_CAN1); // How do I make the alternate function crossplatform? + IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_TX)), IOCFG_AF_PP, GPIO_AF9_CAN1); IOInit(IOGetByTag(IO_TAG(CAN1_RX)), OWNER_DRONECAN, RESOURCE_CAN_RX, 0); - IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_RX)), IOCFG_AF_PP, GPIO_AF9_CAN1); // How do I make the alternate function crossplatform? + IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_RX)), IOCFG_AF_PP, GPIO_AF9_CAN1); #endif @@ -382,7 +382,7 @@ int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { return -CANARD_ERROR_INVALID_ARGUMENT; } - if (rxBufferPopFrame(&RxBuffer, &canRxFrame) == 0) { // Wheres the data? + if (rxBufferPopFrame(&RxBuffer, &canRxFrame) == 0) { rx_frame->id = canRxFrame.header.ExtId; // Process ID to canard format diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c index 049ac2d7b13..7745f7accb1 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c @@ -41,9 +41,9 @@ static void canardSTM32GPIO_Init(void) // Set up the Rx and Tx pins for CAN1 and if present, the standby or listen only pin. #if defined(CAN1_TX) && defined(CAN1_RX) IOInit(IOGetByTag(IO_TAG(CAN1_TX)), OWNER_DRONECAN, RESOURCE_CAN_TX, 0); - IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_TX)), IOCFG_AF_PP, GPIO_AF9_FDCAN1); // How do I make the alternate function crossplatform? + IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_TX)), IOCFG_AF_PP, GPIO_AF9_FDCAN1); IOInit(IOGetByTag(IO_TAG(CAN1_RX)), OWNER_DRONECAN, RESOURCE_CAN_RX, 0); - IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_RX)), IOCFG_AF_PP, GPIO_AF9_FDCAN1); // How do I make the alternate function crossplatform? + IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_RX)), IOCFG_AF_PP, GPIO_AF9_FDCAN1); #endif @@ -354,11 +354,11 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { TxHeader.TxFrameType = FDCAN_DATA_FRAME; } - TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE; // unsure about this one + TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE; // node is not in error-passive state; sender sets ESI to active TxHeader.BitRateSwitch = FDCAN_BRS_OFF; // Disabling FDCAN (using CAN 2.0) TxHeader.FDFormat = FDCAN_CLASSIC_CAN; // Disabling FDCAN (using CAN 2.0) - TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS; // unsure about this one - TxHeader.MessageMarker = 0; // unsure about this one + TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS; // no TX event FIFO allocated; event tracking not needed + TxHeader.MessageMarker = 0; // only meaningful when TX events are enabled; unused here memcpy(TxData, tx_frame->data, TxHeader.DataLength); if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &TxHeader, TxData) == HAL_OK) { From af00a2ff9ecf70396085fc430ccb2e9d0c315f97 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 10:03:38 -0700 Subject: [PATCH 27/34] fix(dronecan): address code review findings (round 4) - Fix F7 canardSTM32CAN1_Init @retval: CANARD_OK is 0, not 1 - Enable AutoRetransmission on H7 to match F7; without it frames lost to bus arbitration are silently dropped, breaking multi-frame transfers - Add volatile qualifier to RX buffer function parameters so the compiler cannot cache index reads across ISR/main-loop boundary - Rename canardSTM32Recieve -> canardSTM32Receive (fix persistent misspelling in header, F7/H7/SITL drivers, and dronecan.c call site) --- src/main/drivers/dronecan/dronecan.c | 2 +- .../dronecan/libcanard/canard_sitl_driver.c | 2 +- .../dronecan/libcanard/canard_stm32_driver.h | 2 +- .../dronecan/libcanard/canard_stm32f7xx_driver.c | 14 +++++++------- .../dronecan/libcanard/canard_stm32h7xx_driver.c | 4 ++-- 5 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index d43bfc2d4ed..e49b485961a 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -138,7 +138,7 @@ void dronecanUpdate(timeUs_t currentTimeUs) for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--) { timestamp = millis() * 1000ULL; - rx_res = canardSTM32Recieve(&rx_frame); + rx_res = canardSTM32Receive(&rx_frame); if (rx_res < 0) { LOG_DEBUG(CAN, "Receive error %d", rx_res); diff --git a/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c b/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c index 8743e779dea..0e23a0fd8d4 100644 --- a/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c @@ -280,7 +280,7 @@ static void sitlCANGetStatsSocketCAN(canardProtocolStatus_t *pProtocolStat) { * @param rx_frame Pointer to frame structure to fill * @retval 0 if no frame available, 1 if frame received, negative on error */ -int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { +int16_t canardSTM32Receive(CanardCANFrame *const rx_frame) { if (rx_frame == NULL) { return -CANARD_ERROR_INVALID_ARGUMENT; } diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h b/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h index a9a7ecd3b0c..7ea72881456 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h +++ b/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h @@ -24,7 +24,7 @@ typedef struct { int16_t canardSTM32CAN1_Init(uint32_t bitrate); -int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame); +int16_t canardSTM32Receive(CanardCANFrame *const rx_frame); int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame); void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat); int32_t canardSTM32GetRxFifoFillLevel(void); diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index b2d6c739331..7dfc3eb06b8 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -211,7 +211,7 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi * Configures GPIO pins, bit timings, a pass-all acceptance filter, * starts the peripheral, and enables RX and TX interrupts. * @param bitrate Desired CAN bus bitrate in bits per second (e.g. 1000000). - * @retval CANARD_OK (1) on success, negative CANARD_ERROR code on failure. + * @retval CANARD_OK (0) on success, negative CANARD_ERROR code on failure. */ int16_t canardSTM32CAN1_Init(uint32_t bitrate) { @@ -292,7 +292,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) * @param rxMsg Pointer to the RxFrame_t to copy into the buffer. * @retval 0 on success, -1 if the buffer is full (frame dropped). */ -static int8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { +static int8_t rxBufferPushFrame(volatile struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { uint8_t next; RxFrame_t *pCurrentRxMsg; @@ -304,7 +304,7 @@ static int8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { if(next == rxBuf->readIndex) { return -1; // rxBuf is full } - pCurrentRxMsg = &rxBuf->rxMsg[rxBuf->writeIndex]; + pCurrentRxMsg = (RxFrame_t *)&rxBuf->rxMsg[rxBuf->writeIndex]; memcpy(pCurrentRxMsg, rxMsg, sizeof(RxFrame_t)); __DMB(); // ensure frame data is visible to main loop before writeIndex advances rxBuf->writeIndex = next; @@ -319,7 +319,7 @@ static int8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { * @param rxMsg Pointer to an RxFrame_t where the frame will be copied. * @retval 0 on success, -1 if the buffer is empty. */ -static int8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { +static int8_t rxBufferPopFrame(volatile struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { uint8_t next; RxFrame_t *pCurrentRxMsg; @@ -331,7 +331,7 @@ static int8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { if (next >= RX_BUFFER_SIZE){ next = 0; } - pCurrentRxMsg = &rxBuf->rxMsg[rxBuf->readIndex]; + pCurrentRxMsg = (RxFrame_t *)&rxBuf->rxMsg[rxBuf->readIndex]; __DMB(); // ensure ISR's frame data writes are visible before we read them memcpy(rxMsg, pCurrentRxMsg, sizeof(RxFrame_t)); rxBuf->readIndex = next; @@ -343,7 +343,7 @@ static int8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) { * @param rxBuf Pointer to the RxBuffer_t ring buffer. * @retval Number of frames available to read (0 to RX_BUFFER_SIZE-1). */ -static uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf) { +static uint8_t rxBufferNumMessages(volatile struct RxBuffer_t *rxBuf) { if(rxBuf->writeIndex < rxBuf->readIndex) return((rxBuf->writeIndex + RX_BUFFER_SIZE) - rxBuf->readIndex); @@ -375,7 +375,7 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { * stored. * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode */ -int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { +int16_t canardSTM32Receive(CanardCANFrame *const rx_frame) { RxFrame_t canRxFrame; if (rx_frame == NULL) { diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c index 7745f7accb1..f3432682b15 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c @@ -206,7 +206,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) hfdcan1.Instance = FDCAN1; hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC; // Initialize in CAN2.0 mode not CAN_FD hfdcan1.Init.Mode = FDCAN_MODE_NORMAL; - hfdcan1.Init.AutoRetransmission = DISABLE; + hfdcan1.Init.AutoRetransmission = ENABLE; // retransmit on arbitration loss, matching F7 behaviour hfdcan1.Init.TransmitPause = DISABLE; hfdcan1.Init.ProtocolException = DISABLE; @@ -283,7 +283,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) * stored. * @retval ret == 1: OK, ret < 0: CANARD_ERROR, ret == 0: Check hfdcan->ErrorCode */ -int16_t canardSTM32Recieve(CanardCANFrame *const rx_frame) { +int16_t canardSTM32Receive(CanardCANFrame *const rx_frame) { if (rx_frame == NULL) { return -CANARD_ERROR_INVALID_ARGUMENT; } From ded9643a952ac272cb6940b91e7cfcc587d9791c Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 10:12:32 -0700 Subject: [PATCH 28/34] style: fix file headers, #ifdef spacing, and tab/space mix in DroneCAN drivers - Correct file header names in canard_stm32f7xx_driver.c and canard_stm32h7xx_driver.c - Remove leading space before #ifdef CAN1_STANDBY in both drivers - Replace all tabs with spaces in dronecan.c (RX loop, 1Hz service block, processCanardTxQueue body) --- src/main/drivers/dronecan/dronecan.c | 316 +++++++++--------- .../libcanard/canard_stm32f7xx_driver.c | 4 +- .../libcanard/canard_stm32h7xx_driver.c | 4 +- 3 files changed, 162 insertions(+), 162 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index e49b485961a..93ecb7e5939 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -135,27 +135,27 @@ void dronecanUpdate(timeUs_t currentTimeUs) case STATE_DRONECAN_NORMAL: processCanardTxQueue(); - for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--) - { - timestamp = millis() * 1000ULL; - rx_res = canardSTM32Receive(&rx_frame); - - if (rx_res < 0) { - LOG_DEBUG(CAN, "Receive error %d", rx_res); - } - else if (rx_res > 0) // Success - process the frame - { - canardHandleRxFrame(&canard, &rx_frame, timestamp); - } - } + for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--) + { + timestamp = millis() * 1000ULL; + rx_res = canardSTM32Receive(&rx_frame); + + if (rx_res < 0) { + LOG_DEBUG(CAN, "Receive error %d", rx_res); + } + else if (rx_res > 0) // Success - process the frame + { + canardHandleRxFrame(&canard, &rx_frame, timestamp); + } + } // Drain any TX frames queued by RX handlers (e.g. GetNodeInfo responses) // in the same task cycle so multi-frame transfers complete before timeout. processCanardTxQueue(); if (currentTimeUs >= next_1hz_service_at) { - next_1hz_service_at += 1000000ULL; - process1HzTasks(currentTimeUs); + next_1hz_service_at += 1000000ULL; + process1HzTasks(currentTimeUs); processCanardTxQueue(); } @@ -184,20 +184,20 @@ void dronecanUpdate(timeUs_t currentTimeUs) /* --- TX and periodic task processing --- */ static void processCanardTxQueue(void) { - for (const CanardCANFrame *tx_frame ; (tx_frame = canardPeekTxQueue(&canard)) != NULL;) + for (const CanardCANFrame *tx_frame ; (tx_frame = canardPeekTxQueue(&canard)) != NULL;) { const int16_t tx_res = canardSTM32Transmit(tx_frame); - if (tx_res < 0) { - LOG_DEBUG(CAN, "Transmit error %d", tx_res); - canardPopTxQueue(&canard); // Error - discard frame - } else if (tx_res > 0) { - canardPopTxQueue(&canard); // Success - remove from queue - } else { - // tx_res == 0: TX FIFO full, retry later - break; - } - } + if (tx_res < 0) { + LOG_DEBUG(CAN, "Transmit error %d", tx_res); + canardPopTxQueue(&canard); // Error - discard frame + } else if (tx_res > 0) { + canardPopTxQueue(&canard); // Success - remove from queue + } else { + // tx_res == 0: TX FIFO full, retry later + break; + } + } } /* @@ -276,30 +276,30 @@ static bool shouldAcceptTransfer(const CanardInstance *ins, CanardTransferType transfer_type, uint8_t source_node_id) { - UNUSED(ins); + UNUSED(ins); UNUSED(source_node_id); if (transfer_type == CanardTransferTypeRequest) { - // check if we want to handle a specific service request - switch (data_type_id) { - case UAVCAN_PROTOCOL_GETNODEINFO_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE; - return true; - } - } - } - if (transfer_type == CanardTransferTypeResponse) { - // check if we want to handle a specific service request - switch (data_type_id) { - } - } - if (transfer_type == CanardTransferTypeBroadcast) { - // see if we want to handle a specific broadcast packet - switch (data_type_id) { - - case UAVCAN_PROTOCOL_NODESTATUS_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE; - return true; - } + // check if we want to handle a specific service request + switch (data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: { + *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE; + return true; + } + } + } + if (transfer_type == CanardTransferTypeResponse) { + // check if we want to handle a specific service request + switch (data_type_id) { + } + } + if (transfer_type == CanardTransferTypeBroadcast) { + // see if we want to handle a specific broadcast packet + switch (data_type_id) { + + case UAVCAN_PROTOCOL_NODESTATUS_ID: { + *out_data_type_signature = UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE; + return true; + } case UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID: { *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE; return true; @@ -320,33 +320,33 @@ static bool shouldAcceptTransfer(const CanardInstance *ins, *out_data_type_signature = UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE; return true; } - } - } - // we don't want any other messages - return false; + } + } + // we don't want any other messages + return false; } /* This callback is invoked by the library when a new message or request or response is received. */ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { - // switch on data type ID to pass to the right handler function - if (transfer->transfer_type == CanardTransferTypeRequest) { - // check if we want to handle a specific service request - switch (transfer->data_type_id) { - case UAVCAN_PROTOCOL_GETNODEINFO_ID: { - handle_GetNodeInfo(ins, transfer); - break; - } - } - } - if (transfer->transfer_type == CanardTransferTypeResponse) { - switch (transfer->data_type_id) { - } - } - if (transfer->transfer_type == CanardTransferTypeBroadcast) { - // check if we want to handle a specific broadcast message - switch (transfer->data_type_id) { + // switch on data type ID to pass to the right handler function + if (transfer->transfer_type == CanardTransferTypeRequest) { + // check if we want to handle a specific service request + switch (transfer->data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: { + handle_GetNodeInfo(ins, transfer); + break; + } + } + } + if (transfer->transfer_type == CanardTransferTypeResponse) { + switch (transfer->data_type_id) { + } + } + if (transfer->transfer_type == CanardTransferTypeBroadcast) { + // check if we want to handle a specific broadcast message + switch (transfer->data_type_id) { case UAVCAN_PROTOCOL_NODESTATUS_ID: handle_NodeStatus(ins, transfer); @@ -373,7 +373,7 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) handle_BatteryInfo(ins, transfer); break; } - } + } } /* --- Message handlers (incoming) --- */ @@ -381,96 +381,96 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) // Canard Handlers ( Many have code copied from libcanard esc_node example: https://github.com/dronecan/libcanard/blob/master/examples/ESCNode/esc_node.c ) static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); + UNUSED(ins); struct uavcan_protocol_NodeStatus nodeStatus; - if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) { - LOG_DEBUG(CAN, "NodeStatus decode failed"); - return; - } - - - switch (nodeStatus.health) { - case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK: - break; - case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING: - break; - case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR: - break; - case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL: - break; - default: - break; - } - - - switch(nodeStatus.mode) { - case UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL: - break; - case UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION: - break; - case UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE: - break; - case UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE: - break; - case UAVCAN_PROTOCOL_NODESTATUS_MODE_OFFLINE: - break; - default: - break; - } + if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) { + LOG_DEBUG(CAN, "NodeStatus decode failed"); + return; + } + + + switch (nodeStatus.health) { + case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK: + break; + case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING: + break; + case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR: + break; + case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL: + break; + default: + break; + } + + + switch(nodeStatus.mode) { + case UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL: + break; + case UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION: + break; + case UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE: + break; + case UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE: + break; + case UAVCAN_PROTOCOL_NODESTATUS_MODE_OFFLINE: + break; + default: + break; + } } static void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); + UNUSED(ins); struct uavcan_equipment_gnss_Auxiliary gnssAuxiliary; - if (uavcan_equipment_gnss_Auxiliary_decode(transfer, &gnssAuxiliary)) { - LOG_DEBUG(CAN, "GNSSAuxiliary decode failed"); - return; - } + if (uavcan_equipment_gnss_Auxiliary_decode(transfer, &gnssAuxiliary)) { + LOG_DEBUG(CAN, "GNSSAuxiliary decode failed"); + return; + } dronecanGPSReceiveGNSSAuxiliary(&gnssAuxiliary); } static void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); + UNUSED(ins); struct uavcan_equipment_gnss_Fix gnssFix; - if (uavcan_equipment_gnss_Fix_decode(transfer, &gnssFix)) { - LOG_DEBUG(CAN, "GNSSFix decode failed"); - return; - } + if (uavcan_equipment_gnss_Fix_decode(transfer, &gnssFix)) { + LOG_DEBUG(CAN, "GNSSFix decode failed"); + return; + } dronecanGPSReceiveGNSSFix(&gnssFix); } static void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); + UNUSED(ins); struct uavcan_equipment_gnss_Fix2 gnssFix2; - if (uavcan_equipment_gnss_Fix2_decode(transfer, &gnssFix2)) { - LOG_DEBUG(CAN, "GNSSFix2 decode failed"); - return; - } + if (uavcan_equipment_gnss_Fix2_decode(transfer, &gnssFix2)) { + LOG_DEBUG(CAN, "GNSSFix2 decode failed"); + return; + } dronecanGPSReceiveGNSSFix2(&gnssFix2); } static void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); + UNUSED(ins); struct uavcan_equipment_gnss_RTCMStream gnssRTCMStream; - if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &gnssRTCMStream)) { - LOG_DEBUG(CAN, "RTCMStream decode failed"); - return; - } + if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &gnssRTCMStream)) { + LOG_DEBUG(CAN, "RTCMStream decode failed"); + return; + } } static void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) { - UNUSED(ins); + UNUSED(ins); struct uavcan_equipment_power_BatteryInfo batteryInfo; - if (uavcan_equipment_power_BatteryInfo_decode(transfer, &batteryInfo)) { - LOG_DEBUG(CAN, "BatteryInfo decode failed"); - return; - } + if (uavcan_equipment_power_BatteryInfo_decode(transfer, &batteryInfo)) { + LOG_DEBUG(CAN, "BatteryInfo decode failed"); + return; + } dronecanBatterySensorReceiveInfo(&batteryInfo); } @@ -479,43 +479,43 @@ static void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) */ static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) { - LOG_DEBUG(CAN, "GetNodeInfo request from %d", transfer->source_node_id); + LOG_DEBUG(CAN, "GetNodeInfo request from %d", transfer->source_node_id); - uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; - struct uavcan_protocol_GetNodeInfoResponse pkt; + uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; + struct uavcan_protocol_GetNodeInfoResponse pkt; - memset(&pkt, 0, sizeof(pkt)); + memset(&pkt, 0, sizeof(pkt)); - node_status.uptime_sec = millis() / 1000ULL; - pkt.status = node_status; + node_status.uptime_sec = millis() / 1000ULL; + pkt.status = node_status; - // fill in your major and minor firmware version - pkt.software_version.major = FC_VERSION_MAJOR; - pkt.software_version.minor = FC_VERSION_MINOR; - pkt.software_version.optional_field_flags = UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT; - pkt.software_version.vcs_commit = strtoul(shortGitRevision, NULL, 16); // need to convert string to integer put git hash in here + // fill in your major and minor firmware version + pkt.software_version.major = FC_VERSION_MAJOR; + pkt.software_version.minor = FC_VERSION_MINOR; + pkt.software_version.optional_field_flags = UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT; + pkt.software_version.vcs_commit = strtoul(shortGitRevision, NULL, 16); // need to convert string to integer put git hash in here - // should fill in hardware version - pkt.hardware_version.major = 1; - pkt.hardware_version.minor = 0; + // should fill in hardware version + pkt.hardware_version.major = 1; + pkt.hardware_version.minor = 0; - // just setting all 16 bytes to 1 for testing - canardSTM32GetUniqueID(pkt.hardware_version.unique_id); + // just setting all 16 bytes to 1 for testing + canardSTM32GetUniqueID(pkt.hardware_version.unique_id); - strncpy((char*)pkt.name.data, FC_FIRMWARE_NAME, sizeof(pkt.name.data)); - pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data)); + strncpy((char*)pkt.name.data, FC_FIRMWARE_NAME, sizeof(pkt.name.data)); + pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data)); - uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); + uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); - canardRequestOrRespond(ins, - transfer->source_node_id, - UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, - UAVCAN_PROTOCOL_GETNODEINFO_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &buffer[0], - total_size); + canardRequestOrRespond(ins, + transfer->source_node_id, + UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, + UAVCAN_PROTOCOL_GETNODEINFO_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + total_size); } #endif diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index 7dfc3eb06b8..53e05ae771c 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -1,5 +1,5 @@ /* - * canard_stm32_driver.c + * canard_stm32f7xx_driver.c * * Created on: Jul 8, 2024 * Author: Roni Kant @@ -73,7 +73,7 @@ static void canardSTM32GPIO_Init(void) #endif - #ifdef CAN1_STANDBY +#ifdef CAN1_STANDBY // Initialize the standby or listen only pin. Set default state to enable CAN. // TODO: Tie the pin state to a configuration option so we can turn CAN on and off. diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c index f3432682b15..3fb5a48d921 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c @@ -1,5 +1,5 @@ /* - * canard_stm32_driver.c + * canard_stm32h7xx_driver.c * * Created on: Jul 8, 2024 * Author: Roni Kant @@ -47,7 +47,7 @@ static void canardSTM32GPIO_Init(void) #endif - #ifdef CAN1_STANDBY +#ifdef CAN1_STANDBY // Initialize the standby or listen only pin. Set default state to enable CAN. // TODO: Tie the pin state to a configuration option so we can turn CAN on and off. From 5636378fdd42b18fd61f53aec79e491c143f8c72 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 10:27:45 -0700 Subject: [PATCH 29/34] fix: resolve SITL linker error and uninitialized status fields in DroneCAN drivers - Add canardSTM32GetTxQueueFillLevel() stub to canard_sitl_driver.c to fix linker error when building SITL target with USE_DRONECAN defined - Add memset in sitlCANGetStatsStub and sitlCANGetStatsSocketCAN to zero all canardProtocolStatus_t fields, preventing stack garbage in CLI output - Fix stale comment in processCanardTxQueue: "TX FIFO full" -> "SW TX queue full" - Remove redundant LOG_DEBUG("Battery Info") from onTransferReceived - Fix 6-space indent to 4-space in CAN1_RX0_IRQHandler --- src/main/drivers/dronecan/dronecan.c | 3 +-- .../dronecan/libcanard/canard_sitl_driver.c | 19 ++++++++++++------- .../libcanard/canard_stm32f7xx_driver.c | 2 +- 3 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 93ecb7e5939..f3c4828a72b 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -194,7 +194,7 @@ static void processCanardTxQueue(void) { } else if (tx_res > 0) { canardPopTxQueue(&canard); // Success - remove from queue } else { - // tx_res == 0: TX FIFO full, retry later + // tx_res == 0: SW TX queue full, retry on next dronecanUpdate cycle break; } } @@ -369,7 +369,6 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) break; case UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID: - LOG_DEBUG(CAN, "Battery Info"); handle_BatteryInfo(ins, transfer); break; } diff --git a/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c b/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c index 0e23a0fd8d4..d2bcad307ad 100644 --- a/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c @@ -106,8 +106,7 @@ static int16_t sitlCANTransmitStub(const CanardCANFrame* const tx_frame) { } static void sitlCANGetStatsStub(canardProtocolStatus_t *pProtocolStat) { - pProtocolStat->BusOff = 0; - pProtocolStat->ErrorPassive = 0; + memset(pProtocolStat, 0, sizeof(*pProtocolStat)); } #ifdef __linux__ @@ -267,11 +266,9 @@ static int16_t sitlCANTransmitSocketCAN(const CanardCANFrame* const tx_frame) { * @param pProtocolStat Pointer to status structure to fill */ static void sitlCANGetStatsSocketCAN(canardProtocolStatus_t *pProtocolStat) { - // SocketCAN doesn't expose bus-off/error-passive directly - // We could check interface flags via netlink, but for SITL testing - // we assume the virtual CAN is always healthy - pProtocolStat->BusOff = 0; - pProtocolStat->ErrorPassive = 0; + // SocketCAN doesn't expose bus-off/error-passive directly. + // Assume the virtual CAN is always healthy for SITL testing. + memset(pProtocolStat, 0, sizeof(*pProtocolStat)); } #endif // __linux__ @@ -357,6 +354,14 @@ int32_t canardSTM32GetRxFifoFillLevel(void) { return 0; } +/** + * @brief Get SW TX queue fill level + * @retval 0 — SITL has no software TX queue + */ +int32_t canardSTM32GetTxQueueFillLevel(void) { + return 0; +} + /** * @brief Recover from bus-off condition */ diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index 53e05ae771c..0d94fb06e06 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -354,7 +354,7 @@ static uint8_t rxBufferNumMessages(volatile struct RxBuffer_t *rxBuf) { * @brief CAN1 RX FIFO0 interrupt handler — delegates to the HAL IRQ handler. */ void CAN1_RX0_IRQHandler(void) { - HAL_CAN_IRQHandler(&hcan1); + HAL_CAN_IRQHandler(&hcan1); } /** From f00a8e83ee9e16edc61d48c9501eb54a608234ca Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 11:00:00 -0700 Subject: [PATCH 30/34] fix: correct SPSC DMB ordering, TX seeding condition, and counter ownership docs - rxBufferPopFrame: snapshot readIndex before __DMB() so the barrier precedes the data dereference, matching the push-side store-release pattern - canardSTM32Transmit: seed HW mailboxes when any mailbox is free (> 0) rather than only when all three are idle (== 3), preventing a permanent TX stall when HAL_CAN_AddTxMessage fails with the SW queue non-empty - Add counter ownership comments: canTxDropped/canTxQueueHWM written by main loop, canRxBufferHWM written by ISR; all are single-instruction atomic reads - H7 AutoRetransmission=ENABLE: add detailed comment explaining frame ordering guarantee (FDCAN TX FIFO is strictly ordered) and the TODO to evaluate whether DISABLE would be safer for the H7 polling model --- .../drivers/dronecan/libcanard/canard_stm32f7xx_driver.c | 8 ++++++-- .../drivers/dronecan/libcanard/canard_stm32h7xx_driver.c | 8 +++++++- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index 0d94fb06e06..ed588d87e7b 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -51,6 +51,9 @@ static struct CanTxQueue_t { static CAN_HandleTypeDef hcan1; +// canTxDropped and canTxQueueHWM are written by main loop (canTxQueuePush), read by main loop only. +// canRxBufferHWM is written by ISR (rxBufferPushFrame), read by main loop only. +// All are uint8/uint16 so reads are single-instruction atomic on Cortex-M. static volatile uint16_t canTxDropped = 0; static volatile uint8_t canTxQueueHWM = 0; static volatile uint8_t canRxBufferHWM = 0; @@ -331,8 +334,9 @@ static int8_t rxBufferPopFrame(volatile struct RxBuffer_t *rxBuf, RxFrame_t *rxM if (next >= RX_BUFFER_SIZE){ next = 0; } - pCurrentRxMsg = (RxFrame_t *)&rxBuf->rxMsg[rxBuf->readIndex]; + uint8_t ridx = rxBuf->readIndex; // snapshot index before barrier __DMB(); // ensure ISR's frame data writes are visible before we read them + pCurrentRxMsg = (RxFrame_t *)&rxBuf->rxMsg[ridx]; memcpy(rxMsg, pCurrentRxMsg, sizeof(RxFrame_t)); rxBuf->readIndex = next; return 0; @@ -550,7 +554,7 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { // higher-priority IRQs like the gyro timer) to preserve the SPSC contract — // ISR is the sole consumer; we become the consumer only during this window. ATOMIC_BLOCK(NVIC_PRIO_CAN) { - if (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 3) { + if (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) > 0) { canTxDrainQueue(&hcan1); } if (!canTxQueueIsEmpty()) { diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c index 3fb5a48d921..db8dc6ba237 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c @@ -206,7 +206,13 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate) hfdcan1.Instance = FDCAN1; hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC; // Initialize in CAN2.0 mode not CAN_FD hfdcan1.Init.Mode = FDCAN_MODE_NORMAL; - hfdcan1.Init.AutoRetransmission = ENABLE; // retransmit on arbitration loss, matching F7 behaviour + // AutoRetransmission=ENABLE: hardware retries on arbitration loss without software intervention. + // Unlike the F7 (ISR + 32-deep SW queue), the H7 has only the HW TX FIFO (no SW queue). + // FDCAN TX FIFO maintains strict submission order regardless of retransmission, so frame + // ordering is preserved. Risk on a heavily loaded bus: a retrying frame can delay later + // FIFO entries but cannot reorder them. Original code used DISABLE (software-controlled + // retry via polling). TODO: evaluate whether DISABLE is safer for the H7 polling model. + hfdcan1.Init.AutoRetransmission = ENABLE; hfdcan1.Init.TransmitPause = DISABLE; hfdcan1.Init.ProtocolException = DISABLE; From 0134e1fbc19f9e6cdfe6c5604f61a1204c6e735a Mon Sep 17 00:00:00 2001 From: daijoubu Date: Sun, 24 May 2026 02:23:03 +0000 Subject: [PATCH 31/34] Potential fix for pull request finding Update comment to reflect unique ID used instead of dummy value. Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- src/main/drivers/dronecan/dronecan.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index f3c4828a72b..4299dac306e 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -498,7 +498,7 @@ static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) pkt.hardware_version.major = 1; pkt.hardware_version.minor = 0; - // just setting all 16 bytes to 1 for testing + // populate the hardware unique ID from the STM32 device unique ID canardSTM32GetUniqueID(pkt.hardware_version.unique_id); strncpy((char*)pkt.name.data, FC_FIRMWARE_NAME, sizeof(pkt.name.data)); From efa2576c57fc73a4886ccb49a4a6957036aa85f7 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Sun, 24 May 2026 16:44:26 -0700 Subject: [PATCH 32/34] fix(dronecan): address Copilot review comments on PR #11560 --- .../dronecan/libcanard/canard_stm32f7xx_driver.c | 14 ++++++++++---- .../dronecan/libcanard/canard_stm32h7xx_driver.c | 8 ++++++-- 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index ed588d87e7b..361a35d151e 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -23,7 +23,7 @@ /* --- Internal types and state --- */ #define RX_BUFFER_SIZE 32 -#define TX_QUEUE_SIZE 32 +#define TX_QUEUE_SIZE 32 // usable capacity is TX_QUEUE_SIZE - 1 (31) due to one-slot-empty scheme struct Timings { uint16_t prescaler; @@ -545,14 +545,20 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { return -CANARD_ERROR_INVALID_ARGUMENT; } + // Classic CAN only, max payload is 8 bytes + if (tx_frame->data_len > 8) { + return -CANARD_ERROR_INVALID_ARGUMENT; + } + if (!canTxQueuePush(tx_frame)) { return 0; // SW queue full - caller retries next cycle } // If all mailboxes are idle, RQCP will never fire to start the ISR chain. - // Seed the HW via canTxDrainQueue while blocking only the CAN TX ISR (not - // higher-priority IRQs like the gyro timer) to preserve the SPSC contract — - // ISR is the sole consumer; we become the consumer only during this window. + // Seed the HW via canTxDrainQueue while masking all interrupts at priority + // >= NVIC_PRIO_CAN via BASEPRI (higher-priority IRQs like gyro timer are + // unaffected) to preserve the SPSC contract — ISR is the sole consumer; + // we become the consumer only during this window. ATOMIC_BLOCK(NVIC_PRIO_CAN) { if (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) > 0) { canTxDrainQueue(&hcan1); diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c index db8dc6ba237..d625cdeaeba 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c @@ -310,8 +310,8 @@ int16_t canardSTM32Receive(CanardCANFrame *const rx_frame) { rx_frame->id |= CANARD_CAN_FRAME_RTR; } - rx_frame->data_len = RxHeader.DataLength; - memcpy(rx_frame->data, RxData, RxHeader.DataLength); + rx_frame->data_len = RxHeader.DataLength > 8 ? 8 : RxHeader.DataLength; + memcpy(rx_frame->data, RxData, rx_frame->data_len); // assume a single interface rx_frame->iface_id = 0; @@ -340,6 +340,10 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) { return -CANARD_ERROR_INVALID_ARGUMENT; // unsupported frame format } + if (tx_frame->data_len > 8) { + return -CANARD_ERROR_INVALID_ARGUMENT; // classic CAN only, max 8 bytes + } + FDCAN_TxHeaderTypeDef TxHeader; uint8_t TxData[8]; From 37c113d3677de0c2a03b8162d3b904901abf448e Mon Sep 17 00:00:00 2001 From: daijoubu Date: Sun, 24 May 2026 17:00:55 -0700 Subject: [PATCH 33/34] fix(dronecan): address sensei-hacker review feedback on PR #11560 - Replace empty switch cases in handle_NodeStatus with (void)nodeStatus + TODO - Remove volatile from main-loop-only canTxDropped/canTxQueueHWM - Use __HAL_CAN_DISABLE_IT() instead of HAL_CAN_DeactivateNotification() in canTxDrainQueue (ISR context safety) --- src/main/drivers/dronecan/dronecan.c | 30 ++----------------- .../libcanard/canard_stm32f7xx_driver.c | 10 +++++-- 2 files changed, 9 insertions(+), 31 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 4299dac306e..b22380b6d77 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -389,34 +389,8 @@ static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { } - switch (nodeStatus.health) { - case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK: - break; - case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING: - break; - case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR: - break; - case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL: - break; - default: - break; - } - - - switch(nodeStatus.mode) { - case UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL: - break; - case UAVCAN_PROTOCOL_NODESTATUS_MODE_INITIALIZATION: - break; - case UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE: - break; - case UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE: - break; - case UAVCAN_PROTOCOL_NODESTATUS_MODE_OFFLINE: - break; - default: - break; - } + // TODO: NodeStatus health/mode monitoring — currently no action taken + (void)nodeStatus; } static void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) { diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c index 361a35d151e..13c52eed388 100644 --- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c +++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c @@ -54,8 +54,9 @@ static CAN_HandleTypeDef hcan1; // canTxDropped and canTxQueueHWM are written by main loop (canTxQueuePush), read by main loop only. // canRxBufferHWM is written by ISR (rxBufferPushFrame), read by main loop only. // All are uint8/uint16 so reads are single-instruction atomic on Cortex-M. -static volatile uint16_t canTxDropped = 0; -static volatile uint8_t canTxQueueHWM = 0; +// Only canRxBufferHWM needs volatile (ISR vs main loop sharing). +static uint16_t canTxDropped = 0; +static uint8_t canTxQueueHWM = 0; static volatile uint8_t canRxBufferHWM = 0; /* --- Initialization --- */ @@ -509,7 +510,10 @@ static void canTxDrainQueue(CAN_HandleTypeDef *hcan) { canTxQueueConsume(); } if (canTxQueueIsEmpty()) { - HAL_CAN_DeactivateNotification(hcan, CAN_IT_TX_MAILBOX_EMPTY); + // Use the register-level macro instead of HAL_CAN_DeactivateNotification: + // this function is called from ISR context (TX mailbox complete callbacks) + // where HAL functions using __HAL_LOCK() may not be safe to call. + __HAL_CAN_DISABLE_IT(hcan, CAN_IT_TX_MAILBOX_EMPTY); } } From e43c5b9daee723cc685e1a9e814a229f0f54afd6 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 28 May 2026 17:59:42 -0700 Subject: [PATCH 34/34] fix(dronecan): restore MSP accessor functions lost during rebase onto maintenance-10.x The -X theirs strategy during rebase lost the upstream accessor functions (dronecanGetState, dronecanGetNodeCount, dronecanGetBitrateKbps, dronecanGetNode) added by PR #11527. These are needed by cli.c and MSP dronecan support. --- src/main/drivers/dronecan/dronecan.c | 36 ++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index b22380b6d77..599056f6acb 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -181,6 +181,42 @@ void dronecanUpdate(timeUs_t currentTimeUs) } +dronecanState_e dronecanGetState(void) +{ + return dronecanState; +} + +uint8_t dronecanGetNodeCount(void) +{ + return activeNodeCount; +} + +uint32_t dronecanGetBitrateKbps(void) +{ + switch (dronecanConfig()->bitRateKbps){ + case DRONECAN_BITRATE_125KBPS: + return 125; + + case DRONECAN_BITRATE_250KBPS: + return 250; + + case DRONECAN_BITRATE_500KBPS: + return 500; + + case DRONECAN_BITRATE_1000KBPS: + return 1000; + + case DRONECAN_BITRATE_COUNT: + return 0; + } + return 0; +} + +const dronecanNodeInfo_t *dronecanGetNode(uint8_t index) { + if (index < activeNodeCount) return &nodeTable[index]; + return NULL; +} + /* --- TX and periodic task processing --- */ static void processCanardTxQueue(void) {