From 3b33ce1bdc2aae8660578f0eb1fbdccc9572f5fe Mon Sep 17 00:00:00 2001 From: daijoubu Date: Sat, 2 May 2026 15:22:23 -0700 Subject: [PATCH 01/30] 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 f78f53a0e30..6de513ded5e 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -110,7 +110,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) { @@ -122,7 +121,6 @@ void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) { return; } dronecanGPSReceiveGNSSFix(&gnssFix); - LOG_DEBUG(CAN, "GNSS Fix received"); } void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) { @@ -134,7 +132,6 @@ void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) { return; } dronecanGPSReceiveGNSSFix2(&gnssFix2); - LOG_DEBUG(CAN, "GNSS Fix2 received"); } void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) { @@ -145,7 +142,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) { @@ -157,7 +153,6 @@ void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) { return; } dronecanBatterySensorReceiveInfo(&batteryInfo); - LOG_DEBUG(CAN, "Battery Info"); } /* @@ -318,12 +313,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 5e7378601582cd2e74ab1b880a085cf8d7bdba95 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Sat, 2 May 2026 15:41:19 -0700 Subject: [PATCH 02/30] 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 e205097bc743b0eff8290d25e212997f19dc573d Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 13 May 2026 17:06:30 -0700 Subject: [PATCH 03/30] 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 6f809432648..2d6f13b0b82 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -127,6 +127,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" @@ -4327,6 +4331,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); @@ -4885,6 +4912,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 f4ef03ac03b07bf28e0a4c47af5c406c4313c41f Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 14 May 2026 19:18:20 -0700 Subject: [PATCH 04/30] 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 2d6f13b0b82..347d8ba361a 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -4348,8 +4348,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 dc1f2fdaa02b69ac1b37a44472dc63ee01cfdce0 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 14 May 2026 19:41:48 -0700 Subject: [PATCH 05/30] 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 169e6581d668258732c191b785720047bdef4000 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 14 May 2026 19:51:56 -0700 Subject: [PATCH 06/30] 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 f536c747466a16bbe3ec1e0c2f05d97dbdae2c97 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 14 May 2026 20:37:29 -0700 Subject: [PATCH 07/30] 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 6de513ded5e..a370727590f 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -313,7 +313,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 b51e06e9ecd810024b994e4a3b71a0028d9c876c Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 14 May 2026 20:53:52 -0700 Subject: [PATCH 08/30] 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 347d8ba361a..814d7a6b9ac 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -4344,7 +4344,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 1311684a6672c06290cbe20b7edfc26ba0488a36 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 14 May 2026 21:06:09 -0700 Subject: [PATCH 09/30] 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 49f538d04c53fc49a82068ee9fa2d0e863a97754 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 14 May 2026 21:12:02 -0700 Subject: [PATCH 10/30] 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 44868d0e10d35a33ecd37c3370d71b8452e0f3ac Mon Sep 17 00:00:00 2001 From: daijoubu Date: Thu, 14 May 2026 21:47:21 -0700 Subject: [PATCH 11/30] 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 814d7a6b9ac..06342fc4074 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -4348,8 +4348,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 11a088e213eb5d0af53630033219dda4dbb00832 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 07:16:19 -0700 Subject: [PATCH 12/30] 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 158aa1ef5083c087b172dd0a30918974f20e4f13 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 07:33:26 -0700 Subject: [PATCH 13/30] 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 a370727590f..809d50cdded 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]; @@ -46,6 +46,8 @@ PG_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig, // 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) { @@ -200,46 +202,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 /* @@ -330,27 +293,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); @@ -359,6 +322,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 @@ -396,6 +403,8 @@ void process1HzTasks(timeUs_t timestamp_usec) send_NodeStatus(); } +/* ── Public API ────────────────────────────────────────────────────────────── */ + void dronecanInit(void) { LOG_DEBUG(CAN, "dronecan Init"); @@ -409,7 +418,7 @@ void dronecanInit(void) case DRONECAN_BITRATE_250KBPS: bitrate = 250000; break; - + case DRONECAN_BITRATE_500KBPS: bitrate = 500000; break; @@ -428,7 +437,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. */ @@ -511,8 +520,8 @@ void dronecanUpdate(timeUs_t currentTimeUs) dronecanState = STATE_DRONECAN_NORMAL; } break; - + } - + } #endif 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 4d8eedb9cafb1999cc391ce0afd0056307a1ff89 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 07:39:10 -0700 Subject: [PATCH 14/30] 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 | 601 ++++++++++++++------------- 1 file changed, 308 insertions(+), 293 deletions(-) diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c index 809d50cdded..267ea584e3e 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -46,160 +46,223 @@ PG_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig, // 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; - // LOG_DEBUG(CAN, "Node Health "); + case DRONECAN_BITRATE_250KBPS: + bitrate = 250000; + break; - 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; - } + case DRONECAN_BITRATE_500KBPS: + bitrate = 500000; + break; - // LOG_DEBUG(CAN, "Node Mode "); + case DRONECAN_BITRATE_1000KBPS: + bitrate = 1000000; + break; - 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; - } + 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; + static enum dronecanState_e dronecanState = STATE_DRONECAN_INIT; + 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) { - printf("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 ───────────────────────────────────────────── */ @@ -214,8 +277,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, @@ -274,7 +336,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 @@ -297,7 +359,6 @@ void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { handle_NodeStatus(ins, transfer); break; - case UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID: handle_GNSSAuxiliary(ins, transfer); break; @@ -322,206 +383,160 @@ void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { } } -/* ── Message senders (outgoing) ────────────────────────────────────────────── */ - -// Canard Senders +/* ── Message handlers (incoming) ───────────────────────────────────────────── */ -/* - 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]; +// Canard Handlers ( Many have code copied from libcanard esc_node example: https://github.com/dronecan/libcanard/blob/master/examples/ESCNode/esc_node.c ) - // 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; - } +static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_protocol_NodeStatus nodeStatus; - 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 + if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) { + LOG_DEBUG(CAN, "NodeStatus decode failed"); + return; + } - // put whatever you like in here for display in GUI - node_status.vendor_specific_status_code = armingFlags; + // LOG_DEBUG(CAN, "Node Health "); - uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); + 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; + } - // 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 Mode "); - canardBroadcast(&canard, - UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, - UAVCAN_PROTOCOL_NODESTATUS_ID, - &transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, - buffer, - len); - // PrintCanStatus(); + 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; +static void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_equipment_gnss_Fix2 gnssFix2; - case DRONECAN_BITRATE_250KBPS: - bitrate = 250000; - break; + if (uavcan_equipment_gnss_Fix2_decode(transfer, &gnssFix2)) { + LOG_DEBUG(CAN, "GNSSFix2 decode failed"); + return; + } + dronecanGPSReceiveGNSSFix2(&gnssFix2); +} - case DRONECAN_BITRATE_500KBPS: - bitrate = 500000; - break; +static void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_equipment_gnss_RTCMStream gnssRTCMStream; - case DRONECAN_BITRATE_1000KBPS: - bitrate = 1000000; - break; + if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &gnssRTCMStream)) { + LOG_DEBUG(CAN, "RTCMStream decode failed"); + return; + } +} - 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_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) { + UNUSED(ins); + struct uavcan_equipment_power_BatteryInfo batteryInfo; - // 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_power_BatteryInfo_decode(transfer, &batteryInfo)) { + LOG_DEBUG(CAN, "BatteryInfo decode failed"); + return; + } + dronecanBatterySensorReceiveInfo(&batteryInfo); } -void dronecanUpdate(timeUs_t currentTimeUs) -{ - static timeUs_t next_1hz_service_at = 0; - static timeUs_t busoffTimeUs = 0; - CanardCANFrame rx_frame; - int numMessagesToProcess = 0; - static enum dronecanState_e dronecanState = STATE_DRONECAN_INIT; - canardProtocolStatus_t protocolStatus = {}; - uint64_t timestamp; - int16_t rx_res; +/* + handle a GetNodeInfo request +*/ - switch(dronecanState) { - case STATE_DRONECAN_INIT: - next_1hz_service_at = currentTimeUs + 1000000ULL; // First 1Hz tick in 1 second - dronecanState = STATE_DRONECAN_NORMAL; - break; +// 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); - case STATE_DRONECAN_NORMAL: - processCanardTxQueue(); + uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; + struct uavcan_protocol_GetNodeInfoResponse pkt; - 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); + memset(&pkt, 0, sizeof(pkt)); - 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(); + node_status.uptime_sec = millis() / 1000ULL; + pkt.status = node_status; - if (currentTimeUs >= next_1hz_service_at) - { - next_1hz_service_at += 1000000ULL; - process1HzTasks(currentTimeUs); - processCanardTxQueue(); - } + // 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 - canardSTM32GetProtocolStatus(&protocolStatus); - if(protocolStatus.BusOff != 0) { - dronecanState = STATE_DRONECAN_BUS_OFF; - busoffTimeUs = currentTimeUs; - } - break; + // should fill in hardware version + pkt.hardware_version.major = 1; + pkt.hardware_version.minor = 0; - 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; + // 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)); + 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); } + #endif From a2d761de7a0c59df51feca586dd52f48f24d554b Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 07:57:25 -0700 Subject: [PATCH 15/30] 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 267ea584e3e..9ecbe5daa62 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -500,7 +500,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 5e8b8e401201e0d040f2e3eb4480b669f6bb5c26 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 08:05:58 -0700 Subject: [PATCH 16/30] 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 9c7c009878bd76b92e4be2aa80457215c8d07efc Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 08:15:42 -0700 Subject: [PATCH 17/30] 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 9ecbe5daa62..b79f5ca8e89 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]; @@ -46,7 +46,7 @@ PG_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig, // 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); @@ -63,7 +63,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) { @@ -185,7 +185,7 @@ void dronecanUpdate(timeUs_t currentTimeUs) } -/* ── TX and periodic task processing ───────────────────────────────────────── */ +/* --- TX and periodic task processing --- */ static void processCanardTxQueue(void) { // Transmitting @@ -222,7 +222,7 @@ static void process1HzTasks(timeUs_t timestamp_usec) send_NodeStatus(); } -/* ── Message senders (outgoing) ────────────────────────────────────────────── */ +/* --- Message senders (outgoing) --- */ // Canard Senders @@ -265,7 +265,7 @@ static void send_NodeStatus(void) { // PrintCanStatus(); } -/* ── Libcanard protocol callbacks ───────────────────────────────────────────── */ +/* --- Libcanard protocol callbacks --- */ // Canard Util /* @@ -383,7 +383,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 02f25fa2944abaced3c5566bcdcf140911dac2b8 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 08:21:45 -0700 Subject: [PATCH 18/30] 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 b79f5ca8e89..b292d342cca 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -188,7 +188,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 06342fc4074..364fd53295d 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -4348,8 +4348,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 98f838511d25108c20529b393fbc2a61d53759bf Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 08:35:54 -0700 Subject: [PATCH 19/30] 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 08257aefa79247c3d7ea496d91bf51266512b5b8 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 08:39:50 -0700 Subject: [PATCH 20/30] 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 b292d342cca..8c0b9843505 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; enum dronecanState_e { STATE_DRONECAN_INIT, @@ -250,7 +250,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 d20666a1833ade7b669fd0ba3e7c488362af8c52 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 08:47:49 -0700 Subject: [PATCH 21/30] 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 1df9b77453d1e28a3cd98b2f571cb6d75f36c16d Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 08:49:12 -0700 Subject: [PATCH 22/30] 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 8c0b9843505..36fb737343a 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -139,8 +139,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); @@ -196,7 +194,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 @@ -232,7 +229,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; @@ -395,46 +391,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 2317e838a8fcc6852d21af264ce16ff55b588d7b Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 09:24:58 -0700 Subject: [PATCH 23/30] 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 6aeec0873d5a7715599d72908a7adaa962cbdbae Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 09:31:02 -0700 Subject: [PATCH 24/30] 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 a97167d96a590e175deb88501e0e5e7440f261c5 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 09:40:05 -0700 Subject: [PATCH 25/30] 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 36fb737343a..500ab15e740 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -480,7 +480,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); @@ -495,7 +494,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 364fd53295d..e7205a2fe21 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -4348,8 +4348,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 7c8287ecc04f22a58186d60cdfe0fee44b755c15 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 09:46:20 -0700 Subject: [PATCH 26/30] 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 500ab15e740..4113c9e8294 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -102,18 +102,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 3992f01ab5364cc6869e17d4d1ec50dc0dcf8dd4 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 10:03:38 -0700 Subject: [PATCH 27/30] 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 4113c9e8294..7d4b29435b1 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -140,7 +140,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 5783032c71ee45013ee9bb46fed2c32678b43500 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 10:12:32 -0700 Subject: [PATCH 28/30] 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 7d4b29435b1..508717271dd 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -137,27 +137,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(); } @@ -186,20 +186,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; + } + } } /* @@ -278,30 +278,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; @@ -322,33 +322,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); @@ -375,7 +375,7 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) handle_BatteryInfo(ins, transfer); break; } - } + } } /* --- Message handlers (incoming) --- */ @@ -383,96 +383,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); } @@ -481,43 +481,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 a4bb7c15b034709ba8f3b84be5bce6df12fe5713 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 10:27:45 -0700 Subject: [PATCH 29/30] 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 508717271dd..7b4f39c2edd 100644 --- a/src/main/drivers/dronecan/dronecan.c +++ b/src/main/drivers/dronecan/dronecan.c @@ -196,7 +196,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; } } @@ -371,7 +371,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 282afb7d110ce3e4de7880f7ce1783322ab43b53 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Fri, 15 May 2026 11:00:00 -0700 Subject: [PATCH 30/30] 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;