From 2d7e92bf3901dc08da35301828b54deefb2da2a5 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Sat, 23 May 2026 07:07:41 -0700
Subject: [PATCH 01/67] dronecan/h7: reduce FDCAN SJW from 8 to 3

SJW=8 was overly conservative (80% of bit time at 1Mbps with 10 quanta).
SJW=3 is the standard value also used by the F7 driver.

Tested with 6037 arm/disarm cycles at 500kbps: TEC=0, REC=0, zero errors.
---
 src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index 77c1f94921f..27fdeece3bd 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -369,7 +369,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 = 8;                        // Not happy with this value, but 1MBPs with unshielded cable?
+    out_timings->sjw = 3;                        // Standard SJW
     out_timings->bs1 = (uint8_t)(solution.bs1);  // The HAL takes care of the 1 bs offset in the register so don't remove it here like AP does.
     out_timings->bs2 = (uint8_t)(solution.bs2);  // The HAL takes care of the 1 bs offset in the register so don't remove it here like AP does.
 

From be177928badde2f15095b78b3216a133050937cc Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Wed, 27 May 2026 19:43:26 -0700
Subject: [PATCH 02/67] dronecan/h7: extend PLL2 block to cover USE_DRONECAN,
 fix PLL2Q for FDCAN
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

PLL2Q was 3 (266 MHz, invalid for FDCAN ≤ 80 MHz). Fix to 10 (80 MHz).
Extend PLL2 guard from USE_SDCARD_SDIO to USE_SDCARD_SDIO || USE_DRONECAN
so H7 boards with CAN but no SD card get PLL2 configured. Adopt upstream
PLL2M/N formula (VCI=1.6 MHz, VCO=800 MHz) and error check on
HAL_RCCEx_PeriphCLKConfig.
---
 src/main/drivers/dronecan/dronecan.c          |  2 +-
 .../libcanard/canard_stm32h7xx_driver.c       | 30 ++++++++-----------
 src/main/target/system_stm32h7xx.c            | 22 ++++++++++----
 3 files changed, 30 insertions(+), 24 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 42e03212b38..42e0a3c3abf 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -495,7 +495,7 @@ void dronecanUpdate(timeUs_t currentTimeUs)
             break;
 
         case STATE_DRONECAN_BUS_OFF:
-            if(currentTimeUs > (busoffTimeUs + 100000)) { // Wait 100 mS
+            if(currentTimeUs > (busoffTimeUs + 1000)) { // Wait 1 mS
                 canardSTM32RecoverFromBusOff();
                 busoffTimeUs = currentTimeUs;
             }
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index 27fdeece3bd..7f2697eb66c 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -163,10 +163,20 @@ 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;
     hfdcan1.Init.TransmitPause = DISABLE;
     hfdcan1.Init.ProtocolException = DISABLE;
 
+    /* Configure FDCAN kernel clock before computing timings */
+    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;
+    }
+    __HAL_RCC_FDCAN_CLK_ENABLE();
+
     ErrorCode = canardSTM32ComputeTimings(bitrate, &out_timings);
     if (ErrorCode != 1)
     {
@@ -188,23 +198,9 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate)
     hfdcan1.Init.ExtFiltersNbr = 1;
     hfdcan1.Init.TxFifoQueueElmtsNbr = 32;
     hfdcan1.Init.TxEventsNbr = 0;
-    hfdcan1.Init.TxBuffersNbr = 5;
+    hfdcan1.Init.TxBuffersNbr = 0;
     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
        
@@ -369,7 +365,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;                        // Standard SJW
+    out_timings->sjw = 1;
     out_timings->bs1 = (uint8_t)(solution.bs1);  // The HAL takes care of the 1 bs offset in the register so don't remove it here like AP does.
     out_timings->bs2 = (uint8_t)(solution.bs2);  // The HAL takes care of the 1 bs offset in the register so don't remove it here like AP does.
 
diff --git a/src/main/target/system_stm32h7xx.c b/src/main/target/system_stm32h7xx.c
index 56a53cefcfb..c55d3acd552 100644
--- a/src/main/target/system_stm32h7xx.c
+++ b/src/main/target/system_stm32h7xx.c
@@ -497,20 +497,30 @@ void SystemClock_Config(void)
     RCC_PeriphClkInit.I2c4ClockSelection = RCC_I2C4CLKSOURCE_D3PCLK1;
     HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphClkInit);
 
-#ifdef USE_SDCARD_SDIO
+#if defined(USE_SDCARD_SDIO) || defined(USE_DRONECAN)
     // PLL2M = HSE_VALUE / 1600000 pins the VCO input to exactly 1.6 MHz for any HSE.
-    // With N=500 this gives VCO=800 MHz: PLL2R/4=200 MHz (SDMMC), PLL2P/2=400 MHz.
+    // With N=500: VCO=800 MHz, PLL2R/4=200 MHz (SDMMC), PLL2Q/10=80 MHz (FDCAN).
     STATIC_ASSERT(HSE_VALUE % 1600000 == 0, HSE_not_a_multiple_of_1600000);
-    RCC_PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_SDMMC;
     RCC_PeriphClkInit.PLL2.PLL2M = HSE_VALUE / 1600000;
     RCC_PeriphClkInit.PLL2.PLL2N = 500;
-    RCC_PeriphClkInit.PLL2.PLL2P = 2; // 400Mhz
-    RCC_PeriphClkInit.PLL2.PLL2Q = 3; // 266Mhz - 133Mhz can be derived from this for for QSPI if flash chip supports the speed.
-    RCC_PeriphClkInit.PLL2.PLL2R = 4; // 200Mhz HAL LIBS REQUIRE 200MHZ SDMMC CLOCK, see HAL_SD_ConfigWideBusOperation, SDMMC_HSpeed_CLK_DIV, SDMMC_NSpeed_CLK_DIV
+    RCC_PeriphClkInit.PLL2.PLL2P = 2;
+    RCC_PeriphClkInit.PLL2.PLL2Q = 10; // 80 MHz for FDCAN
+    RCC_PeriphClkInit.PLL2.PLL2R = 4;  // 200 MHz for SDMMC
     RCC_PeriphClkInit.PLL2.PLL2RGE = RCC_PLL2VCIRANGE_0;
     RCC_PeriphClkInit.PLL2.PLL2VCOSEL = RCC_PLL2VCOWIDE;
     RCC_PeriphClkInit.PLL2.PLL2FRACN = 0;
+
+    uint32_t periphSel = 0;
+#ifdef USE_SDCARD_SDIO
+    periphSel |= RCC_PERIPHCLK_SDMMC;
     RCC_PeriphClkInit.SdmmcClockSelection = RCC_SDMMCCLKSOURCE_PLL2;
+#endif
+
+#ifdef USE_DRONECAN
+    periphSel |= RCC_PERIPHCLK_FDCAN;
+    RCC_PeriphClkInit.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL2;
+#endif
+    RCC_PeriphClkInit.PeriphClockSelection = periphSel;
     if (HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphClkInit) != HAL_OK) {
         Error_Handler();
     }

From 98d76dcf2684918cf3c11766f30b58e66b334ef4 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Thu, 28 May 2026 19:47:01 -0700
Subject: [PATCH 03/67] dronecan/h7: use system PLL2 clock for FDCAN; add
 KAKUTEH7WING CAN support

Remove redundant PeriphClkInitStruct clock config from canardSTM32CAN1_Init.
system_stm32h7xx.c already configures FDCAN to use PLL2Q (80 MHz) when
USE_DRONECAN is defined; duplicating it in the driver overwrites with PLL1.

Also add CAN1 pin definitions and USE_DRONECAN to KAKUTEH7WING target
(PD0/PD1, CAN1_STANDBY PD3 disabled by default).
---
 .../libcanard/canard_stm32h7xx_driver.c       | 24 +++----------------
 src/main/target/KAKUTEH7WING/target.h         |  6 +++++
 2 files changed, 9 insertions(+), 21 deletions(-)

diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index 7f2697eb66c..33aad806b6d 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -142,24 +142,15 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) {
   */
 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.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;
@@ -167,20 +158,11 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate)
     hfdcan1.Init.TransmitPause = DISABLE;
     hfdcan1.Init.ProtocolException = DISABLE;
 
-    /* Configure FDCAN kernel clock before computing timings */
-    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;
-    }
     __HAL_RCC_FDCAN_CLK_ENABLE();
 
-    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;
     }
 
diff --git a/src/main/target/KAKUTEH7WING/target.h b/src/main/target/KAKUTEH7WING/target.h
index 7626b997754..9601a4c77fe 100644
--- a/src/main/target/KAKUTEH7WING/target.h
+++ b/src/main/target/KAKUTEH7WING/target.h
@@ -154,6 +154,12 @@
 #define SERIALRX_PROVIDER   SERIALRX_SBUS
 #define SERIALRX_UART       SERIAL_PORT_USART6
 
+// *************** CANBUS ****************************
+#define USE_DRONECAN
+#define CAN1_RX                 PD0
+#define CAN1_TX                 PD1
+// #define CAN1_STANDBY            PD3
+
 // *************** ADC *****************************
 #define USE_ADC
 #define ADC_INSTANCE                ADC1

From b165d3963728b0c87b0b263a4272ef909c04d1bb Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Thu, 28 May 2026 20:00:26 -0700
Subject: [PATCH 04/67] dronecan/h7: remove BusOff/ErrorPassive LOG_DEBUG spam
 from GetProtocolStatus

---
 .../drivers/dronecan/libcanard/canard_stm32h7xx_driver.c     | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index 33aad806b6d..a9664885341 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -125,12 +125,9 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) {
     }
 
 	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;
 }
@@ -360,8 +357,6 @@ void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){
     HAL_FDCAN_GetProtocolStatus(&hfdcan1, &protocolStatus);
     pProtocolStat->BusOff = protocolStatus.BusOff;
     pProtocolStat->ErrorPassive = protocolStatus.ErrorPassive;
-    LOG_DEBUG(CAN, "BusOff: %lu", protocolStatus.BusOff);
-    LOG_DEBUG(CAN, "ErrorPassive: %lu", protocolStatus.ErrorPassive);
 }
 
 int32_t canardSTM32GetRxFifoFillLevel(void){

From d3cc8e4b848210ea42de1fb1325712706bde5951 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Thu, 28 May 2026 20:44:20 -0700
Subject: [PATCH 05/67] dronecan/h7: fix FDCAN timing clock source and restore
 SJW=3

Use HAL_RCCEx_GetPeriphCLKFreq(RCC_PERIPHCLK_FDCAN) instead of
HAL_RCC_GetPCLK1Freq() for bit timing calculation. FDCAN is clocked
from PLL2Q (80 MHz) configured in system_stm32h7xx.c; using PCLK1
(100 MHz) produced a ~25% baud rate error causing immediate bus-off.

Restore SJW to 3 for better synchronisation tolerance.
---
 src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index a9664885341..67fb6e3cd27 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -239,7 +239,7 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi
     /*
      * Hardware configuration
      */
-    const uint32_t pclk = HAL_RCC_GetPCLK1Freq();
+    const uint32_t pclk = HAL_RCCEx_GetPeriphCLKFreq(RCC_PERIPHCLK_FDCAN);
 
     static const int MaxBS1 = 16;
     static const int MaxBS2 = 8;
@@ -344,7 +344,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 = 1;
+    out_timings->sjw = 3;
     out_timings->bs1 = (uint8_t)(solution.bs1);  // The HAL takes care of the 1 bs offset in the register so don't remove it here like AP does.
     out_timings->bs2 = (uint8_t)(solution.bs2);  // The HAL takes care of the 1 bs offset in the register so don't remove it here like AP does.
 

From f294f3f6b3aa2ce7ee8342ffc8217e41b4235dc9 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Thu, 28 May 2026 22:37:26 -0700
Subject: [PATCH 06/67] dronecan/h7: remove LOG_DEBUG spam and fix PLL2 VCI
 range

Remove high-frequency LOG_DEBUG messages from GNSS Fix/Fix2/Auxiliary
handlers, onTransferReceived, dronecanInit, and gps_dronecan HDOP path
that fired at 25 Hz and flooded the log.

Fix PLL2 VCO input to target 1.6 MHz (PLL2M = HSE/1600000, PLL2N = 500)
rather than 2.0 MHz, keeping the operating point clearly within
VCIRANGE_0 (1-2 MHz) as the original SDCARD-only code did with PLL2M=5.
VCO output remains 800 MHz; FDCAN (80 MHz via PLL2Q=10) and SDMMC
(200 MHz via PLL2R=4) outputs are unchanged.
---
 src/main/drivers/dronecan/dronecan.c | 12 ------------
 src/main/io/gps_dronecan.c           |  2 --
 2 files changed, 14 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 42e0a3c3abf..8f5ce0b6c6c 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -92,7 +92,6 @@ void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) {
 		return;
 	}
     dronecanGPSReceiveGNSSAuxiliary(&gnssAuxiliary);
-    LOG_DEBUG(CAN, "GNSS Auxiliary: Sats=%d HDOP=%.1f", gnssAuxiliary.sats_used, (double)gnssAuxiliary.hdop);
 }
 
 void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) {
@@ -104,7 +103,6 @@ void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) {
 		return;
 	}
     dronecanGPSReceiveGNSSFix(&gnssFix);
-    LOG_DEBUG(CAN, "GNSS Fix received");
 }
 
 void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) {
@@ -116,7 +114,6 @@ void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) {
 		return;
 	}
     dronecanGPSReceiveGNSSFix2(&gnssFix2);
-    LOG_DEBUG(CAN, "GNSS Fix2 received");
 }
 
 void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) {
@@ -127,7 +124,6 @@ void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) {
 		LOG_DEBUG(CAN, "RTCMStream decode failed");
 		return;
 	}
-    LOG_DEBUG(CAN, "GNSS RTCM");
 }
 
 void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
@@ -300,13 +296,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
 		switch (transfer->data_type_id) {
@@ -392,7 +381,6 @@ void process1HzTasks(timeUs_t timestamp_usec)
 
 void dronecanInit(void)
 {
-    LOG_DEBUG(CAN, "dronecan Init");
     uint32_t bitrate = 500000; // At least define 500000
 
     switch (dronecanConfig()->bitRateKbps){
diff --git a/src/main/io/gps_dronecan.c b/src/main/io/gps_dronecan.c
index 849b5fe59b7..717c106443d 100644
--- a/src/main/io/gps_dronecan.c
+++ b/src/main/io/gps_dronecan.c
@@ -152,11 +152,9 @@ void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 * pgnssF
     gpsSolDRV.groundCourse = RADIANS_TO_DECIDEGREES(groundCourse);
     // TODO where to get EPH gpsSolDRV.eph = gpsConstrainEPE(pgnssFix-> / 10);
     // TODO where to get EPV gpsSolDRV.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);
-    LOG_DEBUG(CAN, "Last HDOP %d", lastHDOP);
     if (pgnssFix2->pdop > 0){
         gpsSolDRV.hdop = gpsConstrainHDOP(pgnssFix2->pdop * 100); // Only update if valid.
     } else if((9999 > lastHDOP) && (lastHDOP > 0)) {
-        LOG_DEBUG(CAN, "Updating gpsSolDRV");
         gpsSolDRV.hdop = lastHDOP;
     }
     gpsSolDRV.flags.validVelNE = true;

From a1f4c99c583f1d27a83d8a29ef16e46ecfdf1907 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 07:57:53 -0700
Subject: [PATCH 07/67] dronecan: remove LOG_DEBUG spam from H7/F7 drivers and
 dronecan.c

Drop high-frequency and verbose-but-low-value LOG_DEBUG(CAN messages:
- dronecan.c: Battery Info (x2), GetNodeInfo, NodeStatus, TX success,
  RX loop, commented-out debug blocks
- canard_stm32h7xx_driver.c: timing computation intermediates
  (Baudrate, Max Quanta, Prescaler BS, Prescaler, Timings summary)
- canard_stm32f7xx_driver.c: same timing intermediates, TX success,
  In CAN Init, commented-out clock and RX blocks

Retain error-path messages (decode failed, TX/RX error, init failures)
and the single-line Prescaler/SJW/BS summary logged at init.
---
 src/main/drivers/dronecan/dronecan.c          |  8 ------
 .../libcanard/canard_stm32f7xx_driver.c       | 27 +------------------
 .../libcanard/canard_stm32h7xx_driver.c       |  7 -----
 3 files changed, 1 insertion(+), 41 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 8f5ce0b6c6c..bfa348b2f80 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -135,7 +135,6 @@ void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
 		return;
 	}
     dronecanBatterySensorReceiveInfo(&batteryInfo);
-    LOG_DEBUG(CAN, "Battery Info");
 }
 
 /*
@@ -144,8 +143,6 @@ 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.
 void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
-	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;
 
@@ -192,7 +189,6 @@ void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
 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;
@@ -335,7 +331,6 @@ void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) {
                 break;
             
             case UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID:
-                LOG_DEBUG(CAN, "Battery Info");
                 handle_BatteryInfo(ins, transfer);
                 break;
         }
@@ -353,7 +348,6 @@ 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
@@ -451,8 +445,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);
 
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
index dd7d3e1b357..b8de3a9c87e 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -164,7 +164,6 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) {
 
 	returnCode = HAL_CAN_AddTxMessage(&hcan1, &txHeader, txData, &txMailbox);
     if( returnCode == HAL_OK) {
-		// LOG_DEBUG(CAN, "Successfully sent message with id: %lu", tx_frame->id);
 		return 1;
 	}
 
@@ -222,21 +221,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate)
     // 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");
@@ -320,9 +305,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 +318,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,7 +334,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.
@@ -404,11 +384,8 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi
         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->sjw = 3;
     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.
 
@@ -419,8 +396,6 @@ 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);
-    // LOG_DEBUG(CAN, "BusOff: %lu", pProtocolStat->BusOff);
-    // LOG_DEBUG(CAN, "ErrorPassive: %lu", pProtocolStat->ErrorPassive);
 }
 
 int32_t canardSTM32GetRxFifoFillLevel(void){
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index 67fb6e3cd27..c161794edd6 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -256,8 +256,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 : 17;
-    LOG_DEBUG(CAN, "Baudrate: %lu", target_bitrate);
-    LOG_DEBUG(CAN, "Max Quanta per bit: %i", max_quanta_per_bit);
 
     static const int MaxSamplePointLocation = 900;
 
@@ -272,7 +270,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.
      */
@@ -289,7 +286,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.
@@ -340,9 +336,6 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi
         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;
     out_timings->bs1 = (uint8_t)(solution.bs1);  // The HAL takes care of the 1 bs offset in the register so don't remove it here like AP does.

From c21c7a29647d06d987ddd924d07440a9d10b6015 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 08:28:45 -0700
Subject: [PATCH 08/67] dronecan/h7: set SJW=1 for ISO 11898-1 conformance and
 verified 1Mbps operation

Tested at 1 Mbps on KAKUTEH7WING hardware and confirmed bus operational.
---
 src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index c161794edd6..83c3bbce172 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -337,7 +337,7 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi
     }
 
     out_timings->prescaler = (uint16_t)(prescaler);
-    out_timings->sjw = 3;
+    out_timings->sjw = 1;
     out_timings->bs1 = (uint8_t)(solution.bs1);  // The HAL takes care of the 1 bs offset in the register so don't remove it here like AP does.
     out_timings->bs2 = (uint8_t)(solution.bs2);  // The HAL takes care of the 1 bs offset in the register so don't remove it here like AP does.
 

From 2ea077eecc207301e48b3b7a63f3eaa47387a184 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 08:36:56 -0700
Subject: [PATCH 09/67] dronecan: set SJW=1 in F7 driver; clean up stale
 comments in H7/F7 drivers

Remove CubeMX boilerplate markers, commented-out dead code, and
development-time question comments from both drivers.
---
 .../libcanard/canard_stm32f7xx_driver.c       | 20 +++++--------------
 .../libcanard/canard_stm32h7xx_driver.c       | 17 ++++++++--------
 2 files changed, 13 insertions(+), 24 deletions(-)

diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
index b8de3a9c87e..3b1ce2c2936 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -182,14 +182,10 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) {
   */
 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;
@@ -218,17 +214,13 @@ 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;
-    canardSTM32GPIO_Init();  // Set up the pins for CAN and optional listen only mode
+    canardSTM32GPIO_Init();
     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;
@@ -263,9 +255,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
 
 
@@ -274,7 +266,7 @@ static void canardSTM32GPIO_Init(void)
     // 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?
+    IOConfigGPIO(IOGetByTag(IO_TAG(CAN1_STANDBY)), IOCFG_OUT_PP);
     IOLo(IOGetByTag(IO_TAG(CAN1_STANDBY)));
 #endif
 }
@@ -385,7 +377,7 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi
     }
 
     out_timings->prescaler = (uint16_t)(prescaler);
-    out_timings->sjw = 3;
+    out_timings->sjw = 1;
     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.
 
@@ -403,8 +395,6 @@ int32_t canardSTM32GetRxFifoFillLevel(void){
 }
 
 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
 }
 
 /*
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index 83c3bbce172..8b0044d173c 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -109,11 +109,11 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) {
 		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
+	TxHeader.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
+	TxHeader.BitRateSwitch = FDCAN_BRS_OFF;
+	TxHeader.FDFormat = FDCAN_CLASSIC_CAN;
+	TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
+	TxHeader.MessageMarker = 0;
     if (TxHeader.DataLength <= sizeof(TxData))
     {
 	    memcpy(TxData, tx_frame->data, TxHeader.DataLength);
@@ -188,7 +188,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;
@@ -215,9 +214,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
 
 
@@ -226,7 +225,7 @@ static void canardSTM32GPIO_Init(void)
     // 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?
+    IOConfigGPIO(IOGetByTag(IO_TAG(CAN1_STANDBY)), IOCFG_OUT_PP);
     IOLo(IOGetByTag(IO_TAG(CAN1_STANDBY)));
 #endif
 }

From 5e07d1de5784e60a95416c859f82bdda9123be79 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 09:00:52 -0700
Subject: [PATCH 10/67] dronecan: gate GPS fix handlers on GPS_DRONECAN
 provider

Fix: DroneCAN GNSS messages were being applied to gpsSolDRV regardless
of the configured GPS provider. Guard added in gps_dronecan.c where
it belongs, keeping CAN transport layer unaware of GPS config.
---
 src/main/io/gps_dronecan.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/io/gps_dronecan.c b/src/main/io/gps_dronecan.c
index 717c106443d..bbaf0de1d06 100644
--- a/src/main/io/gps_dronecan.c
+++ b/src/main/io/gps_dronecan.c
@@ -82,8 +82,7 @@ static uint8_t gpsMapFixType(uint8_t dronecanFixType)
 
 void dronecanGPSReceiveGNSSFix(const struct uavcan_equipment_gnss_Fix * pgnssFix)
 {
-    //const mspSensorGpsDataMessage_t * pkt = (const mspSensorGpsDataMessage_t *)bufferPtr;
-
+    if (gpsConfig()->provider != GPS_DRONECAN) return;
     gpsSolDRV.fixType   = gpsMapFixType(pgnssFix->status);
     gpsSolDRV.numSat    = pgnssFix->sats_used;
     gpsSolDRV.llh.lon   = pgnssFix->longitude_deg_1e8 / 10; // convert to deg_1e7
@@ -134,7 +133,7 @@ void dronecanGPSReceiveGNSSFix(const struct uavcan_equipment_gnss_Fix * pgnssFix
 
 void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 * pgnssFix2)
 {
-    //const mspSensorGpsDataMessage_t * pkt = (const mspSensorGpsDataMessage_t *)bufferPtr;
+    if (gpsConfig()->provider != GPS_DRONECAN) return;
 
     gpsSolDRV.fixType   = gpsMapFixType(pgnssFix2->status);
     gpsSolDRV.numSat    = pgnssFix2->sats_used;
@@ -185,6 +184,7 @@ void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 * pgnssF
 
 void dronecanGPSReceiveGNSSAuxiliary(const struct uavcan_equipment_gnss_Auxiliary * pgnssAux)
 {
+    if (gpsConfig()->provider != GPS_DRONECAN) return;
     UNUSED(pgnssAux);
     // No useful information I think...  Placeholder until after testing.
     lastVDOP = pgnssAux->vdop * 100;

From 173b909167f4e31a7779287bac9e02f07e96fe60 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 09:18:31 -0700
Subject: [PATCH 11/67] dronecan/f7: restore SJW=3 (hardware 4 tq); document
 register encoding difference

F7 bxCAN HAL writes SJW directly to BTR register where hardware adds 1,
so stored value 3 gives 4 tq. This wider SJW is needed for reliable bus
operation on F7 targets and is different from H7 where SJW=1 is actual tq.
---
 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 3b1ce2c2936..cd337c4ff60 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -377,7 +377,7 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi
     }
 
     out_timings->prescaler = (uint16_t)(prescaler);
-    out_timings->sjw = 1;
+    out_timings->sjw = 3;  // Register value: hardware SJW = sjw+1 = 4 tq. F7 bxCAN needs wider SJW than H7 FDCAN.
     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 dbe3ace5c28e5dab1c594db4e43082a032b310b6 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 09:58:23 -0700
Subject: [PATCH 12/67] dronecan: add STATE_DRONECAN_FAILED; set on CAN
 peripheral init failure

Prevents state machine from continuing in INIT state when the CAN
peripheral fails to initialize.
---
 src/main/drivers/dronecan/dronecan.c | 7 +++++--
 src/main/drivers/dronecan/dronecan.h | 3 ++-
 2 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index bfa348b2f80..e9189eac8f6 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -402,7 +402,7 @@ void dronecanInit(void)
     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
+        dronecanState = STATE_DRONECAN_FAILED;
         return;
     }  
     /*
@@ -484,7 +484,10 @@ void dronecanUpdate(timeUs_t currentTimeUs)
                 dronecanState = STATE_DRONECAN_NORMAL;
             }
             break;
-    
+
+        case STATE_DRONECAN_FAILED:
+            break;
+
     }
     
 }
diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h
index b0212ec692d..9f53a570337 100644
--- a/src/main/drivers/dronecan/dronecan.h
+++ b/src/main/drivers/dronecan/dronecan.h
@@ -14,7 +14,8 @@ typedef enum {
 typedef enum {
     STATE_DRONECAN_INIT,
     STATE_DRONECAN_NORMAL,
-    STATE_DRONECAN_BUS_OFF
+    STATE_DRONECAN_BUS_OFF,
+    STATE_DRONECAN_FAILED
 } dronecanState_e;
 
 #define DRONECAN_MAX_NODES 32 // Reasonably expected number of devices on the bus.  If this is regularly hit, we could go higher but it consumes more ram.

From 5f55bec04035eee18083feacc518dc2d52b3ce0e Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 10:01:49 -0700
Subject: [PATCH 13/67] dronecan: add FAILED to CLI state name array

Prevents out-of-bounds access when STATE_DRONECAN_FAILED is active.
---
 src/main/fc/cli.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 27713297c86..99a854b35fd 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -4200,7 +4200,7 @@ static void cliStatus(char *cmdline)
 #endif
 
 #ifdef USE_DRONECAN
-    static const char * const dronecanStateNames[] = {"INIT", "NORMAL", "BUS_OFF"};
+    static const char * const dronecanStateNames[] = {"INIT", "NORMAL", "BUS_OFF", "FAILED"};
     cliPrintLinef("DroneCAN: nodeID=%d, bitrate=%u kbps, status=%s, nodes=%d",                                                                                                                                                  
         dronecanConfig()->nodeID,                                                                                                                                                                                                     
         (unsigned)dronecanGetBitrateKbps(),                                                                                                                                                                                                     

From a8140401fb59dde99c0392548d690e47ff41a501 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 10:16:07 -0700
Subject: [PATCH 14/67] dronecan/h7: flush TX FIFO before clearing CCCR.INIT on
 bus-off recovery

Prevents stale pre-bus-off frames from storming the bus on recovery.
---
 src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index 8b0044d173c..870940487bc 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -356,7 +356,8 @@ int32_t canardSTM32GetRxFifoFillLevel(void){
 }
 
 void canardSTM32RecoverFromBusOff(void){
-    CLEAR_BIT(hfdcan1.Instance->CCCR, FDCAN_CCCR_INIT);  // Clear INIT bit to recover from Bus-Off
+    hfdcan1.Instance->TXBCR = 0xFFFFFFFFU;  // Cancel all pending TX requests before recovery
+    CLEAR_BIT(hfdcan1.Instance->CCCR, FDCAN_CCCR_INIT);
 }
 
 /*

From 3865228bded907f18b233b5633fd4c1552d3c36e Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 11:45:36 -0700
Subject: [PATCH 15/67] dronecan: disable AutoRetransmission on H7/F7 to
 prevent TX FIFO stall

With AutoRetransmission=ENABLE, frames that fail on a degraded bus
occupy FIFO slots indefinitely. All 32 slots fill, HAL_FDCAN_AddMessage
returns HAL_ERROR, and all outgoing traffic stalls permanently with no
indication until full bus-off. DroneCAN reliability is handled at the
application layer via periodic republishing.
---
 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 cd337c4ff60..4451177c0f3 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -202,7 +202,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate)
     hcan1.Init.TimeTriggeredMode = DISABLE;
     hcan1.Init.AutoBusOff = ENABLE;
     hcan1.Init.AutoWakeUp = DISABLE;
-    hcan1.Init.AutoRetransmission = ENABLE;
+    hcan1.Init.AutoRetransmission = DISABLE;  // ENABLE fills the TX FIFO on a degraded bus; DroneCAN reliability is handled at the application layer
     hcan1.Init.ReceiveFifoLocked = DISABLE;
     hcan1.Init.TransmitFifoPriority = DISABLE;
   
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index 870940487bc..74f984edf2d 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -151,7 +151,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 = ENABLE;
+    hfdcan1.Init.AutoRetransmission = DISABLE;  // ENABLE fills the 32-slot TX FIFO on a degraded bus; DroneCAN reliability is handled at the application layer
     hfdcan1.Init.TransmitPause = DISABLE;
     hfdcan1.Init.ProtocolException = DISABLE;
 

From 57f85367c5761489f5c252011401e1942f6fd06a Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 11:56:35 -0700
Subject: [PATCH 16/67] dronecan/f7: check canardSTM32ComputeTimings return
 value

Matches the H7 driver pattern. Previously the return value was silently
discarded; if timing computation failed, uninitialized stack bytes were
passed to HAL_CAN_Init.
---
 .../drivers/dronecan/libcanard/canard_stm32f7xx_driver.c    | 6 +++++-
 1 file changed, 5 insertions(+), 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 4451177c0f3..371ee9e0f8a 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -206,7 +206,11 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate)
     hcan1.Init.ReceiveFifoLocked = DISABLE;
     hcan1.Init.TransmitFifoPriority = DISABLE;
   
-    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;

From 8fa8233e040cd56a229624e85d11bea56adc8b3d Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 12:05:41 -0700
Subject: [PATCH 17/67] dronecan: increase bus-off recovery delay from 1ms to
 20ms

The H7 FDCAN 128x11 recessive-bit recovery sequence takes up to 11.264ms
at 125kbps. The 1ms delay was restarting the counter before it could
complete, preventing the node from ever exiting bus-off. 20ms gives safe
margin above worst-case and allows time to detect immediate re-entry.
---
 src/main/drivers/dronecan/dronecan.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index e9189eac8f6..092e0d70f34 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -475,7 +475,7 @@ void dronecanUpdate(timeUs_t currentTimeUs)
             break;
 
         case STATE_DRONECAN_BUS_OFF:
-            if(currentTimeUs > (busoffTimeUs + 1000)) { // Wait 1 mS
+            if(currentTimeUs > (busoffTimeUs + 20000)) { // Wait 20ms: worst-case 128x11 recovery is 11.264ms at 125kbps
                 canardSTM32RecoverFromBusOff();
                 busoffTimeUs = currentTimeUs;
             }

From af897d8e514b866e1da31a402ceea54c24433f70 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 13:15:55 -0700
Subject: [PATCH 18/67] dronecan: move GPS provider guard to dispatch layer in
 dronecan.c

Guard against non-DroneCAN GPS provider at the transport boundary
(handle_GNSS* functions) rather than in each leaf function in
gps_dronecan.c. Also adds the guard to handle_GNSSRCTMStream which
had none. Removes stale UNUSED(pgnssAux) and placeholder comment
from dronecanGPSReceiveGNSSAuxiliary.
---
 src/main/drivers/dronecan/dronecan.c | 4 ++++
 src/main/io/gps_dronecan.c           | 7 -------
 2 files changed, 4 insertions(+), 7 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 092e0d70f34..bd9d579c3a7 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -85,6 +85,7 @@ void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) {
 
 void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) {
 	UNUSED(ins);
+    if (gpsConfig()->provider != GPS_DRONECAN) return;
     struct uavcan_equipment_gnss_Auxiliary gnssAuxiliary;
 
 	if (uavcan_equipment_gnss_Auxiliary_decode(transfer, &gnssAuxiliary)) {
@@ -96,6 +97,7 @@ void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) {
 
 void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) {
 	UNUSED(ins);
+    if (gpsConfig()->provider != GPS_DRONECAN) return;
     struct uavcan_equipment_gnss_Fix gnssFix;
 
 	if (uavcan_equipment_gnss_Fix_decode(transfer, &gnssFix)) {
@@ -107,6 +109,7 @@ void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) {
 
 void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) {
 	UNUSED(ins);
+    if (gpsConfig()->provider != GPS_DRONECAN) return;
     struct uavcan_equipment_gnss_Fix2 gnssFix2;
 
 	if (uavcan_equipment_gnss_Fix2_decode(transfer, &gnssFix2)) {
@@ -118,6 +121,7 @@ void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) {
 
 void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) {
 	UNUSED(ins);
+    if (gpsConfig()->provider != GPS_DRONECAN) return;
     struct uavcan_equipment_gnss_RTCMStream gnssRTCMStream;
 
 	if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &gnssRTCMStream)) {
diff --git a/src/main/io/gps_dronecan.c b/src/main/io/gps_dronecan.c
index bbaf0de1d06..0c3acd20d77 100644
--- a/src/main/io/gps_dronecan.c
+++ b/src/main/io/gps_dronecan.c
@@ -82,7 +82,6 @@ static uint8_t gpsMapFixType(uint8_t dronecanFixType)
 
 void dronecanGPSReceiveGNSSFix(const struct uavcan_equipment_gnss_Fix * pgnssFix)
 {
-    if (gpsConfig()->provider != GPS_DRONECAN) return;
     gpsSolDRV.fixType   = gpsMapFixType(pgnssFix->status);
     gpsSolDRV.numSat    = pgnssFix->sats_used;
     gpsSolDRV.llh.lon   = pgnssFix->longitude_deg_1e8 / 10; // convert to deg_1e7
@@ -133,8 +132,6 @@ void dronecanGPSReceiveGNSSFix(const struct uavcan_equipment_gnss_Fix * pgnssFix
 
 void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 * pgnssFix2)
 {
-    if (gpsConfig()->provider != GPS_DRONECAN) return;
-
     gpsSolDRV.fixType   = gpsMapFixType(pgnssFix2->status);
     gpsSolDRV.numSat    = pgnssFix2->sats_used;
     gpsSolDRV.llh.lon   = pgnssFix2->longitude_deg_1e8 / 10; // convert to deg_1e7
@@ -184,11 +181,7 @@ void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 * pgnssF
 
 void dronecanGPSReceiveGNSSAuxiliary(const struct uavcan_equipment_gnss_Auxiliary * pgnssAux)
 {
-    if (gpsConfig()->provider != GPS_DRONECAN) return;
-    UNUSED(pgnssAux);
-    // No useful information I think...  Placeholder until after testing.
     lastVDOP = pgnssAux->vdop * 100;
     lastHDOP = pgnssAux->hdop * 100;
-    
 }
 #endif
\ No newline at end of file

From 5390248672fa19ba829531222e0e5e39a53f1058 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 13:49:16 -0700
Subject: [PATCH 19/67] dronecan: rate-limit protocol status check to 1Hz in
 NORMAL state
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

canardSTM32GetProtocolStatus() was called on every dronecanUpdate()
invocation (~500Hz) to detect bus-off. Moved into the existing 1Hz
task block — bus-off detection latency of up to 1s is acceptable.
Adds LOG_DEBUG to report BusOff and ErrorPassive flags each second
for bench diagnostics.
---
 src/main/drivers/dronecan/dronecan.c | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index bd9d579c3a7..e41ef75f3cf 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -469,12 +469,13 @@ void dronecanUpdate(timeUs_t currentTimeUs)
 		        next_1hz_service_at += 1000000ULL;
 		        process1HzTasks(currentTimeUs);
                 processCanardTxQueue();
-            }
 
-            canardSTM32GetProtocolStatus(&protocolStatus);
-            if(protocolStatus.BusOff != 0) {
-                dronecanState = STATE_DRONECAN_BUS_OFF;
-                busoffTimeUs = currentTimeUs;
+                canardSTM32GetProtocolStatus(&protocolStatus);
+                LOG_DEBUG(CAN, "CAN status: BusOff=%lu ErrorPassive=%lu", protocolStatus.BusOff, protocolStatus.ErrorPassive);
+                if (protocolStatus.BusOff != 0) {
+                    dronecanState = STATE_DRONECAN_BUS_OFF;
+                    busoffTimeUs = currentTimeUs;
+                }
             }
             break;
 

From da72c03d06967a293c5b24f80ee54c3d518310cc Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 15:08:50 -0700
Subject: [PATCH 20/67] dronecan/f7: implement RecoverFromBusOff to clear
 sticky ESR.BOFF flag

AutoBusOff=ENABLE handles the 128x11 recovery sequence automatically,
but ESR.BOFF is a sticky read-only flag that is NOT cleared when hardware
recovery completes. GetProtocolStatus() reads this flag, so the state
machine was permanently stuck in STATE_DRONECAN_BUS_OFF after any
bus-off event on F7 targets. Stop/Start re-enters init mode which
clears ESR.BOFF, allowing recovery detection to work correctly.
---
 .../drivers/dronecan/libcanard/canard_stm32f7xx_driver.c     | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
index 371ee9e0f8a..b7915d98edf 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -399,6 +399,11 @@ int32_t canardSTM32GetRxFifoFillLevel(void){
 }
 
 void canardSTM32RecoverFromBusOff(void){
+    // AutoBusOff=ENABLE handles the 128x11 recovery sequence automatically.
+    // Stop/Start re-enters init mode which clears the sticky ESR.BOFF flag
+    // so GetProtocolStatus() can detect recovery and return to NORMAL state.
+    HAL_CAN_Stop(&hcan1);
+    HAL_CAN_Start(&hcan1);
 }
 
 /*

From f1fa4c76357a69ff18fa3f21a9c58568d5e8a25d Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 15:15:13 -0700
Subject: [PATCH 21/67] dronecan/h7: fix static_assert comment for PLL2M HSE
 divisibility check
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Comment incorrectly stated '25MHz' as a supported HSE value — 25MHz
fails the assert. CMake always provides HSE_VALUE per-target via
-DHSE_VALUE=<n> so the stm32h7xx_hal_conf.h fallback of 25MHz is never
used. Current targets use 8MHz (default) or 16MHz (KAKUTEH7WING).
---
 src/main/target/system_stm32h7xx.c | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/main/target/system_stm32h7xx.c b/src/main/target/system_stm32h7xx.c
index c55d3acd552..eaf88057cfb 100644
--- a/src/main/target/system_stm32h7xx.c
+++ b/src/main/target/system_stm32h7xx.c
@@ -500,6 +500,9 @@ void SystemClock_Config(void)
 #if defined(USE_SDCARD_SDIO) || defined(USE_DRONECAN)
     // PLL2M = HSE_VALUE / 1600000 pins the VCO input to exactly 1.6 MHz for any HSE.
     // With N=500: VCO=800 MHz, PLL2R/4=200 MHz (SDMMC), PLL2Q/10=80 MHz (FDCAN).
+    // HSE_VALUE must be an exact multiple of 1600000. CMake sets it per-target via -DHSE_VALUE=<n>;
+    // current targets use 8MHz (÷5) and 16MHz (÷10). If adding a target with a non-multiple HSE,
+    // this assert will fire — choose a different VCO input frequency.
     STATIC_ASSERT(HSE_VALUE % 1600000 == 0, HSE_not_a_multiple_of_1600000);
     RCC_PeriphClkInit.PLL2.PLL2M = HSE_VALUE / 1600000;
     RCC_PeriphClkInit.PLL2.PLL2N = 500;

From ef0fe3ab1fa4cf39a4e9261b03bf03e2dead76b4 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 15:28:53 -0700
Subject: [PATCH 22/67] dronecan: rate-limit protocol status check in BUS_OFF
 state to 20ms cadence
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

GetProtocolStatus() was called every dronecanUpdate() cycle (~500Hz) in
BUS_OFF state. Moved inside the 20ms recovery timer block so it runs at
the same cadence as RecoverFromBusOff() — still detects recovery within
20ms but reduces MMIO reads from ~500/sec to ~50/sec.
---
 src/main/drivers/dronecan/dronecan.c | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index e41ef75f3cf..e4f40853db9 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -483,10 +483,10 @@ void dronecanUpdate(timeUs_t currentTimeUs)
             if(currentTimeUs > (busoffTimeUs + 20000)) { // Wait 20ms: worst-case 128x11 recovery is 11.264ms at 125kbps
                 canardSTM32RecoverFromBusOff();
                 busoffTimeUs = currentTimeUs;
-            }
-            canardSTM32GetProtocolStatus(&protocolStatus);
-            if(protocolStatus.BusOff == 0) {
-                dronecanState = STATE_DRONECAN_NORMAL;
+                canardSTM32GetProtocolStatus(&protocolStatus);
+                if(protocolStatus.BusOff == 0) {
+                    dronecanState = STATE_DRONECAN_NORMAL;
+                }
             }
             break;
 

From cca23b3d1b5ad7326c9df4e4d349424fb3932e8a Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 15:31:02 -0700
Subject: [PATCH 23/67] =?UTF-8?q?dronecan/f7:=20revert=20RecoverFromBusOff?=
 =?UTF-8?q?=20HAL=5FCAN=5FStop/Start=20=E2=80=94=20causes=20lockup?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

HAL_CAN_Stop/Start called from the scheduler context with CAN interrupts
active caused a full FC lockup. Reverted to empty stub pending investigation
of a safe mechanism to clear the sticky ESR.BOFF flag on F7.
---
 .../drivers/dronecan/libcanard/canard_stm32f7xx_driver.c   | 7 +++----
 1 file changed, 3 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 b7915d98edf..2f3bc545d94 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -400,10 +400,9 @@ int32_t canardSTM32GetRxFifoFillLevel(void){
 
 void canardSTM32RecoverFromBusOff(void){
     // AutoBusOff=ENABLE handles the 128x11 recovery sequence automatically.
-    // Stop/Start re-enters init mode which clears the sticky ESR.BOFF flag
-    // so GetProtocolStatus() can detect recovery and return to NORMAL state.
-    HAL_CAN_Stop(&hcan1);
-    HAL_CAN_Start(&hcan1);
+    // TODO: ESR.BOFF is a sticky flag not cleared by AutoBusOff recovery.
+    // HAL_CAN_Stop/Start would clear it but caused lockups on F7 when called
+    // from the scheduler context with CAN interrupts active. Needs investigation.
 }
 
 /*

From 5a47b87c193f0ad1f537262f7b3b8b90a99ce84e Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 15:38:27 -0700
Subject: [PATCH 24/67] dronecan: only log CAN status when BusOff or
 ErrorPassive is non-zero

Unconditional 1Hz LOG_DEBUG was flooding the bootlog with healthy status
messages. Now only logs when an error condition is actually present.
---
 src/main/drivers/dronecan/dronecan.c | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index e4f40853db9..4bc8079283e 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -471,7 +471,9 @@ void dronecanUpdate(timeUs_t currentTimeUs)
                 processCanardTxQueue();
 
                 canardSTM32GetProtocolStatus(&protocolStatus);
-                LOG_DEBUG(CAN, "CAN status: BusOff=%lu ErrorPassive=%lu", protocolStatus.BusOff, protocolStatus.ErrorPassive);
+                if (protocolStatus.BusOff != 0 || protocolStatus.ErrorPassive != 0) {
+                    LOG_DEBUG(CAN, "CAN status: BusOff=%lu ErrorPassive=%lu", protocolStatus.BusOff, protocolStatus.ErrorPassive);
+                }
                 if (protocolStatus.BusOff != 0) {
                     dronecanState = STATE_DRONECAN_BUS_OFF;
                     busoffTimeUs = currentTimeUs;

From fe8d8594be780cd59acfdaf74f3da535e4ffee59 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 15:44:35 -0700
Subject: [PATCH 25/67] dronecan/f7: remove TX failure log spam
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

HAL_CAN_AddTxMessage returns non-OK when all mailboxes are busy — a
normal transient condition at startup. The log was noise. Matches the
H7 driver which already handles this path silently.
---
 src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c | 4 +---
 1 file changed, 1 insertion(+), 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 2f3bc545d94..6ccbca40122 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -167,9 +167,7 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) {
 		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
+	// TX failed (mailboxes full or bus error) - return 0 to signal retry needed
 	return 0;
 }
 

From 69f657069480ffd232c0551a25ed48aef9743c65 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 15:50:17 -0700
Subject: [PATCH 26/67] dronecan/gps: guard GNSSAuxiliary DOP fields against
 NaN and overflow

DroneCAN float16 optional fields encode NaN when unpopulated. Without a
guard, NaN * 100 converts to 0 on Cortex-M (ARM VCVT saturation),
permanently blocking the HDOP fallback path. Also passes values through
gpsConstrainHDOP() to prevent uint16_t overflow for extreme DOP values.
---
 src/main/io/gps_dronecan.c | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/main/io/gps_dronecan.c b/src/main/io/gps_dronecan.c
index 0c3acd20d77..c03125a7b97 100644
--- a/src/main/io/gps_dronecan.c
+++ b/src/main/io/gps_dronecan.c
@@ -181,7 +181,13 @@ void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 * pgnssF
 
 void dronecanGPSReceiveGNSSAuxiliary(const struct uavcan_equipment_gnss_Auxiliary * pgnssAux)
 {
-    lastVDOP = pgnssAux->vdop * 100;
-    lastHDOP = pgnssAux->hdop * 100;
+    // DroneCAN float16 optional fields encode NaN when unpopulated; guard before use.
+    // gpsConstrainHDOP clamps to 9999 preventing uint16_t overflow for extreme DOP values.
+    if (!isnan(pgnssAux->hdop)) {
+        lastHDOP = gpsConstrainHDOP((uint32_t)(pgnssAux->hdop * 100));
+    }
+    if (!isnan(pgnssAux->vdop)) {
+        lastVDOP = gpsConstrainHDOP((uint32_t)(pgnssAux->vdop * 100));
+    }
 }
 #endif
\ No newline at end of file

From e6e2c7c8b33e6ce37a6a5ab88c073415d055c652 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 15:54:19 -0700
Subject: [PATCH 27/67] =?UTF-8?q?dronecan/gps:=20remove=20lastVDOP=20?=
 =?UTF-8?q?=E2=80=94=20no=20vdop=20field=20in=20gpsSol=20and=20not=20EPV-c?=
 =?UTF-8?q?ompatible?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

gpsSolDRV has hdop but no vdop field. VDOP and EPV are not interchangeable
(different units, conversion requires receiver UERE). lastVDOP was a dead
store with no valid consumer.
---
 src/main/io/gps_dronecan.c | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/src/main/io/gps_dronecan.c b/src/main/io/gps_dronecan.c
index c03125a7b97..07e595cd208 100644
--- a/src/main/io/gps_dronecan.c
+++ b/src/main/io/gps_dronecan.c
@@ -56,7 +56,6 @@
 
 static bool newDataReady;
 static uint16_t lastHDOP = 9999;
-static uint16_t lastVDOP = 9999;
 
 void gpsRestartDronecan(void)
 {
@@ -186,8 +185,5 @@ void dronecanGPSReceiveGNSSAuxiliary(const struct uavcan_equipment_gnss_Auxiliar
     if (!isnan(pgnssAux->hdop)) {
         lastHDOP = gpsConstrainHDOP((uint32_t)(pgnssAux->hdop * 100));
     }
-    if (!isnan(pgnssAux->vdop)) {
-        lastVDOP = gpsConstrainHDOP((uint32_t)(pgnssAux->vdop * 100));
-    }
 }
 #endif
\ No newline at end of file

From 3aa922f7394cd169677e966955337d5c353580aa Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 15:56:27 -0700
Subject: [PATCH 28/67] dronecan: add STATE_DRONECAN_COUNT sentinel and assert
 dronecanStateNames size
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Guards the CLI state name array against future enum additions — if a new
state is added without updating the array, the build fails immediately.
---
 src/main/drivers/dronecan/dronecan.h | 3 ++-
 src/main/fc/cli.c                    | 1 +
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h
index 9f53a570337..202011b0483 100644
--- a/src/main/drivers/dronecan/dronecan.h
+++ b/src/main/drivers/dronecan/dronecan.h
@@ -15,7 +15,8 @@ typedef enum {
     STATE_DRONECAN_INIT,
     STATE_DRONECAN_NORMAL,
     STATE_DRONECAN_BUS_OFF,
-    STATE_DRONECAN_FAILED
+    STATE_DRONECAN_FAILED,
+    STATE_DRONECAN_COUNT
 } dronecanState_e;
 
 #define DRONECAN_MAX_NODES 32 // Reasonably expected number of devices on the bus.  If this is regularly hit, we could go higher but it consumes more ram.
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 99a854b35fd..ac55eb165ef 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -4201,6 +4201,7 @@ static void cliStatus(char *cmdline)
 
 #ifdef USE_DRONECAN
     static const char * const dronecanStateNames[] = {"INIT", "NORMAL", "BUS_OFF", "FAILED"};
+    STATIC_ASSERT(ARRAY_LENGTH(dronecanStateNames) == STATE_DRONECAN_COUNT, dronecanStateNames_size_mismatch);
     cliPrintLinef("DroneCAN: nodeID=%d, bitrate=%u kbps, status=%s, nodes=%d",                                                                                                                                                  
         dronecanConfig()->nodeID,                                                                                                                                                                                                     
         (unsigned)dronecanGetBitrateKbps(),                                                                                                                                                                                                     

From d2be273f5b5638ceff9b89bfa55ae4bfef56281f Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 15:58:14 -0700
Subject: [PATCH 29/67] dronecan: handle STATE_DRONECAN_COUNT in switch to
 satisfy -Werror=switch

---
 src/main/drivers/dronecan/dronecan.c | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 4bc8079283e..9d99a0c71a1 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -495,6 +495,9 @@ void dronecanUpdate(timeUs_t currentTimeUs)
         case STATE_DRONECAN_FAILED:
             break;
 
+        case STATE_DRONECAN_COUNT:
+            break;
+
     }
     
 }

From 9747c8eaadafb7f0983f614ec55f30314538c2d2 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 17:14:51 -0700
Subject: [PATCH 30/67] dronecan: fix ARRAYLEN macro name in dronecanStateNames
 assert

---
 src/main/fc/cli.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index ac55eb165ef..eb4454ac564 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -4201,7 +4201,7 @@ static void cliStatus(char *cmdline)
 
 #ifdef USE_DRONECAN
     static const char * const dronecanStateNames[] = {"INIT", "NORMAL", "BUS_OFF", "FAILED"};
-    STATIC_ASSERT(ARRAY_LENGTH(dronecanStateNames) == STATE_DRONECAN_COUNT, dronecanStateNames_size_mismatch);
+    STATIC_ASSERT(ARRAYLEN(dronecanStateNames) == STATE_DRONECAN_COUNT, dronecanStateNames_size_mismatch);
     cliPrintLinef("DroneCAN: nodeID=%d, bitrate=%u kbps, status=%s, nodes=%d",                                                                                                                                                  
         dronecanConfig()->nodeID,                                                                                                                                                                                                     
         (unsigned)dronecanGetBitrateKbps(),                                                                                                                                                                                                     

From 97c939ba7f73686386bfceeaddb5414dc4f027c9 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 29 May 2026 17:42:45 -0700
Subject: [PATCH 31/67] dronecan: clamp state index before dronecanStateNames
 lookup in CLI

Guards against out-of-bounds read if dronecanState is ever corrupted
to STATE_DRONECAN_COUNT or beyond.
---
 src/main/fc/cli.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index eb4454ac564..d69e4671b37 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -4205,7 +4205,7 @@ static void cliStatus(char *cmdline)
     cliPrintLinef("DroneCAN: nodeID=%d, bitrate=%u kbps, status=%s, nodes=%d",                                                                                                                                                  
         dronecanConfig()->nodeID,                                                                                                                                                                                                     
         (unsigned)dronecanGetBitrateKbps(),                                                                                                                                                                                                     
-        dronecanStateNames[dronecanGetState()],                                                                                                                                                                                       
+        dronecanStateNames[MIN(dronecanGetState(), STATE_DRONECAN_COUNT - 1)],                                                                                                                                                                                       
         dronecanGetNodeCount()                                                         
     );
 #endif

From 03b16143589061fc22115a55b4146babcf74ad97 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Sat, 30 May 2026 17:22:45 -0700
Subject: [PATCH 32/67] dronecan/f7: document why RecoverFromBusOff is a no-op
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

ABOM (CAN_MCR bit 6) is enabled in canardSTM32CAN1_Init. Per RM0410
ss40.7.6, with ABOM=1 the hardware manages the full bus-off recovery
sequence automatically: after 128x11 recessive bits it cycles INRQ and
clears ESR.BOFF without any software intervention required.

Removes an incorrect TODO asserting ESR.BOFF is sticky — it is a
hardware-managed status bit that clears when bus-off state is left.
---
 .../drivers/dronecan/libcanard/canard_stm32f7xx_driver.c  | 8 ++++----
 1 file changed, 4 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 6ccbca40122..d630e9ab0ef 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -397,10 +397,10 @@ int32_t canardSTM32GetRxFifoFillLevel(void){
 }
 
 void canardSTM32RecoverFromBusOff(void){
-    // AutoBusOff=ENABLE handles the 128x11 recovery sequence automatically.
-    // TODO: ESR.BOFF is a sticky flag not cleared by AutoBusOff recovery.
-    // HAL_CAN_Stop/Start would clear it but caused lockups on F7 when called
-    // from the scheduler context with CAN interrupts active. Needs investigation.
+    // No-op: ABOM (CAN_MCR bit 6) is set in canardSTM32CAN1_Init, so hardware
+    // manages the full bus-off recovery sequence automatically. After 128x11
+    // recessive bits, hardware cycles INRQ and clears ESR.BOFF without software
+    // intervention. See RM0410 ss40.7.6 and CAN_MCR.ABOM, CAN_ESR.BOFF.
 }
 
 /*

From f916fc1be75274bc4f79ce178dab9ac81c68ee7a Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Sat, 30 May 2026 20:25:06 -0700
Subject: [PATCH 33/67] dronecan: fix sign-compare in CLI state name clamp

Cast both MIN() arguments to int to avoid -Werror=sign-compare between
dronecanState_e (unsigned enum) and STATE_DRONECAN_COUNT - 1 (int).
---
 src/main/fc/cli.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index d69e4671b37..7f281a28a46 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -4205,7 +4205,7 @@ static void cliStatus(char *cmdline)
     cliPrintLinef("DroneCAN: nodeID=%d, bitrate=%u kbps, status=%s, nodes=%d",                                                                                                                                                  
         dronecanConfig()->nodeID,                                                                                                                                                                                                     
         (unsigned)dronecanGetBitrateKbps(),                                                                                                                                                                                                     
-        dronecanStateNames[MIN(dronecanGetState(), STATE_DRONECAN_COUNT - 1)],                                                                                                                                                                                       
+        dronecanStateNames[MIN((int)dronecanGetState(), (int)STATE_DRONECAN_COUNT - 1)],                                                                                                                                                                                       
         dronecanGetNodeCount()                                                         
     );
 #endif

From 48c1d7c2e39bdbe1faac1fe1583d98e3e1b598a3 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Sat, 30 May 2026 20:28:27 -0700
Subject: [PATCH 34/67] dronecan: fix %lu format specifier for uint32_t fields

clang treats uint32_t as unsigned int, not unsigned long. Use %u to
match the actual type of BusOff and ErrorPassive in canardProtocolStatus_t.
---
 src/main/drivers/dronecan/dronecan.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 9d99a0c71a1..5994df1c300 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -472,7 +472,7 @@ void dronecanUpdate(timeUs_t currentTimeUs)
 
                 canardSTM32GetProtocolStatus(&protocolStatus);
                 if (protocolStatus.BusOff != 0 || protocolStatus.ErrorPassive != 0) {
-                    LOG_DEBUG(CAN, "CAN status: BusOff=%lu ErrorPassive=%lu", protocolStatus.BusOff, protocolStatus.ErrorPassive);
+                    LOG_DEBUG(CAN, "CAN status: BusOff=%u ErrorPassive=%u", protocolStatus.BusOff, protocolStatus.ErrorPassive);
                 }
                 if (protocolStatus.BusOff != 0) {
                     dronecanState = STATE_DRONECAN_BUS_OFF;

From 60b8f452df35295ec53ad6696cb5fcd48a7d314f Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Sat, 30 May 2026 22:24:47 -0700
Subject: [PATCH 35/67] fix: use PRIu32 for uint32_t format specifiers in
 dronecan LOG_DEBUG

---
 src/main/drivers/dronecan/dronecan.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 5994df1c300..e79144a8c84 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -472,7 +472,7 @@ void dronecanUpdate(timeUs_t currentTimeUs)
 
                 canardSTM32GetProtocolStatus(&protocolStatus);
                 if (protocolStatus.BusOff != 0 || protocolStatus.ErrorPassive != 0) {
-                    LOG_DEBUG(CAN, "CAN status: BusOff=%u ErrorPassive=%u", protocolStatus.BusOff, protocolStatus.ErrorPassive);
+                    LOG_DEBUG(CAN, "CAN status: BusOff=%" PRIu32 " ErrorPassive=%" PRIu32, protocolStatus.BusOff, protocolStatus.ErrorPassive);
                 }
                 if (protocolStatus.BusOff != 0) {
                     dronecanState = STATE_DRONECAN_BUS_OFF;

From 9c1b352e3fa7cf7cea854cb5914e07d3d9bfe50e Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Sun, 31 May 2026 20:04:59 -0700
Subject: [PATCH 36/67] fix(dronecan): send elapsed ms since last seen in
 MSP2_INAV_DRONECAN_NODES

Previously last_seen_ms was the raw millis() timestamp when the node was
last heard from, which equals FC uptime for active nodes. Configurators
had no way to compute elapsed time without knowing current FC millis().

Now sends millis() - last_seen_ms so the field means "ms since this node
was last heard from". Unsigned subtraction handles millis() wraparound
correctly.
---
 src/main/fc/fc_msp.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index e5cfe7e81e8..2053f123a8c 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1899,7 +1899,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
                     .nodeID      = node->nodeID,
                     .health      = node->health,
                     .mode        = node->mode,
-                    .last_seen_ms = node->last_seen_ms,
+                    .last_seen_ms = millis() - node->last_seen_ms,
                 }, sizeof(dronecanNodeStatus_t));
             }
         }

From 1d5338316c5bddf3be00a27a33fc5d2e369f6a10 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Sun, 31 May 2026 20:41:17 -0700
Subject: [PATCH 37/67] fix(dronecan): send elapsed ms since last seen in
 MSP2_INAV_DRONECAN_NODE_INFO

Apply the same fix as MSP2_INAV_DRONECAN_NODES: send millis() - last_seen_ms
so the field means elapsed time since last heard from the node.
---
 src/main/fc/fc_msp.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 2053f123a8c..9da0b461562 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -4608,7 +4608,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
                     sbufWriteU8(dst, node->mode);
                     sbufWriteU32(dst, node->uptime_sec);
                     sbufWriteU16(dst, node->vendor_status_code);
-                    sbufWriteU32(dst, node->last_seen_ms);
+                    sbufWriteU32(dst, millis() - node->last_seen_ms);
                     sbufWriteU8(dst, node->name_len);
                     sbufWriteDataSafe(dst, node->name, 32);
                     found = true;

From f52241c4a716f8b207b5833f5f489295b58c01fe Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Thu, 11 Jun 2026 21:02:37 -0700
Subject: [PATCH 38/67] dronecan/h7: TX queue mode, depth 3, ISR pump, NVIC
 masking
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Switch FDCAN from FIFO to Queue mode so hardware transmits highest-priority
frame first, matching libcanard's scheduler. Reduce staging depth 32→3 so
libcanard's queue remains authoritative between poll cycles.

Add HAL_FDCAN_TxBufferCompleteCallback ISR pump to refill the hardware slot
immediately on TX complete. Wrap all canardBroadcast, canardRequestOrRespond,
and canardCleanupStaleTransfers call sites with NVIC_DisableIRQ/EnableIRQ on
FDCAN1_IT1 to prevent races between the ISR and main task on libcanard's queue.
Masking helpers are no-ops on non-H7 targets.
---
 src/main/drivers/dronecan/dronecan.c          | 40 ++++++++++++++++---
 .../libcanard/canard_stm32h7xx_driver.c       | 11 ++++-
 2 files changed, 43 insertions(+), 8 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index e79144a8c84..31daec2a13f 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -42,6 +42,14 @@ static dronecanState_e dronecanState = STATE_DRONECAN_INIT;
 static uint8_t activeNodeCount = 0;
 static dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES];
 
+#if defined(STM32H7)
+static inline void dronecanMaskTxISR(void)   { NVIC_DisableIRQ(FDCAN1_IT1_IRQn); }
+static inline void dronecanUnmaskTxISR(void) { NVIC_EnableIRQ(FDCAN1_IT1_IRQn); }
+#else
+static inline void dronecanMaskTxISR(void)   {}
+static inline void dronecanUnmaskTxISR(void) {}
+#endif
+
 // 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
 
@@ -173,6 +181,7 @@ void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
 
 	uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer);
 
+    dronecanMaskTxISR();
 	canardRequestOrRespond(ins,
 						   transfer->source_node_id,
 						   UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE,
@@ -182,6 +191,7 @@ void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
 						   CanardResponse,
 						   &buffer[0],
 						   total_size);
+    dronecanUnmaskTxISR();
 }
 
 // Canard Senders
@@ -214,6 +224,7 @@ void send_NodeStatus(void) {
     // loss
     static uint8_t transfer_id;
 
+    dronecanMaskTxISR();
     canardBroadcast(&canard,
                     UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
                     UAVCAN_PROTOCOL_NODESTATUS_ID,
@@ -221,7 +232,8 @@ void send_NodeStatus(void) {
                     CANARD_TRANSFER_PRIORITY_LOW,
                     buffer,
                     len);
-    // PrintCanStatus();
+    dronecanUnmaskTxISR();
+
 }
 
 // Canard Util
@@ -341,7 +353,6 @@ void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) {
 	}
 }
 
-
 void processCanardTxQueue(void) {
 	// Transmitting
 	for (const CanardCANFrame *tx_frame ; (tx_frame = canardPeekTxQueue(&canard)) != NULL;)
@@ -358,9 +369,24 @@ void processCanardTxQueue(void) {
 			break;
 		}
 	}
-
 }
 
+static void processCanardTxQueueSafe(void) {
+    dronecanMaskTxISR();
+    processCanardTxQueue();
+    dronecanUnmaskTxISR();
+}  
+
+#if defined(STM32H7)
+void HAL_FDCAN_TxBufferCompleteCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t BufferIndexes)
+{
+    UNUSED(hfdcan);
+    UNUSED(BufferIndexes);
+    processCanardTxQueue();
+}   
+#endif
+
+
 /*
   This function is called at 1 Hz rate from the main loop.
 */
@@ -369,7 +395,9 @@ void process1HzTasks(timeUs_t timestamp_usec)
    /*
       Purge transfers that are no longer transmitted. This can free up some memory
     */
+    dronecanMaskTxISR();
     canardCleanupStaleTransfers(&canard, timestamp_usec);
+    dronecanUnmaskTxISR();
 
     /*
       Transmit the node status message
@@ -445,7 +473,7 @@ void dronecanUpdate(timeUs_t currentTimeUs)
             break;
 
         case STATE_DRONECAN_NORMAL:
-            processCanardTxQueue();
+            processCanardTxQueueSafe();
 
              for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--)
              {
@@ -462,13 +490,13 @@ void dronecanUpdate(timeUs_t currentTimeUs)
              }
             // 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();
+            processCanardTxQueueSafe();
 
             if (currentTimeUs >= next_1hz_service_at)
             {
 		        next_1hz_service_at += 1000000ULL;
 		        process1HzTasks(currentTimeUs);
-                processCanardTxQueue();
+                processCanardTxQueueSafe();
 
                 canardSTM32GetProtocolStatus(&protocolStatus);
                 if (protocolStatus.BusOff != 0 || protocolStatus.ErrorPassive != 0) {
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index 74f984edf2d..d0ecdd8ed2d 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -175,10 +175,10 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate)
     hfdcan1.Init.RxBufferSize = FDCAN_DATA_BYTES_8;
     hfdcan1.Init.StdFiltersNbr = 0;
     hfdcan1.Init.ExtFiltersNbr = 1;
-    hfdcan1.Init.TxFifoQueueElmtsNbr = 32;
+    hfdcan1.Init.TxFifoQueueElmtsNbr = 3;
     hfdcan1.Init.TxEventsNbr = 0;
     hfdcan1.Init.TxBuffersNbr = 0;
-    hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION;
+    hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_QUEUE_OPERATION;
     hfdcan1.Init.TxElmtSize = FDCAN_DATA_BYTES_8;
 
     canardSTM32GPIO_Init();  // Set up the pins for CAN and optional listen only mode
@@ -201,6 +201,13 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate)
         LOG_ERROR(CAN, "Failed to Start");
         return -CANARD_ERROR_INTERNAL;
     }
+
+    if (HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_TX_COMPLETE,
+        FDCAN_TX_BUFFER0 | FDCAN_TX_BUFFER1 | FDCAN_TX_BUFFER2)) {
+        LOG_ERROR(CAN, "Failed to activate interrupt notification");
+        return -CANARD_ERROR_INTERNAL;
+    }
+
     return CANARD_OK;
 }
 

From 8d3489716a1d29a77c5c6694b522f393f3254279 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Thu, 11 Jun 2026 21:38:04 -0700
Subject: [PATCH 39/67] dronecan: F7 ISR-driven TX, NVIC masking, TEC/REC/LEC
 counters, cliDronecan

Phase 2 of the combined DroneCAN driver rework (folds in #11560 content):

F7 bxCAN ISR-driven TX refill:
- Add NVIC_PRIO_CAN=4 to nvic.h (shared by H7 and F7)
- Enable CAN_IT_TX_MAILBOX_EMPTY and CAN1_TX_IRQn at NVIC_PRIO_CAN
- Add HAL_CAN_TxMailbox{0,1,2}CompleteCallback ISR pumps in dronecan.c
- Wire dronecanMaskTxISR/UnmaskTxISR to NVIC_DisableIRQ/EnableIRQ(CAN1_TX_IRQn)
- Add #else branch for SITL/AT32 (was missing, would break SITL build)

TEC/REC/LEC error counters:
- Extend canardProtocolStatus_t with tec, rec, lec fields
- F7: populate from ESR register (bits 23:16, 31:24, 6:4)
- H7: populate from ECR register (bits 22:16, 14:8) + PSR.LastErrorCode

Typo fix: canardSTM32Recieve -> canardSTM32Receive across all drivers and call sites

Other:
- Add canardSTM32GetTxQueueFillLevel() to all three drivers (returns 0; no SW queue)
- Make canard and memory_pool static in dronecan.c
- Add cliDronecan CLI command showing bus health (BusOff, ErrorPassive, TEC, REC, LEC, fill levels)
---
 src/main/drivers/dronecan/dronecan.c          | 14 +++++++---
 .../dronecan/libcanard/canard_sitl_driver.c   | 15 +++++------
 .../dronecan/libcanard/canard_stm32_driver.h  |  6 ++++-
 .../libcanard/canard_stm32f7xx_driver.c       | 26 ++++++++++++++++---
 .../libcanard/canard_stm32h7xx_driver.c       | 15 ++++++++---
 src/main/drivers/nvic.h                       |  1 +
 src/main/fc/cli.c                             | 26 +++++++++++++++++++
 7 files changed, 83 insertions(+), 20 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 31daec2a13f..4c6c3865bf5 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -27,8 +27,8 @@
 
 /* Private variables ---------------------------------------------------------*/
 
-CanardInstance canard;
-uint8_t memory_pool[1024];
+static CanardInstance canard;
+static uint8_t memory_pool[1024];
 static struct uavcan_protocol_NodeStatus node_status;
 
 PG_REGISTER_WITH_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig, PG_DRONECAN_CONFIG, 0);
@@ -45,6 +45,9 @@ static dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES];
 #if defined(STM32H7)
 static inline void dronecanMaskTxISR(void)   { NVIC_DisableIRQ(FDCAN1_IT1_IRQn); }
 static inline void dronecanUnmaskTxISR(void) { NVIC_EnableIRQ(FDCAN1_IT1_IRQn); }
+#elif defined(STM32F7)
+static inline void dronecanMaskTxISR(void)   { NVIC_DisableIRQ(CAN1_TX_IRQn); }
+static inline void dronecanUnmaskTxISR(void) { NVIC_EnableIRQ(CAN1_TX_IRQn); }
 #else
 static inline void dronecanMaskTxISR(void)   {}
 static inline void dronecanUnmaskTxISR(void) {}
@@ -385,6 +388,11 @@ void HAL_FDCAN_TxBufferCompleteCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t Bu
     processCanardTxQueue();
 }   
 #endif
+#if defined(STM32F7)
+void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { UNUSED(hcan); processCanardTxQueue(); }
+void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { UNUSED(hcan); processCanardTxQueue(); }
+void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { UNUSED(hcan); processCanardTxQueue(); }
+#endif
 
 
 /*
@@ -478,7 +486,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..2f70faa459f 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,7 @@ 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;
+    memset(pProtocolStat, 0, sizeof(*pProtocolStat));
 }
 #endif // __linux__
 
@@ -280,7 +275,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;
     }
@@ -357,6 +352,10 @@ int32_t canardSTM32GetRxFifoFillLevel(void) {
     return 0;
 }
 
+int32_t canardSTM32GetTxQueueFillLevel(void) {
+    return 0;
+}
+
 /**
  * @brief Recover from bus-off condition
  */
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h b/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h
index fe70cddf1d9..c5fd6a5133e 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h
@@ -12,15 +12,19 @@
 typedef struct {
     uint32_t BusOff;
     uint32_t ErrorPassive;
+    uint8_t  tec;
+    uint8_t  rec;
+    uint8_t  lec;
 } canardProtocolStatus_t;
 
 #ifdef USE_DRONECAN
 
 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 canardSTM32GetTxQueueFillLevel(void);
 int32_t canardSTM32GetRxFifoFillLevel(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 d630e9ab0ef..0e0e0026392 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"
 
@@ -93,7 +94,7 @@ uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf) {
   * 		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) {
@@ -240,8 +241,14 @@ 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);
+    if (HAL_CAN_ActivateNotification(&hcan1, CAN_IT_TX_MAILBOX_EMPTY) != HAL_OK) {
+        LOG_ERROR(CAN, "Failed to activate TX interrupt");
+        return -CANARD_ERROR_INTERNAL;
+    }   
+    HAL_NVIC_SetPriority(CAN1_TX_IRQn, NVIC_PRIO_CAN, 0);
+    HAL_NVIC_EnableIRQ(CAN1_TX_IRQn);
 
     return CANARD_OK;
 }
@@ -387,9 +394,16 @@ 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);
+    pProtocolStat->tec          = (uint8_t)((esr >> 16) & 0xFF);
+    pProtocolStat->rec          = (uint8_t)((esr >> 24) & 0xFF);
+    pProtocolStat->lec          = (uint8_t)((esr >> 4) & 0x07);
+}
+
+int32_t canardSTM32GetTxQueueFillLevel(void){
+    return 0;
 }
 
 int32_t canardSTM32GetRxFifoFillLevel(void){
@@ -420,6 +434,10 @@ void CAN1_RX0_IRQHandler(void) {
       HAL_CAN_IRQHandler(&hcan1);
 }
 
+void CAN1_TX_IRQHandler(void) {
+    HAL_CAN_IRQHandler(&hcan1);
+}
+
 void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
     RxFrame_t frame;
     if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &frame.header, frame.data) == HAL_OK) {
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index d0ecdd8ed2d..5927c304ecd 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -40,7 +40,7 @@ static FDCAN_HandleTypeDef hfdcan1;
   * 		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;
 	}
@@ -351,11 +351,18 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi
 }
 
 void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){
-	FDCAN_ProtocolStatusTypeDef protocolStatus = {};
-
+    FDCAN_ProtocolStatusTypeDef protocolStatus = {};
     HAL_FDCAN_GetProtocolStatus(&hfdcan1, &protocolStatus);
-    pProtocolStat->BusOff = protocolStatus.BusOff;
+    pProtocolStat->BusOff       = protocolStatus.BusOff;
     pProtocolStat->ErrorPassive = protocolStatus.ErrorPassive;
+    uint32_t ecr = hfdcan1.Instance->ECR;
+    pProtocolStat->tec          = (uint8_t)((ecr >> 16) & 0x7F);
+    pProtocolStat->rec          = (uint8_t)((ecr >> 8) & 0x7F);
+    pProtocolStat->lec          = (uint8_t)(protocolStatus.LastErrorCode & 0x07);
+}
+
+int32_t canardSTM32GetTxQueueFillLevel(void){
+    return 0;
 }
 
 int32_t canardSTM32GetRxFifoFillLevel(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 7f281a28a46..97e465d1231 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -125,6 +125,7 @@ bool cliMode = false;
 #include "sensors/temperature.h"
 #ifdef USE_DRONECAN
 #include "drivers/dronecan/dronecan.h"
+#include "drivers/dronecan/libcanard/canard_stm32_driver.h"
 #endif
 #ifdef USE_ESC_SENSOR
 #include "sensors/esc_sensor.h"
@@ -4694,6 +4695,28 @@ static void printConfig(const char *cmdline, bool doDiff)
     restoreConfigs();
 }
 
+#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", (long)txFill);
+    cliPrintLinef("  RX buffer:    %ld", (long)rxFill);
+}
+#endif
+
 static void cliDump(char *cmdline)
 {
     printConfig(cmdline, false);
@@ -4939,6 +4962,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 da08d0d3b7a3e7d0308115f2878f6013bf3f5da4 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Thu, 11 Jun 2026 22:48:02 -0700
Subject: [PATCH 40/67] dronecan: fix H7 ISR pump, ECR TEC field, narrow TX
 mask window

Fix H7 ISR pump (was completely non-functional):
- FDCAN_IT_TX_COMPLETE routes to LINE0 by default (ILS=0); enable
  FDCAN1_IT0_IRQn at NVIC_PRIO_CAN and add FDCAN1_IT0_IRQHandler
- Change dronecanMaskTxISR from FDCAN1_IT1_IRQn to FDCAN1_IT0_IRQn

Fix H7 TEC extraction from ECR register:
- ECR[7:0]=TEC, ECR[14:8]=REC, ECR[23:16]=CEL (not TEC)
- Previous code read CEL and labelled it tec

Narrow ISR mask window in processCanardTxQueueSafe:
- Mask only covers canardPeekTxQueue/canardPopTxQueue (linked-list ops)
- HAL transmit call now runs with ISR unmasked
- Re-peek after transmit to detect if ISR already popped the frame

F7 RxBuffer ISR safety:
- writeIndex/readIndex now volatile to prevent compiler caching
- Remove unused file-scope rxMsg variable
- rxBufferPushFrame/PopFrame now int8_t and static
- Log warning on RX buffer full instead of silently dropping

Remove redundant lec & 0x7 mask in cliDronecan (already masked at population)
---
 src/main/drivers/dronecan/dronecan.c          | 36 +++++++++++++++----
 .../libcanard/canard_stm32f7xx_driver.c       | 13 +++----
 .../libcanard/canard_stm32h7xx_driver.c       | 15 ++++++--
 src/main/fc/cli.c                             |  2 +-
 4 files changed, 50 insertions(+), 16 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 4c6c3865bf5..00644c5fc24 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -43,8 +43,8 @@ static uint8_t activeNodeCount = 0;
 static dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES];
 
 #if defined(STM32H7)
-static inline void dronecanMaskTxISR(void)   { NVIC_DisableIRQ(FDCAN1_IT1_IRQn); }
-static inline void dronecanUnmaskTxISR(void) { NVIC_EnableIRQ(FDCAN1_IT1_IRQn); }
+static inline void dronecanMaskTxISR(void)   { NVIC_DisableIRQ(FDCAN1_IT0_IRQn); }
+static inline void dronecanUnmaskTxISR(void) { NVIC_EnableIRQ(FDCAN1_IT0_IRQn); }
 #elif defined(STM32F7)
 static inline void dronecanMaskTxISR(void)   { NVIC_DisableIRQ(CAN1_TX_IRQn); }
 static inline void dronecanUnmaskTxISR(void) { NVIC_EnableIRQ(CAN1_TX_IRQn); }
@@ -375,10 +375,34 @@ void processCanardTxQueue(void) {
 }
 
 static void processCanardTxQueueSafe(void) {
-    dronecanMaskTxISR();
-    processCanardTxQueue();
-    dronecanUnmaskTxISR();
-}  
+    for (;;) {
+        // Mask only for the linked-list peek — not for the HAL transmit call
+        dronecanMaskTxISR();
+        const CanardCANFrame *tx_frame = canardPeekTxQueue(&canard);
+        if (tx_frame == NULL) {
+            dronecanUnmaskTxISR();
+            break;
+        }
+        const CanardCANFrame frame_copy = *tx_frame;
+        dronecanUnmaskTxISR();
+
+        const int16_t tx_res = canardSTM32Transmit(&frame_copy);
+        if (tx_res == 0) {
+            break;  // HW TX full, ISR will refill when a slot opens
+        }
+
+        // Re-mask to pop. If the ISR fired during the transmit call and already
+        // popped this frame, peek will return a different pointer — skip the pop.
+        dronecanMaskTxISR();
+        if (canardPeekTxQueue(&canard) == tx_frame) {
+            if (tx_res < 0) {
+                LOG_DEBUG(CAN, "Transmit error %d", tx_res);
+            }
+            canardPopTxQueue(&canard);
+        }
+        dronecanUnmaskTxISR();
+    }
+}
 
 #if defined(STM32H7)
 void HAL_FDCAN_TxBufferCompleteCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t BufferIndexes)
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
index 0e0e0026392..52c28df7a5d 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;
+    volatile uint8_t readIndex;
     RxFrame_t rxMsg[RX_BUFFER_SIZE];
 } RxBuffer;
 
@@ -43,9 +43,8 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi
 static void canardSTM32GPIO_Init(void);
 
 static CAN_HandleTypeDef hcan1;
-RxFrame_t rxMsg;
 
-uint8_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;
 
@@ -63,7 +62,7 @@ uint8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) {
     return 0;
 }
 
-uint8_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;
 
@@ -441,6 +440,8 @@ void CAN1_TX_IRQHandler(void) {
 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);
+        if (rxBufferPushFrame(&RxBuffer, &frame) != 0) {
+            LOG_WARNING(CAN, "RX buffer full, frame dropped");
+        }
     }
 }
\ No newline at end of file
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index 5927c304ecd..617fa481e69 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_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"
 
@@ -203,11 +204,15 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate)
     }
 
     if (HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_TX_COMPLETE,
-        FDCAN_TX_BUFFER0 | FDCAN_TX_BUFFER1 | FDCAN_TX_BUFFER2)) {
+        FDCAN_TX_BUFFER0 | FDCAN_TX_BUFFER1 | FDCAN_TX_BUFFER2) != HAL_OK) { /* Must match TxFifoQueueElmtsNbr = 3 */
         LOG_ERROR(CAN, "Failed to activate interrupt notification");
         return -CANARD_ERROR_INTERNAL;
     }
 
+    /* FDCAN_IT_TX_COMPLETE routes to LINE0 by default (ILS resets to 0) */
+    HAL_NVIC_SetPriority(FDCAN1_IT0_IRQn, NVIC_PRIO_CAN, 0);
+    HAL_NVIC_EnableIRQ(FDCAN1_IT0_IRQn);
+
     return CANARD_OK;
 }
 
@@ -356,8 +361,8 @@ void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){
     pProtocolStat->BusOff       = protocolStatus.BusOff;
     pProtocolStat->ErrorPassive = protocolStatus.ErrorPassive;
     uint32_t ecr = hfdcan1.Instance->ECR;
-    pProtocolStat->tec          = (uint8_t)((ecr >> 16) & 0x7F);
-    pProtocolStat->rec          = (uint8_t)((ecr >> 8) & 0x7F);
+    pProtocolStat->tec          = (uint8_t)(ecr & 0xFF);           /* ECR[7:0]  */
+    pProtocolStat->rec          = (uint8_t)((ecr >> 8) & 0x7F);   /* ECR[14:8] */
     pProtocolStat->lec          = (uint8_t)(protocolStatus.LastErrorCode & 0x07);
 }
 
@@ -385,4 +390,8 @@ void canardSTM32GetUniqueID(uint8_t id[16]) {
     HALUniqueIDs[1] = HAL_GetUIDw1();
     HALUniqueIDs[2] = HAL_GetUIDw2();
     memcpy(id, HALUniqueIDs, 12);
+}
+
+void FDCAN1_IT0_IRQHandler(void) {
+    HAL_FDCAN_IRQHandler(&hfdcan1);
 }
\ No newline at end of file
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 97e465d1231..593ed5dd1cd 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -4711,7 +4711,7 @@ static void cliDronecan(char *cmdline)
     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("  LEC:          %s (%u)", lecNames[stat.lec], (unsigned)stat.lec);
     cliPrintLinef("  TX queue:     %ld", (long)txFill);
     cliPrintLinef("  RX buffer:    %ld", (long)rxFill);
 }

From 3c889c071e3479b67a443dd64e95a01f27502d2b Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 12 Jun 2026 06:56:20 -0700
Subject: [PATCH 41/67] dronecan: reorder driver files to public API before
 private helpers

Move public canardSTM32* functions above static helpers in H7 and F7
drivers. Move dronecanInit/Update/Get* to the top of dronecan.c with
forward declarations for internal callbacks. Mark rxBufferNumMessages
static in F7 driver. Add ISR-context warning comment to
processCanardTxQueue.
---
 src/main/drivers/dronecan/dronecan.c          | 748 +++++++++---------
 .../libcanard/canard_stm32f7xx_driver.c       | 306 +++----
 .../libcanard/canard_stm32h7xx_driver.c       | 239 +++---
 3 files changed, 661 insertions(+), 632 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 00644c5fc24..5daa3a359f6 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -53,151 +53,248 @@ static inline void dronecanMaskTxISR(void)   {}
 static inline void dronecanUnmaskTxISR(void) {}
 #endif
 
-// 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 ------------------------------------------------------*/
 
-// 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 processCanardTxQueueSafe(void);
+static void process1HzTasks(timeUs_t timestamp_usec);
+bool shouldAcceptTransfer(const CanardInstance *ins, uint64_t *out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, uint8_t source_node_id);
+void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer);
 
-void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) {
-	UNUSED(ins);
-    struct uavcan_protocol_NodeStatus nodeStatus;
+// ---- Public API -------------------------------------------------------------
 
-	if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) {
-		LOG_DEBUG(CAN, "NodeStatus decode failed");
-		return;
-	}
+void dronecanInit(void)
+{
+    uint32_t bitrate = 500000; // At least define 500000
 
-	uint8_t nodeId = transfer->source_node_id;                                                                                                                                                                                                
-    for (uint8_t i = 0; i < activeNodeCount; i++) {
-        if (nodeTable[i].nodeID == nodeId) { 
-            // update health, mode, uptime, vendor_status_code, last_seen_ms                                                                                                                                                                                                 
-            nodeTable[i].health = nodeStatus.health;
-            nodeTable[i].mode = nodeStatus.mode;
-            nodeTable[i].uptime_sec = nodeStatus.uptime_sec;
-            nodeTable[i].vendor_status_code = nodeStatus.vendor_specific_status_code;
-            nodeTable[i].last_seen_ms = millis();                    
-            return;                                                                                                                                                                                                                           
-        }                                                                                       
-    }                                                                                                                                                                                                                                         
-    // new node                                                                                 
-    if (activeNodeCount < DRONECAN_MAX_NODES) {
-        nodeTable[activeNodeCount].nodeID = nodeId;
-        nodeTable[activeNodeCount].health = nodeStatus.health;
-        nodeTable[activeNodeCount].mode = nodeStatus.mode;
-        nodeTable[activeNodeCount].uptime_sec = nodeStatus.uptime_sec;
-        nodeTable[activeNodeCount].vendor_status_code = nodeStatus.vendor_specific_status_code;
-        nodeTable[activeNodeCount].name_len = 0;
-        nodeTable[activeNodeCount].name[0] = 0;
-        nodeTable[activeNodeCount].last_seen_ms = millis();     
-        activeNodeCount++;                                                                                                                                                               
+    switch (dronecanConfig()->bitRateKbps){
+        case DRONECAN_BITRATE_125KBPS:
+            bitrate = 125000;
+            break;
+
+        case DRONECAN_BITRATE_250KBPS:
+            bitrate = 250000;
+            break;
+
+        case DRONECAN_BITRATE_500KBPS:
+            bitrate = 500000;
+            break;
+
+        case DRONECAN_BITRATE_1000KBPS:
+            bitrate = 1000000;
+            break;
+
+        case DRONECAN_BITRATE_COUNT:
+            LOG_ERROR(SYSTEM, "Undefined bitrate set in configuration. 500kbps selected");
+            bitrate = 500000;
+            break;
     }
+    if(canardSTM32CAN1_Init(bitrate) != CANARD_OK)
+    {
+        LOG_ERROR(CAN, "Unable to initialize the CAN peripheral");
+        dronecanState = STATE_DRONECAN_FAILED;
+        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);
-    if (gpsConfig()->provider != GPS_DRONECAN) return;
-    struct uavcan_equipment_gnss_Auxiliary gnssAuxiliary;
+void dronecanUpdate(timeUs_t currentTimeUs)
+{
+    static timeUs_t next_1hz_service_at = 0;
+    static timeUs_t busoffTimeUs = 0;
+    CanardCANFrame rx_frame;
+    int numMessagesToProcess = 0;
+    canardProtocolStatus_t protocolStatus = {};
+    uint64_t timestamp;
+    int16_t rx_res;
 
-	if (uavcan_equipment_gnss_Auxiliary_decode(transfer, &gnssAuxiliary)) {
-		LOG_DEBUG(CAN, "GNSSAuxiliary decode failed");
-		return;
-	}
-    dronecanGPSReceiveGNSSAuxiliary(&gnssAuxiliary);
-}
+    switch(dronecanState) {
+        case STATE_DRONECAN_INIT:
+            next_1hz_service_at = currentTimeUs + 1000000ULL;  // First 1Hz tick in 1 second
+            dronecanState = STATE_DRONECAN_NORMAL;
+            break;
 
-void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) {
-	UNUSED(ins);
-    if (gpsConfig()->provider != GPS_DRONECAN) return;
-    struct uavcan_equipment_gnss_Fix gnssFix;
+        case STATE_DRONECAN_NORMAL:
+            processCanardTxQueueSafe();
 
-	if (uavcan_equipment_gnss_Fix_decode(transfer, &gnssFix)) {
-		LOG_DEBUG(CAN, "GNSSFix decode failed");
-		return;
-	}
-    dronecanGPSReceiveGNSSFix(&gnssFix);
-}
+             for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--)
+             {
+	            timestamp = millis() * 1000ULL;
+	            rx_res = canardSTM32Receive(&rx_frame);
 
-void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) {
-	UNUSED(ins);
-    if (gpsConfig()->provider != GPS_DRONECAN) return;
-    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.
+            processCanardTxQueueSafe();
+
+            if (currentTimeUs >= next_1hz_service_at)
+            {
+		        next_1hz_service_at += 1000000ULL;
+		        process1HzTasks(currentTimeUs);
+                processCanardTxQueueSafe();
+
+                canardSTM32GetProtocolStatus(&protocolStatus);
+                if (protocolStatus.BusOff != 0 || protocolStatus.ErrorPassive != 0) {
+                    LOG_DEBUG(CAN, "CAN status: BusOff=%" PRIu32 " ErrorPassive=%" PRIu32, protocolStatus.BusOff, protocolStatus.ErrorPassive);
+                }
+                if (protocolStatus.BusOff != 0) {
+                    dronecanState = STATE_DRONECAN_BUS_OFF;
+                    busoffTimeUs = currentTimeUs;
+                }
+            }
+            break;
+
+        case STATE_DRONECAN_BUS_OFF:
+            if(currentTimeUs > (busoffTimeUs + 20000)) { // Wait 20ms: worst-case 128x11 recovery is 11.264ms at 125kbps
+                canardSTM32RecoverFromBusOff();
+                busoffTimeUs = currentTimeUs;
+                canardSTM32GetProtocolStatus(&protocolStatus);
+                if(protocolStatus.BusOff == 0) {
+                    dronecanState = STATE_DRONECAN_NORMAL;
+                }
+            }
+            break;
+
+        case STATE_DRONECAN_FAILED:
+            break;
+
+        case STATE_DRONECAN_COUNT:
+            break;
+
+    }
 
-	if (uavcan_equipment_gnss_Fix2_decode(transfer, &gnssFix2)) {
-		LOG_DEBUG(CAN, "GNSSFix2 decode failed");
-		return;
-	}
-    dronecanGPSReceiveGNSSFix2(&gnssFix2);
 }
 
-void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) {
-	UNUSED(ins);
-    if (gpsConfig()->provider != GPS_DRONECAN) return;
-    struct uavcan_equipment_gnss_RTCMStream gnssRTCMStream;
+dronecanState_e dronecanGetState(void)
+{
+    return dronecanState;
+}
 
-	if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &gnssRTCMStream)) {
-		LOG_DEBUG(CAN, "RTCMStream decode failed");
-		return;
-	}
+uint8_t dronecanGetNodeCount(void)
+{
+    return activeNodeCount;
 }
 
-void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
-	UNUSED(ins);
-    struct uavcan_equipment_power_BatteryInfo batteryInfo;
+uint32_t dronecanGetBitrateKbps(void)
+{
+    switch (dronecanConfig()->bitRateKbps){
+        case DRONECAN_BITRATE_125KBPS:
+            return 125;
 
-	if (uavcan_equipment_power_BatteryInfo_decode(transfer, &batteryInfo)) {
-		LOG_DEBUG(CAN, "BatteryInfo decode failed");
-		return;
-	}
-    dronecanBatterySensorReceiveInfo(&batteryInfo);
+        case DRONECAN_BITRATE_250KBPS:
+            return 250;
+
+        case DRONECAN_BITRATE_500KBPS:
+            return 500;
+
+        case DRONECAN_BITRATE_1000KBPS:
+            return 1000;
+
+        case DRONECAN_BITRATE_COUNT:
+            return 0;
+    }
+    return 0;
 }
 
-/*
-  handle a GetNodeInfo request
-*/
+const dronecanNodeInfo_t *dronecanGetNode(uint8_t index) {
+    if (index < activeNodeCount) return &nodeTable[index];
+    return NULL;
+}
 
-// 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) {
-	uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE];
-	struct uavcan_protocol_GetNodeInfoResponse pkt;
+// ---- TX queue ---------------------------------------------------------------
 
-	memset(&pkt, 0, sizeof(pkt));
+/* Called from TX-complete ISR only. Already in interrupt context — no NVIC masking needed.
+   For main-loop use, call processCanardTxQueueSafe() instead. */
+void processCanardTxQueue(void) {
+	// Transmitting
+	for (const CanardCANFrame *tx_frame ; (tx_frame = canardPeekTxQueue(&canard)) != NULL;)
+    {
+        const int16_t tx_res = canardSTM32Transmit(tx_frame);
 
-	node_status.uptime_sec = millis() / 1000ULL;
-	pkt.status = node_status;
+		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;
+		}
+	}
+}
 
-	// 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
+// ---- ISR / HAL callbacks ----------------------------------------------------
 
-	// should fill in hardware version
-	pkt.hardware_version.major = 1;
-	pkt.hardware_version.minor = 0;
+#if defined(STM32H7)
+void HAL_FDCAN_TxBufferCompleteCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t BufferIndexes)
+{
+    UNUSED(hfdcan);
+    UNUSED(BufferIndexes);
+    processCanardTxQueue();
+}
+#endif
+#if defined(STM32F7)
+void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { UNUSED(hcan); processCanardTxQueue(); }
+void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { UNUSED(hcan); processCanardTxQueue(); }
+void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { UNUSED(hcan); processCanardTxQueue(); }
+#endif
 
-	// just setting all 16 bytes to 1 for testing
-	canardSTM32GetUniqueID(pkt.hardware_version.unique_id);
+// ---- Internal helpers -------------------------------------------------------
 
-	strncpy((char*)pkt.name.data, FC_FIRMWARE_NAME, sizeof(pkt.name.data));
-	pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data));
+static void processCanardTxQueueSafe(void) {
+    for (;;) {
+        // Mask only for the linked-list peek — not for the HAL transmit call
+        dronecanMaskTxISR();
+        const CanardCANFrame *tx_frame = canardPeekTxQueue(&canard);
+        if (tx_frame == NULL) {
+            dronecanUnmaskTxISR();
+            break;
+        }
+        const CanardCANFrame frame_copy = *tx_frame;
+        dronecanUnmaskTxISR();
 
-	uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer);
+        const int16_t tx_res = canardSTM32Transmit(&frame_copy);
+        if (tx_res == 0) {
+            break;  // HW TX full, ISR will refill when a slot opens
+        }
 
-    dronecanMaskTxISR();
-	canardRequestOrRespond(ins,
-						   transfer->source_node_id,
-						   UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE,
-						   UAVCAN_PROTOCOL_GETNODEINFO_ID,
-						   &transfer->transfer_id,
-						   transfer->priority,
-						   CanardResponse,
-						   &buffer[0],
-						   total_size);
-    dronecanUnmaskTxISR();
+        // Re-mask to pop. If the ISR fired during the transmit call and already
+        // popped this frame, peek will return a different pointer — skip the pop.
+        dronecanMaskTxISR();
+        if (canardPeekTxQueue(&canard) == tx_frame) {
+            if (tx_res < 0) {
+                LOG_DEBUG(CAN, "Transmit error %d", tx_res);
+            }
+            canardPopTxQueue(&canard);
+        }
+        dronecanUnmaskTxISR();
+    }
 }
 
-// Canard Senders
+// 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
 
 /*
   send the 1Hz NodeStatus message. This is what allows a node to show
@@ -213,7 +310,7 @@ void send_NodeStatus(void) {
     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
 
@@ -239,7 +336,24 @@ void send_NodeStatus(void) {
 
 }
 
-// Canard Util
+/*
+  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
+    */
+    dronecanMaskTxISR();
+    canardCleanupStaleTransfers(&canard, timestamp_usec);
+    dronecanUnmaskTxISR();
+
+    /*
+      Transmit the node status message
+    */
+    send_NodeStatus();
+}
+
 /*
  This callback is invoked by the library when it detects beginning of a new transfer on the bus that can be received
  by the local node.
@@ -249,7 +363,6 @@ void send_NodeStatus(void) {
 
  This function must fill in the out_data_type_signature to be the signature of the message.
  */
-
 bool shouldAcceptTransfer(const CanardInstance *ins,
                                  uint64_t *out_data_type_signature,
                                  uint16_t data_type_id,
@@ -306,6 +419,147 @@ bool shouldAcceptTransfer(const CanardInstance *ins,
 	return false;
 }
 
+// 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) {
+	UNUSED(ins);
+    struct uavcan_protocol_NodeStatus nodeStatus;
+
+	if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) {
+		LOG_DEBUG(CAN, "NodeStatus decode failed");
+		return;
+	}
+
+	uint8_t nodeId = transfer->source_node_id;
+    for (uint8_t i = 0; i < activeNodeCount; i++) {
+        if (nodeTable[i].nodeID == nodeId) {
+            // update health, mode, uptime, vendor_status_code, last_seen_ms
+            nodeTable[i].health = nodeStatus.health;
+            nodeTable[i].mode = nodeStatus.mode;
+            nodeTable[i].uptime_sec = nodeStatus.uptime_sec;
+            nodeTable[i].vendor_status_code = nodeStatus.vendor_specific_status_code;
+            nodeTable[i].last_seen_ms = millis();
+            return;
+        }
+    }
+    // new node
+    if (activeNodeCount < DRONECAN_MAX_NODES) {
+        nodeTable[activeNodeCount].nodeID = nodeId;
+        nodeTable[activeNodeCount].health = nodeStatus.health;
+        nodeTable[activeNodeCount].mode = nodeStatus.mode;
+        nodeTable[activeNodeCount].uptime_sec = nodeStatus.uptime_sec;
+        nodeTable[activeNodeCount].vendor_status_code = nodeStatus.vendor_specific_status_code;
+        nodeTable[activeNodeCount].name_len = 0;
+        nodeTable[activeNodeCount].name[0] = 0;
+        nodeTable[activeNodeCount].last_seen_ms = millis();
+        activeNodeCount++;
+    }
+
+}
+
+void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) {
+	UNUSED(ins);
+    if (gpsConfig()->provider != GPS_DRONECAN) return;
+    struct uavcan_equipment_gnss_Auxiliary gnssAuxiliary;
+
+	if (uavcan_equipment_gnss_Auxiliary_decode(transfer, &gnssAuxiliary)) {
+		LOG_DEBUG(CAN, "GNSSAuxiliary decode failed");
+		return;
+	}
+    dronecanGPSReceiveGNSSAuxiliary(&gnssAuxiliary);
+}
+
+void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) {
+	UNUSED(ins);
+    if (gpsConfig()->provider != GPS_DRONECAN) return;
+    struct uavcan_equipment_gnss_Fix gnssFix;
+
+	if (uavcan_equipment_gnss_Fix_decode(transfer, &gnssFix)) {
+		LOG_DEBUG(CAN, "GNSSFix decode failed");
+		return;
+	}
+    dronecanGPSReceiveGNSSFix(&gnssFix);
+}
+
+void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) {
+	UNUSED(ins);
+    if (gpsConfig()->provider != GPS_DRONECAN) return;
+    struct uavcan_equipment_gnss_Fix2 gnssFix2;
+
+	if (uavcan_equipment_gnss_Fix2_decode(transfer, &gnssFix2)) {
+		LOG_DEBUG(CAN, "GNSSFix2 decode failed");
+		return;
+	}
+    dronecanGPSReceiveGNSSFix2(&gnssFix2);
+}
+
+void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) {
+	UNUSED(ins);
+    if (gpsConfig()->provider != GPS_DRONECAN) return;
+    struct uavcan_equipment_gnss_RTCMStream gnssRTCMStream;
+
+	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;
+
+	if (uavcan_equipment_power_BatteryInfo_decode(transfer, &batteryInfo)) {
+		LOG_DEBUG(CAN, "BatteryInfo decode failed");
+		return;
+	}
+    dronecanBatterySensorReceiveInfo(&batteryInfo);
+}
+
+/*
+  handle a GetNodeInfo request
+*/
+
+// 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) {
+	uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE];
+	struct uavcan_protocol_GetNodeInfoResponse pkt;
+
+	memset(&pkt, 0, sizeof(pkt));
+
+	node_status.uptime_sec = millis() / 1000ULL;
+	pkt.status = node_status;
+
+	// fill in your major and minor firmware version
+	pkt.software_version.major = FC_VERSION_MAJOR;
+	pkt.software_version.minor = FC_VERSION_MINOR;
+	pkt.software_version.optional_field_flags = FC_VERSION_PATCH_LEVEL;
+	pkt.software_version.vcs_commit = strtoul(shortGitRevision, NULL, 16); // need to convert string to integer put git hash in here
+
+	// 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);
+
+	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);
+
+    dronecanMaskTxISR();
+	canardRequestOrRespond(ins,
+						   transfer->source_node_id,
+						   UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE,
+						   UAVCAN_PROTOCOL_GETNODEINFO_ID,
+						   &transfer->transfer_id,
+						   transfer->priority,
+						   CanardResponse,
+						   &buffer[0],
+						   total_size);
+    dronecanUnmaskTxISR();
+}
+
 /*
  This callback is invoked by the library when a new message or request or response is received.
 */
@@ -328,273 +582,31 @@ 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:
                 handle_BatteryInfo(ins, transfer);
                 break;
         }
 	}
 }
-
-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) {
-			canardPopTxQueue(&canard);  // Success - remove from queue
-		} else {
-			// tx_res == 0: TX FIFO full, retry later
-			break;
-		}
-	}
-}
-
-static void processCanardTxQueueSafe(void) {
-    for (;;) {
-        // Mask only for the linked-list peek — not for the HAL transmit call
-        dronecanMaskTxISR();
-        const CanardCANFrame *tx_frame = canardPeekTxQueue(&canard);
-        if (tx_frame == NULL) {
-            dronecanUnmaskTxISR();
-            break;
-        }
-        const CanardCANFrame frame_copy = *tx_frame;
-        dronecanUnmaskTxISR();
-
-        const int16_t tx_res = canardSTM32Transmit(&frame_copy);
-        if (tx_res == 0) {
-            break;  // HW TX full, ISR will refill when a slot opens
-        }
-
-        // Re-mask to pop. If the ISR fired during the transmit call and already
-        // popped this frame, peek will return a different pointer — skip the pop.
-        dronecanMaskTxISR();
-        if (canardPeekTxQueue(&canard) == tx_frame) {
-            if (tx_res < 0) {
-                LOG_DEBUG(CAN, "Transmit error %d", tx_res);
-            }
-            canardPopTxQueue(&canard);
-        }
-        dronecanUnmaskTxISR();
-    }
-}
-
-#if defined(STM32H7)
-void HAL_FDCAN_TxBufferCompleteCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t BufferIndexes)
-{
-    UNUSED(hfdcan);
-    UNUSED(BufferIndexes);
-    processCanardTxQueue();
-}   
-#endif
-#if defined(STM32F7)
-void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { UNUSED(hcan); processCanardTxQueue(); }
-void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { UNUSED(hcan); processCanardTxQueue(); }
-void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { UNUSED(hcan); processCanardTxQueue(); }
-#endif
-
-
-/*
-  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
-    */
-    dronecanMaskTxISR();
-    canardCleanupStaleTransfers(&canard, timestamp_usec);
-    dronecanUnmaskTxISR();
-
-    /*
-      Transmit the node status message
-    */
-    send_NodeStatus();
-}
-
-void dronecanInit(void)
-{
-    uint32_t bitrate = 500000; // At least define 500000
-
-    switch (dronecanConfig()->bitRateKbps){
-        case DRONECAN_BITRATE_125KBPS:
-            bitrate = 125000;
-            break;
-
-        case DRONECAN_BITRATE_250KBPS:
-            bitrate = 250000;
-            break;
-        
-        case DRONECAN_BITRATE_500KBPS:
-            bitrate = 500000;
-            break;
-
-        case DRONECAN_BITRATE_1000KBPS:
-            bitrate = 1000000;
-            break;
-
-        case DRONECAN_BITRATE_COUNT:
-            LOG_ERROR(SYSTEM, "Undefined bitrate set in configuration. 500kbps selected");
-            bitrate = 500000;
-            break;
-    }
-    if(canardSTM32CAN1_Init(bitrate) != CANARD_OK)
-    {
-        LOG_ERROR(CAN, "Unable to initialize the CAN peripheral");
-        dronecanState = STATE_DRONECAN_FAILED;
-        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 dronecanUpdate(timeUs_t currentTimeUs)
-{
-    static timeUs_t next_1hz_service_at = 0;
-    static timeUs_t busoffTimeUs = 0;
-    CanardCANFrame rx_frame;
-    int numMessagesToProcess = 0;
-    canardProtocolStatus_t protocolStatus = {};
-    uint64_t timestamp;
-    int16_t rx_res;
-
-    switch(dronecanState) {
-        case STATE_DRONECAN_INIT:
-            next_1hz_service_at = currentTimeUs + 1000000ULL;  // First 1Hz tick in 1 second
-            dronecanState = STATE_DRONECAN_NORMAL;
-            break;
-
-        case STATE_DRONECAN_NORMAL:
-            processCanardTxQueueSafe();
-
-             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.
-            processCanardTxQueueSafe();
-
-            if (currentTimeUs >= next_1hz_service_at)
-            {
-		        next_1hz_service_at += 1000000ULL;
-		        process1HzTasks(currentTimeUs);
-                processCanardTxQueueSafe();
-
-                canardSTM32GetProtocolStatus(&protocolStatus);
-                if (protocolStatus.BusOff != 0 || protocolStatus.ErrorPassive != 0) {
-                    LOG_DEBUG(CAN, "CAN status: BusOff=%" PRIu32 " ErrorPassive=%" PRIu32, protocolStatus.BusOff, protocolStatus.ErrorPassive);
-                }
-                if (protocolStatus.BusOff != 0) {
-                    dronecanState = STATE_DRONECAN_BUS_OFF;
-                    busoffTimeUs = currentTimeUs;
-                }
-            }
-            break;
-
-        case STATE_DRONECAN_BUS_OFF:
-            if(currentTimeUs > (busoffTimeUs + 20000)) { // Wait 20ms: worst-case 128x11 recovery is 11.264ms at 125kbps
-                canardSTM32RecoverFromBusOff();
-                busoffTimeUs = currentTimeUs;
-                canardSTM32GetProtocolStatus(&protocolStatus);
-                if(protocolStatus.BusOff == 0) {
-                    dronecanState = STATE_DRONECAN_NORMAL;
-                }
-            }
-            break;
-
-        case STATE_DRONECAN_FAILED:
-            break;
-
-        case STATE_DRONECAN_COUNT:
-            break;
-
-    }
-    
-}
-
-dronecanState_e dronecanGetState(void)
-{
-    return dronecanState;
-}
-
-uint8_t dronecanGetNodeCount(void)
-{
-    return activeNodeCount;
-}
-
-uint32_t dronecanGetBitrateKbps(void)
-{
-    switch (dronecanConfig()->bitRateKbps){
-        case DRONECAN_BITRATE_125KBPS:
-            return 125;
-
-        case DRONECAN_BITRATE_250KBPS:
-            return 250;
-        
-        case DRONECAN_BITRATE_500KBPS:
-            return 500;
-
-        case DRONECAN_BITRATE_1000KBPS:
-            return 1000;
-        
-        case DRONECAN_BITRATE_COUNT:
-            return 0;
-    }
-    return 0;
-}
-
-const dronecanNodeInfo_t *dronecanGetNode(uint8_t index) {
-    if (index < activeNodeCount) return &nodeTable[index];
-    return NULL;
-}
 #endif
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
index 52c28df7a5d..7751288bfaf 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -41,50 +41,93 @@ static struct RxBuffer_t {
 
 static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings*out_timings);
 static void canardSTM32GPIO_Init(void);
+static int8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg);
+static int8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg);
+static uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf);
 
 static CAN_HandleTypeDef hcan1;
 
-static int8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) {
-    uint8_t next;
-    RxFrame_t *pCurrentRxMsg;
+// ---- Public API -------------------------------------------------------------
 
-    next = rxBuf->writeIndex + 1;
-    if(next >= RX_BUFFER_SIZE){
-        next = 0;
+/**
+  * @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
+  */
+int16_t canardSTM32CAN1_Init(uint32_t bitrate)
+{
+    struct Timings out_timings;
+
+    __HAL_RCC_CAN1_CLK_ENABLE();
+
+    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 = DISABLE;  // ENABLE fills the TX FIFO on a degraded bus; DroneCAN reliability is handled at the application layer
+    hcan1.Init.ReceiveFifoLocked = DISABLE;
+    hcan1.Init.TransmitFifoPriority = DISABLE;
+
+    if (!canardSTM32ComputeTimings(bitrate, &out_timings))
+    {
+        LOG_ERROR(CAN, "Failed to compute CAN timings for bitrate %lu", (unsigned long)bitrate);
+        return -CANARD_ERROR_INTERNAL;
     }
 
-    if(next == rxBuf->readIndex) {
-        return -1;  // rxBuf is full
+    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);
+
+    canardSTM32GPIO_Init();
+    if (HAL_CAN_Init(&hcan1) != HAL_OK)
+    {
+        LOG_ERROR(CAN, "Failed CAN Init");
+        return -CANARD_ERROR_INTERNAL;
     }
-    pCurrentRxMsg = &rxBuf->rxMsg[rxBuf->writeIndex];
-    memcpy(pCurrentRxMsg, rxMsg, sizeof(RxFrame_t));
-    rxBuf->writeIndex = next;
-    return 0;
-}
 
-static int8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) {
-    uint8_t next;
-    RxFrame_t *pCurrentRxMsg;
+    if (HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig) != HAL_OK) {
+        LOG_ERROR(CAN, "Failed Config Filter");
+        return -CANARD_ERROR_INTERNAL;
+    }
 
-    if(rxBuf->writeIndex == rxBuf->readIndex){
-        return -1;  // Nothing to read
+    if (HAL_CAN_Start(&hcan1) != HAL_OK) {
+        LOG_ERROR(CAN, "Failed to Start");
+        return -CANARD_ERROR_INTERNAL;
     }
 
-    next = rxBuf->readIndex + 1;
-    if (next >= RX_BUFFER_SIZE){
-        next = 0;
+    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;
     }
-    pCurrentRxMsg = &rxBuf->rxMsg[rxBuf->readIndex];
-    memcpy(rxMsg, pCurrentRxMsg, sizeof(RxFrame_t));
-    rxBuf->readIndex = next;
-    return 0;
-}
 
-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);
+    // 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);
+    if (HAL_CAN_ActivateNotification(&hcan1, CAN_IT_TX_MAILBOX_EMPTY) != HAL_OK) {
+        LOG_ERROR(CAN, "Failed to activate TX interrupt");
+        return -CANARD_ERROR_INTERNAL;
+    }
+    HAL_NVIC_SetPriority(CAN1_TX_IRQn, NVIC_PRIO_CAN, 0);
+    HAL_NVIC_EnableIRQ(CAN1_TX_IRQn);
+
+    return CANARD_OK;
 }
 
 /**
@@ -171,87 +214,64 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) {
 	return 0;
 }
 
-/**
-  * @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
-  */
-int16_t canardSTM32CAN1_Init(uint32_t bitrate)
-{
-    struct Timings out_timings;
+void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){
+    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);
+    pProtocolStat->tec          = (uint8_t)((esr >> 16) & 0xFF);
+    pProtocolStat->rec          = (uint8_t)((esr >> 24) & 0xFF);
+    pProtocolStat->lec          = (uint8_t)((esr >> 4) & 0x07);
+}
 
-    __HAL_RCC_CAN1_CLK_ENABLE();
+int32_t canardSTM32GetTxQueueFillLevel(void){
+    return 0;
+}
 
-    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 = DISABLE;  // ENABLE fills the TX FIFO on a degraded bus; DroneCAN reliability is handled at the application layer
-    hcan1.Init.ReceiveFifoLocked = DISABLE;
-    hcan1.Init.TransmitFifoPriority = DISABLE;
-  
-    if (!canardSTM32ComputeTimings(bitrate, &out_timings))
-    {
-        LOG_ERROR(CAN, "Failed to compute CAN timings for bitrate %lu", (unsigned long)bitrate);
-        return -CANARD_ERROR_INTERNAL;
-    }
+int32_t canardSTM32GetRxFifoFillLevel(void){
+    return rxBufferNumMessages(&RxBuffer);
+}
 
-    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);
+void canardSTM32RecoverFromBusOff(void){
+    // No-op: ABOM (CAN_MCR bit 6) is set in canardSTM32CAN1_Init, so hardware
+    // manages the full bus-off recovery sequence automatically. After 128x11
+    // recessive bits, hardware cycles INRQ and clears ESR.BOFF without software
+    // intervention. See RM0410 ss40.7.6 and CAN_MCR.ABOM, CAN_ESR.BOFF.
+}
 
-    canardSTM32GPIO_Init();
-    if (HAL_CAN_Init(&hcan1) != HAL_OK)
-    {
-        LOG_ERROR(CAN, "Failed CAN Init");
-        return -CANARD_ERROR_INTERNAL;
-    }
-    
-    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;
-    }
+/*
+  get a 16 byte unique ID for this node, this should be based on the CPU unique ID or other 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
+    memset(id, 0, 16);
+    HALUniqueIDs[0] = *(uint32_t *)UID_BASE;
+    HALUniqueIDs[1] = *(uint32_t *)(UID_BASE + 4);
+    HALUniqueIDs[2] = *(uint32_t *)(UID_BASE + 8);
+    memcpy(id, HALUniqueIDs, 12);
+}
 
-    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);
-    if (HAL_CAN_ActivateNotification(&hcan1, CAN_IT_TX_MAILBOX_EMPTY) != HAL_OK) {
-        LOG_ERROR(CAN, "Failed to activate TX interrupt");
-        return -CANARD_ERROR_INTERNAL;
-    }   
-    HAL_NVIC_SetPriority(CAN1_TX_IRQn, NVIC_PRIO_CAN, 0);
-    HAL_NVIC_EnableIRQ(CAN1_TX_IRQn);
+// ---- ISR / HAL callbacks ----------------------------------------------------
 
-    return CANARD_OK;
+void CAN1_RX0_IRQHandler(void) {
+      HAL_CAN_IRQHandler(&hcan1);
+}
+
+void CAN1_TX_IRQHandler(void) {
+    HAL_CAN_IRQHandler(&hcan1);
+}
+
+void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
+    RxFrame_t frame;
+    if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &frame.header, frame.data) == HAL_OK) {
+        if (rxBufferPushFrame(&RxBuffer, &frame) != 0) {
+            LOG_WARNING(CAN, "RX buffer full, frame dropped");
+        }
+    }
 }
 
+// ---- Private helpers --------------------------------------------------------
+
 /**
   * @brief GPIO Initialization Function
   * @param None
@@ -278,9 +298,10 @@ static void canardSTM32GPIO_Init(void)
     IOLo(IOGetByTag(IO_TAG(CAN1_STANDBY)));
 #endif
 }
+
 static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings *out_timings)
 {
-    
+
     if (target_bitrate < 1) {
         return false;
     }
@@ -392,56 +413,45 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi
     return true;
 }
 
-void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){
-    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);
-    pProtocolStat->tec          = (uint8_t)((esr >> 16) & 0xFF);
-    pProtocolStat->rec          = (uint8_t)((esr >> 24) & 0xFF);
-    pProtocolStat->lec          = (uint8_t)((esr >> 4) & 0x07);
-}
+static int8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) {
+    uint8_t next;
+    RxFrame_t *pCurrentRxMsg;
 
-int32_t canardSTM32GetTxQueueFillLevel(void){
+    next = rxBuf->writeIndex + 1;
+    if(next >= RX_BUFFER_SIZE){
+        next = 0;
+    }
+
+    if(next == rxBuf->readIndex) {
+        return -1;  // rxBuf is full
+    }
+    pCurrentRxMsg = &rxBuf->rxMsg[rxBuf->writeIndex];
+    memcpy(pCurrentRxMsg, rxMsg, sizeof(RxFrame_t));
+    rxBuf->writeIndex = next;
     return 0;
 }
 
-int32_t canardSTM32GetRxFifoFillLevel(void){
-    return rxBufferNumMessages(&RxBuffer);
-}
+static int8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) {
+    uint8_t next;
+    RxFrame_t *pCurrentRxMsg;
 
-void canardSTM32RecoverFromBusOff(void){
-    // No-op: ABOM (CAN_MCR bit 6) is set in canardSTM32CAN1_Init, so hardware
-    // manages the full bus-off recovery sequence automatically. After 128x11
-    // recessive bits, hardware cycles INRQ and clears ESR.BOFF without software
-    // intervention. See RM0410 ss40.7.6 and CAN_MCR.ABOM, CAN_ESR.BOFF.
-}
+    if(rxBuf->writeIndex == rxBuf->readIndex){
+        return -1;  // Nothing to read
+    }
 
-/*
-  get a 16 byte unique ID for this node, this should be based on the CPU unique ID or other 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
-    memset(id, 0, 16);
-    HALUniqueIDs[0] = *(uint32_t *)UID_BASE;
-    HALUniqueIDs[1] = *(uint32_t *)(UID_BASE + 4);
-    HALUniqueIDs[2] = *(uint32_t *)(UID_BASE + 8);
-    memcpy(id, HALUniqueIDs, 12);
+    next = rxBuf->readIndex + 1;
+    if (next >= RX_BUFFER_SIZE){
+        next = 0;
+    }
+    pCurrentRxMsg = &rxBuf->rxMsg[rxBuf->readIndex];
+    memcpy(rxMsg, pCurrentRxMsg, sizeof(RxFrame_t));
+    rxBuf->readIndex = next;
+    return 0;
 }
 
-void CAN1_RX0_IRQHandler(void) {
-      HAL_CAN_IRQHandler(&hcan1);
-}
+static uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf) {
+    if(rxBuf->writeIndex < rxBuf->readIndex)
+        return((rxBuf->writeIndex + RX_BUFFER_SIZE) - rxBuf->readIndex);
 
-void CAN1_TX_IRQHandler(void) {
-    HAL_CAN_IRQHandler(&hcan1);
+    return (rxBuf->writeIndex - rxBuf->readIndex);
 }
-
-void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
-    RxFrame_t frame;
-    if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &frame.header, frame.data) == HAL_OK) {
-        if (rxBufferPushFrame(&RxBuffer, &frame) != 0) {
-            LOG_WARNING(CAN, "RX buffer full, frame dropped");
-        }
-    }
-}
\ No newline at end of file
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index 617fa481e69..fa6ba12fe53 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -31,6 +31,91 @@ static void canardSTM32GPIO_Init(void);
 
 static FDCAN_HandleTypeDef hfdcan1;
 
+// ---- Public API -------------------------------------------------------------
+
+/**
+  * @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)
+{
+    struct Timings out_timings;
+
+    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;
+    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;  // ENABLE fills the 32-slot TX FIFO on a degraded bus; DroneCAN reliability is handled at the application layer
+    hfdcan1.Init.TransmitPause = DISABLE;
+    hfdcan1.Init.ProtocolException = DISABLE;
+
+    __HAL_RCC_FDCAN_CLK_ENABLE();
+
+    if (!canardSTM32ComputeTimings(bitrate, &out_timings))
+    {
+        LOG_ERROR(CAN, "Failed to compute CAN timings for bitrate %lu", (unsigned long)bitrate);
+        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 = 3;
+    hfdcan1.Init.TxEventsNbr = 0;
+    hfdcan1.Init.TxBuffersNbr = 0;
+    hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_QUEUE_OPERATION;
+    hfdcan1.Init.TxElmtSize = FDCAN_DATA_BYTES_8;
+
+    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;
+    }
+    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;
+    }
+
+    if (HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_TX_COMPLETE,
+        FDCAN_TX_BUFFER0 | FDCAN_TX_BUFFER1 | FDCAN_TX_BUFFER2) != HAL_OK) { /* Must match TxFifoQueueElmtsNbr = 3 */
+        LOG_ERROR(CAN, "Failed to activate interrupt notification");
+        return -CANARD_ERROR_INTERNAL;
+    }
+
+    /* FDCAN_IT_TX_COMPLETE routes to LINE0 by default (ILS resets to 0) */
+    HAL_NVIC_SetPriority(FDCAN1_IT0_IRQn, NVIC_PRIO_CAN, 0);
+    HAL_NVIC_EnableIRQ(FDCAN1_IT0_IRQn);
+
+    return CANARD_OK;
+}
+
 /**
   * @brief  Process CAN message from RxLocation FIFO into rx_frame
   * @param  hfdcan pointer to an FDCAN_HandleTypeDef structure that contains
@@ -133,89 +218,51 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) {
 	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)
-{
-    struct Timings out_timings;
-
-    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;
-    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;  // ENABLE fills the 32-slot TX FIFO on a degraded bus; DroneCAN reliability is handled at the application layer
-    hfdcan1.Init.TransmitPause = DISABLE;
-    hfdcan1.Init.ProtocolException = DISABLE;
-
-    __HAL_RCC_FDCAN_CLK_ENABLE();
-
-    if (!canardSTM32ComputeTimings(bitrate, &out_timings))
-    {
-        LOG_ERROR(CAN, "Failed to compute CAN timings for bitrate %lu", (unsigned long)bitrate);
-        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);
+void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){
+    FDCAN_ProtocolStatusTypeDef protocolStatus = {};
+    HAL_FDCAN_GetProtocolStatus(&hfdcan1, &protocolStatus);
+    pProtocolStat->BusOff       = protocolStatus.BusOff;
+    pProtocolStat->ErrorPassive = protocolStatus.ErrorPassive;
+    uint32_t ecr = hfdcan1.Instance->ECR;
+    pProtocolStat->tec          = (uint8_t)(ecr & 0xFF);           /* ECR[7:0]  */
+    pProtocolStat->rec          = (uint8_t)((ecr >> 8) & 0x7F);   /* ECR[14:8] */
+    pProtocolStat->lec          = (uint8_t)(protocolStatus.LastErrorCode & 0x07);
+}
 
-    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 = 3;
-    hfdcan1.Init.TxEventsNbr = 0;
-    hfdcan1.Init.TxBuffersNbr = 0;
-    hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_QUEUE_OPERATION;
-    hfdcan1.Init.TxElmtSize = FDCAN_DATA_BYTES_8;
+int32_t canardSTM32GetTxQueueFillLevel(void){
+    return 0;
+}
 
-    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;
-    }
-    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;
-    }
+int32_t canardSTM32GetRxFifoFillLevel(void){
+    return (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1, FDCAN_RX_FIFO0));
+}
 
-    if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK) {
-        LOG_ERROR(CAN, "Failed to Start");
-        return -CANARD_ERROR_INTERNAL;
-    }
+void canardSTM32RecoverFromBusOff(void){
+    hfdcan1.Instance->TXBCR = 0xFFFFFFFFU;  // Cancel all pending TX requests before recovery
+    CLEAR_BIT(hfdcan1.Instance->CCCR, FDCAN_CCCR_INIT);
+}
 
-    if (HAL_FDCAN_ActivateNotification(&hfdcan1, FDCAN_IT_TX_COMPLETE,
-        FDCAN_TX_BUFFER0 | FDCAN_TX_BUFFER1 | FDCAN_TX_BUFFER2) != HAL_OK) { /* Must match TxFifoQueueElmtsNbr = 3 */
-        LOG_ERROR(CAN, "Failed to activate interrupt notification");
-        return -CANARD_ERROR_INTERNAL;
-    }
+/*
+  get a 16 byte unique ID for this node, this should be based on the CPU unique ID or other 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
+    memset(id, 0, 16);
+    HALUniqueIDs[0] = HAL_GetUIDw0();
+    HALUniqueIDs[1] = HAL_GetUIDw1();
+    HALUniqueIDs[2] = HAL_GetUIDw2();
+    memcpy(id, HALUniqueIDs, 12);
+}
 
-    /* FDCAN_IT_TX_COMPLETE routes to LINE0 by default (ILS resets to 0) */
-    HAL_NVIC_SetPriority(FDCAN1_IT0_IRQn, NVIC_PRIO_CAN, 0);
-    HAL_NVIC_EnableIRQ(FDCAN1_IT0_IRQn);
+// ---- ISR handler ------------------------------------------------------------
 
-    return CANARD_OK;
+void FDCAN1_IT0_IRQHandler(void) {
+    HAL_FDCAN_IRQHandler(&hfdcan1);
 }
 
+// ---- Private helpers --------------------------------------------------------
+
 /**
   * @brief GPIO Initialization Function
   * @param None
@@ -241,6 +288,7 @@ static void canardSTM32GPIO_Init(void)
     IOLo(IOGetByTag(IO_TAG(CAN1_STANDBY)));
 #endif
 }
+
 static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings *out_timings)
 {
     if (target_bitrate < 1) {
@@ -354,44 +402,3 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi
 
     return true;
 }
-
-void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){
-    FDCAN_ProtocolStatusTypeDef protocolStatus = {};
-    HAL_FDCAN_GetProtocolStatus(&hfdcan1, &protocolStatus);
-    pProtocolStat->BusOff       = protocolStatus.BusOff;
-    pProtocolStat->ErrorPassive = protocolStatus.ErrorPassive;
-    uint32_t ecr = hfdcan1.Instance->ECR;
-    pProtocolStat->tec          = (uint8_t)(ecr & 0xFF);           /* ECR[7:0]  */
-    pProtocolStat->rec          = (uint8_t)((ecr >> 8) & 0x7F);   /* ECR[14:8] */
-    pProtocolStat->lec          = (uint8_t)(protocolStatus.LastErrorCode & 0x07);
-}
-
-int32_t canardSTM32GetTxQueueFillLevel(void){
-    return 0;
-}
-
-int32_t canardSTM32GetRxFifoFillLevel(void){
-    return (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1, FDCAN_RX_FIFO0));
-}
-
-void canardSTM32RecoverFromBusOff(void){
-    hfdcan1.Instance->TXBCR = 0xFFFFFFFFU;  // Cancel all pending TX requests before recovery
-    CLEAR_BIT(hfdcan1.Instance->CCCR, FDCAN_CCCR_INIT);
-}
-
-/*
-  get a 16 byte unique ID for this node, this should be based on the CPU unique ID or other 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
-    memset(id, 0, 16);
-    HALUniqueIDs[0] = HAL_GetUIDw0();
-    HALUniqueIDs[1] = HAL_GetUIDw1();
-    HALUniqueIDs[2] = HAL_GetUIDw2();
-    memcpy(id, HALUniqueIDs, 12);
-}
-
-void FDCAN1_IT0_IRQHandler(void) {
-    HAL_FDCAN_IRQHandler(&hfdcan1);
-}
\ No newline at end of file

From b24900dbab28c012e8d24c5463d5a5a4afe82ec8 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 12 Jun 2026 08:57:44 -0700
Subject: [PATCH 42/67] dronecan: address code review findings

- processCanardTxQueueSafe: simplify by holding NVIC mask across
  transmit call, eliminating the pointer-identity ABA assumption
- F7 rxBuffer: add __DMB() barriers between data write and index
  advance to satisfy C99 memory ordering between ISR and main loop
- F7 ComputeTimings: align max_quanta_per_bit with H7 (17, not 18)
  per Koppe reference recommendation
- H7 GetProtocolStatus: document why ECR is read directly vs HAL
- H7 RecoverFromBusOff: clarify CCCR.INIT clear is defensive no-op
- dronecanUpdate: replace dead STATE_DRONECAN_COUNT case with default
- cli: use PRId32 for int32_t format specifiers; add <inttypes.h>
- SITL: remove Doxygen blocks from private static helpers
---
 src/main/drivers/dronecan/dronecan.c          | 20 +++++---------
 .../dronecan/libcanard/canard_sitl_driver.c   | 26 +------------------
 .../libcanard/canard_stm32f7xx_driver.c       |  4 ++-
 .../libcanard/canard_stm32h7xx_driver.c       |  4 +++
 src/main/fc/cli.c                             |  5 ++--
 5 files changed, 17 insertions(+), 42 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 5daa3a359f6..7667dff8864 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -180,7 +180,7 @@ void dronecanUpdate(timeUs_t currentTimeUs)
         case STATE_DRONECAN_FAILED:
             break;
 
-        case STATE_DRONECAN_COUNT:
+        default:
             break;
 
     }
@@ -265,31 +265,23 @@ void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { UNUSED(hcan);
 
 static void processCanardTxQueueSafe(void) {
     for (;;) {
-        // Mask only for the linked-list peek — not for the HAL transmit call
         dronecanMaskTxISR();
         const CanardCANFrame *tx_frame = canardPeekTxQueue(&canard);
         if (tx_frame == NULL) {
             dronecanUnmaskTxISR();
             break;
         }
-        const CanardCANFrame frame_copy = *tx_frame;
-        dronecanUnmaskTxISR();
-
-        const int16_t tx_res = canardSTM32Transmit(&frame_copy);
-        if (tx_res == 0) {
-            break;  // HW TX full, ISR will refill when a slot opens
-        }
-
-        // Re-mask to pop. If the ISR fired during the transmit call and already
-        // popped this frame, peek will return a different pointer — skip the pop.
-        dronecanMaskTxISR();
-        if (canardPeekTxQueue(&canard) == tx_frame) {
+        const int16_t tx_res = canardSTM32Transmit(tx_frame);  // HAL register write, ~1µs
+        if (tx_res != 0) {
             if (tx_res < 0) {
                 LOG_DEBUG(CAN, "Transmit error %d", tx_res);
             }
             canardPopTxQueue(&canard);
         }
         dronecanUnmaskTxISR();
+        if (tx_res == 0) {
+            break;  // HW TX full, ISR will refill when a slot opens
+        }
     }
 }
 
diff --git a/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c b/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c
index 2f70faa459f..21fb893227c 100644
--- a/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c
@@ -112,11 +112,6 @@ static void sitlCANGetStatsStub(canardProtocolStatus_t *pProtocolStat) {
 #ifdef __linux__
 // SocketCAN implementations
 
-/**
- * @brief Initialize SocketCAN interface
- * @param bitrate CAN bitrate in bps (for logging, actual rate set via ip link)
- * @retval 0 on success, negative on error
- */
 static int16_t sitlCANInitSocketCAN(uint32_t bitrate) {
     struct sockaddr_can addr;
     struct ifreq ifr;
@@ -162,9 +157,6 @@ static int16_t sitlCANInitSocketCAN(uint32_t bitrate) {
     return 0;
 }
 
-/**
- * @brief Convert libcanard frame to Linux CAN frame
- */
 static void sitlCANFrameToLinux(const CanardCANFrame *const src, struct can_frame *const dst) {
     memset(dst, 0, sizeof(struct can_frame));
 
@@ -182,9 +174,6 @@ static void sitlCANFrameToLinux(const CanardCANFrame *const src, struct can_fram
     }
 }
 
-/**
- * @brief Convert Linux CAN frame to libcanard frame
- */
 static void sitlCANFrameFromLinux(const struct can_frame *const src, CanardCANFrame *const dst) {
     memset(dst, 0, sizeof(CanardCANFrame));
 
@@ -202,11 +191,6 @@ static void sitlCANFrameFromLinux(const struct can_frame *const src, CanardCANFr
     }
 }
 
-/**
- * @brief Receive a CAN frame via SocketCAN
- * @param rx_frame Pointer to frame structure to fill
- * @retval 0 if no frame available, 1 if frame received, negative on error
- */
 static int16_t sitlCANReceiveSocketCAN(CanardCANFrame *const rx_frame) {
     struct can_frame frame;
     ssize_t nbytes;
@@ -234,11 +218,6 @@ static int16_t sitlCANReceiveSocketCAN(CanardCANFrame *const rx_frame) {
     return 1;
 }
 
-/**
- * @brief Transmit a CAN frame via SocketCAN
- * @param tx_frame Pointer to frame to transmit
- * @retval 1 on success, 0 if busy, negative on error
- */
 static int16_t sitlCANTransmitSocketCAN(const CanardCANFrame* const tx_frame) {
     struct can_frame frame;
     ssize_t nbytes;
@@ -261,10 +240,7 @@ static int16_t sitlCANTransmitSocketCAN(const CanardCANFrame* const tx_frame) {
     return 1; // Success
 }
 
-/**
- * @brief Get CAN protocol status from SocketCAN
- * @param pProtocolStat Pointer to status structure to fill
- */
+/* Always returns zeroes — SocketCAN provides no per-frame error counters via raw sockets. */
 static void sitlCANGetStatsSocketCAN(canardProtocolStatus_t *pProtocolStat) {
     memset(pProtocolStat, 0, sizeof(*pProtocolStat));
 }
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
index 7751288bfaf..637b306e20c 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -325,7 +325,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 : 17;
     static const int MaxSamplePointLocation = 900;
 
     /*
@@ -427,6 +427,7 @@ static 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 advance
     rxBuf->writeIndex = next;
     return 0;
 }
@@ -443,6 +444,7 @@ static int8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) {
     if (next >= RX_BUFFER_SIZE){
         next = 0;
     }
+    __DMB();  // ensure writeIndex read is complete before reading frame data written by ISR
     pCurrentRxMsg = &rxBuf->rxMsg[rxBuf->readIndex];
     memcpy(rxMsg, pCurrentRxMsg, sizeof(RxFrame_t));
     rxBuf->readIndex = next;
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index fa6ba12fe53..a777b43f826 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -223,6 +223,7 @@ void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){
     HAL_FDCAN_GetProtocolStatus(&hfdcan1, &protocolStatus);
     pProtocolStat->BusOff       = protocolStatus.BusOff;
     pProtocolStat->ErrorPassive = protocolStatus.ErrorPassive;
+    /* HAL provides no accessor for ECR (TEC/REC); read directly. PSR fields (BusOff, ErrorPassive, LEC) come from HAL. */
     uint32_t ecr = hfdcan1.Instance->ECR;
     pProtocolStat->tec          = (uint8_t)(ecr & 0xFF);           /* ECR[7:0]  */
     pProtocolStat->rec          = (uint8_t)((ecr >> 8) & 0x7F);   /* ECR[14:8] */
@@ -239,6 +240,9 @@ int32_t canardSTM32GetRxFifoFillLevel(void){
 
 void canardSTM32RecoverFromBusOff(void){
     hfdcan1.Instance->TXBCR = 0xFFFFFFFFU;  // Cancel all pending TX requests before recovery
+    /* H7 FDCAN does not set CCCR.INIT on bus-off entry (unlike F7 bxCAN ABOM).
+       Hardware runs the 128x11 recessive-bit sequence autonomously. This clear
+       is a defensive no-op in case software previously entered init mode. */
     CLEAR_BIT(hfdcan1.Instance->CCCR, FDCAN_CCCR_INIT);
 }
 
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 593ed5dd1cd..124294e4e5f 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -17,6 +17,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <inttypes.h>
 #include <stdlib.h>
 #include <stdarg.h>
 #include <string.h>
@@ -4712,8 +4713,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], (unsigned)stat.lec);
-    cliPrintLinef("  TX queue:     %ld", (long)txFill);
-    cliPrintLinef("  RX buffer:    %ld", (long)rxFill);
+    cliPrintLinef("  TX queue:     %" PRId32, txFill);
+    cliPrintLinef("  RX buffer:    %" PRId32, rxFill);
 }
 #endif
 

From 5da300c202bbbc8a523c4936494656c6ccebe0a8 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 12 Jun 2026 11:34:59 -0700
Subject: [PATCH 43/67] dronecan: address third code review findings

- Make processCanardTxQueue, shouldAcceptTransfer, onTransferReceived static
- Replace ISR LOG calls with volatile counters (txErrCount, rxDropCount);
  log and clear at 1Hz from main loop via canardSTM32GetAndClearRxDropCount()
- Add canardSTM32GetAndClearRxDropCount() to driver interface; F7 implements
  ring-buffer drop counter, H7/SITL return 0 (no SW ring buffer)
- Check canardBroadcast/canardRequestOrRespond return values; log on OOM
- H7: set RxBuffersNbr=0 (dedicated buffer was unused, wasted message RAM)
- H7: reject unmatched standard-ID frames (FDCAN_REJECT for NonMatchingStd)
- F7: move NVIC_EnableIRQ calls after all ActivateNotification calls succeed
- Fix F7 bxCAN Doxygen: correct @brief, remove wrong @param, fix @retval
- Fix all @retval docs: ret==0 is OK (CANARD_OK), not ret==1
- Cast pid_t to uint32_t before bit-shifting in SITL unique ID generation
---
 src/main/drivers/dronecan/dronecan.c          | 34 ++++++++++++++-----
 .../dronecan/libcanard/canard_sitl_driver.c   | 14 +++++---
 .../dronecan/libcanard/canard_stm32_driver.h  |  1 +
 .../libcanard/canard_stm32f7xx_driver.c       | 28 ++++++++-------
 .../libcanard/canard_stm32h7xx_driver.c       | 14 +++++---
 5 files changed, 61 insertions(+), 30 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 7667dff8864..a75d47dea73 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -41,6 +41,7 @@ PG_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig,
 static dronecanState_e dronecanState = STATE_DRONECAN_INIT;
 static uint8_t activeNodeCount = 0;
 static dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES];
+static volatile uint32_t txErrCount = 0;
 
 #if defined(STM32H7)
 static inline void dronecanMaskTxISR(void)   { NVIC_DisableIRQ(FDCAN1_IT0_IRQn); }
@@ -57,8 +58,8 @@ static inline void dronecanUnmaskTxISR(void) {}
 
 static void processCanardTxQueueSafe(void);
 static void process1HzTasks(timeUs_t timestamp_usec);
-bool shouldAcceptTransfer(const CanardInstance *ins, uint64_t *out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, uint8_t source_node_id);
-void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer);
+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);
 
 // ---- Public API -------------------------------------------------------------
 
@@ -159,6 +160,17 @@ void dronecanUpdate(timeUs_t currentTimeUs)
                 if (protocolStatus.BusOff != 0 || protocolStatus.ErrorPassive != 0) {
                     LOG_DEBUG(CAN, "CAN status: BusOff=%" PRIu32 " ErrorPassive=%" PRIu32, protocolStatus.BusOff, protocolStatus.ErrorPassive);
                 }
+
+                uint32_t rxDrops = canardSTM32GetAndClearRxDropCount();
+                uint32_t txErrs = txErrCount;
+                txErrCount = 0;
+                if (rxDrops > 0) {
+                    LOG_DEBUG(CAN, "RX drops: %" PRIu32, rxDrops);
+                }
+                if (txErrs > 0) {
+                    LOG_DEBUG(CAN, "TX errors: %" PRIu32, txErrs);
+                }
+
                 if (protocolStatus.BusOff != 0) {
                     dronecanState = STATE_DRONECAN_BUS_OFF;
                     busoffTimeUs = currentTimeUs;
@@ -227,14 +239,14 @@ const dronecanNodeInfo_t *dronecanGetNode(uint8_t index) {
 
 /* Called from TX-complete ISR only. Already in interrupt context — no NVIC masking needed.
    For main-loop use, call processCanardTxQueueSafe() instead. */
-void processCanardTxQueue(void) {
+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);
+			txErrCount++;  // logged from main loop at 1Hz
 			canardPopTxQueue(&canard);  // Error - discard frame
 		} else if (tx_res > 0) {
 			canardPopTxQueue(&canard);  // Success - remove from queue
@@ -317,7 +329,7 @@ void send_NodeStatus(void) {
     static uint8_t transfer_id;
 
     dronecanMaskTxISR();
-    canardBroadcast(&canard,
+    const int16_t bc_res = canardBroadcast(&canard,
                     UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
                     UAVCAN_PROTOCOL_NODESTATUS_ID,
                     &transfer_id,
@@ -325,6 +337,9 @@ void send_NodeStatus(void) {
                     buffer,
                     len);
     dronecanUnmaskTxISR();
+    if (bc_res < 0) {
+        LOG_DEBUG(CAN, "NodeStatus broadcast failed: %d", bc_res);
+    }
 
 }
 
@@ -355,7 +370,7 @@ static void process1HzTasks(timeUs_t timestamp_usec)
 
  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,
@@ -540,7 +555,7 @@ void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
 	uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer);
 
     dronecanMaskTxISR();
-	canardRequestOrRespond(ins,
+    const int16_t rr_res = canardRequestOrRespond(ins,
 						   transfer->source_node_id,
 						   UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE,
 						   UAVCAN_PROTOCOL_GETNODEINFO_ID,
@@ -550,12 +565,15 @@ void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
 						   &buffer[0],
 						   total_size);
     dronecanUnmaskTxISR();
+    if (rr_res < 0) {
+        LOG_DEBUG(CAN, "GetNodeInfo response failed: %d", rr_res);
+    }
 }
 
 /*
  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
diff --git a/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c b/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c
index 21fb893227c..9308e12660d 100644
--- a/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c
@@ -328,6 +328,10 @@ int32_t canardSTM32GetRxFifoFillLevel(void) {
     return 0;
 }
 
+uint32_t canardSTM32GetAndClearRxDropCount(void) {
+    return 0;
+}
+
 int32_t canardSTM32GetTxQueueFillLevel(void) {
     return 0;
 }
@@ -359,11 +363,11 @@ void canardSTM32GetUniqueID(uint8_t id[16]) {
 
 #ifdef __linux__
     // Add process ID for uniqueness between multiple SITL instances
-    pid_t pid = getpid();
-    id[4] = (pid >> 24) & 0xFF;
-    id[5] = (pid >> 16) & 0xFF;
-    id[6] = (pid >> 8) & 0xFF;
-    id[7] = pid & 0xFF;
+    uint32_t upid = (uint32_t)getpid();
+    id[4] = (upid >> 24) & 0xFF;
+    id[5] = (upid >> 16) & 0xFF;
+    id[6] = (upid >> 8) & 0xFF;
+    id[7] = upid & 0xFF;
 
     // Add timestamp for additional uniqueness
     struct timespec ts;
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h b/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h
index c5fd6a5133e..9b205f8bbaf 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32_driver.h
@@ -26,6 +26,7 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame);
 void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat);
 int32_t canardSTM32GetTxQueueFillLevel(void);
 int32_t canardSTM32GetRxFifoFillLevel(void);
+uint32_t canardSTM32GetAndClearRxDropCount(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 637b306e20c..d34e3f66be5 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -46,15 +46,14 @@ static int8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg);
 static uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf);
 
 static CAN_HandleTypeDef hcan1;
+static volatile uint32_t rxDropCount = 0;
 
 // ---- Public API -------------------------------------------------------------
 
 /**
-  * @brief FDCAN1 Initialization Function
-  * @param  hfdcan pointer to an FDCAN_HandleTypeDef structure that contains
-  *         the configuration information for the specified FDCAN.
+  * @brief CAN1 (bxCAN) 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 ret == 0: OK (CANARD_OK), ret < 0: CANARD_ERROR
   */
 int16_t canardSTM32CAN1_Init(uint32_t bitrate)
 {
@@ -115,15 +114,14 @@ 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);
     if (HAL_CAN_ActivateNotification(&hcan1, CAN_IT_TX_MAILBOX_EMPTY) != HAL_OK) {
         LOG_ERROR(CAN, "Failed to activate TX interrupt");
         return -CANARD_ERROR_INTERNAL;
     }
+
+    // Enable IRQs only after all ActivateNotification calls succeed
+    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);
 
@@ -134,7 +132,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate)
   * @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
+  * @retval ret == 0: OK (CANARD_OK), ret < 0: CANARD_ERROR
   */
 int16_t canardSTM32Receive(CanardCANFrame *const rx_frame) {
     RxFrame_t canRxFrame;
@@ -170,7 +168,7 @@ int16_t canardSTM32Receive(CanardCANFrame *const rx_frame) {
   * @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
+  * @retval ret == 0: OK (CANARD_OK), ret < 0: CANARD_ERROR
   */
 int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) {
 	CAN_TxHeaderTypeDef txHeader = {};
@@ -265,11 +263,17 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
     RxFrame_t frame;
     if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &frame.header, frame.data) == HAL_OK) {
         if (rxBufferPushFrame(&RxBuffer, &frame) != 0) {
-            LOG_WARNING(CAN, "RX buffer full, frame dropped");
+            rxDropCount++;  // logged from main loop via canardSTM32GetAndClearRxDropCount()
         }
     }
 }
 
+uint32_t canardSTM32GetAndClearRxDropCount(void) {
+    uint32_t count = rxDropCount;
+    rxDropCount = 0;
+    return count;
+}
+
 // ---- Private helpers --------------------------------------------------------
 
 /**
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index a777b43f826..88e2a3925be 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -36,7 +36,7 @@ static FDCAN_HandleTypeDef hfdcan1;
 /**
   * @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 ret == 0: OK (CANARD_OK), ret < 0: CANARD_ERROR
   */
 int16_t canardSTM32CAN1_Init(uint32_t bitrate)
 {
@@ -72,7 +72,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate)
 
     hfdcan1.Init.RxFifo0ElmtsNbr = 30;
     hfdcan1.Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_8;
-    hfdcan1.Init.RxBuffersNbr = 1;
+    hfdcan1.Init.RxBuffersNbr = 0;
     hfdcan1.Init.RxBufferSize = FDCAN_DATA_BYTES_8;
     hfdcan1.Init.StdFiltersNbr = 0;
     hfdcan1.Init.ExtFiltersNbr = 1;
@@ -93,7 +93,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate)
         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) {
+    if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_REJECT, 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;
     }
@@ -124,7 +124,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate)
   *         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
+  * @retval ret == 0: OK (CANARD_OK), ret < 0: CANARD_ERROR
   */
 int16_t canardSTM32Receive(CanardCANFrame *const rx_frame) {
 	if (rx_frame == NULL) {
@@ -164,7 +164,7 @@ int16_t canardSTM32Receive(CanardCANFrame *const rx_frame) {
   * @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
+  * @retval ret == 0: OK (CANARD_OK), ret < 0: CANARD_ERROR
   */
 int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) {
 	if (tx_frame == NULL) {
@@ -230,6 +230,10 @@ void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){
     pProtocolStat->lec          = (uint8_t)(protocolStatus.LastErrorCode & 0x07);
 }
 
+uint32_t canardSTM32GetAndClearRxDropCount(void) {
+    return 0;  // H7 FIFO0 (30 slots) has no software ring buffer; hardware overflow is tracked via GetProtocolStatus
+}
+
 int32_t canardSTM32GetTxQueueFillLevel(void){
     return 0;
 }

From 484e46f84d1e4b27190f76eee3348e0e6142cc76 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 12 Jun 2026 13:13:45 -0700
Subject: [PATCH 44/67] dronecan: address fourth code review findings
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- Wrap processCanardTxQueue in #if STM32H7||STM32F7 — function is
  ISR-only and has no callers in SITL builds; static + no callers
  caused -Werror=unused-function on CI
- Atomic read-clear of txErrCount: hold dronecanMaskTxISR across
  snapshot+zero to prevent ISR increment being silently dropped
- Atomic read-clear of rxDropCount: disable CAN1_RX0_IRQn across
  snapshot+zero in canardSTM32GetAndClearRxDropCount (F7)
- Make all eight file-local handler functions static: send_NodeStatus,
  handle_NodeStatus, handle_GNSSAuxiliary, handle_GNSSFix,
  handle_GNSSFix2, handle_GNSSRCTMStream, handle_BatteryInfo,
  handle_GetNodeInfo
- H7 global filter: FDCAN_REJECT_REMOTE for both RTR frame params
  (was FDCAN_FILTER_REMOTE which passed RTR frames to normal filter)
- H7 Receive: add comment that DataLength==byte count holds only in
  FDCAN_FRAME_CLASSIC mode (FDCAN_DLC_BYTES_0..8 equal 0..8)
- F7 ring buffer: use local index snapshots in push/pop to avoid
  double volatile re-reads of writeIndex/readIndex
---
 src/main/drivers/dronecan/dronecan.c          | 24 +++++++++--------
 .../libcanard/canard_stm32f7xx_driver.c       | 27 +++++++++----------
 .../libcanard/canard_stm32h7xx_driver.c       |  3 ++-
 3 files changed, 27 insertions(+), 27 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index a75d47dea73..36d4c55057e 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -162,8 +162,10 @@ void dronecanUpdate(timeUs_t currentTimeUs)
                 }
 
                 uint32_t rxDrops = canardSTM32GetAndClearRxDropCount();
+                dronecanMaskTxISR();
                 uint32_t txErrs = txErrCount;
                 txErrCount = 0;
+                dronecanUnmaskTxISR();
                 if (rxDrops > 0) {
                     LOG_DEBUG(CAN, "RX drops: %" PRIu32, rxDrops);
                 }
@@ -235,8 +237,9 @@ const dronecanNodeInfo_t *dronecanGetNode(uint8_t index) {
     return NULL;
 }
 
-// ---- TX queue ---------------------------------------------------------------
+// ---- ISR / HAL callbacks ----------------------------------------------------
 
+#if defined(STM32H7) || defined(STM32F7)
 /* Called from TX-complete ISR only. Already in interrupt context — no NVIC masking needed.
    For main-loop use, call processCanardTxQueueSafe() instead. */
 static void processCanardTxQueue(void) {
@@ -256,8 +259,7 @@ static void processCanardTxQueue(void) {
 		}
 	}
 }
-
-// ---- ISR / HAL callbacks ----------------------------------------------------
+#endif
 
 #if defined(STM32H7)
 void HAL_FDCAN_TxBufferCompleteCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t BufferIndexes)
@@ -304,7 +306,7 @@ static void processCanardTxQueueSafe(void) {
   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) {
+static void send_NodeStatus(void) {
     uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE];
 
     node_status.uptime_sec = millis() / 1000UL;
@@ -428,7 +430,7 @@ static bool shouldAcceptTransfer(const CanardInstance *ins,
 
 // 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) {
+static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) {
 	UNUSED(ins);
     struct uavcan_protocol_NodeStatus nodeStatus;
 
@@ -464,7 +466,7 @@ void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) {
 
 }
 
-void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) {
+static void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) {
 	UNUSED(ins);
     if (gpsConfig()->provider != GPS_DRONECAN) return;
     struct uavcan_equipment_gnss_Auxiliary gnssAuxiliary;
@@ -476,7 +478,7 @@ void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) {
     dronecanGPSReceiveGNSSAuxiliary(&gnssAuxiliary);
 }
 
-void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) {
+static void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) {
 	UNUSED(ins);
     if (gpsConfig()->provider != GPS_DRONECAN) return;
     struct uavcan_equipment_gnss_Fix gnssFix;
@@ -488,7 +490,7 @@ void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) {
     dronecanGPSReceiveGNSSFix(&gnssFix);
 }
 
-void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) {
+static void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) {
 	UNUSED(ins);
     if (gpsConfig()->provider != GPS_DRONECAN) return;
     struct uavcan_equipment_gnss_Fix2 gnssFix2;
@@ -500,7 +502,7 @@ void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) {
     dronecanGPSReceiveGNSSFix2(&gnssFix2);
 }
 
-void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) {
+static void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) {
 	UNUSED(ins);
     if (gpsConfig()->provider != GPS_DRONECAN) return;
     struct uavcan_equipment_gnss_RTCMStream gnssRTCMStream;
@@ -511,7 +513,7 @@ void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) {
 	}
 }
 
-void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
+static void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
 	UNUSED(ins);
     struct uavcan_equipment_power_BatteryInfo batteryInfo;
 
@@ -527,7 +529,7 @@ 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.
-void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
+static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
 	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 d34e3f66be5..d8cd507fb87 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -269,8 +269,10 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
 }
 
 uint32_t canardSTM32GetAndClearRxDropCount(void) {
+    HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn);
     uint32_t count = rxDropCount;
     rxDropCount = 0;
+    HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
     return count;
 }
 
@@ -418,39 +420,34 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi
 }
 
 static int8_t rxBufferPushFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) {
-    uint8_t next;
-    RxFrame_t *pCurrentRxMsg;
-
-    next = rxBuf->writeIndex + 1;
-    if(next >= RX_BUFFER_SIZE){
+    uint8_t wi = rxBuf->writeIndex;  // snapshot: only this ISR writes writeIndex
+    uint8_t next = wi + 1;
+    if (next >= RX_BUFFER_SIZE) {
         next = 0;
     }
 
-    if(next == rxBuf->readIndex) {
+    if (next == rxBuf->readIndex) {
         return -1;  // rxBuf is full
     }
-    pCurrentRxMsg = &rxBuf->rxMsg[rxBuf->writeIndex];
-    memcpy(pCurrentRxMsg, rxMsg, sizeof(RxFrame_t));
+    memcpy(&rxBuf->rxMsg[wi], rxMsg, sizeof(RxFrame_t));
     __DMB();  // ensure frame data is visible to main loop before writeIndex advance
     rxBuf->writeIndex = next;
     return 0;
 }
 
 static int8_t rxBufferPopFrame(struct RxBuffer_t *rxBuf, RxFrame_t *rxMsg) {
-    uint8_t next;
-    RxFrame_t *pCurrentRxMsg;
+    uint8_t ri = rxBuf->readIndex;  // snapshot: only main loop writes readIndex
 
-    if(rxBuf->writeIndex == rxBuf->readIndex){
+    if (rxBuf->writeIndex == ri) {
         return -1;  // Nothing to read
     }
 
-    next = rxBuf->readIndex + 1;
-    if (next >= RX_BUFFER_SIZE){
+    uint8_t next = ri + 1;
+    if (next >= RX_BUFFER_SIZE) {
         next = 0;
     }
     __DMB();  // ensure writeIndex read is complete before reading frame data written by ISR
-    pCurrentRxMsg = &rxBuf->rxMsg[rxBuf->readIndex];
-    memcpy(rxMsg, pCurrentRxMsg, sizeof(RxFrame_t));
+    memcpy(rxMsg, &rxBuf->rxMsg[ri], sizeof(RxFrame_t));
     rxBuf->readIndex = next;
     return 0;
 }
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index 88e2a3925be..c0e0b0f7c72 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -93,7 +93,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate)
         LOG_ERROR(CAN, "Failed Config Filter");
         return -CANARD_ERROR_INTERNAL;
     }
-    if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_REJECT, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE) != HAL_OK) {
+    if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_REJECT, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_REJECT_REMOTE, FDCAN_REJECT_REMOTE) != HAL_OK) {
         LOG_ERROR(CAN, "Failed to config FDCAN filter");
         return -CANARD_ERROR_INTERNAL;
     }
@@ -147,6 +147,7 @@ int16_t canardSTM32Receive(CanardCANFrame *const rx_frame) {
 			rx_frame->id |= CANARD_CAN_FRAME_RTR;
 		}
 
+		/* FDCAN_DLC_BYTES_0..8 equal 0..8, so DataLength is the byte count in FDCAN_FRAME_CLASSIC mode. */
 		rx_frame->data_len = RxHeader.DataLength;
 		memcpy(rx_frame->data, RxData, RxHeader.DataLength);
 

From 9c038c4a7e4e442dd3a377044f841b87b2387f5e Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 12 Jun 2026 13:41:22 -0700
Subject: [PATCH 45/67] dronecan: address fifth code review findings

- H7 Transmit: add FDCAN_FRAME_CLASSIC comment on DataLength assignment
  matching the comment already on the receive path
- F7 init: add comment explaining pre-shifted timing register values
- SITL: map RTR flag in sitlCANFrameToLinux/FromLinux, consistent with
  hardware drivers
- SITL canardSTM32Transmit: add ERR frame guard matching hardware drivers
- handle_GNSSRCTMStream: remove dead decode, add comment that RTCM
  forwarding is not yet implemented
- dronecanInit: add default case to bitrate switch for EEPROM corruption
- vendor_specific_status_code: explicit (uint16_t) cast with comment
  acknowledging bits 16-30 of armingFlags are not transmitted
- fport.c: remove dead static volatile frameErrors counter (written but
  never read; triggered -Werror=unused-but-set-variable on GCC 16)
---
 src/main/drivers/dronecan/dronecan.c             | 16 ++++++++--------
 .../dronecan/libcanard/canard_sitl_driver.c      | 10 +++++++++-
 .../dronecan/libcanard/canard_stm32f7xx_driver.c |  1 +
 .../dronecan/libcanard/canard_stm32h7xx_driver.c |  2 +-
 src/main/rx/fport.c                              |  2 --
 5 files changed, 19 insertions(+), 12 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 36d4c55057e..b062e919acd 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -88,6 +88,11 @@ void dronecanInit(void)
             LOG_ERROR(SYSTEM, "Undefined bitrate set in configuration. 500kbps selected");
             bitrate = 500000;
             break;
+
+        default:
+            LOG_ERROR(SYSTEM, "Invalid bitrate setting, defaulting to 500kbps");
+            bitrate = 500000;
+            break;
     }
     if(canardSTM32CAN1_Init(bitrate) != CANARD_OK)
     {
@@ -321,7 +326,7 @@ static void send_NodeStatus(void) {
     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;
+    node_status.vendor_specific_status_code = (uint16_t)(armingFlags & 0xFFFF);  /* field is 16-bit by UAVCAN spec; bits 16-30 of armingFlags are not transmitted */
 
     uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer);
 
@@ -504,13 +509,8 @@ static void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) {
 
 static void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) {
 	UNUSED(ins);
-    if (gpsConfig()->provider != GPS_DRONECAN) return;
-    struct uavcan_equipment_gnss_RTCMStream gnssRTCMStream;
-
-	if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &gnssRTCMStream)) {
-		LOG_DEBUG(CAN, "RTCMStream decode failed");
-		return;
-	}
+	UNUSED(transfer);
+    /* RTCM forwarding not yet implemented. Accepted in shouldAcceptTransfer for future use. */
 }
 
 static void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
diff --git a/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c b/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c
index 9308e12660d..9b02702e2fe 100644
--- a/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c
@@ -167,6 +167,10 @@ static void sitlCANFrameToLinux(const CanardCANFrame *const src, struct can_fram
         dst->can_id = src->id & CANARD_CAN_STD_ID_MASK;
     }
 
+    if (src->id & CANARD_CAN_FRAME_RTR) {
+        dst->can_id |= CAN_RTR_FLAG;
+    }
+
     // Copy data
     dst->can_dlc = src->data_len;
     if (src->data_len > 0) {
@@ -184,6 +188,10 @@ static void sitlCANFrameFromLinux(const struct can_frame *const src, CanardCANFr
         dst->id = src->can_id & CANARD_CAN_STD_ID_MASK;
     }
 
+    if (src->can_id & CAN_RTR_FLAG) {
+        dst->id |= CANARD_CAN_FRAME_RTR;
+    }
+
     // Copy data
     dst->data_len = src->can_dlc;
     if (src->can_dlc > 0) {
@@ -275,7 +283,7 @@ int16_t canardSTM32Receive(CanardCANFrame *const rx_frame) {
  * @retval 1 on success, 0 if busy, negative on error
  */
 int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) {
-    if (tx_frame == NULL) {
+    if (tx_frame == NULL || (tx_frame->id & CANARD_CAN_FRAME_ERR)) {
         return -CANARD_ERROR_INVALID_ARGUMENT;
     }
 
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
index d8cd507fb87..8b367e7a4c4 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -88,6 +88,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate)
     }
 
     hcan1.Init.Prescaler = out_timings.prescaler;
+    /* F7 bxCAN HAL ORs these directly into BTR; values must be pre-shifted to their register positions */
     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;
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index c0e0b0f7c72..9fccf8ea4b1 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -188,7 +188,7 @@ int16_t canardSTM32Transmit(const CanardCANFrame* const tx_frame) {
 		TxHeader.Identifier = tx_frame->id & CANARD_CAN_STD_ID_MASK;
 	}
 
-	TxHeader.DataLength = tx_frame->data_len;
+	TxHeader.DataLength = tx_frame->data_len;  /* FDCAN_DLC_BYTES_0..8 == 0..8; valid only in FDCAN_FRAME_CLASSIC mode */
 
 	if (tx_frame->id & CANARD_CAN_FRAME_RTR) {
 		TxHeader.TxFrameType = FDCAN_REMOTE_FRAME;
diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c
index 0ef3e8b2b63..32f8c638fe2 100644
--- a/src/main/rx/fport.c
+++ b/src/main/rx/fport.c
@@ -149,8 +149,6 @@ static serialPort_t *fportPort;
 
 static void reportFrameError(uint8_t errorReason) {
     UNUSED(errorReason);
-    static volatile uint16_t frameErrors = 0;
-    frameErrors++;
 }
 
 // Receive ISR callback

From 85183b32c671d8b2b42c9b037cb8695e61722a70 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 12 Jun 2026 14:28:44 -0700
Subject: [PATCH 46/67] dronecan: address sixth code review findings

- H7 Receive: add bounds guard on DataLength before assignment to
  data_len (uint8_t); mirrors existing TX path guard; safe no-op in
  FDCAN_FRAME_CLASSIC mode but prevents silent truncation if DLC ever
  exceeds 8
- F7: fix stale comment hfdcan->ErrorCode -> hcan1.ErrorCode
- dronecanInit: fix mixed tab/space indentation in canardInit() call
- dronecanGetBitrateKbps: return 500 for DRONECAN_BITRATE_COUNT and
  default cases, matching what dronecanInit actually selects
- Fix typo: incremeneted -> incremented
- SITL GetRxFifoFillLevel: add comment explaining FIONREAD on SOCK_RAW
  returns next-datagram size only, so result is 0 or 1
---
 src/main/drivers/dronecan/dronecan.c             | 16 ++++++++--------
 .../dronecan/libcanard/canard_sitl_driver.c      |  4 +++-
 .../dronecan/libcanard/canard_stm32f7xx_driver.c |  2 +-
 .../dronecan/libcanard/canard_stm32h7xx_driver.c |  7 +++++--
 4 files changed, 17 insertions(+), 12 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index b062e919acd..2c657af5daf 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -104,11 +104,11 @@ 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
@@ -232,9 +232,9 @@ uint32_t dronecanGetBitrateKbps(void)
             return 1000;
 
         case DRONECAN_BITRATE_COUNT:
-            return 0;
+        default:
+            return 500;
     }
-    return 0;
 }
 
 const dronecanNodeInfo_t *dronecanGetNode(uint8_t index) {
@@ -331,7 +331,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_sitl_driver.c b/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c
index 9b02702e2fe..45c7639b5e4 100644
--- a/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_sitl_driver.c
@@ -327,8 +327,10 @@ int32_t canardSTM32GetRxFifoFillLevel(void) {
 #ifdef __linux__
     if (can_mode == SITL_CAN_MODE_SOCKETCAN && can_socket >= 0) {
         int available;
+        /* FIONREAD on SOCK_RAW returns the byte size of the next pending datagram only,
+           so this yields 0 or 1 — SITL processes at most one frame per scheduler tick. */
         if (ioctl(can_socket, FIONREAD, &available) == 0) {
-            return available / sizeof(struct can_frame);
+            return available / (int)sizeof(struct can_frame);
         }
     }
 #endif
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
index 8b367e7a4c4..9c8f8b472b1 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -161,7 +161,7 @@ int16_t canardSTM32Receive(CanardCANFrame *const rx_frame) {
 		rx_frame->iface_id = 0;
 		return 1;
 	}
-	// Either no CAN msg to be read, or an error that can be read from hfdcan->ErrorCode
+	// Either no CAN msg to be read, or an error that can be read from hcan1.ErrorCode
 	return 0;
 }
 
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index 9fccf8ea4b1..acf902e7056 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -148,8 +148,11 @@ int16_t canardSTM32Receive(CanardCANFrame *const rx_frame) {
 		}
 
 		/* FDCAN_DLC_BYTES_0..8 equal 0..8, so DataLength is the byte count in FDCAN_FRAME_CLASSIC mode. */
-		rx_frame->data_len = RxHeader.DataLength;
-		memcpy(rx_frame->data, RxData, RxHeader.DataLength);
+		if (RxHeader.DataLength > CANARD_CAN_FRAME_MAX_DATA_LEN) {
+			return -CANARD_ERROR_INVALID_ARGUMENT;  /* should never happen in FDCAN_FRAME_CLASSIC mode */
+		}
+		rx_frame->data_len = (uint8_t)RxHeader.DataLength;
+		memcpy(rx_frame->data, RxData, rx_frame->data_len);
 
 		// assume a single interface
 		rx_frame->iface_id = 0;

From 9dd06aee82e59319efa84d105384033605613055 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 12 Jun 2026 14:58:25 -0700
Subject: [PATCH 47/67] dronecan: fix H7 FDCAN filter type to accept all
 extended IDs

FDCAN_FILTER_DUAL with FilterID1=0x0, FilterID2=0x1FFFFFFF only matched
those two exact IDs. Replace with FDCAN_FILTER_MASK (pattern=0, mask=0)
which accepts any extended ID. Also fix stale comments in H7 driver and
dronecanInit.
---
 src/main/drivers/dronecan/dronecan.c                        | 2 +-
 .../drivers/dronecan/libcanard/canard_stm32h7xx_driver.c    | 6 +++---
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 2c657af5daf..12f5dbfb117 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -65,7 +65,7 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer);
 
 void dronecanInit(void)
 {
-    uint32_t bitrate = 500000; // At least define 500000
+    uint32_t bitrate = 500000;
 
     switch (dronecanConfig()->bitRateKbps){
         case DRONECAN_BITRATE_125KBPS:
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index acf902e7056..15e7e885896 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -45,10 +45,10 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate)
     FDCAN_FilterTypeDef sFilterConfig;
     sFilterConfig.IdType = FDCAN_EXTENDED_ID;
     sFilterConfig.FilterIndex = 0;
-    sFilterConfig.FilterType = FDCAN_FILTER_DUAL;
+    sFilterConfig.FilterType = FDCAN_FILTER_MASK; /* ID1=pattern 0x0, ID2=mask 0x0: all bits don't care, accept any extended ID */
     sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
     sFilterConfig.FilterID1 = 0x0;
-    sFilterConfig.FilterID2 = 0x1FFFFFFFU;
+    sFilterConfig.FilterID2 = 0x0;
     hfdcan1.Instance = FDCAN1;
     hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC;  // Initialize in CAN2.0 mode not CAN_FD
     hfdcan1.Init.Mode = FDCAN_MODE_NORMAL;
@@ -235,7 +235,7 @@ void canardSTM32GetProtocolStatus(canardProtocolStatus_t *pProtocolStat){
 }
 
 uint32_t canardSTM32GetAndClearRxDropCount(void) {
-    return 0;  // H7 FIFO0 (30 slots) has no software ring buffer; hardware overflow is tracked via GetProtocolStatus
+    return 0;  // H7 FIFO0 (30 slots) has no software ring buffer; FIFO overflow drops are not currently counted
 }
 
 int32_t canardSTM32GetTxQueueFillLevel(void){

From e0b5f8266324ecee608de2a10041e6e3a6ecdcf7 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 12 Jun 2026 20:42:47 -0700
Subject: [PATCH 48/67] Enforce FIFO ordering to prevent spin errors in multi
 frame messages on bxCan (F7) targets.  Set max quanta per bit to 18
 unconditionally to fix 1MBps on bxCan.

---
 src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c | 4 ++--
 1 file 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 9c8f8b472b1..16b15a99d0a 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -79,7 +79,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate)
     hcan1.Init.AutoWakeUp = DISABLE;
     hcan1.Init.AutoRetransmission = DISABLE;  // ENABLE fills the TX FIFO on a degraded bus; DroneCAN reliability is handled at the application layer
     hcan1.Init.ReceiveFifoLocked = DISABLE;
-    hcan1.Init.TransmitFifoPriority = DISABLE;
+    hcan1.Init.TransmitFifoPriority = ENABLE;
 
     if (!canardSTM32ComputeTimings(bitrate, &out_timings))
     {
@@ -332,7 +332,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 = 18; //(target_bitrate >= 1000000) ? 10 : 17;
     static const int MaxSamplePointLocation = 900;
 
     /*

From cf3f98b5742a3a7c47a962aac511a4f8cb698ea4 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Fri, 12 Jun 2026 20:51:29 -0700
Subject: [PATCH 49/67] Allow FDCAN targets to use 18 quanta per bit solution
 as well at 1 MBps.

---
 src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index 15e7e885896..1403ed9a11d 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -326,7 +326,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 = 18; //(target_bitrate >= 1000000) ? 10 : 17;
 
     static const int MaxSamplePointLocation = 900;
 

From 6079e86ace5c2383c65cbfdcfe5b95497b57cae5 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Sat, 13 Jun 2026 06:54:52 -0700
Subject: [PATCH 50/67] test(dronecan): add bxCAN timing unit tests for
 max_quanta=18 change

17 tests covering canardSTM32ComputeTimings() algorithm at standard
bitrates (125k/250k/500k/1M) for PCLK 54 MHz and 48 MHz. Includes
an 18-quanta regression test that would fail against the old 10-quanta
limit (prescaler=6, 9 tq/bit) and passes with the new unconditional
18-quanta path (prescaler=3, 18 tq/bit at 1 Mbps / 54 MHz).
---
 src/test/unit/CMakeLists.txt           |   3 +
 src/test/unit/bxcan_timing_unittest.cc | 301 +++++++++++++++++++++++++
 2 files changed, 304 insertions(+)
 create mode 100644 src/test/unit/bxcan_timing_unittest.cc

diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt
index 5b43b636a77..e9e2eb7d8a9 100644
--- a/src/test/unit/CMakeLists.txt
+++ b/src/test/unit/CMakeLists.txt
@@ -53,6 +53,9 @@ set_property(SOURCE dronecan_messages_unittest.cc PROPERTY extra_includes
     "../../lib/main/Dronecan/dsdlc_generated/include")
 set_property(SOURCE dronecan_messages_unittest.cc PROPERTY definitions USE_DRONECAN CANARD_ENABLE_TAO_OPTION=0)
 
+# bxCAN timing algorithm tests - self-contained, no driver or HAL dependencies
+# Keep in sync with canard_stm32f7xx_driver.c:canardSTM32ComputeTimings
+
 # Libcanard core tests - CANARD_INTERNAL= makes static functions accessible
 set_property(SOURCE canard_unittest.cc PROPERTY depends
     "drivers/dronecan/libcanard/canard.c")
diff --git a/src/test/unit/bxcan_timing_unittest.cc b/src/test/unit/bxcan_timing_unittest.cc
new file mode 100644
index 00000000000..5d487b7e5e8
--- /dev/null
+++ b/src/test/unit/bxcan_timing_unittest.cc
@@ -0,0 +1,301 @@
+/**
+ * bxCAN Timing Computation Unit Tests
+ *
+ * Regression tests for the timing algorithm in
+ * canard_stm32f7xx_driver.c:canardSTM32ComputeTimings().
+ *
+ * The function is static and reads PCLK via HAL_RCC_GetPCLK1Freq(), so it
+ * cannot be called directly from a unit test.  canardSTM32ComputeTimingsForPCLK()
+ * below is an exact copy with pclk passed as a parameter instead.
+ *
+ * KEEP IN SYNC with canard_stm32f7xx_driver.c.
+ *
+ * Primary test PCLK: 54 MHz — STM32F765 APB1 at SYSCLK=216 MHz / APBprescaler=4.
+ * Secondary test PCLK: 48 MHz — alternate F7 configuration.
+ */
+
+#include <cstdint>
+#include <cstring>
+
+#include "gtest/gtest.h"
+
+// ---------------------------------------------------------------------------
+// Mirror of canard_stm32f7xx_driver.c:canardSTM32ComputeTimings
+// pclk replaces HAL_RCC_GetPCLK1Freq().  All other logic is identical.
+// ---------------------------------------------------------------------------
+
+struct Timings {
+    uint16_t prescaler;
+    uint8_t  sjw;
+    uint8_t  bs1;
+    uint8_t  bs2;
+};
+
+static bool canardSTM32ComputeTimingsForPCLK(const uint32_t pclk,
+                                              const uint32_t target_bitrate,
+                                              struct Timings *out_timings)
+{
+    if (target_bitrate < 1) {
+        return false;
+    }
+
+    static const int MaxBS1 = 16;
+    static const int MaxBS2 = 8;
+
+    const int max_quanta_per_bit = 18;
+    static const int MaxSamplePointLocation = 900;
+
+    const uint32_t prescaler_bs = pclk / target_bitrate;
+
+    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;
+        }
+        bs1_bs2_sum--;
+    }
+
+    const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum);
+    if ((prescaler < 1U) || (prescaler > 1024U)) {
+        return false;
+    }
+
+    struct BsPair {
+        uint8_t  bs1;
+        uint8_t  bs2;
+        uint16_t sample_point_permill;
+    } solution;
+
+    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) {
+        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));
+    }
+
+    if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) ||
+        !((solution.bs1 >= 1) && (solution.bs1 <= MaxBS1) &&
+          (solution.bs2 >= 1) && (solution.bs2 <= MaxBS2))) {
+        return false;
+    }
+
+    out_timings->prescaler = (uint16_t)(prescaler);
+    out_timings->sjw = 3;
+    out_timings->bs1 = (uint8_t)(solution.bs1) - 1;  // HAL adds +1 internally
+    out_timings->bs2 = (uint8_t)(solution.bs2) - 1;  // HAL adds +1 internally
+
+    return true;
+}
+
+// ---------------------------------------------------------------------------
+// Helper: back-calculate bitrate from HAL register values
+// ---------------------------------------------------------------------------
+
+static uint32_t bitrateFromTimings(uint32_t pclk, const struct Timings &t)
+{
+    // HAL bs1/bs2 values are stored with -1 offset; hardware adds +1 back
+    uint32_t total_tq = 1u + (t.bs1 + 1u) + (t.bs2 + 1u);
+    return pclk / (t.prescaler * total_tq);
+}
+
+// ---------------------------------------------------------------------------
+// Test fixture
+// ---------------------------------------------------------------------------
+
+class BxCanTimingTest : public ::testing::Test {
+protected:
+    struct Timings t;
+
+    void SetUp() override { memset(&t, 0, sizeof(t)); }
+};
+
+static const uint32_t PCLK_54M = 54000000U;
+static const uint32_t PCLK_48M = 48000000U;
+
+// ===========================================================================
+// A. Bitrate correctness at PCLK = 54 MHz
+//    All four standard bitrates should resolve to 18 quanta/bit.
+// ===========================================================================
+
+TEST_F(BxCanTimingTest, Pclk54_1Mbps_Succeeds)
+{
+    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 1000000, &t));
+    EXPECT_EQ(bitrateFromTimings(PCLK_54M, t), 1000000U);
+}
+
+TEST_F(BxCanTimingTest, Pclk54_500kbps_Succeeds)
+{
+    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 500000, &t));
+    EXPECT_EQ(bitrateFromTimings(PCLK_54M, t), 500000U);
+}
+
+TEST_F(BxCanTimingTest, Pclk54_250kbps_Succeeds)
+{
+    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 250000, &t));
+    EXPECT_EQ(bitrateFromTimings(PCLK_54M, t), 250000U);
+}
+
+TEST_F(BxCanTimingTest, Pclk54_125kbps_Succeeds)
+{
+    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 125000, &t));
+    EXPECT_EQ(bitrateFromTimings(PCLK_54M, t), 125000U);
+}
+
+// ===========================================================================
+// B. 18-quanta regression — verifies max_quanta_per_bit=18 is in effect
+//
+//    At 54 MHz / 1 Mbps the solver must find 18 quanta/bit → prescaler=3.
+//    The previous limit of 10 quanta would have yielded prescaler=6 (9 quanta).
+// ===========================================================================
+
+TEST_F(BxCanTimingTest, Pclk54_1Mbps_Uses18Quanta)
+{
+    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 1000000, &t));
+
+    // 54 MHz / 3 / 18 quanta = 1 Mbps
+    EXPECT_EQ(t.prescaler, 3u);
+
+    // bs1 raw = t.bs1+1 = 15, bs2 raw = t.bs2+1 = 2 → 1+15+2 = 18 quanta/bit
+    EXPECT_EQ(t.bs1 + 1u, 15u);
+    EXPECT_EQ(t.bs2 + 1u,  2u);
+}
+
+TEST_F(BxCanTimingTest, Pclk54_500kbps_Uses18Quanta)
+{
+    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 500000, &t));
+    EXPECT_EQ(t.prescaler, 6u);
+    EXPECT_EQ(t.bs1 + 1u, 15u);
+    EXPECT_EQ(t.bs2 + 1u,  2u);
+}
+
+TEST_F(BxCanTimingTest, Pclk54_250kbps_Uses18Quanta)
+{
+    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 250000, &t));
+    EXPECT_EQ(t.prescaler, 12u);
+    EXPECT_EQ(t.bs1 + 1u, 15u);
+    EXPECT_EQ(t.bs2 + 1u,  2u);
+}
+
+TEST_F(BxCanTimingTest, Pclk54_125kbps_Uses18Quanta)
+{
+    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 125000, &t));
+    EXPECT_EQ(t.prescaler, 24u);
+    EXPECT_EQ(t.bs1 + 1u, 15u);
+    EXPECT_EQ(t.bs2 + 1u,  2u);
+}
+
+// ===========================================================================
+// C. Bitrate correctness at PCLK = 48 MHz (alternate F7 config)
+//    48 MHz / bitrate is not divisible by 18 for standard rates,
+//    so the solver falls back to 16 quanta/bit at prescaler=3,6,12,24.
+// ===========================================================================
+
+TEST_F(BxCanTimingTest, Pclk48_1Mbps_Succeeds)
+{
+    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_48M, 1000000, &t));
+    EXPECT_EQ(bitrateFromTimings(PCLK_48M, t), 1000000U);
+    // 48 MHz / 3 / 16 quanta = 1 Mbps
+    EXPECT_EQ(t.prescaler, 3u);
+    EXPECT_EQ(t.bs1 + 1u, 13u);
+    EXPECT_EQ(t.bs2 + 1u,  2u);
+}
+
+TEST_F(BxCanTimingTest, Pclk48_500kbps_Succeeds)
+{
+    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_48M, 500000, &t));
+    EXPECT_EQ(bitrateFromTimings(PCLK_48M, t), 500000U);
+    EXPECT_EQ(t.prescaler, 6u);
+}
+
+TEST_F(BxCanTimingTest, Pclk48_250kbps_Succeeds)
+{
+    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_48M, 250000, &t));
+    EXPECT_EQ(bitrateFromTimings(PCLK_48M, t), 250000U);
+    EXPECT_EQ(t.prescaler, 12u);
+}
+
+TEST_F(BxCanTimingTest, Pclk48_125kbps_Succeeds)
+{
+    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_48M, 125000, &t));
+    EXPECT_EQ(bitrateFromTimings(PCLK_48M, t), 125000U);
+    EXPECT_EQ(t.prescaler, 24u);
+}
+
+// ===========================================================================
+// D. Hardware constraint validation across all standard bitrates
+// ===========================================================================
+
+TEST_F(BxCanTimingTest, HwConstraints_AllStandardBitrates)
+{
+    const uint32_t bitrates[] = {125000, 250000, 500000, 1000000};
+    const uint32_t pclks[]    = {PCLK_54M, PCLK_48M};
+
+    for (uint32_t pclk : pclks) {
+        for (uint32_t br : bitrates) {
+            SCOPED_TRACE(testing::Message() << "pclk=" << pclk << " br=" << br);
+            ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(pclk, br, &t));
+
+            // BS1 raw (t.bs1+1) must be in [1..16]
+            EXPECT_GE(t.bs1 + 1u, 1u);
+            EXPECT_LE(t.bs1 + 1u, 16u);
+
+            // BS2 raw (t.bs2+1) must be in [1..8]
+            EXPECT_GE(t.bs2 + 1u, 1u);
+            EXPECT_LE(t.bs2 + 1u, 8u);
+
+            // Prescaler in [1..1024]
+            EXPECT_GE(t.prescaler, 1u);
+            EXPECT_LE(t.prescaler, 1024u);
+
+            // SJW fixed at 3 (hardware SJW = 4 tq)
+            EXPECT_EQ(t.sjw, 3u);
+
+            // Back-calculated bitrate must match the request
+            EXPECT_EQ(bitrateFromTimings(pclk, t), br);
+        }
+    }
+}
+
+TEST_F(BxCanTimingTest, SamplePoint_InValidRange)
+{
+    const uint32_t bitrates[] = {125000, 250000, 500000, 1000000};
+
+    for (uint32_t br : bitrates) {
+        SCOPED_TRACE(br);
+        ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, br, &t));
+
+        uint32_t bs1_raw = t.bs1 + 1u;
+        uint32_t bs2_raw = t.bs2 + 1u;
+        uint32_t total   = 1u + bs1_raw + bs2_raw;
+        uint32_t sp_permill = 1000u * (1u + bs1_raw) / total;
+
+        EXPECT_GE(sp_permill, 750u);  // practical CAN minimum
+        EXPECT_LE(sp_permill, 900u);  // driver MaxSamplePointLocation
+    }
+}
+
+// ===========================================================================
+// E. Invalid and unsolvable inputs
+// ===========================================================================
+
+TEST_F(BxCanTimingTest, Invalid_ZeroBitrate)
+{
+    EXPECT_FALSE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 0, &t));
+}
+
+TEST_F(BxCanTimingTest, Invalid_UnsolvableBitrate)
+{
+    // 999999 bps: prescaler_bs=54 (integer division), but 54M/54=1M ≠ 999999
+    // Final bitrate validation catches it.
+    EXPECT_FALSE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 999999, &t));
+}
+
+TEST_F(BxCanTimingTest, Invalid_ExcessivelyLowBitrate)
+{
+    // prescaler_bs = 54M/100 = 540000 → prescaler = 540000/18 = 30000 > 1024
+    EXPECT_FALSE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 100, &t));
+}

From c1b52a5dcdc94dad30c9e77b9969e5d7bff86084 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Sun, 14 Jun 2026 17:38:01 -0700
Subject: [PATCH 51/67] dronecan: reduce flash waste, improve GPIO config, add
 diagnostics

- Wrap canard.c with USE_DRONECAN guard via platform.h include so the
  libcanard protocol engine is excluded from non-CAN targets (e.g. F722).
  Add comment noting both lines must be preserved on library updates.
- Replace USE_GPS_PROTO_DRONECAN with USE_DRONECAN in gps.c and
  gps_dronecan.c; remove the define from common.h. GPS DroneCAN callbacks
  only make sense when CAN hardware is present.
- Add IOCFG_AF_PP_FAST_UP (VERY_HIGH speed + PULLUP) to io.h for F7/H7,
  matching ST CubeF7 example recommendation for CAN GPIO pins.
- Use IOCFG_AF_PP_FAST_UP for CAN1_TX and CAN1_RX in both the F7 bxCAN
  and H7 FDCAN drivers, replacing the previous LOW speed / no-pull config.
- Add bus-off counter and canard pool allocator stats to dronecan driver
  and expose both in the CLI dronecan command.
---
 src/main/drivers/dronecan/dronecan.c                 | 12 ++++++++++++
 src/main/drivers/dronecan/dronecan.h                 |  7 +++++--
 src/main/drivers/dronecan/libcanard/canard.c         |  7 +++++++
 .../dronecan/libcanard/canard_stm32f7xx_driver.c     |  8 ++++----
 .../dronecan/libcanard/canard_stm32h7xx_driver.c     |  4 ++--
 src/main/drivers/io.h                                |  1 +
 src/main/fc/cli.c                                    |  7 +++++++
 src/main/io/gps.c                                    |  2 +-
 src/main/io/gps_dronecan.c                           |  2 +-
 src/main/target/common.h                             |  1 -
 10 files changed, 40 insertions(+), 11 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 12f5dbfb117..a96e6a75ba1 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -42,6 +42,7 @@ static dronecanState_e dronecanState = STATE_DRONECAN_INIT;
 static uint8_t activeNodeCount = 0;
 static dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES];
 static volatile uint32_t txErrCount = 0;
+static uint32_t busOffCount = 0;
 
 #if defined(STM32H7)
 static inline void dronecanMaskTxISR(void)   { NVIC_DisableIRQ(FDCAN1_IT0_IRQn); }
@@ -181,6 +182,7 @@ void dronecanUpdate(timeUs_t currentTimeUs)
                 if (protocolStatus.BusOff != 0) {
                     dronecanState = STATE_DRONECAN_BUS_OFF;
                     busoffTimeUs = currentTimeUs;
+                    busOffCount++;
                 }
             }
             break;
@@ -242,6 +244,16 @@ const dronecanNodeInfo_t *dronecanGetNode(uint8_t index) {
     return NULL;
 }
 
+uint32_t dronecanGetBusOffCount(void)
+{
+    return busOffCount;
+}
+
+CanardPoolAllocatorStatistics dronecanGetPoolStats(void)
+{
+    return canardGetPoolAllocatorStatistics(&canard);
+}
+
 // ---- ISR / HAL callbacks ----------------------------------------------------
 
 #if defined(STM32H7) || defined(STM32F7)
diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h
index 202011b0483..c69981b9692 100644
--- a/src/main/drivers/dronecan/dronecan.h
+++ b/src/main/drivers/dronecan/dronecan.h
@@ -2,6 +2,7 @@
 
 #include "common/time.h"
 #include "config/parameter_group.h"
+#include "drivers/dronecan/libcanard/canard.h"
 
 typedef enum {
     DRONECAN_BITRATE_125KBPS = 0,
@@ -47,9 +48,11 @@ typedef struct dronecanNodeStatus_s {
 
 void dronecanInit(void);
 void dronecanUpdate(timeUs_t currentTimeUs);
-dronecanState_e dronecanGetState(void);                                               
-uint8_t dronecanGetNodeCount(void);                                                                                                                                                                                                       
+dronecanState_e dronecanGetState(void);
+uint8_t dronecanGetNodeCount(void);
 uint32_t dronecanGetBitrateKbps(void);
 const dronecanNodeInfo_t *dronecanGetNode(uint8_t index);
+uint32_t dronecanGetBusOffCount(void);
+CanardPoolAllocatorStatistics dronecanGetPoolStats(void);
 
 PG_DECLARE(dronecanConfig_t, dronecanConfig);
diff --git a/src/main/drivers/dronecan/libcanard/canard.c b/src/main/drivers/dronecan/libcanard/canard.c
index 784d2ef2637..1dfb026d060 100644
--- a/src/main/drivers/dronecan/libcanard/canard.c
+++ b/src/main/drivers/dronecan/libcanard/canard.c
@@ -24,6 +24,11 @@
  * Documentation: http://uavcan.org/Implementations/Libcanard
  */
 
+// INAV addition: platform.h provides USE_DRONECAN; both lines must be preserved when updating this library.
+#include "platform.h"
+
+#ifdef USE_DRONECAN
+
 #include "canard_internals.h"
 #include <string.h>
 
@@ -1958,3 +1963,5 @@ CANARD_INTERNAL void freeBlock(CanardPoolAllocator* allocator, void* p)
     canard_allocate_sem_give(allocator);
 #endif
 }
+
+#endif
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
index 16b15a99d0a..e783afec91a 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -77,7 +77,7 @@ int16_t canardSTM32CAN1_Init(uint32_t bitrate)
     hcan1.Init.TimeTriggeredMode = DISABLE;
     hcan1.Init.AutoBusOff = ENABLE;
     hcan1.Init.AutoWakeUp = DISABLE;
-    hcan1.Init.AutoRetransmission = DISABLE;  // ENABLE fills the TX FIFO on a degraded bus; DroneCAN reliability is handled at the application layer
+    hcan1.Init.AutoRetransmission = ENABLE;  // ENABLE fills the TX FIFO on a degraded bus; DroneCAN reliability is handled at the application layer
     hcan1.Init.ReceiveFifoLocked = DISABLE;
     hcan1.Init.TransmitFifoPriority = ENABLE;
 
@@ -290,9 +290,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);
+    IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_TX)), IOCFG_AF_PP_FAST_UP, 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);
+    IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_RX)), IOCFG_AF_PP_FAST_UP, GPIO_AF9_CAN1);
 #endif
 
 
@@ -332,7 +332,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 = 18; //(target_bitrate >= 1000000) ? 10 : 17;
+    const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17;
     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 1403ed9a11d..32e558df129 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -285,9 +285,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);
+    IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_TX)), IOCFG_AF_PP_FAST_UP, 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);
+    IOConfigGPIOAF(IOGetByTag(IO_TAG(CAN1_RX)), IOCFG_AF_PP_FAST_UP, GPIO_AF9_FDCAN1);
 #endif
 
 
diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h
index f08da7393dd..581faf7d659 100644
--- a/src/main/drivers/io.h
+++ b/src/main/drivers/io.h
@@ -44,6 +44,7 @@
 #define IOCFG_AF_PP_FAST     IO_CONFIG(GPIO_MODE_AF_PP,     GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLDOWN)
 #define IOCFG_AF_PP          IO_CONFIG(GPIO_MODE_AF_PP,     GPIO_SPEED_FREQ_LOW,  GPIO_NOPULL)
 #define IOCFG_AF_PP_PD       IO_CONFIG(GPIO_MODE_AF_PP,     GPIO_SPEED_FREQ_LOW,  GPIO_PULLDOWN)
+#define IOCFG_AF_PP_FAST_UP  IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP)
 #define IOCFG_AF_PP_UP       IO_CONFIG(GPIO_MODE_AF_PP,     GPIO_SPEED_FREQ_LOW,  GPIO_PULLUP)
 #define IOCFG_AF_OD          IO_CONFIG(GPIO_MODE_AF_OD,     GPIO_SPEED_FREQ_LOW,  GPIO_NOPULL)
 #define IOCFG_AF_OD_UP       IO_CONFIG(GPIO_MODE_AF_OD,     GPIO_SPEED_FREQ_LOW,  GPIO_PULLUP)
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 124294e4e5f..132d7efdc06 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -4707,6 +4707,8 @@ static void cliDronecan(char *cmdline)
     canardSTM32GetProtocolStatus(&stat);
     int32_t txFill = canardSTM32GetTxQueueFillLevel();
     int32_t rxFill = canardSTM32GetRxFifoFillLevel();
+    uint32_t busOffCount = dronecanGetBusOffCount();
+    CanardPoolAllocatorStatistics poolStats = dronecanGetPoolStats();
     cliPrintLine("DroneCAN CAN peripheral status:");
     cliPrintLinef("  BusOff:       %s", stat.BusOff       ? "YES" : "no");
     cliPrintLinef("  ErrorPassive: %s", stat.ErrorPassive ? "YES" : "no");
@@ -4715,6 +4717,11 @@ static void cliDronecan(char *cmdline)
     cliPrintLinef("  LEC:          %s (%u)", lecNames[stat.lec], (unsigned)stat.lec);
     cliPrintLinef("  TX queue:     %" PRId32, txFill);
     cliPrintLinef("  RX buffer:    %" PRId32, rxFill);
+    cliPrintLinef("  BusOff count: %" PRIu32, busOffCount);
+    cliPrintLinef("  Pool blocks:  %u used, %u peak, %u capacity",
+                  poolStats.current_usage_blocks,
+                  poolStats.peak_usage_blocks,
+                  poolStats.capacity_blocks);
 }
 #endif
 
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index b38d22934f5..b2e158c87ec 100755
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -123,7 +123,7 @@ static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = {
 #endif
 
     /* DRONECAN GPS */
-#ifdef USE_GPS_PROTO_DRONECAN
+#ifdef USE_DRONECAN
     {true, 0, &gpsRestartDronecan, &gpsHandleDronecan },
 #else
     {false, 0, NULL, NULL },
diff --git a/src/main/io/gps_dronecan.c b/src/main/io/gps_dronecan.c
index 07e595cd208..2c689d88724 100644
--- a/src/main/io/gps_dronecan.c
+++ b/src/main/io/gps_dronecan.c
@@ -32,7 +32,7 @@
 #include "build/build_config.h"
 
 
-#if defined(USE_GPS_PROTO_DRONECAN)
+#if defined(USE_DRONECAN)
 
 #include "build/debug.h"
 
diff --git a/src/main/target/common.h b/src/main/target/common.h
index bd7730226f6..323111e6344 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -54,7 +54,6 @@
 #define USE_GPS
 #define USE_GPS_PROTO_UBLOX
 #define USE_GPS_PROTO_MSP
-#define USE_GPS_PROTO_DRONECAN
 #define USE_TELEMETRY
 #define USE_TELEMETRY_LTM
 #define USE_GPS_FIX_ESTIMATION

From 08c476817c84731abcf88f32f6ed7d5d4246496b Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Mon, 15 Jun 2026 09:52:38 -0700
Subject: [PATCH 52/67] dronecan: guard STM32 CAN drivers with USE_DRONECAN

Adds #include "platform.h" + #ifdef USE_DRONECAN to both
canard_stm32f7xx_driver.c and canard_stm32h7xx_driver.c, preventing
CAN1_RX0_IRQHandler and related code from linking into non-CAN targets.
---
 .../drivers/dronecan/libcanard/canard_stm32f7xx_driver.c    | 6 ++++++
 .../drivers/dronecan/libcanard/canard_stm32h7xx_driver.c    | 6 ++++++
 2 files changed, 12 insertions(+)

diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
index e783afec91a..5f41deb2824 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -5,6 +5,10 @@
  *      Author: Roni Kant
  */
 
+#include "platform.h"
+
+#ifdef USE_DRONECAN
+
 #include "common/log.h"
 #include "common/time.h"
 #include "drivers/io.h"
@@ -459,3 +463,5 @@ static uint8_t rxBufferNumMessages(struct RxBuffer_t *rxBuf) {
 
     return (rxBuf->writeIndex - rxBuf->readIndex);
 }
+
+#endif
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index 32e558df129..d6063a3a654 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -5,6 +5,10 @@
  *      Author: Roni Kant
  */
 
+#include "platform.h"
+
+#ifdef USE_DRONECAN
+
 #include "common/log.h"
 #include "common/time.h"
 #include "drivers/io.h"
@@ -414,3 +418,5 @@ static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timi
 
     return true;
 }
+
+#endif

From 37ec2baf33fb3b79fed02530452c428e9b39f10b Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Mon, 15 Jun 2026 19:30:25 -0700
Subject: [PATCH 53/67] Set max_quanta_per_bit back to 10 for H7 targets as
 well

---
 src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index d6063a3a654..10c3c43add3 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -330,7 +330,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 = 18; //(target_bitrate >= 1000000) ? 10 : 17;
+    const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17;
 
     static const int MaxSamplePointLocation = 900;
 

From 1139492e37eae5c7f01fe3e0e5b13d15860d7c5b Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Tue, 4 Aug 2026 20:22:24 -0700
Subject: [PATCH 54/67] dronecan: use ATOMIC_BLOCK for TX/RX critical sections,
 guard RX reassembly too

Replace hand-rolled NVIC_DisableIRQ/EnableIRQ pairs with the existing
ATOMIC_BLOCK(NVIC_PRIO_CAN) macro, which saves/restores the actual prior
BASEPRI value instead of a manual counter, so nested critical sections
(e.g. handle_GetNodeInfo's own masking, reached synchronously from within
canardHandleRxFrame) compose correctly without risk of leaving the TX
interrupt permanently masked.

Also wrap canardHandleRxFrame() itself, which was previously unmasked and
could race the TX-complete ISR's freeBlock() calls on the shared canard
pool allocator during multi-frame RX reassembly.
---
 src/main/drivers/dronecan/dronecan.c | 92 ++++++++++++++--------------
 1 file changed, 47 insertions(+), 45 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index a96e6a75ba1..0f671c2425c 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -2,6 +2,8 @@
 #include "common/log.h"
 #include "common/time.h"
 #include "drivers/time.h"
+#include "drivers/nvic.h"
+#include "build/atomic.h"
 #include <stdint.h>
 #include <stdlib.h>
 #include "fc/settings.h"
@@ -44,17 +46,6 @@ static dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES];
 static volatile uint32_t txErrCount = 0;
 static uint32_t busOffCount = 0;
 
-#if defined(STM32H7)
-static inline void dronecanMaskTxISR(void)   { NVIC_DisableIRQ(FDCAN1_IT0_IRQn); }
-static inline void dronecanUnmaskTxISR(void) { NVIC_EnableIRQ(FDCAN1_IT0_IRQn); }
-#elif defined(STM32F7)
-static inline void dronecanMaskTxISR(void)   { NVIC_DisableIRQ(CAN1_TX_IRQn); }
-static inline void dronecanUnmaskTxISR(void) { NVIC_EnableIRQ(CAN1_TX_IRQn); }
-#else
-static inline void dronecanMaskTxISR(void)   {}
-static inline void dronecanUnmaskTxISR(void) {}
-#endif
-
 /* Forward declarations ------------------------------------------------------*/
 
 static void processCanardTxQueueSafe(void);
@@ -149,7 +140,9 @@ void dronecanUpdate(timeUs_t currentTimeUs)
 	             }
 	             else if (rx_res > 0)        // Success - process the frame
 	             {
-		             canardHandleRxFrame(&canard, &rx_frame, timestamp);
+		             ATOMIC_BLOCK(NVIC_PRIO_CAN) {
+		                 canardHandleRxFrame(&canard, &rx_frame, timestamp);
+		             }
 	             }
              }
             // Drain any TX frames queued by RX handlers (e.g. GetNodeInfo responses)
@@ -168,10 +161,11 @@ void dronecanUpdate(timeUs_t currentTimeUs)
                 }
 
                 uint32_t rxDrops = canardSTM32GetAndClearRxDropCount();
-                dronecanMaskTxISR();
-                uint32_t txErrs = txErrCount;
-                txErrCount = 0;
-                dronecanUnmaskTxISR();
+                uint32_t txErrs;
+                ATOMIC_BLOCK(NVIC_PRIO_CAN) {
+                    txErrs = txErrCount;
+                    txErrCount = 0;
+                }
                 if (rxDrops > 0) {
                     LOG_DEBUG(CAN, "RX drops: %" PRIu32, rxDrops);
                 }
@@ -296,22 +290,28 @@ void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { UNUSED(hcan);
 
 static void processCanardTxQueueSafe(void) {
     for (;;) {
-        dronecanMaskTxISR();
-        const CanardCANFrame *tx_frame = canardPeekTxQueue(&canard);
-        if (tx_frame == NULL) {
-            dronecanUnmaskTxISR();
-            break;
-        }
-        const int16_t tx_res = canardSTM32Transmit(tx_frame);  // HAL register write, ~1µs
-        if (tx_res != 0) {
-            if (tx_res < 0) {
-                LOG_DEBUG(CAN, "Transmit error %d", tx_res);
+        bool queueEmpty = false;
+        bool hwFull = false;
+
+        ATOMIC_BLOCK(NVIC_PRIO_CAN) {
+            const CanardCANFrame *tx_frame = canardPeekTxQueue(&canard);
+            if (tx_frame == NULL) {
+                queueEmpty = true;
+            } else {
+                const int16_t tx_res = canardSTM32Transmit(tx_frame);  // HAL register write, ~1µs
+                if (tx_res != 0) {
+                    if (tx_res < 0) {
+                        LOG_DEBUG(CAN, "Transmit error %d", tx_res);
+                    }
+                    canardPopTxQueue(&canard);
+                } else {
+                    hwFull = true;  // HW TX full, ISR will refill when a slot opens
+                }
             }
-            canardPopTxQueue(&canard);
         }
-        dronecanUnmaskTxISR();
-        if (tx_res == 0) {
-            break;  // HW TX full, ISR will refill when a slot opens
+
+        if (queueEmpty || hwFull) {
+            break;
         }
     }
 }
@@ -347,15 +347,16 @@ static void send_NodeStatus(void) {
     // loss
     static uint8_t transfer_id;
 
-    dronecanMaskTxISR();
-    const int16_t bc_res = canardBroadcast(&canard,
-                    UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
-                    UAVCAN_PROTOCOL_NODESTATUS_ID,
-                    &transfer_id,
-                    CANARD_TRANSFER_PRIORITY_LOW,
-                    buffer,
-                    len);
-    dronecanUnmaskTxISR();
+    int16_t bc_res;
+    ATOMIC_BLOCK(NVIC_PRIO_CAN) {
+        bc_res = canardBroadcast(&canard,
+                        UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
+                        UAVCAN_PROTOCOL_NODESTATUS_ID,
+                        &transfer_id,
+                        CANARD_TRANSFER_PRIORITY_LOW,
+                        buffer,
+                        len);
+    }
     if (bc_res < 0) {
         LOG_DEBUG(CAN, "NodeStatus broadcast failed: %d", bc_res);
     }
@@ -370,9 +371,9 @@ static void process1HzTasks(timeUs_t timestamp_usec)
    /*
       Purge transfers that are no longer transmitted. This can free up some memory
     */
-    dronecanMaskTxISR();
-    canardCleanupStaleTransfers(&canard, timestamp_usec);
-    dronecanUnmaskTxISR();
+    ATOMIC_BLOCK(NVIC_PRIO_CAN) {
+        canardCleanupStaleTransfers(&canard, timestamp_usec);
+    }
 
     /*
       Transmit the node status message
@@ -568,8 +569,9 @@ static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer)
 
 	uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer);
 
-    dronecanMaskTxISR();
-    const int16_t rr_res = canardRequestOrRespond(ins,
+    int16_t rr_res;
+    ATOMIC_BLOCK(NVIC_PRIO_CAN) {
+        rr_res = canardRequestOrRespond(ins,
 						   transfer->source_node_id,
 						   UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE,
 						   UAVCAN_PROTOCOL_GETNODEINFO_ID,
@@ -578,7 +580,7 @@ static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer)
 						   CanardResponse,
 						   &buffer[0],
 						   total_size);
-    dronecanUnmaskTxISR();
+    }
     if (rr_res < 0) {
         LOG_DEBUG(CAN, "GetNodeInfo response failed: %d", rr_res);
     }

From 0ba011484a9c3ea5a69780383adc3118dbafa0f0 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Wed, 5 Aug 2026 07:08:01 -0700
Subject: [PATCH 55/67] dronecan: extract shared CAN bit-timing solver, fix
 stale unit test

canardSTM32ComputeTimings() was duplicated verbatim (aside from the PCLK
source, SJW value, and BS1/BS2 register-offset convention) between the F7
bxCAN and H7 FDCAN drivers. Extract the HAL-free quanta/prescaler solve
into canard_stm32_timing.c, shared by both; each driver now applies only
its own peripheral-specific glue on top.

bxcan_timing_unittest.cc previously hand-copied this algorithm into the
test file, hardcoding max_quanta_per_bit=18 with a "keep in sync" comment.
It drifted from the driver's real (target_bitrate >= 1000000) ? 10 : 17
within days of being written and was never updated. Rewritten to call the
real, now-shared canardComputeCanTimingSolution() directly, so there's no
separate copy to keep in sync.
---
 cmake/stm32f7.cmake                           |   1 +
 cmake/stm32h7.cmake                           |   1 +
 .../dronecan/libcanard/canard_stm32_timing.c  | 111 ++++++++
 .../dronecan/libcanard/canard_stm32_timing.h  |  20 ++
 .../libcanard/canard_stm32f7xx_driver.c       | 110 +-------
 .../libcanard/canard_stm32h7xx_driver.c       | 110 +-------
 src/test/unit/CMakeLists.txt                  |   6 +-
 src/test/unit/bxcan_timing_unittest.cc        | 253 +++++++-----------
 8 files changed, 241 insertions(+), 371 deletions(-)
 create mode 100644 src/main/drivers/dronecan/libcanard/canard_stm32_timing.c
 create mode 100644 src/main/drivers/dronecan/libcanard/canard_stm32_timing.h

diff --git a/cmake/stm32f7.cmake b/cmake/stm32f7.cmake
index 48f29038dc2..28c50ee0f83 100644
--- a/cmake/stm32f7.cmake
+++ b/cmake/stm32f7.cmake
@@ -79,6 +79,7 @@ main_sources(STM32F7_SRC
     drivers/serial_uart_hal.c
     drivers/sdcard/sdmmc_sdio_hal.c
     drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+    drivers/dronecan/libcanard/canard_stm32_timing.c
 
 )
 
diff --git a/cmake/stm32h7.cmake b/cmake/stm32h7.cmake
index 7679325d992..ab40a9b7cb9 100644
--- a/cmake/stm32h7.cmake
+++ b/cmake/stm32h7.cmake
@@ -163,6 +163,7 @@ main_sources(STM32H7_SRC
     drivers/sdio.h
     drivers/sdcard/sdmmc_sdio_hal.c
     drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+    drivers/dronecan/libcanard/canard_stm32_timing.c
 
 )
 
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32_timing.c b/src/main/drivers/dronecan/libcanard/canard_stm32_timing.c
new file mode 100644
index 00000000000..409d9b15c1b
--- /dev/null
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32_timing.c
@@ -0,0 +1,111 @@
+#include "canard_stm32_timing.h"
+
+bool canardComputeCanTimingSolution(uint32_t pclk, uint32_t target_bitrate, CanardCanTimingSolution *out_solution)
+{
+    if (target_bitrate < 1) {
+        return false;
+    }
+
+    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 : 17;
+    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;
+    }
+
+    out_solution->prescaler = (uint16_t)prescaler;
+    out_solution->bs1 = solution.bs1;
+    out_solution->bs2 = solution.bs2;
+
+    return true;
+}
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32_timing.h b/src/main/drivers/dronecan/libcanard/canard_stm32_timing.h
new file mode 100644
index 00000000000..9b08c70d38a
--- /dev/null
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32_timing.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/*
+ * Shared, HAL-free CAN bit-timing solver used by both the STM32 bxCAN (F7)
+ * and FDCAN (H7) DroneCAN drivers. Each driver applies its own
+ * peripheral-specific SJW and BS1/BS2 register-offset convention on top of
+ * this solution — see canardSTM32ComputeTimings() in
+ * canard_stm32f7xx_driver.c and canard_stm32h7xx_driver.c.
+ */
+
+typedef struct {
+    uint16_t prescaler;
+    uint8_t  bs1;   // raw solved value, 1..16, no register offset applied
+    uint8_t  bs2;   // raw solved value, 1..8, no register offset applied
+} CanardCanTimingSolution;
+
+bool canardComputeCanTimingSolution(uint32_t pclk, uint32_t target_bitrate, CanardCanTimingSolution *out_solution);
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
index 5f41deb2824..dc9aa4f7684 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c
@@ -15,6 +15,7 @@
 #include "drivers/nvic.h"
 #include "canard.h"
 #include "canard_stm32_driver.h"
+#include "canard_stm32_timing.h"
 
 #include "stm32f7xx_hal.h"
 #include "stm32f7xx_hal_def.h"
@@ -312,114 +313,15 @@ static void canardSTM32GPIO_Init(void)
 
 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 : 17;
-    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)))
-    {
+    CanardCanTimingSolution sol;
+    if (!canardComputeCanTimingSolution(HAL_RCC_GetPCLK1Freq(), target_bitrate, &sol)) {
         return false;
     }
 
-    out_timings->prescaler = (uint16_t)(prescaler);
+    out_timings->prescaler = sol.prescaler;
     out_timings->sjw = 3;  // Register value: hardware SJW = sjw+1 = 4 tq. F7 bxCAN needs wider SJW than H7 FDCAN.
-    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.
+    out_timings->bs1 = sol.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 = sol.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;
 }
diff --git a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
index 10c3c43add3..3d8a4c7ef71 100644
--- a/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
+++ b/src/main/drivers/dronecan/libcanard/canard_stm32h7xx_driver.c
@@ -15,6 +15,7 @@
 #include "drivers/nvic.h"
 #include "canard.h"
 #include "canard_stm32_driver.h"
+#include "canard_stm32_timing.h"
 
 #include "stm32h7xx_hal.h"
 #include "stm32h7xx_hal_def.h"
@@ -307,114 +308,15 @@ static void canardSTM32GPIO_Init(void)
 
 static bool canardSTM32ComputeTimings(const uint32_t target_bitrate, struct Timings *out_timings)
 {
-    if (target_bitrate < 1) {
+    CanardCanTimingSolution sol;
+    if (!canardComputeCanTimingSolution(HAL_RCCEx_GetPeriphCLKFreq(RCC_PERIPHCLK_FDCAN), target_bitrate, &sol)) {
         return false;
     }
 
-    /*
-     * Hardware configuration
-     */
-    const uint32_t pclk = HAL_RCCEx_GetPeriphCLKFreq(RCC_PERIPHCLK_FDCAN);
-
-    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 : 17;
-
-    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;
-    }
-
-    out_timings->prescaler = (uint16_t)(prescaler);
+    out_timings->prescaler = sol.prescaler;
     out_timings->sjw = 1;
-    out_timings->bs1 = (uint8_t)(solution.bs1);  // The HAL takes care of the 1 bs offset in the register so don't remove it here like AP does.
-    out_timings->bs2 = (uint8_t)(solution.bs2);  // The HAL takes care of the 1 bs offset in the register so don't remove it here like AP does.
+    out_timings->bs1 = sol.bs1;  // The HAL takes care of the 1 bs offset in the register so don't remove it here like AP does.
+    out_timings->bs2 = sol.bs2;  // The HAL takes care of the 1 bs offset in the register so don't remove it here like AP does.
 
     return true;
 }
diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt
index e9e2eb7d8a9..1a83a9934c2 100644
--- a/src/test/unit/CMakeLists.txt
+++ b/src/test/unit/CMakeLists.txt
@@ -53,8 +53,10 @@ set_property(SOURCE dronecan_messages_unittest.cc PROPERTY extra_includes
     "../../lib/main/Dronecan/dsdlc_generated/include")
 set_property(SOURCE dronecan_messages_unittest.cc PROPERTY definitions USE_DRONECAN CANARD_ENABLE_TAO_OPTION=0)
 
-# bxCAN timing algorithm tests - self-contained, no driver or HAL dependencies
-# Keep in sync with canard_stm32f7xx_driver.c:canardSTM32ComputeTimings
+# CAN bit-timing solver tests - links the real, HAL-free timing core shared
+# by the F7 (bxCAN) and H7 (FDCAN) drivers, so there is nothing to keep in sync
+set_property(SOURCE bxcan_timing_unittest.cc PROPERTY depends
+    "drivers/dronecan/libcanard/canard_stm32_timing.c")
 
 # Libcanard core tests - CANARD_INTERNAL= makes static functions accessible
 set_property(SOURCE canard_unittest.cc PROPERTY depends
diff --git a/src/test/unit/bxcan_timing_unittest.cc b/src/test/unit/bxcan_timing_unittest.cc
index 5d487b7e5e8..3879d1714a6 100644
--- a/src/test/unit/bxcan_timing_unittest.cc
+++ b/src/test/unit/bxcan_timing_unittest.cc
@@ -1,14 +1,15 @@
 /**
- * bxCAN Timing Computation Unit Tests
+ * CAN Bit-Timing Solver Unit Tests
  *
- * Regression tests for the timing algorithm in
- * canard_stm32f7xx_driver.c:canardSTM32ComputeTimings().
+ * Regression tests for canardComputeCanTimingSolution() in
+ * canard_stm32_timing.c — the HAL-free timing core shared by both the
+ * STM32 bxCAN (F7) and FDCAN (H7) DroneCAN drivers. Each driver applies its
+ * own peripheral-specific SJW and BS1/BS2 register-offset convention on top
+ * of this solution; see canardSTM32ComputeTimings() in
+ * canard_stm32f7xx_driver.c and canard_stm32h7xx_driver.c.
  *
- * The function is static and reads PCLK via HAL_RCC_GetPCLK1Freq(), so it
- * cannot be called directly from a unit test.  canardSTM32ComputeTimingsForPCLK()
- * below is an exact copy with pclk passed as a parameter instead.
- *
- * KEEP IN SYNC with canard_stm32f7xx_driver.c.
+ * This test links the real production source directly (see CMakeLists.txt),
+ * so there is no separate copy of the algorithm to keep in sync.
  *
  * Primary test PCLK: 54 MHz — STM32F765 APB1 at SYSCLK=216 MHz / APBprescaler=4.
  * Secondary test PCLK: 48 MHz — alternate F7 configuration.
@@ -19,87 +20,18 @@
 
 #include "gtest/gtest.h"
 
-// ---------------------------------------------------------------------------
-// Mirror of canard_stm32f7xx_driver.c:canardSTM32ComputeTimings
-// pclk replaces HAL_RCC_GetPCLK1Freq().  All other logic is identical.
-// ---------------------------------------------------------------------------
-
-struct Timings {
-    uint16_t prescaler;
-    uint8_t  sjw;
-    uint8_t  bs1;
-    uint8_t  bs2;
-};
-
-static bool canardSTM32ComputeTimingsForPCLK(const uint32_t pclk,
-                                              const uint32_t target_bitrate,
-                                              struct Timings *out_timings)
-{
-    if (target_bitrate < 1) {
-        return false;
-    }
-
-    static const int MaxBS1 = 16;
-    static const int MaxBS2 = 8;
-
-    const int max_quanta_per_bit = 18;
-    static const int MaxSamplePointLocation = 900;
-
-    const uint32_t prescaler_bs = pclk / target_bitrate;
-
-    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;
-        }
-        bs1_bs2_sum--;
-    }
-
-    const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum);
-    if ((prescaler < 1U) || (prescaler > 1024U)) {
-        return false;
-    }
-
-    struct BsPair {
-        uint8_t  bs1;
-        uint8_t  bs2;
-        uint16_t sample_point_permill;
-    } solution;
-
-    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) {
-        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));
-    }
-
-    if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) ||
-        !((solution.bs1 >= 1) && (solution.bs1 <= MaxBS1) &&
-          (solution.bs2 >= 1) && (solution.bs2 <= MaxBS2))) {
-        return false;
-    }
-
-    out_timings->prescaler = (uint16_t)(prescaler);
-    out_timings->sjw = 3;
-    out_timings->bs1 = (uint8_t)(solution.bs1) - 1;  // HAL adds +1 internally
-    out_timings->bs2 = (uint8_t)(solution.bs2) - 1;  // HAL adds +1 internally
-
-    return true;
+extern "C" {
+#include "drivers/dronecan/libcanard/canard_stm32_timing.h"
 }
 
 // ---------------------------------------------------------------------------
-// Helper: back-calculate bitrate from HAL register values
+// Helper: back-calculate bitrate from a solved timing solution
 // ---------------------------------------------------------------------------
 
-static uint32_t bitrateFromTimings(uint32_t pclk, const struct Timings &t)
+static uint32_t bitrateFromSolution(uint32_t pclk, const CanardCanTimingSolution &s)
 {
-    // HAL bs1/bs2 values are stored with -1 offset; hardware adds +1 back
-    uint32_t total_tq = 1u + (t.bs1 + 1u) + (t.bs2 + 1u);
-    return pclk / (t.prescaler * total_tq);
+    uint32_t total_tq = 1u + s.bs1 + s.bs2;
+    return pclk / (s.prescaler * total_tq);
 }
 
 // ---------------------------------------------------------------------------
@@ -108,9 +40,9 @@ static uint32_t bitrateFromTimings(uint32_t pclk, const struct Timings &t)
 
 class BxCanTimingTest : public ::testing::Test {
 protected:
-    struct Timings t;
+    CanardCanTimingSolution sol;
 
-    void SetUp() override { memset(&t, 0, sizeof(t)); }
+    void SetUp() override { memset(&sol, 0, sizeof(sol)); }
 };
 
 static const uint32_t PCLK_54M = 54000000U;
@@ -118,111 +50,113 @@ static const uint32_t PCLK_48M = 48000000U;
 
 // ===========================================================================
 // A. Bitrate correctness at PCLK = 54 MHz
-//    All four standard bitrates should resolve to 18 quanta/bit.
 // ===========================================================================
 
 TEST_F(BxCanTimingTest, Pclk54_1Mbps_Succeeds)
 {
-    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 1000000, &t));
-    EXPECT_EQ(bitrateFromTimings(PCLK_54M, t), 1000000U);
+    ASSERT_TRUE(canardComputeCanTimingSolution(PCLK_54M, 1000000, &sol));
+    EXPECT_EQ(bitrateFromSolution(PCLK_54M, sol), 1000000U);
 }
 
 TEST_F(BxCanTimingTest, Pclk54_500kbps_Succeeds)
 {
-    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 500000, &t));
-    EXPECT_EQ(bitrateFromTimings(PCLK_54M, t), 500000U);
+    ASSERT_TRUE(canardComputeCanTimingSolution(PCLK_54M, 500000, &sol));
+    EXPECT_EQ(bitrateFromSolution(PCLK_54M, sol), 500000U);
 }
 
 TEST_F(BxCanTimingTest, Pclk54_250kbps_Succeeds)
 {
-    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 250000, &t));
-    EXPECT_EQ(bitrateFromTimings(PCLK_54M, t), 250000U);
+    ASSERT_TRUE(canardComputeCanTimingSolution(PCLK_54M, 250000, &sol));
+    EXPECT_EQ(bitrateFromSolution(PCLK_54M, sol), 250000U);
 }
 
 TEST_F(BxCanTimingTest, Pclk54_125kbps_Succeeds)
 {
-    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 125000, &t));
-    EXPECT_EQ(bitrateFromTimings(PCLK_54M, t), 125000U);
+    ASSERT_TRUE(canardComputeCanTimingSolution(PCLK_54M, 125000, &sol));
+    EXPECT_EQ(bitrateFromSolution(PCLK_54M, sol), 125000U);
 }
 
 // ===========================================================================
-// B. 18-quanta regression — verifies max_quanta_per_bit=18 is in effect
+// B. Quanta-cap regression — verifies the dual max_quanta_per_bit is in effect
 //
-//    At 54 MHz / 1 Mbps the solver must find 18 quanta/bit → prescaler=3.
-//    The previous limit of 10 quanta would have yielded prescaler=6 (9 quanta).
+//    Driver caps at 10 quanta/bit for >= 1 Mbps, 17 quanta/bit below that.
+//    At 54 MHz / 1 Mbps the 10-cap yields 9 quanta/bit (prescaler=6) — not
+//    the 18 quanta/bit (prescaler=3) an unconditional cap would give.
+//    500/250 kbps land on 12 quanta/bit; 125 kbps lands on 16 quanta/bit —
+//    all below the 17 cap, since 54 MHz isn't evenly divisible at 17 quanta
+//    for these rates.
 // ===========================================================================
 
-TEST_F(BxCanTimingTest, Pclk54_1Mbps_Uses18Quanta)
+TEST_F(BxCanTimingTest, Pclk54_1Mbps_Uses9Quanta)
 {
-    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 1000000, &t));
-
-    // 54 MHz / 3 / 18 quanta = 1 Mbps
-    EXPECT_EQ(t.prescaler, 3u);
+    ASSERT_TRUE(canardComputeCanTimingSolution(PCLK_54M, 1000000, &sol));
 
-    // bs1 raw = t.bs1+1 = 15, bs2 raw = t.bs2+1 = 2 → 1+15+2 = 18 quanta/bit
-    EXPECT_EQ(t.bs1 + 1u, 15u);
-    EXPECT_EQ(t.bs2 + 1u,  2u);
+    // 54 MHz / 6 / 9 quanta = 1 Mbps
+    EXPECT_EQ(sol.prescaler, 6u);
+    EXPECT_EQ(sol.bs1, 7u);
+    EXPECT_EQ(sol.bs2, 1u);
 }
 
-TEST_F(BxCanTimingTest, Pclk54_500kbps_Uses18Quanta)
+TEST_F(BxCanTimingTest, Pclk54_500kbps_Uses12Quanta)
 {
-    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 500000, &t));
-    EXPECT_EQ(t.prescaler, 6u);
-    EXPECT_EQ(t.bs1 + 1u, 15u);
-    EXPECT_EQ(t.bs2 + 1u,  2u);
+    ASSERT_TRUE(canardComputeCanTimingSolution(PCLK_54M, 500000, &sol));
+    EXPECT_EQ(sol.prescaler, 9u);
+    EXPECT_EQ(sol.bs1, 9u);
+    EXPECT_EQ(sol.bs2, 2u);
 }
 
-TEST_F(BxCanTimingTest, Pclk54_250kbps_Uses18Quanta)
+TEST_F(BxCanTimingTest, Pclk54_250kbps_Uses12Quanta)
 {
-    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 250000, &t));
-    EXPECT_EQ(t.prescaler, 12u);
-    EXPECT_EQ(t.bs1 + 1u, 15u);
-    EXPECT_EQ(t.bs2 + 1u,  2u);
+    ASSERT_TRUE(canardComputeCanTimingSolution(PCLK_54M, 250000, &sol));
+    EXPECT_EQ(sol.prescaler, 18u);
+    EXPECT_EQ(sol.bs1, 9u);
+    EXPECT_EQ(sol.bs2, 2u);
 }
 
-TEST_F(BxCanTimingTest, Pclk54_125kbps_Uses18Quanta)
+TEST_F(BxCanTimingTest, Pclk54_125kbps_Uses16Quanta)
 {
-    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 125000, &t));
-    EXPECT_EQ(t.prescaler, 24u);
-    EXPECT_EQ(t.bs1 + 1u, 15u);
-    EXPECT_EQ(t.bs2 + 1u,  2u);
+    ASSERT_TRUE(canardComputeCanTimingSolution(PCLK_54M, 125000, &sol));
+    EXPECT_EQ(sol.prescaler, 27u);
+    EXPECT_EQ(sol.bs1, 13u);
+    EXPECT_EQ(sol.bs2, 2u);
 }
 
 // ===========================================================================
 // C. Bitrate correctness at PCLK = 48 MHz (alternate F7 config)
-//    48 MHz / bitrate is not divisible by 18 for standard rates,
-//    so the solver falls back to 16 quanta/bit at prescaler=3,6,12,24.
+//    At 1 Mbps the 10-quanta cap resolves to 8 quanta/bit (prescaler=6).
+//    48 MHz / bitrate is not divisible by 17 quanta for 500/250/125 kbps,
+//    so those fall back to 16 quanta/bit at prescaler=6,12,24.
 // ===========================================================================
 
 TEST_F(BxCanTimingTest, Pclk48_1Mbps_Succeeds)
 {
-    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_48M, 1000000, &t));
-    EXPECT_EQ(bitrateFromTimings(PCLK_48M, t), 1000000U);
-    // 48 MHz / 3 / 16 quanta = 1 Mbps
-    EXPECT_EQ(t.prescaler, 3u);
-    EXPECT_EQ(t.bs1 + 1u, 13u);
-    EXPECT_EQ(t.bs2 + 1u,  2u);
+    ASSERT_TRUE(canardComputeCanTimingSolution(PCLK_48M, 1000000, &sol));
+    EXPECT_EQ(bitrateFromSolution(PCLK_48M, sol), 1000000U);
+    // 48 MHz / 6 / 8 quanta = 1 Mbps
+    EXPECT_EQ(sol.prescaler, 6u);
+    EXPECT_EQ(sol.bs1, 6u);
+    EXPECT_EQ(sol.bs2, 1u);
 }
 
 TEST_F(BxCanTimingTest, Pclk48_500kbps_Succeeds)
 {
-    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_48M, 500000, &t));
-    EXPECT_EQ(bitrateFromTimings(PCLK_48M, t), 500000U);
-    EXPECT_EQ(t.prescaler, 6u);
+    ASSERT_TRUE(canardComputeCanTimingSolution(PCLK_48M, 500000, &sol));
+    EXPECT_EQ(bitrateFromSolution(PCLK_48M, sol), 500000U);
+    EXPECT_EQ(sol.prescaler, 6u);
 }
 
 TEST_F(BxCanTimingTest, Pclk48_250kbps_Succeeds)
 {
-    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_48M, 250000, &t));
-    EXPECT_EQ(bitrateFromTimings(PCLK_48M, t), 250000U);
-    EXPECT_EQ(t.prescaler, 12u);
+    ASSERT_TRUE(canardComputeCanTimingSolution(PCLK_48M, 250000, &sol));
+    EXPECT_EQ(bitrateFromSolution(PCLK_48M, sol), 250000U);
+    EXPECT_EQ(sol.prescaler, 12u);
 }
 
 TEST_F(BxCanTimingTest, Pclk48_125kbps_Succeeds)
 {
-    ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_48M, 125000, &t));
-    EXPECT_EQ(bitrateFromTimings(PCLK_48M, t), 125000U);
-    EXPECT_EQ(t.prescaler, 24u);
+    ASSERT_TRUE(canardComputeCanTimingSolution(PCLK_48M, 125000, &sol));
+    EXPECT_EQ(bitrateFromSolution(PCLK_48M, sol), 125000U);
+    EXPECT_EQ(sol.prescaler, 24u);
 }
 
 // ===========================================================================
@@ -237,25 +171,22 @@ TEST_F(BxCanTimingTest, HwConstraints_AllStandardBitrates)
     for (uint32_t pclk : pclks) {
         for (uint32_t br : bitrates) {
             SCOPED_TRACE(testing::Message() << "pclk=" << pclk << " br=" << br);
-            ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(pclk, br, &t));
+            ASSERT_TRUE(canardComputeCanTimingSolution(pclk, br, &sol));
 
-            // BS1 raw (t.bs1+1) must be in [1..16]
-            EXPECT_GE(t.bs1 + 1u, 1u);
-            EXPECT_LE(t.bs1 + 1u, 16u);
+            // BS1 must be in [1..16]
+            EXPECT_GE(sol.bs1, 1u);
+            EXPECT_LE(sol.bs1, 16u);
 
-            // BS2 raw (t.bs2+1) must be in [1..8]
-            EXPECT_GE(t.bs2 + 1u, 1u);
-            EXPECT_LE(t.bs2 + 1u, 8u);
+            // BS2 must be in [1..8]
+            EXPECT_GE(sol.bs2, 1u);
+            EXPECT_LE(sol.bs2, 8u);
 
             // Prescaler in [1..1024]
-            EXPECT_GE(t.prescaler, 1u);
-            EXPECT_LE(t.prescaler, 1024u);
-
-            // SJW fixed at 3 (hardware SJW = 4 tq)
-            EXPECT_EQ(t.sjw, 3u);
+            EXPECT_GE(sol.prescaler, 1u);
+            EXPECT_LE(sol.prescaler, 1024u);
 
             // Back-calculated bitrate must match the request
-            EXPECT_EQ(bitrateFromTimings(pclk, t), br);
+            EXPECT_EQ(bitrateFromSolution(pclk, sol), br);
         }
     }
 }
@@ -266,12 +197,10 @@ TEST_F(BxCanTimingTest, SamplePoint_InValidRange)
 
     for (uint32_t br : bitrates) {
         SCOPED_TRACE(br);
-        ASSERT_TRUE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, br, &t));
+        ASSERT_TRUE(canardComputeCanTimingSolution(PCLK_54M, br, &sol));
 
-        uint32_t bs1_raw = t.bs1 + 1u;
-        uint32_t bs2_raw = t.bs2 + 1u;
-        uint32_t total   = 1u + bs1_raw + bs2_raw;
-        uint32_t sp_permill = 1000u * (1u + bs1_raw) / total;
+        uint32_t total   = 1u + sol.bs1 + sol.bs2;
+        uint32_t sp_permill = 1000u * (1u + sol.bs1) / total;
 
         EXPECT_GE(sp_permill, 750u);  // practical CAN minimum
         EXPECT_LE(sp_permill, 900u);  // driver MaxSamplePointLocation
@@ -284,18 +213,20 @@ TEST_F(BxCanTimingTest, SamplePoint_InValidRange)
 
 TEST_F(BxCanTimingTest, Invalid_ZeroBitrate)
 {
-    EXPECT_FALSE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 0, &t));
+    EXPECT_FALSE(canardComputeCanTimingSolution(PCLK_54M, 0, &sol));
 }
 
 TEST_F(BxCanTimingTest, Invalid_UnsolvableBitrate)
 {
-    // 999999 bps: prescaler_bs=54 (integer division), but 54M/54=1M ≠ 999999
-    // Final bitrate validation catches it.
-    EXPECT_FALSE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 999999, &t));
+    // 999999 bps: the solver finds a valid quanta/prescaler split, but the
+    // resulting bitrate (54 MHz / 6 / 9 quanta = 1000000) doesn't match the
+    // requested 999999 — final bitrate validation catches it.
+    EXPECT_FALSE(canardComputeCanTimingSolution(PCLK_54M, 999999, &sol));
 }
 
 TEST_F(BxCanTimingTest, Invalid_ExcessivelyLowBitrate)
 {
-    // prescaler_bs = 54M/100 = 540000 → prescaler = 540000/18 = 30000 > 1024
-    EXPECT_FALSE(canardSTM32ComputeTimingsForPCLK(PCLK_54M, 100, &t));
+    // prescaler_bs = 54M/100 = 540000 → even at max quanta the required
+    // prescaler exceeds the 1024 hardware limit.
+    EXPECT_FALSE(canardComputeCanTimingSolution(PCLK_54M, 100, &sol));
 }

From 3bfbebb7a9834ef70f2a0f78cc9114f1f68f463e Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Wed, 5 Aug 2026 07:08:07 -0700
Subject: [PATCH 56/67] test(dronecan): assert a real known value in
 CRC_KnownValues

Previously only checked that crcAddByte() changed its input at all
(!= 0xFFFF, != 0x0000), which a subtly wrong polynomial or bit order
would still satisfy. Assert the actual CRC-16/CCITT-FALSE value for a
single byte instead.
---
 src/test/unit/canard_unittest.cc | 11 +++--------
 1 file changed, 3 insertions(+), 8 deletions(-)

diff --git a/src/test/unit/canard_unittest.cc b/src/test/unit/canard_unittest.cc
index dfc7df430d8..24208b6abb9 100644
--- a/src/test/unit/canard_unittest.cc
+++ b/src/test/unit/canard_unittest.cc
@@ -263,14 +263,9 @@ TEST_F(CanardTest, MemPool_PeakTracking)
 
 TEST_F(CanardTest, CRC_KnownValues)
 {
-    // CRC-16/CCITT (init 0xFFFF, poly 0x1021)
-    uint16_t crc = 0xFFFF;
-
-    // Single byte: CRC of "A" (0x41)
-    crc = crcAddByte(0xFFFF, 0x41);
-    // Verify it produces a non-trivial value
-    EXPECT_NE(crc, 0xFFFF);
-    EXPECT_NE(crc, 0x0000);
+    // CRC-16/CCITT-FALSE (init 0xFFFF, poly 0x1021), single byte 0x41 ('A')
+    uint16_t crc = crcAddByte(0xFFFF, 0x41);
+    EXPECT_EQ(crc, 0xB915);
 }
 
 TEST_F(CanardTest, CRC_AddString)

From 75f4bae5ce035082bd02bb124928658c2fc339a9 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Sun, 16 Aug 2026 18:47:16 -0700
Subject: [PATCH 57/67] feat(dronecan): request and accept GetNodeInfo from a
 target node

Add on-demand GetNodeInfo support: request a target node's software/
hardware version and name over DroneCAN, decode the response into the
node table, and expose it to the configurator/CLI via MSP
(MSP2_INAV_DRONECAN_NODE_INFO). This is the first on-demand (as opposed
to broadcast-only) DroneCAN service request INAV makes - GetNodeInfo
uses canardRequestOrRespond() under ATOMIC_BLOCK(NVIC_PRIO_CAN), matching
the ISR masking the H7/F7 driver rework established for TX.

MSP2_INAV_DRONECAN_NODE_INFO response grew to 71 bytes to carry the
decoded version/name fields (docs/msp regenerated to match;
MSP2_DRONECAN_NODE_INFO_SIZE replaces an inline field-count literal
wherever the response size was checked, including the msp_protocol_v2_inav.h
constant's own location, which moved out of fc_msp.c).

Also: per-node transfer_id (rather than one shared counter across all
nodes) so concurrent/overlapping GetNodeInfo requests to different nodes
don't cross-contaminate; node name storage extended to 80 bytes with
overflow logging; dronecanGetNodeByID() added to eliminate node-table
lookups duplicated across handlers; bus-off recovery now gives up after
50 attempts and enters STATE_DRONECAN_FAILED instead of retrying forever.

Full unit test suite passes: GetNodeInfo/SoftwareVersion/HardwareVersion/
RTCMStream response decode, shouldAcceptTransfer dispatch (GAP-S1/S2),
and node-table tests.

Squashed from the original feature/dronecan-getnodeinfo commit sequence
(24 commits - the initial multi-phase implementation, several rounds of
code-review fixups, and one rebase-artifact cleanup, "fixup: remove
orphaned TX loop and duplicate process1HzTasks from rebase artifact" -
into this single commit for a clean PR diff. No functional changes from
the squash itself.
---
 docs/development/msp/README.md                |  19 +-
 docs/development/msp/msp_messages.json        |  64 ++-
 src/main/drivers/dronecan/dronecan.c          | 128 ++++-
 src/main/drivers/dronecan/dronecan.h          |  15 +-
 src/main/fc/fc_msp.c                          |  47 +-
 src/main/msp/msp_protocol_v2_inav.h           |   7 +-
 src/test/unit/CMakeLists.txt                  |  38 ++
 .../unit/dronecan_application_unittest.cc     | 447 ++++++++++++++++++
 .../unit/dronecan_getnodeinfo_unittest.cc     | 362 ++++++++++++++
 9 files changed, 1069 insertions(+), 58 deletions(-)
 create mode 100644 src/test/unit/dronecan_application_unittest.cc
 create mode 100644 src/test/unit/dronecan_getnodeinfo_unittest.cc

diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md
index fafcf5d4762..56d177ffbad 100644
--- a/docs/development/msp/README.md
+++ b/docs/development/msp/README.md
@@ -4170,10 +4170,10 @@ When the MSP JSON specification changes, bump `msp_messages.json` version:
 | `nodeCount` | `uint8_t` | 1 | Number of detected DroneCAN nodes |
 | `nodeData` | `dronecanNodeStatus_t[]` | array | Array of per-node status records, one per detected node. Each record: nodeID(1)+health(1)+mode(1)+last_seen_ms(4) = 7 bytes. Full detail available via MSP2_INAV_DRONECAN_NODE_INFO. |
 
-**Notes:** Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 7 bytes each: nodeID(1)+health(1)+mode(1)+last_seen_ms(4). Maximum payload 1 + (DRONECAN_MAX_NODES * 7) = 225 bytes. Full node detail including uptime, vendor status, and name is available via MSP2_INAV_DRONECAN_NODE_INFO.
+**Notes:** Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 7 bytes each: nodeID(1)+health(1)+mode(1)+elapsed_ms(4), where elapsed_ms is milliseconds since the node was last seen. Maximum payload 1 + (DRONECAN_MAX_NODES * 7) = 225 bytes. Full node detail including uptime, vendor status, and name is available via MSP2_INAV_DRONECAN_NODE_INFO.
 
 ## <a id="msp2_inav_dronecan_node_info"></a>`MSP2_INAV_DRONECAN_NODE_INFO (8259 / 0x2043)`
-**Description:** Returns full status detail for a single DroneCAN node by ID.  
+**Description:** Returns full status detail for a single DroneCAN node by ID, including software and hardware version data retrieved via the DroneCAN GetNodeInfo service.  
   
 **Request Payload:**
 |Field|C Type|Size (Bytes)|Description|
@@ -4188,11 +4188,18 @@ When the MSP JSON specification changes, bump `msp_messages.json` version:
 | `mode` | `uint8_t` | 1 | - | Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE |
 | `uptime_sec` | `uint32_t` | 4 | s | Node uptime in seconds |
 | `vendor_status_code` | `uint16_t` | 2 | - | Vendor-specific status code |
-| `last_seen_ms` | `uint32_t` | 4 | ms | FC millisecond timestamp when this node was last seen |
+| `elapsed_ms` | `uint32_t` | 4 | ms | Milliseconds elapsed since this node was last seen (millis() - last_seen_ms at time of request) |
 | `name_len` | `uint8_t` | 1 | - | Length of node name string (0 if unknown) |
-| `name` | `char[32]` | 32 | - | Node name up to 32 bytes, zero-padded |
-
-**Notes:** Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table.
+| `name` | `char[80]` | 80 | - | Node name up to 80 bytes, zero-padded |
+| `sw_major` | `uint8_t` | 1 | - | Software version major (from GetNodeInfo response) |
+| `sw_minor` | `uint8_t` | 1 | - | Software version minor (from GetNodeInfo response) |
+| `sw_optional_field_flags` | `uint8_t` | 1 | - | UAVCAN SoftwareVersion optional_field_flags: bit 0 = vcs_commit valid, bit 1 = image_crc valid |
+| `sw_vcs_commit` | `uint32_t` | 4 | - | Git commit hash (valid when sw_optional_field_flags bit 0 is set) |
+| `hw_major` | `uint8_t` | 1 | - | Hardware version major (from GetNodeInfo response) |
+| `hw_minor` | `uint8_t` | 1 | - | Hardware version minor (from GetNodeInfo response) |
+| `hw_unique_id` | `uint8_t[16]` | 16 | - | 128-bit hardware unique ID (from GetNodeInfo response) |
+
+**Notes:** Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table. Response is always 119 bytes.
 
 ## <a id="msp2_inav_led_strip_config_ex"></a>`MSP2_INAV_LED_STRIP_CONFIG_EX (8264 / 0x2048)`
 **Description:** Retrieves the full configuration for each LED on the strip using the `ledConfig_t` structure. Supersedes `MSP_LED_STRIP_CONFIG`.  
diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json
index 3572d5462d4..d303bc2928b 100644
--- a/docs/development/msp/msp_messages.json
+++ b/docs/development/msp/msp_messages.json
@@ -9840,7 +9840,7 @@
                 ]
             },
             "variable_len": true,
-            "notes": "Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 7 bytes each: nodeID(1)+health(1)+mode(1)+last_seen_ms(4). Maximum payload 1 + (DRONECAN_MAX_NODES * 7) = 225 bytes. Full node detail including uptime, vendor status, and name is available via MSP2_INAV_DRONECAN_NODE_INFO.",
+            "notes": "Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 7 bytes each: nodeID(1)+health(1)+mode(1)+elapsed_ms(4), where elapsed_ms is milliseconds since the node was last seen. Maximum payload 1 + (DRONECAN_MAX_NODES * 7) = 225 bytes. Full node detail including uptime, vendor status, and name is available via MSP2_INAV_DRONECAN_NODE_INFO.",
             "description": "Returns the list of all detected DroneCAN nodes with their current status."
         },                                                                                                        
         "MSP2_INAV_DRONECAN_NODE_INFO": {                                                   
@@ -9889,10 +9889,10 @@
                         "units": ""                                                         
                     },
                     {
-                        "name": "last_seen_ms",
-                        "ctype": "uint32_t",                                                                      
-                        "desc": "FC millisecond timestamp when this node was last seen",
-                        "units": "ms"                                                                             
+                        "name": "elapsed_ms",
+                        "ctype": "uint32_t",
+                        "desc": "Milliseconds elapsed since this node was last seen (millis() - last_seen_ms at time of request)",
+                        "units": "ms"
                     },                                                                      
                     {
                         "name": "name_len",
@@ -9902,14 +9902,56 @@
                     },                                                                      
                     {
                         "name": "name",
-                        "ctype": "char[32]",
-                        "desc": "Node name up to 32 bytes, zero-padded",                                          
+                        "ctype": "char[80]",
+                        "desc": "Node name up to 80 bytes, zero-padded",
+                        "units": ""
+                    },
+                    {
+                        "name": "sw_major",
+                        "ctype": "uint8_t",
+                        "desc": "Software version major (from GetNodeInfo response)",
+                        "units": ""
+                    },
+                    {
+                        "name": "sw_minor",
+                        "ctype": "uint8_t",
+                        "desc": "Software version minor (from GetNodeInfo response)",
+                        "units": ""
+                    },
+                    {
+                        "name": "sw_optional_field_flags",
+                        "ctype": "uint8_t",
+                        "desc": "UAVCAN SoftwareVersion optional_field_flags: bit 0 = vcs_commit valid, bit 1 = image_crc valid",
                         "units": ""
-                    }                                                                                             
-                ]                                                                           
+                    },
+                    {
+                        "name": "sw_vcs_commit",
+                        "ctype": "uint32_t",
+                        "desc": "Git commit hash (valid when sw_optional_field_flags bit 0 is set)",
+                        "units": ""
+                    },
+                    {
+                        "name": "hw_major",
+                        "ctype": "uint8_t",
+                        "desc": "Hardware version major (from GetNodeInfo response)",
+                        "units": ""
+                    },
+                    {
+                        "name": "hw_minor",
+                        "ctype": "uint8_t",
+                        "desc": "Hardware version minor (from GetNodeInfo response)",
+                        "units": ""
+                    },
+                    {
+                        "name": "hw_unique_id",
+                        "ctype": "uint8_t[16]",
+                        "desc": "128-bit hardware unique ID (from GetNodeInfo response)",
+                        "units": ""
+                    }
+                ]
             },
-            "notes": "Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table.",                                                                                                     
-            "description": "Returns full status detail for a single DroneCAN node by ID."
+            "notes": "Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table. Response is always 119 bytes.",
+            "description": "Returns full status detail for a single DroneCAN node by ID, including software and hardware version data retrieved via the DroneCAN GetNodeInfo service."
         },                                        
         "MSP2_INAV_LED_STRIP_CONFIG_EX": {
             "code": 8264,
diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 0f671c2425c..8fa6bd60944 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -41,17 +41,30 @@ PG_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig,
 );
 
 static dronecanState_e dronecanState = STATE_DRONECAN_INIT;
+#ifdef UNIT_TEST
+uint8_t activeNodeCount = 0;
+dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES];
+static volatile uint32_t txErrCount = 0;
+static uint32_t busOffCount = 0;
+#else
 static uint8_t activeNodeCount = 0;
 static dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES];
 static volatile uint32_t txErrCount = 0;
 static uint32_t busOffCount = 0;
+#endif
 
 /* Forward declarations ------------------------------------------------------*/
 
 static void processCanardTxQueueSafe(void);
 static void process1HzTasks(timeUs_t timestamp_usec);
+#ifdef UNIT_TEST
+bool shouldAcceptTransfer(const CanardInstance *ins, uint64_t *out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, uint8_t source_node_id);
+void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer);
+void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer);
+#else
 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);
+#endif
 
 // ---- Public API -------------------------------------------------------------
 
@@ -183,11 +196,18 @@ void dronecanUpdate(timeUs_t currentTimeUs)
 
         case STATE_DRONECAN_BUS_OFF:
             if(currentTimeUs > (busoffTimeUs + 20000)) { // Wait 20ms: worst-case 128x11 recovery is 11.264ms at 125kbps
+                static uint8_t busoff_retries = 0;
                 canardSTM32RecoverFromBusOff();
                 busoffTimeUs = currentTimeUs;
                 canardSTM32GetProtocolStatus(&protocolStatus);
                 if(protocolStatus.BusOff == 0) {
+                    busoff_retries = 0;
                     dronecanState = STATE_DRONECAN_NORMAL;
+                } else if (++busoff_retries >= 50) {
+                    // ~1 second of 20ms recovery attempts with no success — permanent fault
+                    busoff_retries = 0;
+                    dronecanState = STATE_DRONECAN_FAILED;
+                    LOG_DEBUG(CAN, "DroneCAN: bus-off recovery failed after 50 attempts, entering FAILED state");
                 }
             }
             break;
@@ -319,6 +339,57 @@ static void processCanardTxQueueSafe(void) {
 // 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
 
+static dronecanNodeInfo_t *findNodeByID(uint8_t nodeID) {
+    for (uint8_t i = 0; i < activeNodeCount; i++) {
+        if (nodeTable[i].nodeID == nodeID) {
+            return &nodeTable[i];
+        }
+    }
+    return NULL;
+}
+
+const dronecanNodeInfo_t *dronecanGetNodeByID(uint8_t nodeID) {
+    return findNodeByID(nodeID);
+}
+
+static void handle_GetNodeInfoResponse(CanardInstance *ins, CanardRxTransfer *transfer) {
+    UNUSED(ins);
+    struct uavcan_protocol_GetNodeInfoResponse resp;
+
+    if (uavcan_protocol_GetNodeInfoResponse_decode(transfer, &resp)) {
+        LOG_DEBUG(CAN, "GetNodeInfoResponse decode failed");
+        return;
+    }
+
+    uint8_t nodeID = transfer->source_node_id;
+    dronecanNodeInfo_t *node = findNodeByID(nodeID);
+    if (!node) {
+        LOG_DEBUG(CAN, "GetNodeInfoResponse from unknown node %u", nodeID);
+        return;
+    }
+
+    if (transfer->transfer_id != ((node->getNodeInfo_transfer_id - 1) & 0x1F)) {
+        LOG_DEBUG(CAN, "GetNodeInfoResponse from node %u: stale tid %u", nodeID, transfer->transfer_id);
+        return;
+    }
+
+    uint8_t len = resp.name.len < sizeof(node->name) ? resp.name.len : sizeof(node->name);
+    node->name_len = len;
+    memcpy(node->name, resp.name.data, len);
+
+    node->sw_major = resp.software_version.major;
+    node->sw_minor = resp.software_version.minor;
+    node->sw_optional_field_flags = resp.software_version.optional_field_flags;
+    node->sw_vcs_commit = (resp.software_version.optional_field_flags & UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT)
+                         ? resp.software_version.vcs_commit : 0;
+
+    node->hw_major = resp.hardware_version.major;
+    node->hw_minor = resp.hardware_version.minor;
+    memcpy(node->hw_unique_id, resp.hardware_version.unique_id, sizeof(node->hw_unique_id));
+}
+// Canard Handlers and 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
@@ -390,7 +461,11 @@ static void process1HzTasks(timeUs_t timestamp_usec)
 
  This function must fill in the out_data_type_signature to be the signature of the message.
  */
+#ifdef UNIT_TEST
+bool shouldAcceptTransfer(const CanardInstance *ins,
+#else
 static bool shouldAcceptTransfer(const CanardInstance *ins,
+#endif
                                  uint64_t *out_data_type_signature,
                                  uint16_t data_type_id,
                                  CanardTransferType transfer_type,
@@ -408,8 +483,11 @@ static bool shouldAcceptTransfer(const CanardInstance *ins,
 		}
 	}
 	if (transfer_type == CanardTransferTypeResponse) {
-		// 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_RESPONSE_SIGNATURE;
+			return true;
+		}
 		}
 	}
 	if (transfer_type == CanardTransferTypeBroadcast) {
@@ -448,8 +526,11 @@ static bool shouldAcceptTransfer(const CanardInstance *ins,
 
 // Canard Handlers ( Many have code copied from libcanard esc_node example: https://github.com/dronecan/libcanard/blob/master/examples/ESCNode/esc_node.c )
 
+#ifdef UNIT_TEST
+void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) {
+#else
 static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) {
-	UNUSED(ins);
+#endif
     struct uavcan_protocol_NodeStatus nodeStatus;
 
 	if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) {
@@ -458,30 +539,39 @@ static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) {
 	}
 
 	uint8_t nodeId = transfer->source_node_id;
-    for (uint8_t i = 0; i < activeNodeCount; i++) {
-        if (nodeTable[i].nodeID == nodeId) {
-            // update health, mode, uptime, vendor_status_code, last_seen_ms
-            nodeTable[i].health = nodeStatus.health;
-            nodeTable[i].mode = nodeStatus.mode;
-            nodeTable[i].uptime_sec = nodeStatus.uptime_sec;
-            nodeTable[i].vendor_status_code = nodeStatus.vendor_specific_status_code;
-            nodeTable[i].last_seen_ms = millis();
-            return;
-        }
+    dronecanNodeInfo_t *node = findNodeByID(nodeId);
+    if (node) {
+        node->health = nodeStatus.health;
+        node->mode = nodeStatus.mode;
+        node->uptime_sec = nodeStatus.uptime_sec;
+        node->vendor_status_code = nodeStatus.vendor_specific_status_code;
+        node->last_seen_ms = millis();
+        return;
     }
     // new node
     if (activeNodeCount < DRONECAN_MAX_NODES) {
+        memset(&nodeTable[activeNodeCount], 0, sizeof(dronecanNodeInfo_t));
         nodeTable[activeNodeCount].nodeID = nodeId;
         nodeTable[activeNodeCount].health = nodeStatus.health;
         nodeTable[activeNodeCount].mode = nodeStatus.mode;
         nodeTable[activeNodeCount].uptime_sec = nodeStatus.uptime_sec;
         nodeTable[activeNodeCount].vendor_status_code = nodeStatus.vendor_specific_status_code;
-        nodeTable[activeNodeCount].name_len = 0;
-        nodeTable[activeNodeCount].name[0] = 0;
         nodeTable[activeNodeCount].last_seen_ms = millis();
         activeNodeCount++;
-    }
 
+        int16_t res;
+        ATOMIC_BLOCK(NVIC_PRIO_CAN) {
+            res = canardRequestOrRespond(ins, nodeId,
+                UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, UAVCAN_PROTOCOL_GETNODEINFO_ID,
+                &nodeTable[activeNodeCount - 1].getNodeInfo_transfer_id,
+                CANARD_TRANSFER_PRIORITY_LOW, CanardRequest, NULL, 0);
+        }
+        if (res < 0) {
+            LOG_DEBUG(CAN, "GetNodeInfo request failed for node %u: %d", nodeId, res);
+        }
+    } else {
+        LOG_DEBUG(CAN, "DroneCAN: node table full (%u nodes), ignoring node %u", DRONECAN_MAX_NODES, nodeId);
+    }
 }
 
 static void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) {
@@ -589,7 +679,11 @@ static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer)
 /*
  This callback is invoked by the library when a new message or request or response is received.
 */
+#ifdef UNIT_TEST
+void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) {
+#else
 static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) {
+#endif
 	// 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
@@ -602,6 +696,9 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
 	}
 	if (transfer->transfer_type == CanardTransferTypeResponse) {
 		switch (transfer->data_type_id) {
+        case UAVCAN_PROTOCOL_GETNODEINFO_ID:
+            handle_GetNodeInfoResponse(ins, transfer);
+            break;
 		}
 	}
 	if (transfer->transfer_type == CanardTransferTypeBroadcast) {
@@ -635,4 +732,5 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
         }
 	}
 }
+
 #endif
diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h
index c69981b9692..c4029cb0244 100644
--- a/src/main/drivers/dronecan/dronecan.h
+++ b/src/main/drivers/dronecan/dronecan.h
@@ -35,7 +35,19 @@ typedef struct dronecanNodeInfo_s {
     uint16_t vendor_status_code;
     uint32_t last_seen_ms;
     uint8_t name_len;
-    char name[32];
+    char name[80];
+    /* Software version (from GetNodeInfo response)*/
+    uint8_t sw_major;
+    uint8_t sw_minor;
+    uint8_t sw_optional_field_flags;
+    uint32_t sw_vcs_commit;
+    /* Hardware version (from GetNodeInfo response)*/
+    uint8_t hw_major;
+    uint8_t hw_minor;
+    uint8_t hw_unique_id[16];
+    /* Canard transfer ID for outgoing GetNodeInfo requests to this node.
+     * Must be per-node: Canard forbids sharing a counter across different dst_node_id. */
+    uint8_t getNodeInfo_transfer_id;
 } dronecanNodeInfo_t;
 
 // Wire format for MSP2_INAV_DRONECAN_NODES records (7 bytes each, packed).
@@ -54,5 +66,6 @@ uint32_t dronecanGetBitrateKbps(void);
 const dronecanNodeInfo_t *dronecanGetNode(uint8_t index);
 uint32_t dronecanGetBusOffCount(void);
 CanardPoolAllocatorStatistics dronecanGetPoolStats(void);
+const dronecanNodeInfo_t *dronecanGetNodeByID(uint8_t nodeID);
 
 PG_DECLARE(dronecanConfig_t, dronecanConfig);
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 919665397a3..1f6f627bcdc 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -4626,33 +4626,32 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
                 *ret = MSP_RESULT_ERROR;
                 break;
             }
-            uint8_t nodeId = sbufReadU8(src);
-            uint8_t count = dronecanGetNodeCount();
-            bool found = false;
-            for (uint8_t i = 0; i < count; i++) {
-                const dronecanNodeInfo_t *node = dronecanGetNode(i);
-                if (node->nodeID == nodeId) {
-                    found = true;
-                    if (sbufBytesRemaining(dst) < 46) {
-                        *ret = MSP_RESULT_ERROR;
-                        break;
-                    }
-                    sbufWriteU8(dst, node->nodeID);
-                    sbufWriteU8(dst, node->health);
-                    sbufWriteU8(dst, node->mode);
-                    sbufWriteU32(dst, node->uptime_sec);
-                    sbufWriteU16(dst, node->vendor_status_code);
-                    sbufWriteU32(dst, millis() - node->last_seen_ms);
-                    sbufWriteU8(dst, node->name_len);
-                    sbufWriteDataSafe(dst, node->name, 32);
-                    found = true;
-                    *ret = MSP_RESULT_ACK;
-                    break;
-                }
+            uint8_t nodeID = sbufReadU8(src);
+            const dronecanNodeInfo_t *node = dronecanGetNodeByID(nodeID);
+            if (!node) {
+                *ret = MSP_RESULT_ERROR;
+                break;
             }
-            if (!found) {
+            if (sbufBytesRemaining(dst) < MSP2_DRONECAN_NODE_INFO_SIZE) {
                 *ret = MSP_RESULT_ERROR;
+                break;
             }
+            sbufWriteU8(dst, node->nodeID);
+            sbufWriteU8(dst, node->health);
+            sbufWriteU8(dst, node->mode);
+            sbufWriteU32(dst, node->uptime_sec);
+            sbufWriteU16(dst, node->vendor_status_code);
+            sbufWriteU32(dst, millis() - node->last_seen_ms);
+            sbufWriteU8(dst, node->name_len);
+            sbufWriteDataSafe(dst, node->name, 80);
+            sbufWriteU8(dst,  node->sw_major);
+            sbufWriteU8(dst,  node->sw_minor);
+            sbufWriteU8(dst,  node->sw_optional_field_flags);
+            sbufWriteU32(dst, node->sw_vcs_commit);
+            sbufWriteU8(dst,  node->hw_major);
+            sbufWriteU8(dst,  node->hw_minor);
+            sbufWriteDataSafe(dst, node->hw_unique_id, 16);
+            *ret = MSP_RESULT_ACK;
         }
         break;
 #endif
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index ea2604e28b4..a85c02d1859 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -97,7 +97,12 @@
 #define MSP2_INAV_ESC_TELEM                     0x2041
 
 #define MSP2_INAV_DRONECAN_NODES                0x2042
-#define MSP2_INAV_DRONECAN_NODE_INFO            0x2043 
+#define MSP2_INAV_DRONECAN_NODE_INFO            0x2043
+// MSP2_INAV_DRONECAN_NODE_INFO reply size:
+// nodeID(1)+health(1)+mode(1)+uptime_sec(4)+vendor_status_code(2)+elapsed_ms(4)
+// +name_len(1)+name(80)+sw_major(1)+sw_minor(1)+sw_optional_field_flags(1)
+// +sw_vcs_commit(4)+hw_major(1)+hw_minor(1)+hw_unique_id(16) = 119
+#define MSP2_DRONECAN_NODE_INFO_SIZE            119
 
 #define MSP2_INAV_LED_STRIP_CONFIG_EX           0x2048
 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX       0x2049
diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt
index 4eb2d0da1a7..703295793aa 100644
--- a/src/test/unit/CMakeLists.txt
+++ b/src/test/unit/CMakeLists.txt
@@ -53,6 +53,44 @@ set_property(SOURCE dronecan_messages_unittest.cc PROPERTY extra_includes
     "../../lib/main/Dronecan/dsdlc_generated/include")
 set_property(SOURCE dronecan_messages_unittest.cc PROPERTY definitions USE_DRONECAN CANARD_ENABLE_TAO_OPTION=0)
 
+# GetNodeInfo, SoftwareVersion, HardwareVersion, RTCMStream tests
+set_property(SOURCE dronecan_getnodeinfo_unittest.cc PROPERTY depends
+    "drivers/dronecan/libcanard/canard.c")
+set_property(SOURCE dronecan_getnodeinfo_unittest.cc PROPERTY extra_sources
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.GetNodeInfo_res.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.GetNodeInfo_req.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.SoftwareVersion.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.HardwareVersion.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.NodeStatus.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.RTCMStream.c")
+set_property(SOURCE dronecan_getnodeinfo_unittest.cc PROPERTY extra_includes
+    "../../lib/main/Dronecan/dsdlc_generated/include")
+set_property(SOURCE dronecan_getnodeinfo_unittest.cc PROPERTY definitions USE_DRONECAN CANARD_ENABLE_TAO_OPTION=0)
+
+# DroneCAN application-layer tests - compiles dronecan.c with INAV stubs.
+# UNIT_TEST exposes activeNodeCount and nodeTable as non-static for SetUp reset.
+set_property(SOURCE dronecan_application_unittest.cc PROPERTY depends
+    "drivers/dronecan/dronecan.c"
+    "drivers/dronecan/libcanard/canard.c")
+set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_sources
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.NodeStatus.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.GetNodeInfo_res.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.GetNodeInfo_req.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.SoftwareVersion.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.HardwareVersion.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Fix2.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Fix.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Auxiliary.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.power.BatteryInfo.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.RTCMStream.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.Timestamp.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.ECEFPositionVelocity.c")
+set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_includes
+    "../../lib/main/Dronecan/dsdlc_generated/include")
+set_property(SOURCE dronecan_application_unittest.cc PROPERTY definitions
+    USE_DRONECAN CANARD_ENABLE_TAO_OPTION=0
+    FC_VERSION_MAJOR=10 FC_VERSION_MINOR=0 FC_VERSION_PATCH_LEVEL=0)
+
 # CAN bit-timing solver tests - links the real, HAL-free timing core shared
 # by the F7 (bxCAN) and H7 (FDCAN) drivers, so there is nothing to keep in sync
 set_property(SOURCE bxcan_timing_unittest.cc PROPERTY depends
diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc
new file mode 100644
index 00000000000..89492d795f2
--- /dev/null
+++ b/src/test/unit/dronecan_application_unittest.cc
@@ -0,0 +1,447 @@
+/**
+ * DroneCAN Application-Layer Unit Tests
+ *
+ * Tests node table management and transfer acceptance filter using the real
+ * dronecan.c compiled against INAV stubs. The UNIT_TEST build makes
+ * activeNodeCount and nodeTable non-static so tests can reset state in SetUp.
+ *
+ * Coverage:
+ *   GAP-N1  New node ID → added to table; no slot if table full
+ *   GAP-N2  Subsequent NodeStatus from same node → fields updated in place
+ *   GAP-N3  last_seen_ms follows controllable millis() value
+ *   GAP-N4  33rd unique node → table overflow rejected, count stays at 32
+ *   GAP-S1  shouldAcceptTransfer: NodeStatus ✓, GetNodeInfo request ✓,
+ *           GetNodeInfo response ✓, unknown ID ✗
+ */
+
+#include "gtest/gtest.h"
+
+extern "C" {
+#include <stdint.h>
+#include <string.h>
+#include <stdbool.h>
+
+#include "platform.h"
+
+/* DSDL types used by dronecan.c handlers */
+#include "uavcan.protocol.NodeStatus.h"
+#include "uavcan.protocol.GetNodeInfo.h"
+
+/* Canard core and STM32 driver declarations */
+#include "drivers/dronecan/libcanard/canard.h"
+#include "drivers/dronecan/libcanard/canard_stm32_driver.h"
+
+/* INAV headers pulled in by dronecan.c — included here so the types are
+   available when we define stub globals below. */
+#include "io/gps.h"
+#include "sensors/battery_sensor_dronecan.h"
+#include "fc/runtime_config.h"
+#include "sensors/diagnostics.h"
+#include "build/version.h"
+#include "common/log.h"
+
+/* Public API we test against */
+#include "drivers/dronecan/dronecan.h"
+
+/* Private state made non-static in UNIT_TEST builds */
+extern uint8_t activeNodeCount;
+extern dronecanNodeInfo_t nodeTable[];
+
+/* Private functions not exposed in dronecan.h */
+void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer);
+bool shouldAcceptTransfer(const CanardInstance *ins,
+                          uint64_t *out_data_type_signature,
+                          uint16_t data_type_id,
+                          CanardTransferType transfer_type,
+                          uint8_t source_node_id);
+void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer);
+
+/* =========================================================================
+ * Stubs — provide every symbol dronecan.c references that isn't supplied by
+ * the compiled dependencies (dronecan.c, canard.c, DSDL .c files).
+ * ========================================================================= */
+
+/* Controllable time source */
+static uint32_t mock_time_ms = 0;
+uint32_t millis(void) { return mock_time_ms; }
+
+/* Arming state — dronecan.c reads this for send_NodeStatus vendor code */
+uint32_t armingFlags = 0;
+
+/* GPS config — provider != GPS_DRONECAN so all GPS handlers return early */
+gpsConfig_t gpsConfig_System;
+gpsConfig_t gpsConfig_Copy;
+
+/* Hardware health — dronecan.c reads this in send_NodeStatus */
+bool isHardwareHealthy(void) { return true; }
+
+/* Logging — USE_LOG is unconditionally defined by target/common.h (pulled in
+   via platform.h), so LOG_ERROR/LOG_DEBUG in dronecan.c expand to real _logf()
+   calls. Stubbed as a no-op rather than linking common/log.c, which would pull
+   in drivers/serial.h, msp/msp.h, msp/msp_serial.h, fc/config.h and
+   config/feature.h — unrelated production dependencies this test has no need
+   for. Tests don't assert on logging output. */
+void _logf(logTopic_e topic, unsigned level, const char *fmt, ...) { (void)topic; (void)level; (void)fmt; }
+
+/* GPS and battery DroneCAN receive stubs */
+void dronecanGPSReceiveGNSSFix(const struct uavcan_equipment_gnss_Fix *p) { (void)p; }
+void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 *p) { (void)p; }
+void dronecanGPSReceiveGNSSAuxiliary(const struct uavcan_equipment_gnss_Auxiliary *p) { (void)p; }
+void dronecanBatterySensorReceiveInfo(struct uavcan_equipment_power_BatteryInfo *p) { (void)p; }
+
+/* STM32 CAN driver stubs */
+int16_t canardSTM32CAN1_Init(uint32_t b) { (void)b; return CANARD_OK; }
+int16_t canardSTM32Receive(CanardCANFrame *f) { (void)f; return 0; }
+uint32_t canardSTM32GetAndClearRxDropCount(void) { return 0; }
+int16_t canardSTM32Transmit(const CanardCANFrame *f) { (void)f; return 1; }
+void    canardSTM32GetProtocolStatus(canardProtocolStatus_t *s) { memset(s, 0, sizeof(*s)); }
+int32_t canardSTM32GetRxFifoFillLevel(void) { return 0; }
+void    canardSTM32RecoverFromBusOff(void) {}
+void    canardSTM32GetUniqueID(uint8_t id[16]) { memset(id, 0, 16); }
+
+/* Version strings declared in build/version.h */
+const char* const shortGitRevision = "00000000";
+const char* const compilerVersion  = "test";
+const char* const targetName       = "TEST";
+const char* const buildDate        = "Jan 01 2026";
+const char* const buildTime        = "00:00:00";
+
+} /* extern "C" */
+
+/* =========================================================================
+ * Helper: encode a NodeStatus and build a single-frame CanardRxTransfer.
+ * buf must be at least UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE bytes.
+ * ========================================================================= */
+static CanardRxTransfer makeNodeStatusTransfer(
+        uint8_t  nodeId,
+        uint32_t uptime_sec,
+        uint8_t  health,
+        uint8_t  mode,
+        uint16_t vendor_code,
+        uint8_t *buf)
+{
+    struct uavcan_protocol_NodeStatus ns;
+    memset(&ns, 0, sizeof(ns));
+    ns.uptime_sec                    = uptime_sec;
+    ns.health                        = health;
+    ns.mode                          = mode;
+    ns.vendor_specific_status_code   = vendor_code;
+
+    uint32_t len = uavcan_protocol_NodeStatus_encode(&ns, buf);
+
+    CanardRxTransfer xfer;
+    memset(&xfer, 0, sizeof(xfer));
+    xfer.transfer_type  = CanardTransferTypeBroadcast;
+    xfer.data_type_id   = UAVCAN_PROTOCOL_NODESTATUS_ID;
+    xfer.source_node_id = nodeId;
+    xfer.payload_head   = buf;
+    xfer.payload_len    = (uint16_t)len;
+    return xfer;
+}
+
+/* =========================================================================
+ * Node table tests (GAP-N1 … GAP-N4)
+ * ========================================================================= */
+
+class DroneCANNodeTableTest : public ::testing::Test {
+protected:
+    CanardInstance ins;
+    uint8_t memory_pool[4096]; /* generous pool: 32 nodes × 1 frame each */
+    uint8_t buf[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE + 4];
+
+    void SetUp() override {
+        activeNodeCount = 0;
+        memset(nodeTable, 0, sizeof(dronecanNodeInfo_t) * DRONECAN_MAX_NODES);
+        mock_time_ms = 0;
+        canardInit(&ins, memory_pool, sizeof(memory_pool),
+                   onTransferReceived, shouldAcceptTransfer, NULL);
+        canardSetLocalNodeID(&ins, 1); /* FC node ID required for canardRequestOrRespond */
+    }
+};
+
+/* GAP-N1: First NodeStatus from an unseen node ID → entry added to table */
+TEST_F(DroneCANNodeTableTest, NewNodeAddedOnFirstStatus)
+{
+    ASSERT_EQ(dronecanGetNodeCount(), 0u);
+
+    CanardRxTransfer xfer = makeNodeStatusTransfer(
+            10, 100,
+            UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK,
+            UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL,
+            0xABCD, buf);
+    handle_NodeStatus(&ins, &xfer);
+
+    EXPECT_EQ(dronecanGetNodeCount(), 1u);
+
+    const dronecanNodeInfo_t *node = dronecanGetNode(0);
+    ASSERT_NE(node, nullptr);
+    EXPECT_EQ(node->nodeID,             10u);
+    EXPECT_EQ(node->health,             UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK);
+    EXPECT_EQ(node->mode,               UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL);
+    EXPECT_EQ(node->uptime_sec,         100u);
+    EXPECT_EQ(node->vendor_status_code, 0xABCDu);
+    EXPECT_EQ(node->name_len,           0u);
+    EXPECT_EQ(node->name[0],            '\0');
+}
+
+/* GAP-N1 (second node): Two distinct IDs → two separate entries */
+TEST_F(DroneCANNodeTableTest, TwoDistinctNodesStoredSeparately)
+{
+    CanardRxTransfer x1 = makeNodeStatusTransfer(10, 100, 0, 0, 0, buf);
+    handle_NodeStatus(&ins, &x1);
+    CanardRxTransfer x2 = makeNodeStatusTransfer(20, 200, 0, 0, 0, buf);
+    handle_NodeStatus(&ins, &x2);
+
+    EXPECT_EQ(dronecanGetNodeCount(), 2u);
+    EXPECT_EQ(dronecanGetNode(0)->nodeID, 10u);
+    EXPECT_EQ(dronecanGetNode(1)->nodeID, 20u);
+}
+
+/* GAP-N2: Second NodeStatus from the same node → fields updated, no new entry */
+TEST_F(DroneCANNodeTableTest, ExistingNodeUpdatedInPlace)
+{
+    CanardRxTransfer x1 = makeNodeStatusTransfer(
+            10, 100,
+            UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK,
+            UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL,
+            0x0000, buf);
+    handle_NodeStatus(&ins, &x1);
+    ASSERT_EQ(dronecanGetNodeCount(), 1u);
+
+    CanardRxTransfer x2 = makeNodeStatusTransfer(
+            10, 500,
+            UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING,
+            UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE,
+            0xBEEF, buf);
+    handle_NodeStatus(&ins, &x2);
+
+    EXPECT_EQ(dronecanGetNodeCount(), 1u);   /* still one node */
+
+    const dronecanNodeInfo_t *node = dronecanGetNode(0);
+    ASSERT_NE(node, nullptr);
+    EXPECT_EQ(node->health,             UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING);
+    EXPECT_EQ(node->mode,               UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE);
+    EXPECT_EQ(node->uptime_sec,         500u);
+    EXPECT_EQ(node->vendor_status_code, 0xBEEFu);
+}
+
+/* GAP-N3: last_seen_ms is set from millis() at the time of each call */
+TEST_F(DroneCANNodeTableTest, LastSeenMsFollowsMillis)
+{
+    mock_time_ms = 1000;
+    CanardRxTransfer x1 = makeNodeStatusTransfer(20, 10, 0, 0, 0, buf);
+    handle_NodeStatus(&ins, &x1);
+
+    const dronecanNodeInfo_t *node = dronecanGetNode(0);
+    ASSERT_NE(node, nullptr);
+    EXPECT_EQ(node->last_seen_ms, 1000u);
+
+    mock_time_ms = 2500;
+    CanardRxTransfer x2 = makeNodeStatusTransfer(20, 20, 0, 0, 0, buf);
+    handle_NodeStatus(&ins, &x2);
+
+    EXPECT_EQ(node->last_seen_ms, 2500u);
+}
+
+/* GAP-N3: last_seen_ms for a new node also uses current millis() */
+TEST_F(DroneCANNodeTableTest, LastSeenMsSetOnInsert)
+{
+    mock_time_ms = 9999;
+    CanardRxTransfer xfer = makeNodeStatusTransfer(5, 0, 0, 0, 0, buf);
+    handle_NodeStatus(&ins, &xfer);
+
+    const dronecanNodeInfo_t *node = dronecanGetNode(0);
+    ASSERT_NE(node, nullptr);
+    EXPECT_EQ(node->last_seen_ms, 9999u);
+}
+
+/* GAP-N4: Fill the table to DRONECAN_MAX_NODES, then a 33rd node is silently
+   dropped — count stays at 32 and the overflow ID is not present. */
+TEST_F(DroneCANNodeTableTest, TableFullNodeRejected)
+{
+    for (uint8_t i = 1; i <= DRONECAN_MAX_NODES; i++) {
+        CanardRxTransfer xfer = makeNodeStatusTransfer(i, 0, 0, 0, 0, buf);
+        handle_NodeStatus(&ins, &xfer);
+    }
+    ASSERT_EQ(dronecanGetNodeCount(), (uint8_t)DRONECAN_MAX_NODES);
+
+    /* Try to add a 33rd node (ID 100, not in 1..32) */
+    CanardRxTransfer overflow = makeNodeStatusTransfer(100, 0, 0, 0, 0, buf);
+    handle_NodeStatus(&ins, &overflow);
+
+    EXPECT_EQ(dronecanGetNodeCount(), (uint8_t)DRONECAN_MAX_NODES);
+
+    for (uint8_t i = 0; i < DRONECAN_MAX_NODES; i++) {
+        const dronecanNodeInfo_t *n = dronecanGetNode(i);
+        ASSERT_NE(n, nullptr);
+        EXPECT_NE(n->nodeID, 100u) << "overflow node ID 100 should not be in slot " << (int)i;
+    }
+}
+
+/* GAP-N4 boundary: dronecanGetNode at index == DRONECAN_MAX_NODES returns NULL */
+TEST_F(DroneCANNodeTableTest, GetNodeOutOfBoundsReturnsNull)
+{
+    EXPECT_EQ(dronecanGetNode(DRONECAN_MAX_NODES), nullptr);
+    EXPECT_EQ(dronecanGetNode(255), nullptr);
+}
+
+/* =========================================================================
+ * shouldAcceptTransfer tests (GAP-S1)
+ * ========================================================================= */
+
+/* shouldAcceptTransfer does not use the CanardInstance — pass NULL. */
+
+TEST(DroneCANShouldAcceptTransfer, AcceptsNodeStatusBroadcast)
+{
+    uint64_t signature = 0;
+    bool accept = shouldAcceptTransfer(
+            nullptr, &signature,
+            UAVCAN_PROTOCOL_NODESTATUS_ID,
+            CanardTransferTypeBroadcast,
+            42);
+
+    EXPECT_TRUE(accept);
+    EXPECT_EQ(signature, UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE);
+}
+
+TEST(DroneCANShouldAcceptTransfer, AcceptsGetNodeInfoRequest)
+{
+    /* The FC handles incoming GetNodeInfo requests and sends a response */
+    uint64_t signature = 0;
+    bool accept = shouldAcceptTransfer(
+            nullptr, &signature,
+            UAVCAN_PROTOCOL_GETNODEINFO_ID,
+            CanardTransferTypeRequest,
+            42);
+
+    EXPECT_TRUE(accept);
+    EXPECT_EQ(signature, UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE);
+}
+
+TEST(DroneCANShouldAcceptTransfer, AcceptsGetNodeInfoResponse)
+{
+    /* Phase 3: FC now accepts GetNodeInfo responses so handle_GetNodeInfoResponse
+       can populate the node table with name and version data. */
+    uint64_t signature = 0;
+    bool accept = shouldAcceptTransfer(
+            nullptr, &signature,
+            UAVCAN_PROTOCOL_GETNODEINFO_ID,
+            CanardTransferTypeResponse,
+            42);
+
+    EXPECT_TRUE(accept);
+    EXPECT_EQ(signature, UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE);
+}
+
+TEST(DroneCANShouldAcceptTransfer, RejectsUnknownBroadcastId)
+{
+    uint64_t signature = 0;
+    bool accept = shouldAcceptTransfer(
+            nullptr, &signature,
+            0xFFFF,                        /* not a real UAVCAN data type ID */
+            CanardTransferTypeBroadcast,
+            42);
+
+    EXPECT_FALSE(accept);
+}
+
+TEST(DroneCANShouldAcceptTransfer, RejectsUnknownResponseId)
+{
+    uint64_t signature = 0;
+    bool accept = shouldAcceptTransfer(
+            nullptr, &signature,
+            0xFFFF,
+            CanardTransferTypeResponse,
+            42);
+
+    EXPECT_FALSE(accept);
+}
+
+/* =========================================================================
+ * onTransferReceived dispatch test (GAP-S2)
+ *
+ * Verifies that a GetNodeInfo response transfer is dispatched to
+ * handle_GetNodeInfoResponse and populates the node table entry.
+ * Written before Phase 4 — fails until the handler is implemented.
+ * ========================================================================= */
+
+class DroneCANDispatchTest : public ::testing::Test {
+protected:
+    CanardInstance ins;
+    uint8_t memory_pool[4096];
+    uint8_t buf[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE + 16];
+
+    void SetUp() override {
+        activeNodeCount = 0;
+        memset(nodeTable, 0, sizeof(dronecanNodeInfo_t) * DRONECAN_MAX_NODES);
+        mock_time_ms = 0;
+        canardInit(&ins, memory_pool, sizeof(memory_pool),
+                   onTransferReceived, shouldAcceptTransfer, NULL);
+        canardSetLocalNodeID(&ins, 1);
+    }
+};
+
+/* GAP-S2: GetNodeInfo response → handler populates name and version fields */
+TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesNodeTableEntry)
+{
+    /* Pre-insert node 42 via a NodeStatus so the table has a slot for it */
+    uint8_t ns_buf[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE + 4];
+    CanardRxTransfer ns_xfer = makeNodeStatusTransfer(42, 10, 0, 0, 0, ns_buf);
+    handle_NodeStatus(&ins, &ns_xfer);
+    ASSERT_EQ(dronecanGetNodeCount(), 1u);
+
+    /* Build a GetNodeInfo response from node 42 */
+    struct uavcan_protocol_GetNodeInfoResponse resp;
+    memset(&resp, 0, sizeof(resp));
+
+    resp.status.uptime_sec = 10;
+    resp.status.health     = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK;
+    resp.status.mode       = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
+
+    resp.software_version.major                = 1;
+    resp.software_version.minor                = 7;
+    resp.software_version.optional_field_flags = 1; /* vcs_commit valid */
+    resp.software_version.vcs_commit           = 0xDEADBEEF;
+
+    resp.hardware_version.major = 2;
+    resp.hardware_version.minor = 0;
+    for (int i = 0; i < 16; i++) {
+        resp.hardware_version.unique_id[i] = (uint8_t)(0xA0 + i);
+    }
+
+    const char *name = "com.example.gps";
+    resp.name.len = (uint8_t)strlen(name);
+    memcpy(resp.name.data, name, resp.name.len);
+
+    uint32_t encoded_len = uavcan_protocol_GetNodeInfoResponse_encode(&resp, buf);
+
+    CanardRxTransfer xfer;
+    memset(&xfer, 0, sizeof(xfer));
+    xfer.transfer_type  = CanardTransferTypeResponse;
+    xfer.data_type_id   = UAVCAN_PROTOCOL_GETNODEINFO_ID;
+    xfer.source_node_id = 42;
+    xfer.payload_head   = buf;
+    xfer.payload_len    = (uint16_t)encoded_len;
+
+    onTransferReceived(&ins, &xfer);
+
+    /* Verify the node table entry was populated */
+    const dronecanNodeInfo_t *node = dronecanGetNode(0);
+    ASSERT_NE(node, nullptr);
+    EXPECT_EQ(node->nodeID, 42u);
+
+    EXPECT_EQ(node->name_len, (uint8_t)strlen(name));
+    EXPECT_EQ(0, memcmp(node->name, name, node->name_len));
+
+    EXPECT_EQ(node->sw_major,                1u);
+    EXPECT_EQ(node->sw_minor,                7u);
+    EXPECT_EQ(node->sw_optional_field_flags,  1u);
+    EXPECT_EQ(node->sw_vcs_commit,           0xDEADBEEFu);
+
+    EXPECT_EQ(node->hw_major, 2u);
+    EXPECT_EQ(node->hw_minor, 0u);
+    for (int i = 0; i < 16; i++) {
+        EXPECT_EQ(node->hw_unique_id[i], (uint8_t)(0xA0 + i))
+            << "unique_id mismatch at byte " << i;
+    }
+}
diff --git a/src/test/unit/dronecan_getnodeinfo_unittest.cc b/src/test/unit/dronecan_getnodeinfo_unittest.cc
new file mode 100644
index 00000000000..f9a8e5fa7ea
--- /dev/null
+++ b/src/test/unit/dronecan_getnodeinfo_unittest.cc
@@ -0,0 +1,362 @@
+/**
+ * DroneCAN GetNodeInfo and Service Message Unit Tests
+ *
+ * Covers coverage gaps identified in audit 2026-06-01:
+ *   GAP-D1  GetNodeInfoResponse encode/decode round-trip
+ *   GAP-D2  RTCMStream encode/decode
+ *   GAP-D3  SoftwareVersion optional_field_flags wire behaviour
+ *           (vcs_commit/image_crc are ALWAYS encoded; flags are app-level hint)
+ *
+ * Node table logic (GAP-N1..N4), MSP byte-layout (GAP-M1..M2), and
+ * shouldAcceptTransfer dispatch (GAP-S1..S3) require dronecan.c to be
+ * compiled with mocked INAV dependencies. That infrastructure belongs in a
+ * separate dronecan_application_unittest.cc — tracked in the project todo.
+ */
+
+#include <cstdint>
+#include <cstring>
+
+extern "C" {
+#include "drivers/dronecan/libcanard/canard.h"
+#include "uavcan.protocol.GetNodeInfo.h"
+#include "uavcan.protocol.GetNodeInfo_res.h"
+#include "uavcan.protocol.GetNodeInfo_req.h"
+#include "uavcan.protocol.SoftwareVersion.h"
+#include "uavcan.protocol.HardwareVersion.h"
+#include "uavcan.protocol.NodeStatus.h"
+#include "uavcan.equipment.gnss.RTCMStream.h"
+}
+
+#include "gtest/gtest.h"
+
+class DroneCANGetNodeInfoTest : public ::testing::Test {
+protected:
+    void SetUp() override {
+        memset(buffer, 0, sizeof(buffer));
+    }
+
+    CanardRxTransfer makeTransfer(uint32_t len) {
+        CanardRxTransfer transfer;
+        memset(&transfer, 0, sizeof(transfer));
+        transfer.payload_len = len;
+        transfer.payload_head = buffer;
+        transfer.payload_middle = NULL;
+        transfer.payload_tail = NULL;
+        return transfer;
+    }
+
+    // Buffer large enough for the largest GetNodeInfo response (377 bytes).
+    uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE + 16];
+};
+
+// ===========================================================================
+// GetNodeInfoResponse encode/decode (GAP-D1)
+// ===========================================================================
+
+TEST_F(DroneCANGetNodeInfoTest, GetNodeInfoResponse_RoundTrip)
+{
+    struct uavcan_protocol_GetNodeInfoResponse tx;
+    memset(&tx, 0, sizeof(tx));
+
+    // NodeStatus
+    tx.status.uptime_sec              = 12345;
+    tx.status.health                  = 1; // WARNING
+    tx.status.mode                    = 0; // OPERATIONAL
+    tx.status.vendor_specific_status_code = 0xABCD;
+
+    // SoftwareVersion
+    tx.software_version.major              = 1;
+    tx.software_version.minor              = 7;
+    tx.software_version.optional_field_flags = 1; // vcs_commit valid
+    tx.software_version.vcs_commit         = 0xDEADBEEF;
+    tx.software_version.image_crc          = 0; // not flagged
+
+    // HardwareVersion
+    tx.hardware_version.major = 2;
+    tx.hardware_version.minor = 0;
+    for (int i = 0; i < 16; i++) {
+        tx.hardware_version.unique_id[i] = (uint8_t)(0x10 + i);
+    }
+    tx.hardware_version.certificate_of_authenticity.len = 0;
+
+    // Name
+    const char *name = "com.example.sensor";
+    tx.name.len = (uint8_t)strlen(name);
+    memcpy(tx.name.data, name, tx.name.len);
+
+    uint32_t encoded_len = uavcan_protocol_GetNodeInfoResponse_encode(&tx, buffer);
+
+    EXPECT_GT(encoded_len, 0u);
+    EXPECT_LE(encoded_len, (uint32_t)UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE);
+
+    CanardRxTransfer transfer = makeTransfer(encoded_len);
+    struct uavcan_protocol_GetNodeInfoResponse rx;
+    memset(&rx, 0, sizeof(rx));
+    bool decode_failed = uavcan_protocol_GetNodeInfoResponse_decode(&transfer, &rx);
+
+    EXPECT_FALSE(decode_failed);
+
+    EXPECT_EQ(rx.status.uptime_sec,               tx.status.uptime_sec);
+    EXPECT_EQ(rx.status.health,                   tx.status.health);
+    EXPECT_EQ(rx.status.mode,                     tx.status.mode);
+    EXPECT_EQ(rx.status.vendor_specific_status_code, tx.status.vendor_specific_status_code);
+
+    EXPECT_EQ(rx.software_version.major,               tx.software_version.major);
+    EXPECT_EQ(rx.software_version.minor,               tx.software_version.minor);
+    EXPECT_EQ(rx.software_version.optional_field_flags, tx.software_version.optional_field_flags);
+    EXPECT_EQ(rx.software_version.vcs_commit,          tx.software_version.vcs_commit);
+
+    EXPECT_EQ(rx.hardware_version.major, tx.hardware_version.major);
+    EXPECT_EQ(rx.hardware_version.minor, tx.hardware_version.minor);
+    for (int i = 0; i < 16; i++) {
+        EXPECT_EQ(rx.hardware_version.unique_id[i], tx.hardware_version.unique_id[i])
+            << "unique_id mismatch at byte " << i;
+    }
+
+    EXPECT_EQ(rx.name.len, tx.name.len);
+    EXPECT_EQ(0, memcmp(rx.name.data, tx.name.data, tx.name.len));
+}
+
+TEST_F(DroneCANGetNodeInfoTest, GetNodeInfoResponse_EmptyName)
+{
+    // TAO-encoded name length is inferred from remaining payload when len=0.
+    // A zero-length name must decode without error and name.len must be 0.
+    struct uavcan_protocol_GetNodeInfoResponse tx;
+    memset(&tx, 0, sizeof(tx));
+    tx.name.len = 0;
+
+    uint32_t encoded_len = uavcan_protocol_GetNodeInfoResponse_encode(&tx, buffer);
+    EXPECT_GT(encoded_len, 0u);
+
+    CanardRxTransfer transfer = makeTransfer(encoded_len);
+    struct uavcan_protocol_GetNodeInfoResponse rx;
+    memset(&rx, 0xFF, sizeof(rx));
+    bool decode_failed = uavcan_protocol_GetNodeInfoResponse_decode(&transfer, &rx);
+
+    EXPECT_FALSE(decode_failed);
+    EXPECT_EQ(rx.name.len, 0u);
+}
+
+TEST_F(DroneCANGetNodeInfoTest, GetNodeInfoResponse_MaxLengthName)
+{
+    struct uavcan_protocol_GetNodeInfoResponse tx;
+    memset(&tx, 0, sizeof(tx));
+    tx.name.len = 80;
+    for (int i = 0; i < 80; i++) {
+        tx.name.data[i] = (uint8_t)('a' + (i % 26));
+    }
+
+    uint32_t encoded_len = uavcan_protocol_GetNodeInfoResponse_encode(&tx, buffer);
+    EXPECT_LE(encoded_len, (uint32_t)UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE);
+
+    CanardRxTransfer transfer = makeTransfer(encoded_len);
+    struct uavcan_protocol_GetNodeInfoResponse rx;
+    memset(&rx, 0, sizeof(rx));
+    bool decode_failed = uavcan_protocol_GetNodeInfoResponse_decode(&transfer, &rx);
+
+    EXPECT_FALSE(decode_failed);
+    EXPECT_EQ(rx.name.len, 80u);
+    EXPECT_EQ(0, memcmp(rx.name.data, tx.name.data, 80));
+}
+
+// ===========================================================================
+// SoftwareVersion optional_field_flags (GAP-D3)
+//
+// The DSDL-generated encoder writes vcs_commit and image_crc unconditionally
+// (always 15 bytes on the wire). optional_field_flags is an app-level hint
+// that tells the receiver which fields are meaningful — it does NOT gate the
+// wire encoding. Tests here document this behaviour and ensure that
+// handle_GetNodeInfoResponse checks the flag before storing vcs_commit.
+// ===========================================================================
+
+TEST_F(DroneCANGetNodeInfoTest, SoftwareVersion_AlwaysEncodesAllFields)
+{
+    // Even with flags=0, vcs_commit and image_crc bytes are present on wire.
+    // Verify that a non-zero vcs_commit set with flags=0 still survives the
+    // round-trip — the application must use the flag to decide whether to use
+    // the value, not rely on the decoder zeroing it out.
+    struct uavcan_protocol_SoftwareVersion tx;
+    memset(&tx, 0, sizeof(tx));
+    tx.major               = 3;
+    tx.minor               = 1;
+    tx.optional_field_flags = 0; // neither field is flagged as valid
+    tx.vcs_commit          = 0xCAFEBABE; // present on wire, but not flagged
+    tx.image_crc           = 0;
+
+    uint32_t encoded_len = uavcan_protocol_SoftwareVersion_encode(&tx, buffer);
+    EXPECT_GT(encoded_len, 0u);
+
+    CanardRxTransfer transfer = makeTransfer(encoded_len);
+    struct uavcan_protocol_SoftwareVersion rx;
+    memset(&rx, 0, sizeof(rx));
+    bool decode_failed = uavcan_protocol_SoftwareVersion_decode(&transfer, &rx);
+
+    EXPECT_FALSE(decode_failed);
+    EXPECT_EQ(rx.major,                tx.major);
+    EXPECT_EQ(rx.minor,                tx.minor);
+    EXPECT_EQ(rx.optional_field_flags, 0u);
+    // vcs_commit IS decoded (wire is always 15 bytes) but flags=0 means
+    // the application must NOT trust it — assert the flag is checked:
+    EXPECT_EQ(rx.optional_field_flags & 1u, 0u) << "vcs_commit flag must not be set";
+}
+
+TEST_F(DroneCANGetNodeInfoTest, SoftwareVersion_VCSCommitFlaggedAndValid)
+{
+    struct uavcan_protocol_SoftwareVersion tx;
+    memset(&tx, 0, sizeof(tx));
+    tx.major               = 1;
+    tx.minor               = 5;
+    tx.optional_field_flags = 1; // VCS_COMMIT valid
+    tx.vcs_commit          = 0xDEADBEEF;
+    tx.image_crc           = 0;
+
+    uint32_t encoded_len = uavcan_protocol_SoftwareVersion_encode(&tx, buffer);
+    CanardRxTransfer transfer = makeTransfer(encoded_len);
+    struct uavcan_protocol_SoftwareVersion rx;
+    memset(&rx, 0, sizeof(rx));
+
+    EXPECT_FALSE(uavcan_protocol_SoftwareVersion_decode(&transfer, &rx));
+    EXPECT_EQ(rx.optional_field_flags & 1u, 1u);
+    EXPECT_EQ(rx.vcs_commit, 0xDEADBEEFu);
+}
+
+TEST_F(DroneCANGetNodeInfoTest, SoftwareVersion_BothOptionalFieldsFlagged)
+{
+    struct uavcan_protocol_SoftwareVersion tx;
+    memset(&tx, 0, sizeof(tx));
+    tx.major               = 2;
+    tx.minor               = 0;
+    tx.optional_field_flags = 3; // VCS_COMMIT and IMAGE_CRC both valid
+    tx.vcs_commit          = 0x12345678;
+    tx.image_crc           = 0xABCDEF0123456789ULL;
+
+    uint32_t encoded_len = uavcan_protocol_SoftwareVersion_encode(&tx, buffer);
+    CanardRxTransfer transfer = makeTransfer(encoded_len);
+    struct uavcan_protocol_SoftwareVersion rx;
+    memset(&rx, 0, sizeof(rx));
+
+    EXPECT_FALSE(uavcan_protocol_SoftwareVersion_decode(&transfer, &rx));
+    EXPECT_EQ(rx.optional_field_flags, 3u);
+    EXPECT_EQ(rx.vcs_commit, 0x12345678u);
+    EXPECT_EQ(rx.image_crc,  0xABCDEF0123456789ULL);
+}
+
+// ===========================================================================
+// HardwareVersion unique_id (part of GAP-D1)
+// ===========================================================================
+
+TEST_F(DroneCANGetNodeInfoTest, HardwareVersion_UniqueIdRoundTrip)
+{
+    struct uavcan_protocol_HardwareVersion tx;
+    memset(&tx, 0, sizeof(tx));
+    tx.major = 1;
+    tx.minor = 0;
+    for (int i = 0; i < 16; i++) {
+        tx.unique_id[i] = (uint8_t)(0xA0 + i);
+    }
+    tx.certificate_of_authenticity.len = 0;
+
+    uint32_t encoded_len = uavcan_protocol_HardwareVersion_encode(&tx, buffer);
+    EXPECT_GT(encoded_len, 0u);
+
+    CanardRxTransfer transfer = makeTransfer(encoded_len);
+    struct uavcan_protocol_HardwareVersion rx;
+    memset(&rx, 0, sizeof(rx));
+
+    EXPECT_FALSE(uavcan_protocol_HardwareVersion_decode(&transfer, &rx));
+    EXPECT_EQ(rx.major, tx.major);
+    EXPECT_EQ(rx.minor, tx.minor);
+    for (int i = 0; i < 16; i++) {
+        EXPECT_EQ(rx.unique_id[i], tx.unique_id[i]) << "unique_id mismatch at byte " << i;
+    }
+    EXPECT_EQ(rx.certificate_of_authenticity.len, 0u);
+}
+
+TEST_F(DroneCANGetNodeInfoTest, HardwareVersion_ZeroUniqueId)
+{
+    struct uavcan_protocol_HardwareVersion tx;
+    memset(&tx, 0, sizeof(tx));
+    // All unique_id bytes zero — valid for nodes that don't implement unique ID.
+
+    uint32_t encoded_len = uavcan_protocol_HardwareVersion_encode(&tx, buffer);
+    CanardRxTransfer transfer = makeTransfer(encoded_len);
+    struct uavcan_protocol_HardwareVersion rx;
+    memset(&rx, 0xFF, sizeof(rx));
+
+    EXPECT_FALSE(uavcan_protocol_HardwareVersion_decode(&transfer, &rx));
+    for (int i = 0; i < 16; i++) {
+        EXPECT_EQ(rx.unique_id[i], 0u) << "unique_id byte " << i << " should be zero";
+    }
+}
+
+// ===========================================================================
+// RTCMStream encode/decode (GAP-D2)
+// ===========================================================================
+
+TEST_F(DroneCANGetNodeInfoTest, RTCMStream_BasicEncodeDecode)
+{
+    struct uavcan_equipment_gnss_RTCMStream tx;
+    memset(&tx, 0, sizeof(tx));
+    tx.protocol_id = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM3;
+
+    const uint8_t payload[] = {0xD3, 0x00, 0x13, 0x3E, 0xD0, 0x00,
+                                0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+                                0x00, 0x00, 0x00, 0x00, 0x70};
+    tx.data.len = sizeof(payload);
+    memcpy(tx.data.data, payload, sizeof(payload));
+
+    uint32_t encoded_len = uavcan_equipment_gnss_RTCMStream_encode(&tx, buffer);
+
+    EXPECT_GT(encoded_len, 0u);
+    EXPECT_LE(encoded_len, (uint32_t)UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_MAX_SIZE);
+
+    CanardRxTransfer transfer = makeTransfer(encoded_len);
+    struct uavcan_equipment_gnss_RTCMStream rx;
+    memset(&rx, 0, sizeof(rx));
+
+    EXPECT_FALSE(uavcan_equipment_gnss_RTCMStream_decode(&transfer, &rx));
+    EXPECT_EQ(rx.protocol_id, UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM3);
+    EXPECT_EQ(rx.data.len, tx.data.len);
+    EXPECT_EQ(0, memcmp(rx.data.data, tx.data.data, tx.data.len));
+}
+
+TEST_F(DroneCANGetNodeInfoTest, RTCMStream_EmptyPayload)
+{
+    struct uavcan_equipment_gnss_RTCMStream tx;
+    memset(&tx, 0, sizeof(tx));
+    tx.protocol_id = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM2;
+    tx.data.len    = 0;
+
+    uint32_t encoded_len = uavcan_equipment_gnss_RTCMStream_encode(&tx, buffer);
+    CanardRxTransfer transfer = makeTransfer(encoded_len);
+    struct uavcan_equipment_gnss_RTCMStream rx;
+    memset(&rx, 0xFF, sizeof(rx));
+
+    EXPECT_FALSE(uavcan_equipment_gnss_RTCMStream_decode(&transfer, &rx));
+    EXPECT_EQ(rx.protocol_id, UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM2);
+    EXPECT_EQ(rx.data.len, 0u);
+}
+
+// ===========================================================================
+// Constants (extend GAP-S2: signatures must match DSDL spec)
+// ===========================================================================
+
+TEST(DroneCANGetNodeInfoConstants, Signatures)
+{
+    EXPECT_EQ(UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE,          0xEE468A8121C46A9EULL);
+    EXPECT_EQ(UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE, 0xEE468A8121C46A9EULL);
+    EXPECT_EQ(UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_SIGNATURE,     0x1F56030ECB171501ULL);
+}
+
+TEST(DroneCANGetNodeInfoConstants, IDs)
+{
+    EXPECT_EQ(UAVCAN_PROTOCOL_GETNODEINFO_ID,         1u);
+    EXPECT_EQ(UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID,    1062u);
+}
+
+TEST(DroneCANGetNodeInfoConstants, MessageSizes)
+{
+    // Response max size accounts for 80-char name + all nested structs.
+    EXPECT_EQ(UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE, 377);
+    EXPECT_EQ(UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_MAX_SIZE,     130);
+}

From 738f9ea31d32fd05f79350bc658598bebe6a764e Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Sun, 16 Aug 2026 18:49:00 -0700
Subject: [PATCH 58/67] feat(dronecan): on-demand GetSet, ExecuteOpcode,
 RestartNode via shared async slot

Extend on-demand DroneCAN service requests beyond GetNodeInfo to param
GetSet, ExecuteOpcode, and RestartNode. All four services now share a
single in-flight async request slot (dronecanAsyncSlot) rather than
per-service state, since only one on-demand request is ever outstanding
at a time in practice: dronecanAsyncRequest() encodes and sends whichever
service's request (masked under ATOMIC_BLOCK against the CAN TX ISR), and
one response handler decodes whichever service's response arrives,
guarded by service_id/node_id/transfer_id matching so a stale or
mismatched response can't be misattributed to the wrong in-flight request.
A timeout (DRONECAN_ASYNC_TIMEOUT_MS) expires a request that never gets a
response, so the slot can't wedge waiting forever.

GetSet: full int/float/bool/string value union plus min/max NumericValue
range, exposed through MSP so the configurator can read/write a remote
node's parameters and see their valid range. ExecuteOpcode/RestartNode:
simple ok/fail response, for triggering a remote node's save/erase
opcodes or a restart.

Full unit test suite passes: response-decode coverage for GetSet
(int/float/bool/string/empty), ExecuteOpcode, and RestartNode, plus the
async-slot dispatch tests (GAP-S2) updated for the new shared-slot
architecture.

Squashed from the original feature/dronecan-param-getset commit sequence
(20 commits - the initial async-slot/GetSet/ExecuteOpcode/RestartNode
implementation plus several rounds of code-review fixups) into this
single commit for a clean PR diff. No functional changes from the squash
itself.
---
 docs/development/msp/README.md                |  63 +--
 docs/development/msp/msp_messages.json        | 153 +++---
 src/main/drivers/dronecan/dronecan.c          | 383 +++++++++++----
 src/main/drivers/dronecan/dronecan.h          | 113 ++++-
 src/main/fc/fc_msp.c                          | 197 ++++++--
 src/main/msp/msp_protocol_v2_inav.h           |   8 +-
 src/test/unit/CMakeLists.txt                  |  11 +-
 .../unit/dronecan_application_unittest.cc     | 446 +++++++++++++++++-
 8 files changed, 1108 insertions(+), 266 deletions(-)

diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md
index 56d177ffbad..6231331dc7a 100644
--- a/docs/development/msp/README.md
+++ b/docs/development/msp/README.md
@@ -418,7 +418,8 @@ When the MSP JSON specification changes, bump `msp_messages.json` version:
 [8256 - MSP2_INAV_ESC_RPM](#msp2_inav_esc_rpm)  
 [8257 - MSP2_INAV_ESC_TELEM](#msp2_inav_esc_telem)  
 [8258 - MSP2_INAV_DRONECAN_NODES](#msp2_inav_dronecan_nodes)  
-[8259 - MSP2_INAV_DRONECAN_NODE_INFO](#msp2_inav_dronecan_node_info)  
+[8259 - MSP2_INAV_DRONECAN_ASYNC_REQUEST](#msp2_inav_dronecan_async_request)  
+[8260 - MSP2_INAV_DRONECAN_ASYNC_RESULT](#msp2_inav_dronecan_async_result)  
 [8264 - MSP2_INAV_LED_STRIP_CONFIG_EX](#msp2_inav_led_strip_config_ex)  
 [8265 - MSP2_INAV_SET_LED_STRIP_CONFIG_EX](#msp2_inav_set_led_strip_config_ex)  
 [8266 - MSP2_INAV_FW_APPROACH](#msp2_inav_fw_approach)  
@@ -4165,41 +4166,49 @@ When the MSP JSON specification changes, bump `msp_messages.json` version:
 **Request Payload:** **None**  
   
 **Reply Payload:**
-|Field|C Type|Size (Bytes)|Description|
-|---|---|---|---|
-| `nodeCount` | `uint8_t` | 1 | Number of detected DroneCAN nodes |
-| `nodeData` | `dronecanNodeStatus_t[]` | array | Array of per-node status records, one per detected node. Each record: nodeID(1)+health(1)+mode(1)+last_seen_ms(4) = 7 bytes. Full detail available via MSP2_INAV_DRONECAN_NODE_INFO. |
+|Field|C Type|Size (Bytes)|Units|Description|
+|---|---|---|---|---|
+| `nodeCount` | `uint8_t` | 1 | - | Number of detected DroneCAN nodes |
+| `nodeID` | `uint8_t[]` | array | - | [per node] DroneCAN node ID (1-127) |
+| `health` | `uint8_t` | 1 | - | [per node] Node health: 0=OK, 1=WARNING, 2=ERROR, 3=CRITICAL |
+| `mode` | `uint8_t` | 1 | - | [per node] Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE |
+| `last_seen_ms` | `uint32_t` | 4 | ms | [per node] Milliseconds since this node was last seen (FC-local timestamp delta) |
+| `uptime_sec` | `uint32_t` | 4 | s | [per node] Node uptime in seconds (from NodeStatus broadcast) |
+| `vendor_status_code` | `uint16_t` | 2 | - | [per node] Vendor-specific status code |
 
-**Notes:** Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 7 bytes each: nodeID(1)+health(1)+mode(1)+elapsed_ms(4), where elapsed_ms is milliseconds since the node was last seen. Maximum payload 1 + (DRONECAN_MAX_NODES * 7) = 225 bytes. Full node detail including uptime, vendor status, and name is available via MSP2_INAV_DRONECAN_NODE_INFO.
+**Notes:** Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 13 bytes each: nodeID(1)+health(1)+mode(1)+last_seen_ms(4)+uptime_sec(4)+vendor_status_code(2). Maximum payload 1 + (DRONECAN_MAX_NODES * 13) = 417 bytes. For full node detail (name, SW/HW version, unique ID) use MSP2_INAV_DRONECAN_ASYNC_REQUEST with service_id=DRONECAN_SERVICE_GETNODEINFO(1).
 
-## <a id="msp2_inav_dronecan_node_info"></a>`MSP2_INAV_DRONECAN_NODE_INFO (8259 / 0x2043)`
-**Description:** Returns full status detail for a single DroneCAN node by ID, including software and hardware version data retrieved via the DroneCAN GetNodeInfo service.  
+## <a id="msp2_inav_dronecan_async_request"></a>`MSP2_INAV_DRONECAN_ASYNC_REQUEST (8259 / 0x2043)`
+**Description:** Initiates an asynchronous DroneCAN service request (GetNodeInfo, ParamGetSet, ExecuteOpcode, RestartNode) to a specific node. Result retrieved via MSP2_INAV_DRONECAN_ASYNC_RESULT.  
   
 **Request Payload:**
 |Field|C Type|Size (Bytes)|Description|
 |---|---|---|---|
-| `nodeID` | `uint8_t` | 1 | DroneCAN node ID to query (1-127) |
+| `service_id` | `uint16_t` | 2 | Service to invoke: 1=GETNODEINFO, 5=RESTART_NODE, 10=EXECUTE_OPCODE, 11=PARAM_GETSET. Transmitted as u16 for MSP alignment; only low 8 bits used. |
+| `nodeID` | `uint8_t` | 1 | Target DroneCAN node ID (1-127) |
   
 **Reply Payload:**
-|Field|C Type|Size (Bytes)|Units|Description|
-|---|---|---|---|---|
-| `nodeID` | `uint8_t` | 1 | - | DroneCAN node ID |
-| `health` | `uint8_t` | 1 | - | Node health: 0=OK, 1=WARNING, 2=ERROR, 3=CRITICAL |
-| `mode` | `uint8_t` | 1 | - | Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE |
-| `uptime_sec` | `uint32_t` | 4 | s | Node uptime in seconds |
-| `vendor_status_code` | `uint16_t` | 2 | - | Vendor-specific status code |
-| `elapsed_ms` | `uint32_t` | 4 | ms | Milliseconds elapsed since this node was last seen (millis() - last_seen_ms at time of request) |
-| `name_len` | `uint8_t` | 1 | - | Length of node name string (0 if unknown) |
-| `name` | `char[80]` | 80 | - | Node name up to 80 bytes, zero-padded |
-| `sw_major` | `uint8_t` | 1 | - | Software version major (from GetNodeInfo response) |
-| `sw_minor` | `uint8_t` | 1 | - | Software version minor (from GetNodeInfo response) |
-| `sw_optional_field_flags` | `uint8_t` | 1 | - | UAVCAN SoftwareVersion optional_field_flags: bit 0 = vcs_commit valid, bit 1 = image_crc valid |
-| `sw_vcs_commit` | `uint32_t` | 4 | - | Git commit hash (valid when sw_optional_field_flags bit 0 is set) |
-| `hw_major` | `uint8_t` | 1 | - | Hardware version major (from GetNodeInfo response) |
-| `hw_minor` | `uint8_t` | 1 | - | Hardware version minor (from GetNodeInfo response) |
-| `hw_unique_id` | `uint8_t[16]` | 16 | - | 128-bit hardware unique ID (from GetNodeInfo response) |
+|Field|C Type|Size (Bytes)|Description|
+|---|---|---|---|
+| `accepted` | `uint8_t` | 1 | 0=request accepted; 1=busy (slot in use) or unrecognised service_id; 0xFF=bus not in STATE_DRONECAN_NORMAL (not ready) |
+| `seq` | `uint8_t` | 1 | Sequence number; correlate with MSP2_INAV_DRONECAN_ASYNC_RESULT to verify the result belongs to this request |
+
+**Notes:** Requires `USE_DRONECAN`. Initiates an async DroneCAN service request; poll MSP2_INAV_DRONECAN_ASYNC_RESULT at ~100ms intervals until state=READY(2) or ERROR(3). Only one request in-flight at a time. Service-specific request fields follow the common header in the request payload: EXECUTE_OPCODE appends opcode(u8); PARAM_GETSET appends index(u16)+is_write(u8) and optionally value_type(u8)+value(variable) for writes, then req_name_len(u8)+req_name(bytes) for named lookup. Param value encoding: INT=lo(u32)+hi(u32), FLOAT=raw(u32), BOOL=u8, STRING=len(u8)+data. Requests time out after DRONECAN_ASYNC_TIMEOUT_MS (2000ms). If bus is not in STATE_DRONECAN_NORMAL, returns accepted=0xFF without dispatching.
+
+## <a id="msp2_inav_dronecan_async_result"></a>`MSP2_INAV_DRONECAN_ASYNC_RESULT (8260 / 0x2044)`
+**Description:** Polls the result of the most recent MSP2_INAV_DRONECAN_ASYNC_REQUEST. Poll at ~100ms intervals until state is READY(2) or ERROR(3).  
+
+**Request Payload:** **None**  
+  
+**Reply Payload:**
+|Field|C Type|Size (Bytes)|Description|
+|---|---|---|---|
+| `state` | `uint8_t` | 1 | Async slot state: 0=IDLE, 1=PENDING, 2=READY, 3=ERROR |
+| `seq` | `uint8_t` | 1 | Sequence number matching the originating MSP2_INAV_DRONECAN_ASYNC_REQUEST reply |
+| `service_id` | `uint16_t` | 2 | Service ID of the in-flight or just-completed request |
+| `node_id` | `uint8_t` | 1 | Node ID of the target |
 
-**Notes:** Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table. Response is always 119 bytes.
+**Notes:** Requires `USE_DRONECAN`. When state=READY(2), service-specific result fields follow the 5-byte common header. GETNODEINFO: name_len(u8)+name(bytes)+sw_major(u8)+sw_minor(u8)+sw_optional_field_flags(u8)+sw_vcs_commit(u32)+hw_major(u8)+hw_minor(u8)+hw_unique_id(u8[16]). PARAM_GETSET: name_len(u8)+name(bytes)+type(u8)+value(variable)+min_type(u8)+min(variable)+max_type(u8)+max(variable); value/min/max encoding: INT=lo(u32)+hi(u32), FLOAT=raw(u32), BOOL=u8, STRING=len(u8)+data; EMPTY(0) min/max type means no bound is present. EXECUTE_OPCODE and RESTART_NODE: ok(u8) where 1=success. Reading result when state=READY transitions slot back to IDLE.
 
 ## <a id="msp2_inav_led_strip_config_ex"></a>`MSP2_INAV_LED_STRIP_CONFIG_EX (8264 / 0x2048)`
 **Description:** Retrieves the full configuration for each LED on the strip using the `ledConfig_t` structure. Supersedes `MSP_LED_STRIP_CONFIG`.  
diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json
index d303bc2928b..196970c0a7e 100644
--- a/docs/development/msp/msp_messages.json
+++ b/docs/development/msp/msp_messages.json
@@ -9830,128 +9830,123 @@
                         "units": ""
                     },
                     {
-                        "name": "nodeData",
-                        "desc": "Array of per-node status records, one per detected node. Each record: nodeID(1)+health(1)+mode(1)+last_seen_ms(4) = 7 bytes. Full detail available via MSP2_INAV_DRONECAN_NODE_INFO.",
-                        "ctype": "dronecanNodeStatus_t",
-                        "array": true,
-                        "array_size": 0,
-                        "units": ""
-                    }
-                ]
-            },
-            "variable_len": true,
-            "notes": "Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 7 bytes each: nodeID(1)+health(1)+mode(1)+elapsed_ms(4), where elapsed_ms is milliseconds since the node was last seen. Maximum payload 1 + (DRONECAN_MAX_NODES * 7) = 225 bytes. Full node detail including uptime, vendor status, and name is available via MSP2_INAV_DRONECAN_NODE_INFO.",
-            "description": "Returns the list of all detected DroneCAN nodes with their current status."
-        },                                                                                                        
-        "MSP2_INAV_DRONECAN_NODE_INFO": {                                                   
-            "code": 8259,                                                                                         
-            "mspv": 2,                                                                                            
-            "request": {
-                "payload": [                                                                                      
-                    {                                                                       
                         "name": "nodeID",
                         "ctype": "uint8_t",
-                        "desc": "DroneCAN node ID to query (1-127)",
-                        "units": ""                                                                               
-                    }
-                ]                                                                                                 
-            },                                                                              
-            "reply": {
-                "payload": [
-                    {
-                        "name": "nodeID",
-                        "ctype": "uint8_t",                                                                       
-                        "desc": "DroneCAN node ID",
-                        "units": ""                                                                               
-                    },                                                                      
+                        "desc": "[per node] DroneCAN node ID (1-127)",
+                        "units": "",
+                        "array": true,
+                        "array_size": 0
+                    },
                     {
                         "name": "health",
                         "ctype": "uint8_t",
-                        "desc": "Node health: 0=OK, 1=WARNING, 2=ERROR, 3=CRITICAL",
+                        "desc": "[per node] Node health: 0=OK, 1=WARNING, 2=ERROR, 3=CRITICAL",
                         "units": ""
                     },
                     {
                         "name": "mode",
-                        "ctype": "uint8_t",                                                                       
-                        "desc": "Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE",                                                                                                       
-                        "units": ""                                                         
-                    },                                                                                            
-                    {
-                        "name": "uptime_sec",                                                                     
-                        "ctype": "uint32_t",                                                
-                        "desc": "Node uptime in seconds",
-                        "units": "s"
+                        "ctype": "uint8_t",
+                        "desc": "[per node] Node mode: 0=OPERATIONAL, 1=INITIALIZATION, 2=MAINTENANCE, 3=SOFTWARE_UPDATE, 7=OFFLINE",
+                        "units": ""
                     },
                     {
-                        "name": "vendor_status_code",                                                             
-                        "ctype": "uint16_t",
-                        "desc": "Vendor-specific status code",                                                    
-                        "units": ""                                                         
+                        "name": "last_seen_ms",
+                        "ctype": "uint32_t",
+                        "desc": "[per node] Milliseconds since this node was last seen (FC-local timestamp delta)",
+                        "units": "ms"
                     },
                     {
-                        "name": "elapsed_ms",
+                        "name": "uptime_sec",
                         "ctype": "uint32_t",
-                        "desc": "Milliseconds elapsed since this node was last seen (millis() - last_seen_ms at time of request)",
-                        "units": "ms"
-                    },                                                                      
+                        "desc": "[per node] Node uptime in seconds (from NodeStatus broadcast)",
+                        "units": "s"
+                    },
                     {
-                        "name": "name_len",
-                        "ctype": "uint8_t",                                                                       
-                        "desc": "Length of node name string (0 if unknown)",
-                        "units": ""                                                                               
-                    },                                                                      
+                        "name": "vendor_status_code",
+                        "ctype": "uint16_t",
+                        "desc": "[per node] Vendor-specific status code",
+                        "units": ""
+                    }
+                ]
+            },
+            "variable_len": true,
+            "notes": "Requires `USE_DRONECAN`. Response is `nodeCount` followed by `nodeCount` records of 13 bytes each: nodeID(1)+health(1)+mode(1)+last_seen_ms(4)+uptime_sec(4)+vendor_status_code(2). Maximum payload 1 + (DRONECAN_MAX_NODES * 13) = 417 bytes. For full node detail (name, SW/HW version, unique ID) use MSP2_INAV_DRONECAN_ASYNC_REQUEST with service_id=DRONECAN_SERVICE_GETNODEINFO(1).",
+            "description": "Returns the list of all detected DroneCAN nodes with their current status."
+        },
+        "MSP2_INAV_DRONECAN_ASYNC_REQUEST": {
+            "code": 8259,
+            "mspv": 2,
+            "request": {
+                "payload": [
                     {
-                        "name": "name",
-                        "ctype": "char[80]",
-                        "desc": "Node name up to 80 bytes, zero-padded",
+                        "name": "service_id",
+                        "ctype": "uint16_t",
+                        "desc": "Service to invoke: 1=GETNODEINFO, 5=RESTART_NODE, 10=EXECUTE_OPCODE, 11=PARAM_GETSET. Transmitted as u16 for MSP alignment; only low 8 bits used.",
                         "units": ""
                     },
                     {
-                        "name": "sw_major",
+                        "name": "nodeID",
                         "ctype": "uint8_t",
-                        "desc": "Software version major (from GetNodeInfo response)",
+                        "desc": "Target DroneCAN node ID (1-127)",
                         "units": ""
-                    },
+                    }
+                ]
+            },
+            "reply": {
+                "payload": [
                     {
-                        "name": "sw_minor",
+                        "name": "accepted",
                         "ctype": "uint8_t",
-                        "desc": "Software version minor (from GetNodeInfo response)",
+                        "desc": "0=request accepted; 1=busy (slot in use) or unrecognised service_id; 0xFF=bus not in STATE_DRONECAN_NORMAL (not ready)",
                         "units": ""
                     },
                     {
-                        "name": "sw_optional_field_flags",
+                        "name": "seq",
                         "ctype": "uint8_t",
-                        "desc": "UAVCAN SoftwareVersion optional_field_flags: bit 0 = vcs_commit valid, bit 1 = image_crc valid",
+                        "desc": "Sequence number; correlate with MSP2_INAV_DRONECAN_ASYNC_RESULT to verify the result belongs to this request",
                         "units": ""
-                    },
+                    }
+                ]
+            },
+            "variable_len": true,
+            "notes": "Requires `USE_DRONECAN`. Initiates an async DroneCAN service request; poll MSP2_INAV_DRONECAN_ASYNC_RESULT at ~100ms intervals until state=READY(2) or ERROR(3). Only one request in-flight at a time. Service-specific request fields follow the common header in the request payload: EXECUTE_OPCODE appends opcode(u8); PARAM_GETSET appends index(u16)+is_write(u8) and optionally value_type(u8)+value(variable) for writes, then req_name_len(u8)+req_name(bytes) for named lookup. Param value encoding: INT=lo(u32)+hi(u32), FLOAT=raw(u32), BOOL=u8, STRING=len(u8)+data. Requests time out after DRONECAN_ASYNC_TIMEOUT_MS (2000ms). If bus is not in STATE_DRONECAN_NORMAL, returns accepted=0xFF without dispatching.",
+            "description": "Initiates an asynchronous DroneCAN service request (GetNodeInfo, ParamGetSet, ExecuteOpcode, RestartNode) to a specific node. Result retrieved via MSP2_INAV_DRONECAN_ASYNC_RESULT."
+        },
+        "MSP2_INAV_DRONECAN_ASYNC_RESULT": {
+            "code": 8260,
+            "mspv": 2,
+            "request": null,
+            "reply": {
+                "payload": [
                     {
-                        "name": "sw_vcs_commit",
-                        "ctype": "uint32_t",
-                        "desc": "Git commit hash (valid when sw_optional_field_flags bit 0 is set)",
+                        "name": "state",
+                        "ctype": "uint8_t",
+                        "desc": "Async slot state: 0=IDLE, 1=PENDING, 2=READY, 3=ERROR",
                         "units": ""
                     },
                     {
-                        "name": "hw_major",
+                        "name": "seq",
                         "ctype": "uint8_t",
-                        "desc": "Hardware version major (from GetNodeInfo response)",
+                        "desc": "Sequence number matching the originating MSP2_INAV_DRONECAN_ASYNC_REQUEST reply",
                         "units": ""
                     },
                     {
-                        "name": "hw_minor",
-                        "ctype": "uint8_t",
-                        "desc": "Hardware version minor (from GetNodeInfo response)",
+                        "name": "service_id",
+                        "ctype": "uint16_t",
+                        "desc": "Service ID of the in-flight or just-completed request",
                         "units": ""
                     },
                     {
-                        "name": "hw_unique_id",
-                        "ctype": "uint8_t[16]",
-                        "desc": "128-bit hardware unique ID (from GetNodeInfo response)",
+                        "name": "node_id",
+                        "ctype": "uint8_t",
+                        "desc": "Node ID of the target",
                         "units": ""
                     }
                 ]
             },
-            "notes": "Requires `USE_DRONECAN`. Returns `MSP_RESULT_ERROR` if the requested node ID is not in the node table. Response is always 119 bytes.",
-            "description": "Returns full status detail for a single DroneCAN node by ID, including software and hardware version data retrieved via the DroneCAN GetNodeInfo service."
+            "variable_len": true,
+            "notes": "Requires `USE_DRONECAN`. When state=READY(2), service-specific result fields follow the 5-byte common header. GETNODEINFO: name_len(u8)+name(bytes)+sw_major(u8)+sw_minor(u8)+sw_optional_field_flags(u8)+sw_vcs_commit(u32)+hw_major(u8)+hw_minor(u8)+hw_unique_id(u8[16]). PARAM_GETSET: name_len(u8)+name(bytes)+type(u8)+value(variable)+min_type(u8)+min(variable)+max_type(u8)+max(variable); value/min/max encoding: INT=lo(u32)+hi(u32), FLOAT=raw(u32), BOOL=u8, STRING=len(u8)+data; EMPTY(0) min/max type means no bound is present. EXECUTE_OPCODE and RESTART_NODE: ok(u8) where 1=success. Reading result when state=READY transitions slot back to IDLE.",
+            "description": "Polls the result of the most recent MSP2_INAV_DRONECAN_ASYNC_REQUEST. Poll at ~100ms intervals until state is READY(2) or ERROR(3)."
         },                                        
         "MSP2_INAV_LED_STRIP_CONFIG_EX": {
             "code": 8264,
diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 8fa6bd60944..77ce978fde1 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -41,6 +41,8 @@ PG_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig,
 );
 
 static dronecanState_e dronecanState = STATE_DRONECAN_INIT;
+dronecanAsyncSlot_t dronecanAsyncSlot = { .state = DRONECAN_ASYNC_IDLE };
+
 #ifdef UNIT_TEST
 uint8_t activeNodeCount = 0;
 dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES];
@@ -143,6 +145,12 @@ void dronecanUpdate(timeUs_t currentTimeUs)
         case STATE_DRONECAN_NORMAL:
             processCanardTxQueueSafe();
 
+            // Check for and expire any pending async requests that have timed out.
+            if (dronecanAsyncSlot.state == DRONECAN_ASYNC_PENDING &&
+                millis() - dronecanAsyncSlot.requested_at_ms >= DRONECAN_ASYNC_TIMEOUT_MS) {
+                dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR;
+            }
+
              for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--)
              {
 	            timestamp = millis() * 1000ULL;
@@ -352,41 +360,260 @@ const dronecanNodeInfo_t *dronecanGetNodeByID(uint8_t nodeID) {
     return findNodeByID(nodeID);
 }
 
-static void handle_GetNodeInfoResponse(CanardInstance *ins, CanardRxTransfer *transfer) {
-    UNUSED(ins);
-    struct uavcan_protocol_GetNodeInfoResponse resp;
 
-    if (uavcan_protocol_GetNodeInfoResponse_decode(transfer, &resp)) {
-        LOG_DEBUG(CAN, "GetNodeInfoResponse decode failed");
-        return;
+bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *payload)
+{
+    if (dronecanAsyncSlot.state == DRONECAN_ASYNC_PENDING &&
+        millis() - dronecanAsyncSlot.requested_at_ms < DRONECAN_ASYNC_TIMEOUT_MS) {
+        return false;
     }
 
-    uint8_t nodeID = transfer->source_node_id;
-    dronecanNodeInfo_t *node = findNodeByID(nodeID);
-    if (!node) {
-        LOG_DEBUG(CAN, "GetNodeInfoResponse from unknown node %u", nodeID);
-        return;
+    // PARAM_GETSET_REQUEST is the largest payload; zero-init prevents garbage in UAVCAN reserved bits
+    uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_REQUEST_MAX_SIZE];
+    memset(buffer, 0, sizeof(buffer));
+    uint16_t len = 0;
+    uint64_t signature = 0;
+    const uint8_t *buf_ptr = NULL;
+
+    switch (service_id) {
+        case DRONECAN_SERVICE_GETNODEINFO:
+            signature = UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE;
+            len = 0;
+            break;
+
+        case DRONECAN_SERVICE_PARAM_GETSET: {
+            if (!payload) return false;
+            const dronecanParamRequest_t *req = (const dronecanParamRequest_t *)payload;
+            struct uavcan_protocol_param_GetSetRequest getset;
+            memset(&getset, 0, sizeof(getset));
+            getset.index = req->index;
+            if (req->is_write) {
+                getset.value.union_tag = (enum uavcan_protocol_param_Value_type_t)req->value_type;
+                switch (req->value_type) {
+                    case DRONECAN_PARAM_TYPE_INT:
+                        getset.value.integer_value = req->value_int;
+                        break;
+                    case DRONECAN_PARAM_TYPE_FLOAT:
+                        getset.value.real_value = req->value_float;
+                        break;
+                    case DRONECAN_PARAM_TYPE_BOOL:
+                        getset.value.boolean_value = req->value_bool;
+                        break;
+                    case DRONECAN_PARAM_TYPE_STRING: {
+                        uint8_t slen = req->value_str_len < sizeof(getset.value.string_value.data)
+                                       ? req->value_str_len : sizeof(getset.value.string_value.data);
+                        getset.value.string_value.len = slen;
+                        memcpy(getset.value.string_value.data, req->value_str, slen);
+                        break;
+                    }
+                    default:
+                        getset.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY;
+                        break;
+                }
+            }
+            uint8_t nlen = req->req_name_len < sizeof(getset.name.data)
+                           ? req->req_name_len : sizeof(getset.name.data);
+            getset.name.len = nlen;
+            memcpy(getset.name.data, req->req_name, nlen);
+            len = uavcan_protocol_param_GetSetRequest_encode(&getset, buffer);
+            buf_ptr = buffer;
+            signature = UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE;
+            break;
+        }
+
+        case DRONECAN_SERVICE_EXECUTE_OPCODE: {
+            if (!payload) return false;
+            const uint8_t *opcode = (const uint8_t *)payload;
+            struct uavcan_protocol_param_ExecuteOpcodeRequest req;
+            memset(&req, 0, sizeof(req));
+            req.opcode = *opcode;
+            req.argument = 0;
+            len = uavcan_protocol_param_ExecuteOpcodeRequest_encode(&req, buffer);
+            buf_ptr = buffer;
+            signature = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE;
+            break;
+        }
+
+        case DRONECAN_SERVICE_RESTART_NODE: {
+            struct uavcan_protocol_RestartNodeRequest req;
+            memset(&req, 0, sizeof(req));
+            req.magic_number = UAVCAN_PROTOCOL_RESTARTNODE_REQUEST_MAGIC_NUMBER;
+            len = uavcan_protocol_RestartNodeRequest_encode(&req, buffer);
+            buf_ptr = buffer;
+            signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE;
+            break;
+        }
+
+        default:
+            return false;
     }
 
-    if (transfer->transfer_id != ((node->getNodeInfo_transfer_id - 1) & 0x1F)) {
-        LOG_DEBUG(CAN, "GetNodeInfoResponse from node %u: stale tid %u", nodeID, transfer->transfer_id);
-        return;
+    // buf_ptr remains NULL only for GETNODEINFO (zero-length request); libcanard accepts NULL with len=0
+    int16_t res;
+    ATOMIC_BLOCK(NVIC_PRIO_CAN) {
+        res = canardRequestOrRespond(&canard, node_id, signature, service_id,
+            &dronecanAsyncSlot.transfer_id, CANARD_TRANSFER_PRIORITY_MEDIUM, CanardRequest,
+            buf_ptr, len);
     }
 
-    uint8_t len = resp.name.len < sizeof(node->name) ? resp.name.len : sizeof(node->name);
-    node->name_len = len;
-    memcpy(node->name, resp.name.data, len);
+    if (res < 0) {
+        LOG_WARNING(CAN, "dronecanAsyncRequest: service %u node %u failed: %d", service_id, node_id, res);
+        return false;
+    }
 
-    node->sw_major = resp.software_version.major;
-    node->sw_minor = resp.software_version.minor;
-    node->sw_optional_field_flags = resp.software_version.optional_field_flags;
-    node->sw_vcs_commit = (resp.software_version.optional_field_flags & UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT)
-                         ? resp.software_version.vcs_commit : 0;
+    dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING;
+    dronecanAsyncSlot.seq++;
+    dronecanAsyncSlot.service_id = service_id;
+    dronecanAsyncSlot.node_id = node_id;
+    dronecanAsyncSlot.requested_at_ms = millis();
+    return true;
+}
 
-    node->hw_major = resp.hardware_version.major;
-    node->hw_minor = resp.hardware_version.minor;
-    memcpy(node->hw_unique_id, resp.hardware_version.unique_id, sizeof(node->hw_unique_id));
+/*
+    Handle responses for any pending async service request
+    (GETNODEINFO, PARAM_GETSET, EXECUTE_OPCODE, RESTART_NODE).
+    A single handler serialises all on-demand service requests through
+    one shared slot, avoiding the need for per-service response queues.
+*/
+static void handle_AsyncServiceResponse(CanardInstance *ins, CanardRxTransfer *transfer)
+{
+    UNUSED(ins);
+
+    if (dronecanAsyncSlot.state != DRONECAN_ASYNC_PENDING) // timed out or already received
+        return;
+    if (transfer->data_type_id != dronecanAsyncSlot.service_id) // response service_id does not match the pending request
+        return;
+    if (transfer->source_node_id != dronecanAsyncSlot.node_id) // response received for different node_id
+        return;
+    // UAVCAN requires matching transfer_id to guard against stale frames (e.g. after bus-off recovery).
+    // canardRequestOrRespond increments the slot's transfer_id after sending, so the in-flight id is (transfer_id-1) mod 32.
+    if (transfer->transfer_id != ((dronecanAsyncSlot.transfer_id - 1) & 0x1F))
+        return;
+
+    switch (dronecanAsyncSlot.service_id) {
+        case DRONECAN_SERVICE_GETNODEINFO: {
+            struct uavcan_protocol_GetNodeInfoResponse resp;
+            if (uavcan_protocol_GetNodeInfoResponse_decode(transfer, &resp)) {
+                LOG_WARNING(CAN, "GetNodeInfoResponse decode failed");
+                dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR;
+                return;
+            }
+            dronecanGetNodeInfoResult_t *r = &dronecanAsyncSlot.result.node_info;
+            uint8_t len = resp.name.len < (sizeof(r->name) - 1) ? resp.name.len : (sizeof(r->name) - 1);
+            r->name_len = len;
+            memcpy(r->name, resp.name.data, len);
+            r->name[len] = '\0';
+            r->sw_major = resp.software_version.major;
+            r->sw_minor = resp.software_version.minor;
+            r->sw_optional_field_flags = resp.software_version.optional_field_flags;
+            r->sw_vcs_commit = (resp.software_version.optional_field_flags &
+                                UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT)
+                               ? resp.software_version.vcs_commit : 0;
+            r->hw_major = resp.hardware_version.major;
+            r->hw_minor = resp.hardware_version.minor;
+            memcpy(r->hw_unique_id, resp.hardware_version.unique_id, 16);
+            dronecanAsyncSlot.state = DRONECAN_ASYNC_READY;
+            break;
+        }
+
+        case DRONECAN_SERVICE_PARAM_GETSET: {
+            struct uavcan_protocol_param_GetSetResponse resp;
+            if (uavcan_protocol_param_GetSetResponse_decode(transfer, &resp)) {
+                LOG_WARNING(CAN, "ParamGetSetResponse decode failed");
+                dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR;
+                return;
+            }
+            dronecanParamResult_t *r = &dronecanAsyncSlot.result.param;
+            uint8_t name_len = resp.name.len < (sizeof(r->name) - 1) ? resp.name.len : (sizeof(r->name) - 1);
+            r->name_len = name_len;
+            memcpy(r->name, resp.name.data, name_len);
+            r->name[name_len] = '\0';
+            r->type = (uint8_t)resp.value.union_tag;
+            switch (resp.value.union_tag) {
+                case UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE:
+                    r->value_int = resp.value.integer_value;
+                    break;
+                case UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE:
+                    r->value_float = resp.value.real_value;
+                    break;
+                case UAVCAN_PROTOCOL_PARAM_VALUE_BOOLEAN_VALUE:
+                    r->value_bool = resp.value.boolean_value;
+                    break;
+                case UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE: {
+                    uint8_t slen = resp.value.string_value.len < (sizeof(r->value_str) - 1)
+                                   ? resp.value.string_value.len : (sizeof(r->value_str) - 1);
+                    r->value_str_len = slen;
+                    memcpy(r->value_str, resp.value.string_value.data, slen);
+                    r->value_str[slen] = '\0';
+                    break;
+                }
+                default:
+                    r->type = DRONECAN_PARAM_TYPE_EMPTY;
+                    break;
+            }
+            r->min_type = DRONECAN_PARAM_TYPE_EMPTY;
+            r->min_int  = 0;
+            r->min_float = 0.0f;
+            switch (resp.min_value.union_tag) {
+                case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE:
+                    r->min_type = DRONECAN_PARAM_TYPE_INT;
+                    r->min_int  = resp.min_value.integer_value;
+                    break;
+                case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE:
+                    r->min_type  = DRONECAN_PARAM_TYPE_FLOAT;
+                    r->min_float = resp.min_value.real_value;
+                    break;
+                default:
+                    break;
+            }
+            r->max_type  = DRONECAN_PARAM_TYPE_EMPTY;
+            r->max_int   = 0;
+            r->max_float = 0.0f;
+            switch (resp.max_value.union_tag) {
+                case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE:
+                    r->max_type = DRONECAN_PARAM_TYPE_INT;
+                    r->max_int  = resp.max_value.integer_value;
+                    break;
+                case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE:
+                    r->max_type  = DRONECAN_PARAM_TYPE_FLOAT;
+                    r->max_float = resp.max_value.real_value;
+                    break;
+                default:
+                    break;
+            }
+            dronecanAsyncSlot.state = DRONECAN_ASYNC_READY;
+            break;
+        }
+
+        case DRONECAN_SERVICE_EXECUTE_OPCODE: {
+            struct uavcan_protocol_param_ExecuteOpcodeResponse resp;
+            if (uavcan_protocol_param_ExecuteOpcodeResponse_decode(transfer, &resp)) {
+                LOG_WARNING(CAN, "ExecuteOpcodeResponse decode failed");
+                dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR;
+                return;
+            }
+            dronecanAsyncSlot.result.simple.ok = resp.ok;
+            dronecanAsyncSlot.state = DRONECAN_ASYNC_READY;
+            break;
+        }
+
+        case DRONECAN_SERVICE_RESTART_NODE: {
+            struct uavcan_protocol_RestartNodeResponse resp;
+            if (uavcan_protocol_RestartNodeResponse_decode(transfer, &resp)) {
+                LOG_WARNING(CAN, "RestartNodeResponse decode failed");
+                dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR;
+                return;
+            }
+            dronecanAsyncSlot.result.simple.ok = resp.ok;
+            dronecanAsyncSlot.state = DRONECAN_ASYNC_READY;
+            break;
+        }
+
+        default:
+            break;
+    }
 }
+
 // Canard Handlers and Senders
 
 
@@ -446,6 +673,16 @@ static void process1HzTasks(timeUs_t timestamp_usec)
         canardCleanupStaleTransfers(&canard, timestamp_usec);
     }
 
+    // Remove nodes that have stopped broadcasting NodeStatus
+    for (uint8_t i = 0; i < activeNodeCount; ) {
+        if (millis() - nodeTable[i].last_seen_ms > DRONECAN_NODE_STALE_TIMEOUT_MS) {
+            nodeTable[i] = nodeTable[activeNodeCount - 1];
+            activeNodeCount--;
+        } else {
+            i++;
+        }
+    }
+
     /*
       Transmit the node status message
     */
@@ -471,57 +708,54 @@ 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) {
-		switch (data_type_id) {
-		case UAVCAN_PROTOCOL_GETNODEINFO_ID: {
-			*out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE;
-			return true;
-		}
-		}
-	}
-	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;
+        switch (data_type_id) {
+        case UAVCAN_PROTOCOL_GETNODEINFO_ID:
+            *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE;
             return true;
         }
-        case UAVCAN_EQUIPMENT_GNSS_FIX_ID: {
-            *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_FIX_SIGNATURE;
+    }
+    if (transfer_type == CanardTransferTypeResponse) {
+        switch (data_type_id) {
+        case UAVCAN_PROTOCOL_GETNODEINFO_ID:
+            *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_SIGNATURE;
+            return true;
+        case UAVCAN_PROTOCOL_PARAM_GETSET_ID:
+            *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE;
+            return true;
+        case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID:
+            *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE;
+            return true;
+        case UAVCAN_PROTOCOL_RESTARTNODE_ID:
+            *out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE;
             return true;
         }
-        case UAVCAN_EQUIPMENT_GNSS_FIX2_ID: {
+    }
+    if (transfer_type == CanardTransferTypeBroadcast) {
+        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;
+        case UAVCAN_EQUIPMENT_GNSS_FIX_ID:
+            *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_FIX_SIGNATURE;
+            return true;
+        case UAVCAN_EQUIPMENT_GNSS_FIX2_ID:
             *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE;
             return true;
-        }
-        case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID: {
+        case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID:
             *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_SIGNATURE;
             return true;
-        }
-        case UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID: {
+        case UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID:
             *out_data_type_signature = UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE;
             return true;
         }
-		}
-	}
-	// we don't want any other messages
-	return false;
+    }
+    return false;
 }
 
 // Canard Handlers ( Many have code copied from libcanard esc_node example: https://github.com/dronecan/libcanard/blob/master/examples/ESCNode/esc_node.c )
@@ -531,6 +765,7 @@ void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) {
 #else
 static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) {
 #endif
+    UNUSED(ins);
     struct uavcan_protocol_NodeStatus nodeStatus;
 
 	if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) {
@@ -559,16 +794,6 @@ static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) {
         nodeTable[activeNodeCount].last_seen_ms = millis();
         activeNodeCount++;
 
-        int16_t res;
-        ATOMIC_BLOCK(NVIC_PRIO_CAN) {
-            res = canardRequestOrRespond(ins, nodeId,
-                UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, UAVCAN_PROTOCOL_GETNODEINFO_ID,
-                &nodeTable[activeNodeCount - 1].getNodeInfo_transfer_id,
-                CANARD_TRANSFER_PRIORITY_LOW, CanardRequest, NULL, 0);
-        }
-        if (res < 0) {
-            LOG_DEBUG(CAN, "GetNodeInfo request failed for node %u: %d", nodeId, res);
-        }
     } else {
         LOG_DEBUG(CAN, "DroneCAN: node table full (%u nodes), ignoring node %u", DRONECAN_MAX_NODES, nodeId);
     }
@@ -694,13 +919,11 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
 		}
 		}
 	}
-	if (transfer->transfer_type == CanardTransferTypeResponse) {
-		switch (transfer->data_type_id) {
-        case UAVCAN_PROTOCOL_GETNODEINFO_ID:
-            handle_GetNodeInfoResponse(ins, transfer);
-            break;
-		}
-	}
+
+    if (transfer->transfer_type == CanardTransferTypeResponse) {
+        handle_AsyncServiceResponse(&canard, transfer);
+    }
+
 	if (transfer->transfer_type == CanardTransferTypeBroadcast) {
 		// check if we want to handle a specific broadcast message
 		switch (transfer->data_type_id) {
diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h
index c4029cb0244..8722bccb916 100644
--- a/src/main/drivers/dronecan/dronecan.h
+++ b/src/main/drivers/dronecan/dronecan.h
@@ -28,35 +28,100 @@ typedef struct dronecanConfig_s {
 } dronecanConfig_t;
 
 typedef struct dronecanNodeInfo_s {
-    uint8_t nodeID;
-    uint8_t health;
-    uint8_t mode;
+    uint8_t  nodeID;
+    uint8_t  health;
+    uint8_t  mode;
     uint32_t uptime_sec;
     uint16_t vendor_status_code;
     uint32_t last_seen_ms;
-    uint8_t name_len;
-    char name[80];
-    /* Software version (from GetNodeInfo response)*/
-    uint8_t sw_major;
-    uint8_t sw_minor;
-    uint8_t sw_optional_field_flags;
-    uint32_t sw_vcs_commit;
-    /* Hardware version (from GetNodeInfo response)*/
-    uint8_t hw_major;
-    uint8_t hw_minor;
-    uint8_t hw_unique_id[16];
-    /* Canard transfer ID for outgoing GetNodeInfo requests to this node.
-     * Must be per-node: Canard forbids sharing a counter across different dst_node_id. */
-    uint8_t getNodeInfo_transfer_id;
 } dronecanNodeInfo_t;
 
-// Wire format for MSP2_INAV_DRONECAN_NODES records (7 bytes each, packed).
-typedef struct dronecanNodeStatus_s {
-    uint8_t nodeID;
-    uint8_t health;
-    uint8_t mode;
-    uint32_t last_seen_ms;
-} __attribute__((packed)) dronecanNodeStatus_t;
+typedef enum {
+    DRONECAN_ASYNC_IDLE = 0,
+    DRONECAN_ASYNC_PENDING,
+    DRONECAN_ASYNC_READY,
+    DRONECAN_ASYNC_ERROR,
+} dronecanAsyncState_e;
+
+#define DRONECAN_SERVICE_GETNODEINFO     1
+#define DRONECAN_SERVICE_RESTART_NODE    5
+#define DRONECAN_SERVICE_EXECUTE_OPCODE  10
+#define DRONECAN_SERVICE_PARAM_GETSET    11
+
+#define DRONECAN_ASYNC_TIMEOUT_MS      2000
+#define DRONECAN_NODE_STALE_TIMEOUT_MS 10000  // Remove node from table if no NodeStatus received for this long
+#define DRONECAN_STATE_NOT_READY       0xFF   // MSP sentinel: bus not in STATE_NORMAL; outside dronecanAsyncState_e range
+
+#define DRONECAN_PARAM_TYPE_EMPTY   0
+#define DRONECAN_PARAM_TYPE_INT     1
+#define DRONECAN_PARAM_TYPE_FLOAT   2
+#define DRONECAN_PARAM_TYPE_BOOL    3
+#define DRONECAN_PARAM_TYPE_STRING  4
+
+typedef struct dronecanParamRequest_s {
+    uint16_t index;
+    uint8_t  is_write;
+    uint8_t  value_type;
+    int64_t  value_int;
+    float    value_float;
+    uint8_t  value_bool;
+    uint8_t  value_str_len;
+    char     value_str[128];
+    uint8_t  req_name_len;
+    char     req_name[92];
+} dronecanParamRequest_t;
+
+typedef struct dronecanGetNodeInfoResult_s {
+    uint8_t  sw_major;
+    uint8_t  sw_minor;
+    uint8_t  sw_optional_field_flags;
+    uint32_t sw_vcs_commit;
+    uint8_t  hw_major;
+    uint8_t  hw_minor;
+    uint8_t  hw_unique_id[16];
+    uint8_t  name_len;
+    char     name[81]; // 80 bytes max + null terminator
+} dronecanGetNodeInfoResult_t;
+
+typedef struct dronecanParamResult_s {
+    uint8_t type;
+    int64_t value_int;
+    float   value_float;
+    uint8_t value_bool;
+    uint8_t value_str_len;
+    char    value_str[128];
+    uint8_t name_len;
+    char    name[93]; // 92 bytes max per UAVCAN param.GetSet DSDL + null terminator
+    // NumericValue range from the GetSet response; DRONECAN_PARAM_TYPE_EMPTY means not provided.
+    // Only INT and FLOAT variants are valid — BOOL and STRING have no numeric range.
+    uint8_t min_type;
+    int64_t min_int;
+    float   min_float;
+    uint8_t max_type;
+    int64_t max_int;
+    float   max_float;
+} dronecanParamResult_t;
+
+typedef struct dronecanSimpleResult_s {
+    bool ok;
+} dronecanSimpleResult_t;
+
+typedef struct dronecanAsyncSlot_s {
+    dronecanAsyncState_e state;
+    uint8_t  seq;
+    uint8_t service_id;
+    uint8_t  node_id;
+    uint8_t  transfer_id;
+    uint32_t requested_at_ms;
+    union {
+        dronecanGetNodeInfoResult_t node_info;
+        dronecanParamResult_t       param;
+        dronecanSimpleResult_t      simple;
+    } result;
+} dronecanAsyncSlot_t;
+
+extern dronecanAsyncSlot_t dronecanAsyncSlot;
+bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *payload);
 
 void dronecanInit(void);
 void dronecanUpdate(timeUs_t currentTimeUs);
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 1f6f627bcdc..db86f748f34 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1929,12 +1929,12 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             sbufWriteU8(dst, count);
             for (uint8_t i = 0; i < count; i++) {
                 const dronecanNodeInfo_t *node = dronecanGetNode(i);
-                sbufWriteDataSafe(dst, &(dronecanNodeStatus_t){
-                    .nodeID      = node->nodeID,
-                    .health      = node->health,
-                    .mode        = node->mode,
-                    .last_seen_ms = millis() - node->last_seen_ms,
-                }, sizeof(dronecanNodeStatus_t));
+                sbufWriteU8(dst,  node->nodeID);
+                sbufWriteU8(dst,  node->health);
+                sbufWriteU8(dst,  node->mode);
+                sbufWriteU32(dst, millis() - node->last_seen_ms);
+                sbufWriteU32(dst, node->uptime_sec);
+                sbufWriteU16(dst, node->vendor_status_code);
             }
         }
         break;
@@ -4620,37 +4620,176 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
         break;
 
 #ifdef USE_DRONECAN
-    case MSP2_INAV_DRONECAN_NODE_INFO:
+    case MSP2_INAV_DRONECAN_ASYNC_REQUEST:
         {
-            if (sbufBytesRemaining(src) < 1) {
+            if (sbufBytesRemaining(src) < 3) {
                 *ret = MSP_RESULT_ERROR;
                 break;
             }
+            uint8_t service_id = (uint8_t)sbufReadU16(src); // MSP uses u16 for protocol compat; UAVCAN service IDs are 8-bit
             uint8_t nodeID = sbufReadU8(src);
-            const dronecanNodeInfo_t *node = dronecanGetNodeByID(nodeID);
-            if (!node) {
-                *ret = MSP_RESULT_ERROR;
+
+            if (dronecanGetState() != STATE_DRONECAN_NORMAL) {
+                sbufWriteU8(dst, DRONECAN_STATE_NOT_READY);
+                sbufWriteU8(dst, 0);
+                *ret = MSP_RESULT_ACK;
                 break;
             }
-            if (sbufBytesRemaining(dst) < MSP2_DRONECAN_NODE_INFO_SIZE) {
-                *ret = MSP_RESULT_ERROR;
-                break;
+
+            bool accepted = false;
+            if (service_id == DRONECAN_SERVICE_GETNODEINFO) {
+                accepted = dronecanAsyncRequest(service_id, nodeID, NULL);
+            } else if (service_id == DRONECAN_SERVICE_PARAM_GETSET) {
+                if (sbufBytesRemaining(src) < 3) { // index(2) + is_write(1) minimum
+                    *ret = MSP_RESULT_ERROR;
+                    break;
+                }
+                dronecanParamRequest_t req;
+                memset(&req, 0, sizeof(req));
+                req.index    = sbufReadU16(src);
+                req.is_write = sbufReadU8(src);
+                if (req.is_write && sbufBytesRemaining(src) >= 1) {
+                    req.value_type = sbufReadU8(src);
+                    switch (req.value_type) {
+                        case DRONECAN_PARAM_TYPE_INT:
+                            if (sbufBytesRemaining(src) >= 8) {
+                                uint64_t tmp;
+                                sbufReadData(src, &tmp, sizeof(tmp));
+                                sbufAdvance(src, sizeof(tmp));
+                                req.value_int = (int64_t)tmp;
+                            }
+                            break;
+                        case DRONECAN_PARAM_TYPE_FLOAT:
+                            if (sbufBytesRemaining(src) >= 4) {
+                                uint32_t raw = sbufReadU32(src);
+                                memcpy(&req.value_float, &raw, 4);
+                            }
+                            break;
+                        case DRONECAN_PARAM_TYPE_BOOL:
+                            if (sbufBytesRemaining(src) >= 1)
+                                req.value_bool = sbufReadU8(src);
+                            break;
+                        case DRONECAN_PARAM_TYPE_STRING:
+                            if (sbufBytesRemaining(src) >= 1) {
+                                req.value_str_len = sbufReadU8(src);
+                                if (req.value_str_len > sizeof(req.value_str))
+                                    req.value_str_len = sizeof(req.value_str);
+                                if (sbufBytesRemaining(src) >= req.value_str_len) {
+                                    sbufReadData(src, req.value_str, req.value_str_len);
+                                    sbufAdvance(src, req.value_str_len);
+                                }
+                            }
+                            break;
+                    }
+                }
+                if (sbufBytesRemaining(src) >= 1) {
+                    req.req_name_len = sbufReadU8(src);
+                    if (req.req_name_len > sizeof(req.req_name))
+                        req.req_name_len = sizeof(req.req_name);
+                    if (sbufBytesRemaining(src) >= req.req_name_len) {
+                        sbufReadData(src, req.req_name, req.req_name_len);
+                        sbufAdvance(src, req.req_name_len);
+                    }
+                }
+                accepted = dronecanAsyncRequest(service_id, nodeID, &req);
+            } else if (service_id == DRONECAN_SERVICE_EXECUTE_OPCODE) {
+                if (sbufBytesRemaining(src) < 1) {
+                    *ret = MSP_RESULT_ERROR;
+                    break;
+                }
+                uint8_t opcode = sbufReadU8(src);
+                accepted = dronecanAsyncRequest(service_id, nodeID, &opcode);
+            } else if (service_id == DRONECAN_SERVICE_RESTART_NODE) {
+                accepted = dronecanAsyncRequest(service_id, nodeID, NULL);
+            }
+
+            sbufWriteU8(dst, accepted ? 0 : 1); // 0=accepted, 1=busy or unrecognised service_id
+            sbufWriteU8(dst, dronecanAsyncSlot.seq);
+            *ret = MSP_RESULT_ACK;
+        }
+        break;
+
+    case MSP2_INAV_DRONECAN_ASYNC_RESULT:
+        {
+            sbufWriteU8(dst, (uint8_t)dronecanAsyncSlot.state);
+            sbufWriteU8(dst, dronecanAsyncSlot.seq);
+            sbufWriteU16(dst, dronecanAsyncSlot.service_id);
+            sbufWriteU8(dst, dronecanAsyncSlot.node_id);
+
+            if (dronecanAsyncSlot.state == DRONECAN_ASYNC_READY) {
+                switch (dronecanAsyncSlot.service_id) {
+                    case DRONECAN_SERVICE_GETNODEINFO: {
+                        const dronecanGetNodeInfoResult_t *r = &dronecanAsyncSlot.result.node_info;
+                        sbufWriteU8(dst, r->name_len);
+                        sbufWriteDataSafe(dst, r->name, r->name_len);
+                        sbufWriteU8(dst,  r->sw_major);
+                        sbufWriteU8(dst,  r->sw_minor);
+                        sbufWriteU8(dst,  r->sw_optional_field_flags);
+                        sbufWriteU32(dst, r->sw_vcs_commit);
+                        sbufWriteU8(dst,  r->hw_major);
+                        sbufWriteU8(dst,  r->hw_minor);
+                        sbufWriteDataSafe(dst, r->hw_unique_id, 16);
+                        break;
+                    }
+                    case DRONECAN_SERVICE_PARAM_GETSET: {
+                        const dronecanParamResult_t *r = &dronecanAsyncSlot.result.param;
+                        sbufWriteU8(dst, r->name_len);
+                        sbufWriteDataSafe(dst, r->name, r->name_len);
+                        sbufWriteU8(dst, r->type);
+                        switch (r->type) {
+                            case DRONECAN_PARAM_TYPE_INT: {
+                                uint64_t tmp;
+                                memcpy(&tmp, &r->value_int, sizeof(tmp));
+                                sbufWriteData(dst, &tmp, sizeof(tmp));
+                                break;
+                            }
+                            case DRONECAN_PARAM_TYPE_FLOAT: {
+                                uint32_t raw;
+                                memcpy(&raw, &r->value_float, 4);
+                                sbufWriteU32(dst, raw);
+                                break;
+                            }
+                            case DRONECAN_PARAM_TYPE_BOOL:
+                                sbufWriteU8(dst, r->value_bool);
+                                break;
+                            case DRONECAN_PARAM_TYPE_STRING:
+                                sbufWriteU8(dst, r->value_str_len);
+                                sbufWriteDataSafe(dst, r->value_str, r->value_str_len);
+                                break;
+                            default:
+                                break;
+                        }
+                        sbufWriteU8(dst, r->min_type);
+                        if (r->min_type == DRONECAN_PARAM_TYPE_INT) {
+                            uint64_t utmp;
+                            memcpy(&utmp, &r->min_int, sizeof(utmp));
+                            sbufWriteData(dst, &utmp, sizeof(utmp));
+                        } else if (r->min_type == DRONECAN_PARAM_TYPE_FLOAT) {
+                            uint32_t raw;
+                            memcpy(&raw, &r->min_float, 4);
+                            sbufWriteU32(dst, raw);
+                        }
+                        sbufWriteU8(dst, r->max_type);
+                        if (r->max_type == DRONECAN_PARAM_TYPE_INT) {
+                            uint64_t utmp;
+                            memcpy(&utmp, &r->max_int, sizeof(utmp));
+                            sbufWriteData(dst, &utmp, sizeof(utmp));
+                        } else if (r->max_type == DRONECAN_PARAM_TYPE_FLOAT) {
+                            uint32_t raw;
+                            memcpy(&raw, &r->max_float, 4);
+                            sbufWriteU32(dst, raw);
+                        }
+                        break;
+                    }
+                    case DRONECAN_SERVICE_EXECUTE_OPCODE:
+                    case DRONECAN_SERVICE_RESTART_NODE:
+                        sbufWriteU8(dst, dronecanAsyncSlot.result.simple.ok ? 1 : 0);
+                        break;
+                }
+                dronecanAsyncSlot.state = DRONECAN_ASYNC_IDLE;
+            } else if (dronecanAsyncSlot.state == DRONECAN_ASYNC_ERROR) {
+                dronecanAsyncSlot.state = DRONECAN_ASYNC_IDLE;
             }
-            sbufWriteU8(dst, node->nodeID);
-            sbufWriteU8(dst, node->health);
-            sbufWriteU8(dst, node->mode);
-            sbufWriteU32(dst, node->uptime_sec);
-            sbufWriteU16(dst, node->vendor_status_code);
-            sbufWriteU32(dst, millis() - node->last_seen_ms);
-            sbufWriteU8(dst, node->name_len);
-            sbufWriteDataSafe(dst, node->name, 80);
-            sbufWriteU8(dst,  node->sw_major);
-            sbufWriteU8(dst,  node->sw_minor);
-            sbufWriteU8(dst,  node->sw_optional_field_flags);
-            sbufWriteU32(dst, node->sw_vcs_commit);
-            sbufWriteU8(dst,  node->hw_major);
-            sbufWriteU8(dst,  node->hw_minor);
-            sbufWriteDataSafe(dst, node->hw_unique_id, 16);
             *ret = MSP_RESULT_ACK;
         }
         break;
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index a85c02d1859..5a9a4db5a5d 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -97,12 +97,8 @@
 #define MSP2_INAV_ESC_TELEM                     0x2041
 
 #define MSP2_INAV_DRONECAN_NODES                0x2042
-#define MSP2_INAV_DRONECAN_NODE_INFO            0x2043
-// MSP2_INAV_DRONECAN_NODE_INFO reply size:
-// nodeID(1)+health(1)+mode(1)+uptime_sec(4)+vendor_status_code(2)+elapsed_ms(4)
-// +name_len(1)+name(80)+sw_major(1)+sw_minor(1)+sw_optional_field_flags(1)
-// +sw_vcs_commit(4)+hw_major(1)+hw_minor(1)+hw_unique_id(16) = 119
-#define MSP2_DRONECAN_NODE_INFO_SIZE            119
+#define MSP2_INAV_DRONECAN_ASYNC_REQUEST        0x2043
+#define MSP2_INAV_DRONECAN_ASYNC_RESULT         0x2044
 
 #define MSP2_INAV_LED_STRIP_CONFIG_EX           0x2048
 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX       0x2049
diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt
index 703295793aa..3a4f3798d9e 100644
--- a/src/test/unit/CMakeLists.txt
+++ b/src/test/unit/CMakeLists.txt
@@ -84,7 +84,16 @@ set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_sources
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.power.BatteryInfo.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.RTCMStream.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.Timestamp.c"
-    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.ECEFPositionVelocity.c")
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.ECEFPositionVelocity.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.GetSet_req.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.GetSet_res.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.ExecuteOpcode_req.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.ExecuteOpcode_res.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.RestartNode_req.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.RestartNode_res.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.Value.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.NumericValue.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.Empty.c")
 set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_includes
     "../../lib/main/Dronecan/dsdlc_generated/include")
 set_property(SOURCE dronecan_application_unittest.cc PROPERTY definitions
diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc
index 89492d795f2..41ab67b1ffd 100644
--- a/src/test/unit/dronecan_application_unittest.cc
+++ b/src/test/unit/dronecan_application_unittest.cc
@@ -26,6 +26,9 @@ extern "C" {
 /* DSDL types used by dronecan.c handlers */
 #include "uavcan.protocol.NodeStatus.h"
 #include "uavcan.protocol.GetNodeInfo.h"
+#include "uavcan.protocol.param.GetSet_res.h"
+#include "uavcan.protocol.param.ExecuteOpcode_res.h"
+#include "uavcan.protocol.RestartNode_res.h"
 
 /* Canard core and STM32 driver declarations */
 #include "drivers/dronecan/libcanard/canard.h"
@@ -139,6 +142,65 @@ static CanardRxTransfer makeNodeStatusTransfer(
     return xfer;
 }
 
+/* =========================================================================
+ * Helpers: encode response structs and build CanardRxTransfer objects.
+ * ========================================================================= */
+
+static CanardRxTransfer makeParamGetSetTransfer(
+        uint8_t source_node_id, uint8_t transfer_id,
+        struct uavcan_protocol_param_GetSetResponse *resp,
+        uint8_t *buf)
+{
+    uint32_t len = uavcan_protocol_param_GetSetResponse_encode(resp, buf);
+    CanardRxTransfer xfer;
+    memset(&xfer, 0, sizeof(xfer));
+    xfer.transfer_type  = CanardTransferTypeResponse;
+    xfer.data_type_id   = UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_ID;
+    xfer.source_node_id = source_node_id;
+    xfer.transfer_id    = transfer_id;
+    xfer.payload_head   = buf;
+    xfer.payload_len    = (uint16_t)len;
+    return xfer;
+}
+
+static CanardRxTransfer makeExecuteOpcodeTransfer(
+        uint8_t source_node_id, uint8_t transfer_id,
+        bool ok, uint8_t *buf)
+{
+    struct uavcan_protocol_param_ExecuteOpcodeResponse resp;
+    memset(&resp, 0, sizeof(resp));
+    resp.ok = ok;
+    uint32_t len = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&resp, buf);
+    CanardRxTransfer xfer;
+    memset(&xfer, 0, sizeof(xfer));
+    xfer.transfer_type  = CanardTransferTypeResponse;
+    xfer.data_type_id   = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_ID;
+    xfer.source_node_id = source_node_id;
+    xfer.transfer_id    = transfer_id;
+    xfer.payload_head   = buf;
+    xfer.payload_len    = (uint16_t)len;
+    return xfer;
+}
+
+static CanardRxTransfer makeRestartNodeTransfer(
+        uint8_t source_node_id, uint8_t transfer_id,
+        bool ok, uint8_t *buf)
+{
+    struct uavcan_protocol_RestartNodeResponse resp;
+    memset(&resp, 0, sizeof(resp));
+    resp.ok = ok;
+    uint32_t len = uavcan_protocol_RestartNodeResponse_encode(&resp, buf);
+    CanardRxTransfer xfer;
+    memset(&xfer, 0, sizeof(xfer));
+    xfer.transfer_type  = CanardTransferTypeResponse;
+    xfer.data_type_id   = UAVCAN_PROTOCOL_RESTARTNODE_RESPONSE_ID;
+    xfer.source_node_id = source_node_id;
+    xfer.transfer_id    = transfer_id;
+    xfer.payload_head   = buf;
+    xfer.payload_len    = (uint16_t)len;
+    return xfer;
+}
+
 /* =========================================================================
  * Node table tests (GAP-N1 … GAP-N4)
  * ========================================================================= */
@@ -180,8 +242,6 @@ TEST_F(DroneCANNodeTableTest, NewNodeAddedOnFirstStatus)
     EXPECT_EQ(node->mode,               UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL);
     EXPECT_EQ(node->uptime_sec,         100u);
     EXPECT_EQ(node->vendor_status_code, 0xABCDu);
-    EXPECT_EQ(node->name_len,           0u);
-    EXPECT_EQ(node->name[0],            '\0');
 }
 
 /* GAP-N1 (second node): Two distinct IDs → two separate entries */
@@ -357,6 +417,45 @@ TEST(DroneCANShouldAcceptTransfer, RejectsUnknownResponseId)
     EXPECT_FALSE(accept);
 }
 
+TEST(DroneCANShouldAcceptTransfer, AcceptsParamGetSetResponse)
+{
+    uint64_t signature = 0;
+    bool accept = shouldAcceptTransfer(
+            nullptr, &signature,
+            UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_ID,
+            CanardTransferTypeResponse,
+            42);
+
+    EXPECT_TRUE(accept);
+    EXPECT_EQ(signature, UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_SIGNATURE);
+}
+
+TEST(DroneCANShouldAcceptTransfer, AcceptsExecuteOpcodeResponse)
+{
+    uint64_t signature = 0;
+    bool accept = shouldAcceptTransfer(
+            nullptr, &signature,
+            UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_ID,
+            CanardTransferTypeResponse,
+            42);
+
+    EXPECT_TRUE(accept);
+    EXPECT_EQ(signature, UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_SIGNATURE);
+}
+
+TEST(DroneCANShouldAcceptTransfer, AcceptsRestartNodeResponse)
+{
+    uint64_t signature = 0;
+    bool accept = shouldAcceptTransfer(
+            nullptr, &signature,
+            UAVCAN_PROTOCOL_RESTARTNODE_RESPONSE_ID,
+            CanardTransferTypeResponse,
+            42);
+
+    EXPECT_TRUE(accept);
+    EXPECT_EQ(signature, UAVCAN_PROTOCOL_RESTARTNODE_RESPONSE_SIGNATURE);
+}
+
 /* =========================================================================
  * onTransferReceived dispatch test (GAP-S2)
  *
@@ -374,6 +473,8 @@ class DroneCANDispatchTest : public ::testing::Test {
     void SetUp() override {
         activeNodeCount = 0;
         memset(nodeTable, 0, sizeof(dronecanNodeInfo_t) * DRONECAN_MAX_NODES);
+        memset(&dronecanAsyncSlot, 0, sizeof(dronecanAsyncSlot));
+        dronecanAsyncSlot.state = DRONECAN_ASYNC_IDLE;
         mock_time_ms = 0;
         canardInit(&ins, memory_pool, sizeof(memory_pool),
                    onTransferReceived, shouldAcceptTransfer, NULL);
@@ -381,15 +482,26 @@ class DroneCANDispatchTest : public ::testing::Test {
     }
 };
 
-/* GAP-S2: GetNodeInfo response → handler populates name and version fields */
-TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesNodeTableEntry)
+/* GAP-S2: GetNodeInfo response → handler populates async slot result.
+ * The node table (dronecanNodeInfo_t) holds only NodeStatus-level fields since
+ * commit 96f8a4bd9 stripped the GetNodeInfo fields to save ~3.5 KB RAM and
+ * replaced auto-fetch with the on-demand async slot pattern. */
+TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesAsyncSlot)
 {
-    /* Pre-insert node 42 via a NodeStatus so the table has a slot for it */
+    /* Pre-insert node 42 via a NodeStatus (node table is independent of async slot) */
     uint8_t ns_buf[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE + 4];
     CanardRxTransfer ns_xfer = makeNodeStatusTransfer(42, 10, 0, 0, 0, ns_buf);
     handle_NodeStatus(&ins, &ns_xfer);
     ASSERT_EQ(dronecanGetNodeCount(), 1u);
 
+    /* Prime the async slot — handle_AsyncServiceResponse guards on state, service_id,
+     * node_id, and transfer_id. The guard checks transfer_id == (slot.transfer_id-1)&0x1F,
+     * so set transfer_id=1 so the expected in-flight id is 0 (matching xfer.transfer_id). */
+    dronecanAsyncSlot.state       = DRONECAN_ASYNC_PENDING;
+    dronecanAsyncSlot.service_id  = DRONECAN_SERVICE_GETNODEINFO;
+    dronecanAsyncSlot.node_id     = 42;
+    dronecanAsyncSlot.transfer_id = 1;
+
     /* Build a GetNodeInfo response from node 42 */
     struct uavcan_protocol_GetNodeInfoResponse resp;
     memset(&resp, 0, sizeof(resp));
@@ -405,9 +517,8 @@ TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesNodeTableEntry)
 
     resp.hardware_version.major = 2;
     resp.hardware_version.minor = 0;
-    for (int i = 0; i < 16; i++) {
+    for (int i = 0; i < 16; i++)
         resp.hardware_version.unique_id[i] = (uint8_t)(0xA0 + i);
-    }
 
     const char *name = "com.example.gps";
     resp.name.len = (uint8_t)strlen(name);
@@ -425,23 +536,318 @@ TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesNodeTableEntry)
 
     onTransferReceived(&ins, &xfer);
 
-    /* Verify the node table entry was populated */
+    /* Slot must now be READY */
+    EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY);
+
+    /* Result fields populated from the GetNodeInfo response */
+    const dronecanGetNodeInfoResult_t *r = &dronecanAsyncSlot.result.node_info;
+    EXPECT_EQ(r->name_len, (uint8_t)strlen(name));
+    EXPECT_EQ(0, memcmp(r->name, name, r->name_len));
+
+    EXPECT_EQ(r->sw_major,                1u);
+    EXPECT_EQ(r->sw_minor,                7u);
+    EXPECT_EQ(r->sw_optional_field_flags,  1u);
+    EXPECT_EQ(r->sw_vcs_commit,           0xDEADBEEFu);
+
+    EXPECT_EQ(r->hw_major, 2u);
+    EXPECT_EQ(r->hw_minor, 0u);
+    for (int i = 0; i < 16; i++)
+        EXPECT_EQ(r->hw_unique_id[i], (uint8_t)(0xA0 + i))
+            << "unique_id mismatch at byte " << i;
+
+    /* Node table entry still exists (populated by the preceding NodeStatus) */
     const dronecanNodeInfo_t *node = dronecanGetNode(0);
     ASSERT_NE(node, nullptr);
     EXPECT_EQ(node->nodeID, 42u);
+}
 
-    EXPECT_EQ(node->name_len, (uint8_t)strlen(name));
-    EXPECT_EQ(0, memcmp(node->name, name, node->name_len));
+/* =========================================================================
+ * Async service response guard rejection tests (GAP-S3)
+ *
+ * handle_AsyncServiceResponse has four guards before decoding the payload.
+ * Each test confirms a mismatched guard leaves the slot state unchanged.
+ * ========================================================================= */
 
-    EXPECT_EQ(node->sw_major,                1u);
-    EXPECT_EQ(node->sw_minor,                7u);
-    EXPECT_EQ(node->sw_optional_field_flags,  1u);
-    EXPECT_EQ(node->sw_vcs_commit,           0xDEADBEEFu);
+/* GAP-S3a: Slot in IDLE state → response silently ignored */
+TEST_F(DroneCANDispatchTest, AsyncSlot_IdleState_IgnoresParamGetSetResponse)
+{
+    /* slot stays IDLE (SetUp default); send a valid PARAM_GETSET response */
+    struct uavcan_protocol_param_GetSetResponse resp;
+    memset(&resp, 0, sizeof(resp));
+    resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
+    resp.value.integer_value = 7;
 
-    EXPECT_EQ(node->hw_major, 2u);
-    EXPECT_EQ(node->hw_minor, 0u);
-    for (int i = 0; i < 16; i++) {
-        EXPECT_EQ(node->hw_unique_id[i], (uint8_t)(0xA0 + i))
-            << "unique_id mismatch at byte " << i;
-    }
+    CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf);
+    onTransferReceived(&ins, &xfer);
+
+    EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_IDLE);
+}
+
+/* GAP-S3b: Slot PENDING but response comes from the wrong node ID */
+TEST_F(DroneCANDispatchTest, AsyncSlot_WrongNodeId_IgnoresResponse)
+{
+    dronecanAsyncSlot.state      = DRONECAN_ASYNC_PENDING;
+    dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET;
+    dronecanAsyncSlot.node_id    = 42;
+    dronecanAsyncSlot.transfer_id = 1; /* guard expects in-flight id (1-1)&0x1F = 0 */
+
+    struct uavcan_protocol_param_GetSetResponse resp;
+    memset(&resp, 0, sizeof(resp));
+    resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
+
+    /* source_node_id = 99, not 42 */
+    CanardRxTransfer xfer = makeParamGetSetTransfer(99, 0, &resp, buf);
+    onTransferReceived(&ins, &xfer);
+
+    EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_PENDING);
+}
+
+/* GAP-S3c: Slot PENDING but transfer_id does not match the in-flight id */
+TEST_F(DroneCANDispatchTest, AsyncSlot_WrongTransferId_IgnoresResponse)
+{
+    dronecanAsyncSlot.state      = DRONECAN_ASYNC_PENDING;
+    dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET;
+    dronecanAsyncSlot.node_id    = 42;
+    dronecanAsyncSlot.transfer_id = 1; /* guard expects xfer.transfer_id == 0 */
+
+    struct uavcan_protocol_param_GetSetResponse resp;
+    memset(&resp, 0, sizeof(resp));
+    resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
+
+    /* xfer.transfer_id = 5, which != (1-1)&0x1F = 0 */
+    CanardRxTransfer xfer = makeParamGetSetTransfer(42, 5, &resp, buf);
+    onTransferReceived(&ins, &xfer);
+
+    EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_PENDING);
+}
+
+/* GAP-S3d: Slot PENDING for GETNODEINFO; a PARAM_GETSET response arrives →
+ * data_type_id mismatch rejects it before any decode. */
+TEST_F(DroneCANDispatchTest, AsyncSlot_WrongServiceId_IgnoresResponse)
+{
+    dronecanAsyncSlot.state      = DRONECAN_ASYNC_PENDING;
+    dronecanAsyncSlot.service_id = DRONECAN_SERVICE_GETNODEINFO;
+    dronecanAsyncSlot.node_id    = 42;
+    dronecanAsyncSlot.transfer_id = 1;
+
+    struct uavcan_protocol_param_GetSetResponse resp;
+    memset(&resp, 0, sizeof(resp));
+    resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
+
+    /* xfer.data_type_id == PARAM_GETSET(11) != slot.service_id(GETNODEINFO=1) */
+    CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf);
+    onTransferReceived(&ins, &xfer);
+
+    EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_PENDING);
+}
+
+/* =========================================================================
+ * PARAM_GETSET response decode tests (GAP-S4)
+ * ========================================================================= */
+
+/* GAP-S4a: Integer value with integer min/max range */
+TEST_F(DroneCANDispatchTest, ParamGetSetIntResponse_PopulatesSlot)
+{
+    dronecanAsyncSlot.state      = DRONECAN_ASYNC_PENDING;
+    dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET;
+    dronecanAsyncSlot.node_id    = 42;
+    dronecanAsyncSlot.transfer_id = 1;
+
+    struct uavcan_protocol_param_GetSetResponse resp;
+    memset(&resp, 0, sizeof(resp));
+    resp.value.union_tag    = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE;
+    resp.value.integer_value = 42;
+    resp.min_value.union_tag   = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE;
+    resp.min_value.integer_value = 0;
+    resp.max_value.union_tag   = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE;
+    resp.max_value.integer_value = 100;
+    const char *name = "MOT_SPIN_MIN";
+    resp.name.len = (uint8_t)strlen(name);
+    memcpy(resp.name.data, name, resp.name.len);
+
+    CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf);
+    onTransferReceived(&ins, &xfer);
+
+    ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY);
+    const dronecanParamResult_t *r = &dronecanAsyncSlot.result.param;
+    EXPECT_EQ(r->type,       (uint8_t)DRONECAN_PARAM_TYPE_INT);
+    EXPECT_EQ(r->value_int,  42);
+    EXPECT_EQ(r->name_len,   (uint8_t)strlen(name));
+    EXPECT_EQ(0, memcmp(r->name, name, r->name_len));
+    EXPECT_EQ(r->min_type,   (uint8_t)DRONECAN_PARAM_TYPE_INT);
+    EXPECT_EQ(r->min_int,    0);
+    EXPECT_EQ(r->max_type,   (uint8_t)DRONECAN_PARAM_TYPE_INT);
+    EXPECT_EQ(r->max_int,    100);
+}
+
+/* GAP-S4b: Float value with float min/max range */
+TEST_F(DroneCANDispatchTest, ParamGetSetFloatResponse_PopulatesSlot)
+{
+    dronecanAsyncSlot.state      = DRONECAN_ASYNC_PENDING;
+    dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET;
+    dronecanAsyncSlot.node_id    = 42;
+    dronecanAsyncSlot.transfer_id = 1;
+
+    struct uavcan_protocol_param_GetSetResponse resp;
+    memset(&resp, 0, sizeof(resp));
+    resp.value.union_tag   = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE;
+    resp.value.real_value  = 3.14f;
+    resp.min_value.union_tag  = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE;
+    resp.min_value.real_value = 0.0f;
+    resp.max_value.union_tag  = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE;
+    resp.max_value.real_value = 10.0f;
+
+    CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf);
+    onTransferReceived(&ins, &xfer);
+
+    ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY);
+    const dronecanParamResult_t *r = &dronecanAsyncSlot.result.param;
+    EXPECT_EQ(r->type,       (uint8_t)DRONECAN_PARAM_TYPE_FLOAT);
+    EXPECT_FLOAT_EQ(r->value_float, 3.14f);
+    EXPECT_EQ(r->min_type,   (uint8_t)DRONECAN_PARAM_TYPE_FLOAT);
+    EXPECT_FLOAT_EQ(r->min_float, 0.0f);
+    EXPECT_EQ(r->max_type,   (uint8_t)DRONECAN_PARAM_TYPE_FLOAT);
+    EXPECT_FLOAT_EQ(r->max_float, 10.0f);
+}
+
+/* GAP-S4c: Boolean value (no numeric range) */
+TEST_F(DroneCANDispatchTest, ParamGetSetBoolResponse_PopulatesSlot)
+{
+    dronecanAsyncSlot.state      = DRONECAN_ASYNC_PENDING;
+    dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET;
+    dronecanAsyncSlot.node_id    = 42;
+    dronecanAsyncSlot.transfer_id = 1;
+
+    struct uavcan_protocol_param_GetSetResponse resp;
+    memset(&resp, 0, sizeof(resp));
+    resp.value.union_tag    = UAVCAN_PROTOCOL_PARAM_VALUE_BOOLEAN_VALUE;
+    resp.value.boolean_value = 1;
+    /* min/max remain EMPTY (memset to 0 = UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_EMPTY) */
+
+    CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf);
+    onTransferReceived(&ins, &xfer);
+
+    ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY);
+    const dronecanParamResult_t *r = &dronecanAsyncSlot.result.param;
+    EXPECT_EQ(r->type,       (uint8_t)DRONECAN_PARAM_TYPE_BOOL);
+    EXPECT_EQ(r->value_bool, 1u);
+    EXPECT_EQ(r->min_type,   (uint8_t)DRONECAN_PARAM_TYPE_EMPTY);
+    EXPECT_EQ(r->max_type,   (uint8_t)DRONECAN_PARAM_TYPE_EMPTY);
+}
+
+/* GAP-S4d: String value */
+TEST_F(DroneCANDispatchTest, ParamGetSetStringResponse_PopulatesSlot)
+{
+    dronecanAsyncSlot.state      = DRONECAN_ASYNC_PENDING;
+    dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET;
+    dronecanAsyncSlot.node_id    = 42;
+    dronecanAsyncSlot.transfer_id = 1;
+
+    struct uavcan_protocol_param_GetSetResponse resp;
+    memset(&resp, 0, sizeof(resp));
+    resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE;
+    const char *str = "hello";
+    resp.value.string_value.len = (uint8_t)strlen(str);
+    memcpy(resp.value.string_value.data, str, resp.value.string_value.len);
+
+    CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf);
+    onTransferReceived(&ins, &xfer);
+
+    ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY);
+    const dronecanParamResult_t *r = &dronecanAsyncSlot.result.param;
+    EXPECT_EQ(r->type,          (uint8_t)DRONECAN_PARAM_TYPE_STRING);
+    EXPECT_EQ(r->value_str_len, (uint8_t)strlen(str));
+    EXPECT_EQ(0, memcmp(r->value_str, str, r->value_str_len));
+}
+
+/* GAP-S4e: Empty value (unknown union_tag) → type forced to DRONECAN_PARAM_TYPE_EMPTY */
+TEST_F(DroneCANDispatchTest, ParamGetSetEmptyResponse_SetsEmptyType)
+{
+    dronecanAsyncSlot.state      = DRONECAN_ASYNC_PENDING;
+    dronecanAsyncSlot.service_id = DRONECAN_SERVICE_PARAM_GETSET;
+    dronecanAsyncSlot.node_id    = 42;
+    dronecanAsyncSlot.transfer_id = 1;
+
+    struct uavcan_protocol_param_GetSetResponse resp;
+    memset(&resp, 0, sizeof(resp));
+    /* union_tag == 0 == UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY */
+    resp.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY;
+
+    CanardRxTransfer xfer = makeParamGetSetTransfer(42, 0, &resp, buf);
+    onTransferReceived(&ins, &xfer);
+
+    ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY);
+    EXPECT_EQ(dronecanAsyncSlot.result.param.type, (uint8_t)DRONECAN_PARAM_TYPE_EMPTY);
+}
+
+/* =========================================================================
+ * EXECUTE_OPCODE response decode tests (GAP-S5)
+ * ========================================================================= */
+
+/* GAP-S5a: ok=true */
+TEST_F(DroneCANDispatchTest, ExecuteOpcodeOkResponse_PopulatesSlot)
+{
+    dronecanAsyncSlot.state      = DRONECAN_ASYNC_PENDING;
+    dronecanAsyncSlot.service_id = DRONECAN_SERVICE_EXECUTE_OPCODE;
+    dronecanAsyncSlot.node_id    = 42;
+    dronecanAsyncSlot.transfer_id = 1;
+
+    CanardRxTransfer xfer = makeExecuteOpcodeTransfer(42, 0, true, buf);
+    onTransferReceived(&ins, &xfer);
+
+    ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY);
+    EXPECT_TRUE(dronecanAsyncSlot.result.simple.ok);
+}
+
+/* GAP-S5b: ok=false */
+TEST_F(DroneCANDispatchTest, ExecuteOpcodeFailResponse_PopulatesSlot)
+{
+    dronecanAsyncSlot.state      = DRONECAN_ASYNC_PENDING;
+    dronecanAsyncSlot.service_id = DRONECAN_SERVICE_EXECUTE_OPCODE;
+    dronecanAsyncSlot.node_id    = 42;
+    dronecanAsyncSlot.transfer_id = 1;
+
+    CanardRxTransfer xfer = makeExecuteOpcodeTransfer(42, 0, false, buf);
+    onTransferReceived(&ins, &xfer);
+
+    ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY);
+    EXPECT_FALSE(dronecanAsyncSlot.result.simple.ok);
+}
+
+/* =========================================================================
+ * RESTART_NODE response decode test (GAP-S6)
+ * ========================================================================= */
+
+/* GAP-S6: ok=true */
+TEST_F(DroneCANDispatchTest, RestartNodeOkResponse_PopulatesSlot)
+{
+    dronecanAsyncSlot.state      = DRONECAN_ASYNC_PENDING;
+    dronecanAsyncSlot.service_id = DRONECAN_SERVICE_RESTART_NODE;
+    dronecanAsyncSlot.node_id    = 42;
+    dronecanAsyncSlot.transfer_id = 1;
+
+    CanardRxTransfer xfer = makeRestartNodeTransfer(42, 0, true, buf);
+    onTransferReceived(&ins, &xfer);
+
+    ASSERT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_READY);
+    EXPECT_TRUE(dronecanAsyncSlot.result.simple.ok);
+}
+
+/* =========================================================================
+ * dronecanAsyncRequest re-entry guard test (GAP-S7)
+ * ========================================================================= */
+
+/* GAP-S7: A second async request is rejected while one is already in flight.
+ * Uses RESTART_NODE (no null-payload check) so the re-entry guard is the only
+ * reason dronecanAsyncRequest returns false.  Slot PENDING with
+ * requested_at_ms=0 and mock_time_ms=0 keeps the timeout condition satisfied
+ * (0 < DRONECAN_ASYNC_TIMEOUT_MS), so the guard fires before touching the bus. */
+TEST_F(DroneCANDispatchTest, AsyncRequest_RejectedWhilePending)
+{
+    dronecanAsyncSlot.state           = DRONECAN_ASYNC_PENDING;
+    dronecanAsyncSlot.requested_at_ms = 0;
+    mock_time_ms = 0;
+
+    EXPECT_FALSE(dronecanAsyncRequest(DRONECAN_SERVICE_RESTART_NODE, 42, nullptr));
+    EXPECT_EQ(dronecanAsyncSlot.state, DRONECAN_ASYNC_PENDING);
 }

From 74a9d21aa12e2005213e035ad163601b7ea56e6a Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Sat, 15 Aug 2026 22:27:59 -0700
Subject: [PATCH 59/67] refactor(dronecan): extract async param/service client
 into its own module

Split dronecanAsyncRequest() and the response handler (GetNodeInfo,
ParamGetSet, ExecuteOpcode, RestartNode - a single shared slot
serialising all on-demand service requests) out of dronecan.c into
dronecan_async.c/.h. dronecanAsyncSlot's definition and the response
handler move too; dronecan.h keeps declaring dronecanAsyncRequest()/
dronecanAsyncSlot since fc_msp.c is an external caller of both.

dronecan.c's onTransferReceived() now calls
dronecanAsyncHandleServiceResponse() (renamed from the static
handle_AsyncServiceResponse for external linkage), and
STATE_DRONECAN_NORMAL calls the new dronecanAsyncCheckTimeout()
instead of carrying the timeout-expiry check inline.

Also flips the file-scope `canard` CanardInstance from static to
plain external linkage, since dronecan_async.c needs `extern
CanardInstance canard` to reach it. (On the branch this was originally
authored on, that linkage change had already landed earlier, as part
of the DNA server work - rebasing this extraction back to sit directly
on param-getset instead means picking it up here.)

Cherry-picked from feature/dronecan-actuator-control (original commit
e577393b6) onto feature/dronecan-param-getset: this is general
dronecan.c restructuring in async-request/GetNodeInfo/ParamGetSet/
ExecuteOpcode/RestartNode territory - this branch's own scope - not
actuator-control-specific, so it belongs here rather than riding along
with unrelated actuator-output work.

Full unit test suite (29 tests in dronecan_application_unittest, full
suite otherwise unchanged) passes. SITL builds clean with -Werror.
---
 src/main/CMakeLists.txt                    |   2 +
 src/main/drivers/dronecan/dronecan.c       | 266 +------------------
 src/main/drivers/dronecan/dronecan_async.c | 294 +++++++++++++++++++++
 src/main/drivers/dronecan/dronecan_async.h |  16 ++
 src/test/unit/CMakeLists.txt               |   1 +
 5 files changed, 317 insertions(+), 262 deletions(-)
 create mode 100644 src/main/drivers/dronecan/dronecan_async.c
 create mode 100644 src/main/drivers/dronecan/dronecan_async.h

diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index f4ab6479a5f..6913a438caf 100755
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -171,6 +171,8 @@ main_sources(COMMON_SRC
     drivers/dronecan/libcanard/canard.h
     drivers/dronecan/libcanard/canard_stm32_driver.h
     drivers/dronecan/dronecan.c
+    drivers/dronecan/dronecan_async.c
+    drivers/dronecan/dronecan_async.h
     drivers/dronecan/dronecan.h
      
     drivers/display.c
diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 77ce978fde1..9fa5a0434c6 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -26,10 +26,11 @@
 #include <string.h>
 #include <inttypes.h>
 #include <dronecan_msgs.h>
+#include "dronecan_async.h"
 
 /* Private variables ---------------------------------------------------------*/
 
-static CanardInstance canard;
+CanardInstance canard; /* non-static: dronecan_async.c needs extern access */
 static uint8_t memory_pool[1024];
 static struct uavcan_protocol_NodeStatus node_status;
 
@@ -41,7 +42,6 @@ PG_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig,
 );
 
 static dronecanState_e dronecanState = STATE_DRONECAN_INIT;
-dronecanAsyncSlot_t dronecanAsyncSlot = { .state = DRONECAN_ASYNC_IDLE };
 
 #ifdef UNIT_TEST
 uint8_t activeNodeCount = 0;
@@ -145,11 +145,7 @@ void dronecanUpdate(timeUs_t currentTimeUs)
         case STATE_DRONECAN_NORMAL:
             processCanardTxQueueSafe();
 
-            // Check for and expire any pending async requests that have timed out.
-            if (dronecanAsyncSlot.state == DRONECAN_ASYNC_PENDING &&
-                millis() - dronecanAsyncSlot.requested_at_ms >= DRONECAN_ASYNC_TIMEOUT_MS) {
-                dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR;
-            }
+            dronecanAsyncCheckTimeout();
 
              for (numMessagesToProcess = canardSTM32GetRxFifoFillLevel(); numMessagesToProcess > 0; numMessagesToProcess--)
              {
@@ -360,260 +356,6 @@ const dronecanNodeInfo_t *dronecanGetNodeByID(uint8_t nodeID) {
     return findNodeByID(nodeID);
 }
 
-
-bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *payload)
-{
-    if (dronecanAsyncSlot.state == DRONECAN_ASYNC_PENDING &&
-        millis() - dronecanAsyncSlot.requested_at_ms < DRONECAN_ASYNC_TIMEOUT_MS) {
-        return false;
-    }
-
-    // PARAM_GETSET_REQUEST is the largest payload; zero-init prevents garbage in UAVCAN reserved bits
-    uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_REQUEST_MAX_SIZE];
-    memset(buffer, 0, sizeof(buffer));
-    uint16_t len = 0;
-    uint64_t signature = 0;
-    const uint8_t *buf_ptr = NULL;
-
-    switch (service_id) {
-        case DRONECAN_SERVICE_GETNODEINFO:
-            signature = UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE;
-            len = 0;
-            break;
-
-        case DRONECAN_SERVICE_PARAM_GETSET: {
-            if (!payload) return false;
-            const dronecanParamRequest_t *req = (const dronecanParamRequest_t *)payload;
-            struct uavcan_protocol_param_GetSetRequest getset;
-            memset(&getset, 0, sizeof(getset));
-            getset.index = req->index;
-            if (req->is_write) {
-                getset.value.union_tag = (enum uavcan_protocol_param_Value_type_t)req->value_type;
-                switch (req->value_type) {
-                    case DRONECAN_PARAM_TYPE_INT:
-                        getset.value.integer_value = req->value_int;
-                        break;
-                    case DRONECAN_PARAM_TYPE_FLOAT:
-                        getset.value.real_value = req->value_float;
-                        break;
-                    case DRONECAN_PARAM_TYPE_BOOL:
-                        getset.value.boolean_value = req->value_bool;
-                        break;
-                    case DRONECAN_PARAM_TYPE_STRING: {
-                        uint8_t slen = req->value_str_len < sizeof(getset.value.string_value.data)
-                                       ? req->value_str_len : sizeof(getset.value.string_value.data);
-                        getset.value.string_value.len = slen;
-                        memcpy(getset.value.string_value.data, req->value_str, slen);
-                        break;
-                    }
-                    default:
-                        getset.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY;
-                        break;
-                }
-            }
-            uint8_t nlen = req->req_name_len < sizeof(getset.name.data)
-                           ? req->req_name_len : sizeof(getset.name.data);
-            getset.name.len = nlen;
-            memcpy(getset.name.data, req->req_name, nlen);
-            len = uavcan_protocol_param_GetSetRequest_encode(&getset, buffer);
-            buf_ptr = buffer;
-            signature = UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE;
-            break;
-        }
-
-        case DRONECAN_SERVICE_EXECUTE_OPCODE: {
-            if (!payload) return false;
-            const uint8_t *opcode = (const uint8_t *)payload;
-            struct uavcan_protocol_param_ExecuteOpcodeRequest req;
-            memset(&req, 0, sizeof(req));
-            req.opcode = *opcode;
-            req.argument = 0;
-            len = uavcan_protocol_param_ExecuteOpcodeRequest_encode(&req, buffer);
-            buf_ptr = buffer;
-            signature = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE;
-            break;
-        }
-
-        case DRONECAN_SERVICE_RESTART_NODE: {
-            struct uavcan_protocol_RestartNodeRequest req;
-            memset(&req, 0, sizeof(req));
-            req.magic_number = UAVCAN_PROTOCOL_RESTARTNODE_REQUEST_MAGIC_NUMBER;
-            len = uavcan_protocol_RestartNodeRequest_encode(&req, buffer);
-            buf_ptr = buffer;
-            signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE;
-            break;
-        }
-
-        default:
-            return false;
-    }
-
-    // buf_ptr remains NULL only for GETNODEINFO (zero-length request); libcanard accepts NULL with len=0
-    int16_t res;
-    ATOMIC_BLOCK(NVIC_PRIO_CAN) {
-        res = canardRequestOrRespond(&canard, node_id, signature, service_id,
-            &dronecanAsyncSlot.transfer_id, CANARD_TRANSFER_PRIORITY_MEDIUM, CanardRequest,
-            buf_ptr, len);
-    }
-
-    if (res < 0) {
-        LOG_WARNING(CAN, "dronecanAsyncRequest: service %u node %u failed: %d", service_id, node_id, res);
-        return false;
-    }
-
-    dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING;
-    dronecanAsyncSlot.seq++;
-    dronecanAsyncSlot.service_id = service_id;
-    dronecanAsyncSlot.node_id = node_id;
-    dronecanAsyncSlot.requested_at_ms = millis();
-    return true;
-}
-
-/*
-    Handle responses for any pending async service request
-    (GETNODEINFO, PARAM_GETSET, EXECUTE_OPCODE, RESTART_NODE).
-    A single handler serialises all on-demand service requests through
-    one shared slot, avoiding the need for per-service response queues.
-*/
-static void handle_AsyncServiceResponse(CanardInstance *ins, CanardRxTransfer *transfer)
-{
-    UNUSED(ins);
-
-    if (dronecanAsyncSlot.state != DRONECAN_ASYNC_PENDING) // timed out or already received
-        return;
-    if (transfer->data_type_id != dronecanAsyncSlot.service_id) // response service_id does not match the pending request
-        return;
-    if (transfer->source_node_id != dronecanAsyncSlot.node_id) // response received for different node_id
-        return;
-    // UAVCAN requires matching transfer_id to guard against stale frames (e.g. after bus-off recovery).
-    // canardRequestOrRespond increments the slot's transfer_id after sending, so the in-flight id is (transfer_id-1) mod 32.
-    if (transfer->transfer_id != ((dronecanAsyncSlot.transfer_id - 1) & 0x1F))
-        return;
-
-    switch (dronecanAsyncSlot.service_id) {
-        case DRONECAN_SERVICE_GETNODEINFO: {
-            struct uavcan_protocol_GetNodeInfoResponse resp;
-            if (uavcan_protocol_GetNodeInfoResponse_decode(transfer, &resp)) {
-                LOG_WARNING(CAN, "GetNodeInfoResponse decode failed");
-                dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR;
-                return;
-            }
-            dronecanGetNodeInfoResult_t *r = &dronecanAsyncSlot.result.node_info;
-            uint8_t len = resp.name.len < (sizeof(r->name) - 1) ? resp.name.len : (sizeof(r->name) - 1);
-            r->name_len = len;
-            memcpy(r->name, resp.name.data, len);
-            r->name[len] = '\0';
-            r->sw_major = resp.software_version.major;
-            r->sw_minor = resp.software_version.minor;
-            r->sw_optional_field_flags = resp.software_version.optional_field_flags;
-            r->sw_vcs_commit = (resp.software_version.optional_field_flags &
-                                UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT)
-                               ? resp.software_version.vcs_commit : 0;
-            r->hw_major = resp.hardware_version.major;
-            r->hw_minor = resp.hardware_version.minor;
-            memcpy(r->hw_unique_id, resp.hardware_version.unique_id, 16);
-            dronecanAsyncSlot.state = DRONECAN_ASYNC_READY;
-            break;
-        }
-
-        case DRONECAN_SERVICE_PARAM_GETSET: {
-            struct uavcan_protocol_param_GetSetResponse resp;
-            if (uavcan_protocol_param_GetSetResponse_decode(transfer, &resp)) {
-                LOG_WARNING(CAN, "ParamGetSetResponse decode failed");
-                dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR;
-                return;
-            }
-            dronecanParamResult_t *r = &dronecanAsyncSlot.result.param;
-            uint8_t name_len = resp.name.len < (sizeof(r->name) - 1) ? resp.name.len : (sizeof(r->name) - 1);
-            r->name_len = name_len;
-            memcpy(r->name, resp.name.data, name_len);
-            r->name[name_len] = '\0';
-            r->type = (uint8_t)resp.value.union_tag;
-            switch (resp.value.union_tag) {
-                case UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE:
-                    r->value_int = resp.value.integer_value;
-                    break;
-                case UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE:
-                    r->value_float = resp.value.real_value;
-                    break;
-                case UAVCAN_PROTOCOL_PARAM_VALUE_BOOLEAN_VALUE:
-                    r->value_bool = resp.value.boolean_value;
-                    break;
-                case UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE: {
-                    uint8_t slen = resp.value.string_value.len < (sizeof(r->value_str) - 1)
-                                   ? resp.value.string_value.len : (sizeof(r->value_str) - 1);
-                    r->value_str_len = slen;
-                    memcpy(r->value_str, resp.value.string_value.data, slen);
-                    r->value_str[slen] = '\0';
-                    break;
-                }
-                default:
-                    r->type = DRONECAN_PARAM_TYPE_EMPTY;
-                    break;
-            }
-            r->min_type = DRONECAN_PARAM_TYPE_EMPTY;
-            r->min_int  = 0;
-            r->min_float = 0.0f;
-            switch (resp.min_value.union_tag) {
-                case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE:
-                    r->min_type = DRONECAN_PARAM_TYPE_INT;
-                    r->min_int  = resp.min_value.integer_value;
-                    break;
-                case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE:
-                    r->min_type  = DRONECAN_PARAM_TYPE_FLOAT;
-                    r->min_float = resp.min_value.real_value;
-                    break;
-                default:
-                    break;
-            }
-            r->max_type  = DRONECAN_PARAM_TYPE_EMPTY;
-            r->max_int   = 0;
-            r->max_float = 0.0f;
-            switch (resp.max_value.union_tag) {
-                case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE:
-                    r->max_type = DRONECAN_PARAM_TYPE_INT;
-                    r->max_int  = resp.max_value.integer_value;
-                    break;
-                case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE:
-                    r->max_type  = DRONECAN_PARAM_TYPE_FLOAT;
-                    r->max_float = resp.max_value.real_value;
-                    break;
-                default:
-                    break;
-            }
-            dronecanAsyncSlot.state = DRONECAN_ASYNC_READY;
-            break;
-        }
-
-        case DRONECAN_SERVICE_EXECUTE_OPCODE: {
-            struct uavcan_protocol_param_ExecuteOpcodeResponse resp;
-            if (uavcan_protocol_param_ExecuteOpcodeResponse_decode(transfer, &resp)) {
-                LOG_WARNING(CAN, "ExecuteOpcodeResponse decode failed");
-                dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR;
-                return;
-            }
-            dronecanAsyncSlot.result.simple.ok = resp.ok;
-            dronecanAsyncSlot.state = DRONECAN_ASYNC_READY;
-            break;
-        }
-
-        case DRONECAN_SERVICE_RESTART_NODE: {
-            struct uavcan_protocol_RestartNodeResponse resp;
-            if (uavcan_protocol_RestartNodeResponse_decode(transfer, &resp)) {
-                LOG_WARNING(CAN, "RestartNodeResponse decode failed");
-                dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR;
-                return;
-            }
-            dronecanAsyncSlot.result.simple.ok = resp.ok;
-            dronecanAsyncSlot.state = DRONECAN_ASYNC_READY;
-            break;
-        }
-
-        default:
-            break;
-    }
-}
-
 // Canard Handlers and Senders
 
 
@@ -921,7 +663,7 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
 	}
 
     if (transfer->transfer_type == CanardTransferTypeResponse) {
-        handle_AsyncServiceResponse(&canard, transfer);
+        dronecanAsyncHandleServiceResponse(&canard, transfer);
     }
 
 	if (transfer->transfer_type == CanardTransferTypeBroadcast) {
diff --git a/src/main/drivers/dronecan/dronecan_async.c b/src/main/drivers/dronecan/dronecan_async.c
new file mode 100644
index 00000000000..a6fd03a8467
--- /dev/null
+++ b/src/main/drivers/dronecan/dronecan_async.c
@@ -0,0 +1,294 @@
+#include "platform.h"
+#if defined(USE_DRONECAN)
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "build/atomic.h"
+
+#include "common/log.h"
+#include "common/time.h"
+#include "common/utils.h"
+
+#include "drivers/time.h"
+#include "drivers/nvic.h"
+
+#include "libcanard/canard.h"
+
+#include <dronecan_msgs.h>
+
+#include "dronecan.h"
+#include "dronecan_async.h"
+
+extern CanardInstance canard; /* the FC's own canard instance, owned by dronecan.c */
+
+dronecanAsyncSlot_t dronecanAsyncSlot = { .state = DRONECAN_ASYNC_IDLE };
+
+/*
+    Send an asynchronous request for data from a dronecan node such as
+    a configuration parameter or the node info
+*/
+bool dronecanAsyncRequest(uint8_t service_id, uint8_t node_id, const void *payload)
+{
+    if (dronecanAsyncSlot.state == DRONECAN_ASYNC_PENDING &&
+        millis() - dronecanAsyncSlot.requested_at_ms < DRONECAN_ASYNC_TIMEOUT_MS) {
+        return false;
+    }
+
+    // PARAM_GETSET_REQUEST is the largest payload; zero-init prevents garbage in UAVCAN reserved bits
+    uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_REQUEST_MAX_SIZE];
+    memset(buffer, 0, sizeof(buffer));
+    uint16_t len = 0;
+    uint64_t signature = 0;
+    const uint8_t *buf_ptr = NULL;
+
+    switch (service_id) {
+        case DRONECAN_SERVICE_GETNODEINFO:
+            signature = UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE;
+            len = 0;
+            break;
+
+        case DRONECAN_SERVICE_PARAM_GETSET: {
+            if (!payload) return false;
+            const dronecanParamRequest_t *req = (const dronecanParamRequest_t *)payload;
+            struct uavcan_protocol_param_GetSetRequest getset;
+            memset(&getset, 0, sizeof(getset));
+            getset.index = req->index;
+            if (req->is_write) {
+                getset.value.union_tag = (enum uavcan_protocol_param_Value_type_t)req->value_type;
+                switch (req->value_type) {
+                    case DRONECAN_PARAM_TYPE_INT:
+                        getset.value.integer_value = req->value_int;
+                        break;
+                    case DRONECAN_PARAM_TYPE_FLOAT:
+                        getset.value.real_value = req->value_float;
+                        break;
+                    case DRONECAN_PARAM_TYPE_BOOL:
+                        getset.value.boolean_value = req->value_bool;
+                        break;
+                    case DRONECAN_PARAM_TYPE_STRING: {
+                        uint8_t slen = req->value_str_len < sizeof(getset.value.string_value.data)
+                                       ? req->value_str_len : sizeof(getset.value.string_value.data);
+                        getset.value.string_value.len = slen;
+                        memcpy(getset.value.string_value.data, req->value_str, slen);
+                        break;
+                    }
+                    default:
+                        getset.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY;
+                        break;
+                }
+            }
+            uint8_t nlen = req->req_name_len < sizeof(getset.name.data)
+                           ? req->req_name_len : sizeof(getset.name.data);
+            getset.name.len = nlen;
+            memcpy(getset.name.data, req->req_name, nlen);
+            len = uavcan_protocol_param_GetSetRequest_encode(&getset, buffer);
+            buf_ptr = buffer;
+            signature = UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE;
+            break;
+        }
+
+        case DRONECAN_SERVICE_EXECUTE_OPCODE: {
+            if (!payload) return false;
+            const uint8_t *opcode = (const uint8_t *)payload;
+            struct uavcan_protocol_param_ExecuteOpcodeRequest req;
+            memset(&req, 0, sizeof(req));
+            req.opcode = *opcode;
+            req.argument = 0;
+            len = uavcan_protocol_param_ExecuteOpcodeRequest_encode(&req, buffer);
+            buf_ptr = buffer;
+            signature = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE;
+            break;
+        }
+
+        case DRONECAN_SERVICE_RESTART_NODE: {
+            struct uavcan_protocol_RestartNodeRequest req;
+            memset(&req, 0, sizeof(req));
+            req.magic_number = UAVCAN_PROTOCOL_RESTARTNODE_REQUEST_MAGIC_NUMBER;
+            len = uavcan_protocol_RestartNodeRequest_encode(&req, buffer);
+            buf_ptr = buffer;
+            signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE;
+            break;
+        }
+
+        default:
+            return false;
+    }
+
+    // buf_ptr remains NULL only for GETNODEINFO (zero-length request); libcanard accepts NULL with len=0
+    int16_t res;
+    ATOMIC_BLOCK(NVIC_PRIO_CAN) {
+        res = canardRequestOrRespond(&canard, node_id, signature, service_id,
+            &dronecanAsyncSlot.transfer_id, CANARD_TRANSFER_PRIORITY_MEDIUM, CanardRequest,
+            buf_ptr, len);
+    }
+
+    if (res < 0) {
+        LOG_WARNING(CAN, "dronecanAsyncRequest: service %u node %u failed: %d", service_id, node_id, res);
+        return false;
+    }
+
+    dronecanAsyncSlot.state = DRONECAN_ASYNC_PENDING;
+    dronecanAsyncSlot.seq++;
+    dronecanAsyncSlot.service_id = service_id;
+    dronecanAsyncSlot.node_id = node_id;
+    dronecanAsyncSlot.requested_at_ms = millis();
+    return true;
+}
+
+void dronecanAsyncCheckTimeout(void)
+{
+    // Check for and expire any pending async requests that have timed out.
+    if (dronecanAsyncSlot.state == DRONECAN_ASYNC_PENDING &&
+        millis() - dronecanAsyncSlot.requested_at_ms >= DRONECAN_ASYNC_TIMEOUT_MS) {
+        dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR;
+    }
+}
+
+/*
+    Handle responses for any pending async service request
+    (GETNODEINFO, PARAM_GETSET, EXECUTE_OPCODE, RESTART_NODE).
+    A single handler serialises all on-demand service requests through
+    one shared slot, avoiding the need for per-service response queues.
+*/
+void dronecanAsyncHandleServiceResponse(CanardInstance *ins, CanardRxTransfer *transfer)
+{
+    UNUSED(ins);
+
+    if (dronecanAsyncSlot.state != DRONECAN_ASYNC_PENDING) // timed out or already received
+        return;
+    if (transfer->data_type_id != dronecanAsyncSlot.service_id) // response service_id does not match the pending request
+        return;
+    if (transfer->source_node_id != dronecanAsyncSlot.node_id) // response received for different node_id
+        return;
+    // UAVCAN requires matching transfer_id to guard against stale frames (e.g. after bus-off recovery).
+    // canardRequestOrRespond increments the slot's transfer_id after sending, so the in-flight id is (transfer_id-1) mod 32.
+    if (transfer->transfer_id != ((dronecanAsyncSlot.transfer_id - 1) & 0x1F))
+        return;
+
+    switch (dronecanAsyncSlot.service_id) {
+        case DRONECAN_SERVICE_GETNODEINFO: {
+            struct uavcan_protocol_GetNodeInfoResponse resp;
+            if (uavcan_protocol_GetNodeInfoResponse_decode(transfer, &resp)) {
+                LOG_WARNING(CAN, "GetNodeInfoResponse decode failed");
+                dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR;
+                return;
+            }
+            dronecanGetNodeInfoResult_t *r = &dronecanAsyncSlot.result.node_info;
+            uint8_t len = resp.name.len < (sizeof(r->name) - 1) ? resp.name.len : (sizeof(r->name) - 1);
+            r->name_len = len;
+            memcpy(r->name, resp.name.data, len);
+            r->name[len] = '\0';
+            r->sw_major = resp.software_version.major;
+            r->sw_minor = resp.software_version.minor;
+            r->sw_optional_field_flags = resp.software_version.optional_field_flags;
+            r->sw_vcs_commit = (resp.software_version.optional_field_flags &
+                                UAVCAN_PROTOCOL_SOFTWAREVERSION_OPTIONAL_FIELD_FLAG_VCS_COMMIT)
+                               ? resp.software_version.vcs_commit : 0;
+            r->hw_major = resp.hardware_version.major;
+            r->hw_minor = resp.hardware_version.minor;
+            memcpy(r->hw_unique_id, resp.hardware_version.unique_id, 16);
+            dronecanAsyncSlot.state = DRONECAN_ASYNC_READY;
+            break;
+        }
+
+        case DRONECAN_SERVICE_PARAM_GETSET: {
+            struct uavcan_protocol_param_GetSetResponse resp;
+            if (uavcan_protocol_param_GetSetResponse_decode(transfer, &resp)) {
+                LOG_WARNING(CAN, "ParamGetSetResponse decode failed");
+                dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR;
+                return;
+            }
+            dronecanParamResult_t *r = &dronecanAsyncSlot.result.param;
+            uint8_t name_len = resp.name.len < (sizeof(r->name) - 1) ? resp.name.len : (sizeof(r->name) - 1);
+            r->name_len = name_len;
+            memcpy(r->name, resp.name.data, name_len);
+            r->name[name_len] = '\0';
+            r->type = (uint8_t)resp.value.union_tag;
+            switch (resp.value.union_tag) {
+                case UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE:
+                    r->value_int = resp.value.integer_value;
+                    break;
+                case UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE:
+                    r->value_float = resp.value.real_value;
+                    break;
+                case UAVCAN_PROTOCOL_PARAM_VALUE_BOOLEAN_VALUE:
+                    r->value_bool = resp.value.boolean_value;
+                    break;
+                case UAVCAN_PROTOCOL_PARAM_VALUE_STRING_VALUE: {
+                    uint8_t slen = resp.value.string_value.len < (sizeof(r->value_str) - 1)
+                                   ? resp.value.string_value.len : (sizeof(r->value_str) - 1);
+                    r->value_str_len = slen;
+                    memcpy(r->value_str, resp.value.string_value.data, slen);
+                    r->value_str[slen] = '\0';
+                    break;
+                }
+                default:
+                    r->type = DRONECAN_PARAM_TYPE_EMPTY;
+                    break;
+            }
+            r->min_type = DRONECAN_PARAM_TYPE_EMPTY;
+            r->min_int  = 0;
+            r->min_float = 0.0f;
+            switch (resp.min_value.union_tag) {
+                case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE:
+                    r->min_type = DRONECAN_PARAM_TYPE_INT;
+                    r->min_int  = resp.min_value.integer_value;
+                    break;
+                case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE:
+                    r->min_type  = DRONECAN_PARAM_TYPE_FLOAT;
+                    r->min_float = resp.min_value.real_value;
+                    break;
+                default:
+                    break;
+            }
+            r->max_type  = DRONECAN_PARAM_TYPE_EMPTY;
+            r->max_int   = 0;
+            r->max_float = 0.0f;
+            switch (resp.max_value.union_tag) {
+                case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_INTEGER_VALUE:
+                    r->max_type = DRONECAN_PARAM_TYPE_INT;
+                    r->max_int  = resp.max_value.integer_value;
+                    break;
+                case UAVCAN_PROTOCOL_PARAM_NUMERICVALUE_REAL_VALUE:
+                    r->max_type  = DRONECAN_PARAM_TYPE_FLOAT;
+                    r->max_float = resp.max_value.real_value;
+                    break;
+                default:
+                    break;
+            }
+            dronecanAsyncSlot.state = DRONECAN_ASYNC_READY;
+            break;
+        }
+
+        case DRONECAN_SERVICE_EXECUTE_OPCODE: {
+            struct uavcan_protocol_param_ExecuteOpcodeResponse resp;
+            if (uavcan_protocol_param_ExecuteOpcodeResponse_decode(transfer, &resp)) {
+                LOG_WARNING(CAN, "ExecuteOpcodeResponse decode failed");
+                dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR;
+                return;
+            }
+            dronecanAsyncSlot.result.simple.ok = resp.ok;
+            dronecanAsyncSlot.state = DRONECAN_ASYNC_READY;
+            break;
+        }
+
+        case DRONECAN_SERVICE_RESTART_NODE: {
+            struct uavcan_protocol_RestartNodeResponse resp;
+            if (uavcan_protocol_RestartNodeResponse_decode(transfer, &resp)) {
+                LOG_WARNING(CAN, "RestartNodeResponse decode failed");
+                dronecanAsyncSlot.state = DRONECAN_ASYNC_ERROR;
+                return;
+            }
+            dronecanAsyncSlot.result.simple.ok = resp.ok;
+            dronecanAsyncSlot.state = DRONECAN_ASYNC_READY;
+            break;
+        }
+
+        default:
+            break;
+    }
+}
+
+#endif // USE_DRONECAN
diff --git a/src/main/drivers/dronecan/dronecan_async.h b/src/main/drivers/dronecan/dronecan_async.h
new file mode 100644
index 00000000000..39631e496b9
--- /dev/null
+++ b/src/main/drivers/dronecan/dronecan_async.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include "libcanard/canard.h"
+
+#ifdef USE_DRONECAN
+
+/* Called from onTransferReceived() for every CanardTransferTypeResponse
+   frame - matches it against the single in-flight async request slot
+   (dronecanAsyncSlot, declared in dronecan.h) and decodes the response. */
+void dronecanAsyncHandleServiceResponse(CanardInstance *ins, CanardRxTransfer *transfer);
+
+/* Called once per dronecanUpdate() tick while in STATE_DRONECAN_NORMAL -
+   expires a pending request that never got a response. */
+void dronecanAsyncCheckTimeout(void);
+
+#endif // USE_DRONECAN
diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt
index 3a4f3798d9e..80bd535875c 100644
--- a/src/test/unit/CMakeLists.txt
+++ b/src/test/unit/CMakeLists.txt
@@ -71,6 +71,7 @@ set_property(SOURCE dronecan_getnodeinfo_unittest.cc PROPERTY definitions USE_DR
 # UNIT_TEST exposes activeNodeCount and nodeTable as non-static for SetUp reset.
 set_property(SOURCE dronecan_application_unittest.cc PROPERTY depends
     "drivers/dronecan/dronecan.c"
+    "drivers/dronecan/dronecan_async.c"
     "drivers/dronecan/libcanard/canard.c")
 set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_sources
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.NodeStatus.c"

From 6b6a5c9447544537953878e67c1bb5ff737789cf Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Sun, 16 Aug 2026 19:12:27 -0700
Subject: [PATCH 60/67] feat(dronecan): add Dynamic Node Allocation (DNA)
 server

Add a DNA (Dynamic Node Allocation) server so peripheral nodes with no
configured node ID (anonymous, node ID 0) can request one dynamically
over DroneCAN, instead of every device on the bus needing a fixed ID
set by hand. Implements the standard three-stage UID handshake
(uavcan.protocol.dynamic_node_id.Allocation broadcasts assembling a
peripheral's 16-byte unique ID across up to three frames), matching
already-assigned peripherals back to their stored allocation so a
peripheral doesn't get handed a different ID every boot, and honouring
a peripheral's preferred node ID when it's free. Allocation starts from
node ID 125 downward (126/127 reserved for network maintenance nodes,
per spec) and re-checks the live node table before handing out an ID so
it won't collide with a node already active on the bus.

Node allocations persist to EEPROM (via Parameter Groups) so a
peripheral keeps the same node ID across FC power cycles, not just
within a session; persistence writes happen on disarm rather than on
every allocation. Gated behind a new dronecan_use_dna_server CLI
setting so it's opt-in.

Extracted into its own dronecan_dna_server.c/.h module (mirroring the
file-per-concern pattern already used elsewhere in the driver) rather
than living inline in dronecan.c.

Full unit test suite passes: DNA-1 through DNA-9 covering the stage
handshake, timeout/reset behavior, preferred-ID honouring, existing-
peripheral re-matching, allocation table exhaustion, and the
FC's-own-node-ID skip guard.

Squashed from the original feature/dronecan-dna-server commit sequence
(30 commits - the initial multi-pass implementation, spec-correctness
fixes, EEPROM persistence and settings-gate additions, several rounds
of code-review fixups, and a cluster of commits restoring code
(NVIC masking, txErrCount/bc_res, vendor_specific_status_code masking,
memory_pool's static qualifier, busOffCount and its accessors) that had
been silently dropped by an earlier rebase-conflict resolution against
param-getset - into this single commit for a clean PR diff. No
functional changes from the squash itself; the single-frame full-UID
stage-1 delivery fix (a distinct, later-discovered regression with its
own dedicated differential test) is kept as a separate commit rather
than folded in here.
---
 docs/DroneCAN-Driver.md                       |  77 ++-
 docs/DroneCAN.md                              |  63 +-
 docs/Settings.md                              |  10 +
 src/main/CMakeLists.txt                       |   2 +
 src/main/config/parameter_group_ids.h         |   3 +-
 src/main/drivers/dronecan/dronecan.c          |  50 +-
 src/main/drivers/dronecan/dronecan.h          |   1 +
 .../drivers/dronecan/dronecan_dna_server.c    | 287 ++++++++++
 .../drivers/dronecan/dronecan_dna_server.h    |  27 +
 src/main/fc/fc_msp.c                          |  62 +-
 src/main/fc/settings.yaml                     |   5 +
 src/test/unit/CMakeLists.txt                  |  15 +-
 .../unit/dronecan_application_unittest.cc     |   2 +
 src/test/unit/dronecan_dna_server_unittest.cc | 539 ++++++++++++++++++
 14 files changed, 1101 insertions(+), 42 deletions(-)
 create mode 100644 src/main/drivers/dronecan/dronecan_dna_server.c
 create mode 100644 src/main/drivers/dronecan/dronecan_dna_server.h
 create mode 100644 src/test/unit/dronecan_dna_server_unittest.cc

diff --git a/docs/DroneCAN-Driver.md b/docs/DroneCAN-Driver.md
index d4f2a5e9945..b1fb9317aa6 100644
--- a/docs/DroneCAN-Driver.md
+++ b/docs/DroneCAN-Driver.md
@@ -1,8 +1,8 @@
 # DroneCAN Driver Documentation
 
-**Last Updated:** 2026-02-16
+**Last Updated:** 2026-06-06
 **Status:** Complete
-**Branch:** feature-dronecan-sitl
+**Branch:** feature/dronecan-dna-server
 
 ---
 
@@ -115,6 +115,7 @@ CAN Bus ──[CAN interrupt]──> canardSTM32Receive() ──> shouldAcceptTr
 | `UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID` | `handle_GNSSRCTMStream()` | RTCM Stream | Broadcast | Variable | RTK correction data |
 | `UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID` | `handle_BatteryInfo()` | Battery Info | Broadcast | 1-10 Hz | Integrated with battery system |
 | `UAVCAN_PROTOCOL_GETNODEINFO_ID` | `handle_GetNodeInfo()` | GetNodeInfo | Request/Response | On demand | Responds with FC firmware version |
+| `UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID` | `dronecanDnaHandleAllocation()` | Dynamic Node Allocation | Broadcast | On connect | Assigns node IDs to anonymous peripherals via 3-stage UID handshake |
 
 ### Handler-Based Architecture
 
@@ -282,15 +283,9 @@ The driver uses the following settings from `settings.yaml`:
 
 | Setting | Description | Type | Valid Range | Default |
 |---|---|---|---|---|
-| `dronecan_mode` | Enable/disable DroneCAN | bool | 0/1 | 0 |
-| `dronecan_node_id` | This FC's CAN node ID | uint8 | 1-125 | 0 |
-| `dronecan_baudrate` | CAN bus bitrate | enum | 0-3 | 2 (500kbps) |
-
-**Bitrate Mapping:**
-- 0 = 125 kbps
-- 1 = 250 kbps
-- 2 = 500 kbps (default)
-- 3 = 1000 kbps
+| `dronecan_node_id` | This FC's CAN node ID | uint8 | 1–127 | 1 |
+| `dronecan_bitrate_kbps` | CAN bus bitrate in kbps | enum | 125, 250, 500, 1000 | 1000 |
+| `dronecan_use_dna_server` | Enable automatic node ID assignment | bool | ON/OFF | ON |
 
 #### Accessing Configuration
 
@@ -549,6 +544,65 @@ Total: 1 + nodeCount × 30 bytes.
 
 ---
 
+## DNA Server
+
+The DNA server (`dronecan_dna_server.c`) implements a non-redundant (single-master) Dynamic Node Allocation allocator per UAVCAN Specification §6.4.9. It is guarded by `dronecanConfig()->dronecanUseDNAServer` and can be disabled via `set dronecan_use_dna_server = OFF`.
+
+### Three-stage handshake
+
+A peripheral that has no node ID broadcasts anonymous Allocation messages carrying up to 6 bytes of its 16-byte hardware unique ID per stage:
+
+| Stage | `first_part_of_unique_id` | Bytes sent |
+|-------|--------------------------|------------|
+| 1 | `true` | bytes 0–5 (+ optional preferred node ID) |
+| 2 | `false` | bytes 6–11 |
+| 3 | `false` | bytes 12–15 |
+
+Each stage must arrive within 500 ms (`FOLLOWUP_TIMEOUT_MS`) of the previous one, or the accumulator resets and the peripheral must start over from Stage 1.
+
+After Stage 3 the server has the full 16-byte UID and calls `dnaLookupOrAssignNode()`.
+
+### Node ID assignment
+
+`dnaLookupOrAssignNode()` applies the following priority order:
+
+1. **Existing entry** — if the UID is already in the allocation table and the stored node ID isn't currently claimed by another live node, return the previously assigned node ID. If it *is* claimed by someone else, fall through to reassignment below.
+2. **Preferred ID** — if the peripheral included a preferred node ID in Stage 1 (range 1–125; 126–127 are reserved for network maintenance tools), search upward from that ID first, then downward from `preferred - 1`, assigning the first available ID found either way.
+3. **Top-down fallback** — if there's no preference, or the preferred-ID search found nothing free, scan from node ID 125 downward and assign the first available ID.
+
+The FC's own node ID is never assigned to a peripheral. If all 32 table slots are full and no existing entry matches, the allocation fails silently.
+
+### Allocation table persistence
+
+The allocation table (`dnaServerData_t`) is stored in a Parameter Group (`PG_DRONECAN_DNA_SERVER`) backed by flash. A new entry calls `saveConfig()` (deferred, silent, only while disarmed) so the same peripheral receives the same node ID on every boot without re-negotiating.
+
+The table holds up to `DRONECAN_MAX_NODES` (32) entries, each storing:
+
+```c
+typedef struct {
+    uint8_t uniqueId[16];   // Full 16-byte hardware UID
+    uint8_t nodeId;         // Assigned CAN node ID (1-127)
+} dnaAllocationEntry_t;
+```
+
+### Entry point
+
+`dronecanDnaHandleAllocation()` is called from `onTransferReceived()` when a `UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID` broadcast arrives, but only while `dronecanUseDNAServer` is true:
+
+```c
+case UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID:
+    if (dronecanConfig()->dronecanUseDNAServer) {
+        dronecanDnaHandleAllocation(ins, transfer);
+    }
+    break;
+```
+
+### Unit tests
+
+Thirteen tests in `src/test/unit/dronecan_dna_server_unittest.cc` cover the full handshake, UID re-use, table-full rejection, non-broadcast source rejection, stage ordering, timeout reset, FC node ID exclusion, preferred node ID honouring, top-down sequential assignment, reserved-range and taken-preferred-ID fallback, and live-network reassignment of a stored ID (DNA-1 through DNA-13).
+
+---
+
 ## Adding New Message Types
 
 To add support for a new DroneCAN message type:
@@ -909,6 +963,7 @@ void broadcastNodeStatus(void) {
 
 | Date | Version | Changes |
 |---|---|---|
+| 2026-06-06 | 1.3 | Added DNA server section; corrected settings table (`dronecan_bitrate_kbps`, `dronecan_use_dna_server`); added DNA allocation to message type table |
 | 2026-04-30 | 1.2 | Added node table (`dronecanNodeInfo_t`), accessor functions, CLI status output, and MSP commands (0x2042/0x2043) |
 | 2026-02-18 | 1.1 | Added error recovery, graceful disable behavior, and safe initialization documentation |
 | 2026-02-16 | 1.0 | Initial version - handler-based architecture documentation |
diff --git a/docs/DroneCAN.md b/docs/DroneCAN.md
index 3a0312d2d56..6e8d4761be5 100644
--- a/docs/DroneCAN.md
+++ b/docs/DroneCAN.md
@@ -11,7 +11,7 @@ DroneCAN (formerly UAVCAN v0) is a lightweight protocol designed for reliable co
 | Battery Current | Supported | Current sensing from DroneCAN battery monitors |
 | Parameter Get/Set | Planned | Remote parameter configuration |
 | ESC Control | Planned | Motor control via DroneCAN ESCs |
-| Dynamic Node Assignment | Planned | Manage node IDs dynamically to minimize first time configuration |
+| Dynamic Node Assignment | Supported | Automatically assign CAN node IDs to plug-and-play DroneCAN peripherals |
 
 ## Supported Hardware
 
@@ -38,10 +38,11 @@ save
 
 | Setting | Values | Default | Description |
 |---------|--------|---------|-------------|
-| `dronecan_node_id` | 1-127 | 10 | CAN node ID for the flight controller |
-| `dronecan_bitrate` | 125KBPS, 250KBPS, 500KBPS, 1000KBPS | 1000KBPS | CAN bus bitrate |
+| `dronecan_node_id` | 1-127 | 1 | CAN node ID for the flight controller |
+| `dronecan_bitrate_kbps` | 125, 250, 500, 1000 | 1000 | CAN bus bitrate in kbps |
+| `dronecan_use_dna_server` | ON, OFF | ON | Enable automatic node ID assignment for plug-and-play peripherals |
 
-All peripherals need to have the node ID and the bitrate set manually through the dronecan_gui for now.  You can use your flight controller as a CAN interface by loading an Ardupilot image on it.  Once the set up is complete, you can reflash it to Inav.
+With `dronecan_use_dna_server = ON` (the default), peripherals that support Dynamic Node Allocation and have their node ID set to 0 (anonymous/unset) negotiate their node IDs automatically at power-up. Only the CAN bitrate needs to match across the bus. Peripherals that do not support DNA, or that already have a static node ID configured, will not send Allocation requests and must have their node ID set manually via a tool such as dronecan_gui. You can use your flight controller as a CAN interface by loading an ArduPilot image on it; once configuration is complete, reflash to INAV.
 
 ### GPS via DroneCAN
 
@@ -154,13 +155,14 @@ Setting up multiple DroneCAN peripherals on a single CAN bus:
 
 ```
 # Flight Controller Configuration
-set dronecan_node_id = 10           # Flight controller = node 10
-set dronecan_bitrate = 1000KBPS
+set dronecan_node_id = 1            # Flight controller = node 1
+set dronecan_bitrate_kbps = 1000
+set dronecan_use_dna_server = ON    # Enable plug-and-play node assignment (default)
 
-# Configure GPS (from node 1)
+# Configure GPS
 set gps_provider = DRONECAN
 
-# Configure battery monitor (from node 2)
+# Configure battery monitor
 set bat_voltage_src = CAN
 feature CURRENT_METER
 set current_meter_type = CAN
@@ -168,10 +170,13 @@ set current_meter_type = CAN
 save
 ```
 
-**Peripheral Configuration (using dronecan_gui or similar tool):**
-- **GPS Receiver:** Node ID = 1, Bitrate = 1000 KBPS
-- **Battery Monitor:** Node ID = 2, Bitrate = 1000 KBPS
-- **Potential Future Peripheral:** Node ID = 3, etc.
+**Peripheral Configuration:**
+
+With `dronecan_use_dna_server = ON`, peripherals that support Dynamic Node Allocation negotiate their node IDs automatically — no manual per-device configuration needed. Just set the CAN bitrate to match on every device.
+
+For peripherals that do not support DNA, use dronecan_gui to assign a static node ID:
+- **GPS Receiver:** Node ID = 2, Bitrate = 1000 KBPS
+- **Battery Monitor:** Node ID = 3, Bitrate = 1000 KBPS
 
 **CAN Bus Layout:**
 ```
@@ -295,6 +300,40 @@ bat_voltage_src = CAN
 current_meter_type = CAN
 ```
 
+## Dynamic Node Allocation (Plug and Play)
+
+DroneCAN supports a three-stage handshake protocol (defined in UAVCAN Specification §6.4.9) that lets peripherals negotiate a unique node ID at power-up without any manual configuration. INAV implements a non-redundant (single-master) DNA server.
+
+### How it works
+
+1. The peripheral powers on without a node ID (anonymous mode).
+2. It broadcasts an Allocation request containing the first 6 bytes of its 16-byte hardware unique ID, optionally including a preferred node ID.
+3. INAV responds with an echo of the bytes received so far, prompting the peripheral to send the next 6 bytes.
+4. After three stages (6 + 6 + 4 bytes), INAV has the full UID and assigns a node ID.
+5. INAV stores the UID→node ID mapping in flash so the same peripheral receives the same ID on every power cycle.
+
+All three stages must arrive within 500 ms of each other, or the handshake resets.
+
+### Preferred node IDs
+
+A peripheral may include a preferred node ID in its Stage 1 request. INAV honours the request if the ID is in the valid range (1–127) and not already assigned. If the requested ID is unavailable, INAV falls back to the next free ID in sequence.
+
+### Allocation table
+
+INAV stores up to 32 UID→node ID mappings in persistent flash storage. The table survives power cycles, so peripherals receive consistent node IDs across reboots. The table is managed automatically and is not directly user-configurable.
+
+The flight controller's own node ID is never assigned to a peripheral.
+
+### Enabling / disabling
+
+```
+set dronecan_use_dna_server = ON    # default — plug-and-play enabled
+set dronecan_use_dna_server = OFF   # static node IDs only
+save
+```
+
+---
+
 ## Hardware Setup
 
 ### Wiring
diff --git a/docs/Settings.md b/docs/Settings.md
index c8eb8bb0716..970636e5673 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -905,6 +905,16 @@ Unique identifier for this device. Valid values are 1 to 127. 126 and 127 are re
 
 ---
 
+### dronecan_use_dna_server
+
+Enable the DNA server to manage plug and play dronecan devices
+
+| Default | Min | Max |
+| --- | --- | --- |
+| ON | OFF | ON |
+
+---
+
 ### dshot_beeper_enabled
 
 Whether using DShot motors as beepers is enabled
diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 6913a438caf..7c91ad1f12f 100755
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -173,6 +173,8 @@ main_sources(COMMON_SRC
     drivers/dronecan/dronecan.c
     drivers/dronecan/dronecan_async.c
     drivers/dronecan/dronecan_async.h
+    drivers/dronecan/dronecan_dna_server.c
+    drivers/dronecan/dronecan_dna_server.h
     drivers/dronecan/dronecan.h
      
     drivers/display.c
diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
index da1be28507d..362280f30ca 100644
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@@ -133,7 +133,8 @@
 #define PG_GEOZONES 1043
 #define PG_GEOZONE_VERTICES 1044
 #define PG_DRONECAN_CONFIG 1045
-#define PG_INAV_END PG_DRONECAN_CONFIG
+#define PG_DRONECAN_DNA_SERVER 1046     // Separate PG so we don't wipe user settings if the allocation table changes
+#define PG_INAV_END PG_DRONECAN_DNA_SERVER
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047
diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 9fa5a0434c6..425e015b6da 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -27,6 +27,7 @@
 #include <inttypes.h>
 #include <dronecan_msgs.h>
 #include "dronecan_async.h"
+#include "dronecan_dna_server.h"
 
 /* Private variables ---------------------------------------------------------*/
 
@@ -38,7 +39,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig, PG_DRONECAN_CO
 
 PG_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig,
     .nodeID = SETTING_DRONECAN_NODE_ID_DEFAULT,
-    .bitRateKbps = SETTING_DRONECAN_BITRATE_KBPS_DEFAULT
+    .bitRateKbps = SETTING_DRONECAN_BITRATE_KBPS_DEFAULT,
+    .dronecanUseDNAServer = SETTING_DRONECAN_USE_DNA_SERVER_DEFAULT
 );
 
 static dronecanState_e dronecanState = STATE_DRONECAN_INIT;
@@ -122,7 +124,7 @@ void dronecanInit(void)
     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");
+	      LOG_WARNING(CAN, "Node ID is 0, this node is anonymous and can't transmit most messages. Please update this in config");
     }
 }
 
@@ -153,7 +155,7 @@ void dronecanUpdate(timeUs_t currentTimeUs)
 	            rx_res = canardSTM32Receive(&rx_frame);
 
 	             if (rx_res < 0) {
-		             LOG_DEBUG(CAN, "Receive error %d", rx_res);
+		             LOG_WARNING(CAN, "Receive error %d", rx_res);
 	             }
 	             else if (rx_res > 0)        // Success - process the frame
 	             {
@@ -211,7 +213,7 @@ void dronecanUpdate(timeUs_t currentTimeUs)
                     // ~1 second of 20ms recovery attempts with no success — permanent fault
                     busoff_retries = 0;
                     dronecanState = STATE_DRONECAN_FAILED;
-                    LOG_DEBUG(CAN, "DroneCAN: bus-off recovery failed after 50 attempts, entering FAILED state");
+                    LOG_ERROR(CAN, "DroneCAN: bus-off recovery failed after 50 attempts, entering FAILED state");
                 }
             }
             break;
@@ -325,7 +327,7 @@ static void processCanardTxQueueSafe(void) {
                 const int16_t tx_res = canardSTM32Transmit(tx_frame);  // HAL register write, ~1µs
                 if (tx_res != 0) {
                     if (tx_res < 0) {
-                        LOG_DEBUG(CAN, "Transmit error %d", tx_res);
+                        LOG_WARNING(CAN, "Transmit error %d", tx_res);
                     }
                     canardPopTxQueue(&canard);
                 } else {
@@ -480,6 +482,9 @@ static bool shouldAcceptTransfer(const CanardInstance *ins,
         case UAVCAN_PROTOCOL_NODESTATUS_ID:
             *out_data_type_signature = UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE;
             return true;
+        case UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID:
+            *out_data_type_signature = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE;
+            return true;
         case UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID:
             *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE;
             return true;
@@ -508,15 +513,16 @@ void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) {
 static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) {
 #endif
     UNUSED(ins);
+
     struct uavcan_protocol_NodeStatus nodeStatus;
 
 	if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) {
-		LOG_DEBUG(CAN, "NodeStatus decode failed");
+		LOG_WARNING(CAN, "NodeStatus decode failed");
 		return;
 	}
 
-	uint8_t nodeId = transfer->source_node_id;
-    dronecanNodeInfo_t *node = findNodeByID(nodeId);
+	uint8_t nodeID = transfer->source_node_id;
+    dronecanNodeInfo_t *node = findNodeByID(nodeID);
     if (node) {
         node->health = nodeStatus.health;
         node->mode = nodeStatus.mode;
@@ -528,7 +534,7 @@ static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) {
     // new node
     if (activeNodeCount < DRONECAN_MAX_NODES) {
         memset(&nodeTable[activeNodeCount], 0, sizeof(dronecanNodeInfo_t));
-        nodeTable[activeNodeCount].nodeID = nodeId;
+        nodeTable[activeNodeCount].nodeID = nodeID;
         nodeTable[activeNodeCount].health = nodeStatus.health;
         nodeTable[activeNodeCount].mode = nodeStatus.mode;
         nodeTable[activeNodeCount].uptime_sec = nodeStatus.uptime_sec;
@@ -537,7 +543,7 @@ static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) {
         activeNodeCount++;
 
     } else {
-        LOG_DEBUG(CAN, "DroneCAN: node table full (%u nodes), ignoring node %u", DRONECAN_MAX_NODES, nodeId);
+        LOG_WARNING(CAN, "DroneCAN: node table full (%u nodes), ignoring node %u", DRONECAN_MAX_NODES, nodeID);
     }
 }
 
@@ -547,7 +553,7 @@ static void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer
     struct uavcan_equipment_gnss_Auxiliary gnssAuxiliary;
 
 	if (uavcan_equipment_gnss_Auxiliary_decode(transfer, &gnssAuxiliary)) {
-		LOG_DEBUG(CAN, "GNSSAuxiliary decode failed");
+		LOG_WARNING(CAN, "GNSSAuxiliary decode failed");
 		return;
 	}
     dronecanGPSReceiveGNSSAuxiliary(&gnssAuxiliary);
@@ -559,7 +565,7 @@ static void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) {
     struct uavcan_equipment_gnss_Fix gnssFix;
 
 	if (uavcan_equipment_gnss_Fix_decode(transfer, &gnssFix)) {
-		LOG_DEBUG(CAN, "GNSSFix decode failed");
+		LOG_WARNING(CAN, "GNSSFix decode failed");
 		return;
 	}
     dronecanGPSReceiveGNSSFix(&gnssFix);
@@ -571,7 +577,7 @@ static void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) {
     struct uavcan_equipment_gnss_Fix2 gnssFix2;
 
 	if (uavcan_equipment_gnss_Fix2_decode(transfer, &gnssFix2)) {
-		LOG_DEBUG(CAN, "GNSSFix2 decode failed");
+		LOG_WARNING(CAN, "GNSSFix2 decode failed");
 		return;
 	}
     dronecanGPSReceiveGNSSFix2(&gnssFix2);
@@ -579,8 +585,13 @@ static void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) {
 
 static void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) {
 	UNUSED(ins);
-	UNUSED(transfer);
-    /* RTCM forwarding not yet implemented. Accepted in shouldAcceptTransfer for future use. */
+    if (gpsConfig()->provider != GPS_DRONECAN) return;
+    struct uavcan_equipment_gnss_RTCMStream gnssRTCMStream;
+
+	if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &gnssRTCMStream)) {
+		LOG_WARNING(CAN, "RTCMStream decode failed");
+		return;
+	}
 }
 
 static void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
@@ -588,7 +599,7 @@ static void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer)
     struct uavcan_equipment_power_BatteryInfo batteryInfo;
 
 	if (uavcan_equipment_power_BatteryInfo_decode(transfer, &batteryInfo)) {
-		LOG_DEBUG(CAN, "BatteryInfo decode failed");
+		LOG_WARNING(CAN, "BatteryInfo decode failed");
 		return;
 	}
     dronecanBatterySensorReceiveInfo(&batteryInfo);
@@ -674,6 +685,13 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
                 handle_NodeStatus(ins, transfer);
                 break;
 
+            case UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID:
+                if (dronecanConfig()->dronecanUseDNAServer) {
+                    ATOMIC_BLOCK(NVIC_PRIO_CAN) {
+                        dronecanDnaHandleAllocation(ins, transfer);
+                    }
+                }
+                break;
 
             case UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID:
                 handle_GNSSAuxiliary(ins, transfer);
diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h
index 8722bccb916..7af7e0f392b 100644
--- a/src/main/drivers/dronecan/dronecan.h
+++ b/src/main/drivers/dronecan/dronecan.h
@@ -25,6 +25,7 @@ typedef enum {
 typedef struct dronecanConfig_s {
     uint8_t nodeID;
     dronecanBitrate_e bitRateKbps;
+    bool dronecanUseDNAServer;
 } dronecanConfig_t;
 
 typedef struct dronecanNodeInfo_s {
diff --git a/src/main/drivers/dronecan/dronecan_dna_server.c b/src/main/drivers/dronecan/dronecan_dna_server.c
new file mode 100644
index 00000000000..58c07fe83eb
--- /dev/null
+++ b/src/main/drivers/dronecan/dronecan_dna_server.c
@@ -0,0 +1,287 @@
+#include "platform.h"
+#include "common/log.h"
+#include "drivers/time.h"
+
+#if defined(USE_DRONECAN)
+
+#include <string.h>
+#include <dronecan_msgs.h>
+#include "fc/config.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+#include "dronecan.h"
+#include "dronecan_dna_server.h"
+
+#define DNA_INVALID_STAGE          -1
+#define DNA_STAGE_1                1
+#define DNA_STAGE_2                2
+#define DNA_STAGE_3                3
+
+#define DNA_STAGE3_UID_LEN  (DNA_UNIQUE_ID_LENGTH - UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST * 2)
+_Static_assert(DNA_STAGE3_UID_LEN > 0, "DNA_UNIQUE_ID_LENGTH too small for 3-stage protocol");
+
+
+PG_REGISTER(dnaServerData_t, dnaServerData, PG_DRONECAN_DNA_SERVER, 0);
+
+static int8_t detectRequestStage(struct uavcan_protocol_dynamic_node_id_Allocation *msg);
+static int8_t getExpectedStage(uint8_t currentUniqueIdLength);
+static uint8_t dnaLookupOrAssignNode(const uint8_t *uid, uint8_t requestedNodeId);
+static void dnaSendResponse(uint8_t nodeId, const uint8_t *uid, uint8_t uidLen);
+
+/*
+    Entry point for DroneCAN Dynamic Node Allocation messages.
+    Called from onTransferReceived when a UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID
+    broadcast is received.
+
+    Assembles the complete unique identifier from up to three request stages,
+    then assigns or confirms a node ID for the requesting peripheral.
+    Follows the UAVCAN specification for non-redundant (single-master) allocators.
+*/
+void dronecanDnaHandleAllocation(CanardInstance *ins, CanardRxTransfer *transfer)
+{
+    UNUSED(ins);
+
+    struct uavcan_protocol_dynamic_node_id_Allocation dynamicAllocation;
+
+    static struct {
+        uint8_t len;
+        uint8_t data[DNA_UNIQUE_ID_LENGTH];
+    } currentUniqueId;
+    static uint8_t requestedNodeId = CANARD_BROADCAST_NODE_ID;
+
+    static uint32_t lastMessageTimestamp = 0;
+    int8_t request_stage;
+
+    if (transfer->source_node_id != CANARD_BROADCAST_NODE_ID)
+        return;
+
+    if ((millis() - lastMessageTimestamp) > UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_FOLLOWUP_TIMEOUT_MS) {
+        memset(currentUniqueId.data, 0, DNA_UNIQUE_ID_LENGTH);
+        currentUniqueId.len = 0;
+        requestedNodeId = CANARD_BROADCAST_NODE_ID;
+    }
+
+    if (uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &dynamicAllocation)) {
+        LOG_ERROR(CAN, "DNA decode failed");
+        return;
+    }
+
+    request_stage = detectRequestStage(&dynamicAllocation);
+    if (request_stage == DNA_INVALID_STAGE)
+    {
+        LOG_WARNING(CAN, "DNA malformed request (invalid stage)");
+        return;
+    }
+    const int8_t expected_stage = getExpectedStage(currentUniqueId.len);
+    if (request_stage != expected_stage)
+    {
+        LOG_WARNING(CAN, "DNA stage mismatch (got %d, expected %d)", request_stage, expected_stage);
+        return;
+    }
+    if (request_stage == DNA_STAGE_1)
+        requestedNodeId = dynamicAllocation.node_id;   // Store requested node id as it's only on stage 1
+
+    if (dynamicAllocation.unique_id.len > DNA_UNIQUE_ID_LENGTH - currentUniqueId.len)
+    {
+        LOG_WARNING(CAN, "DNA malformed request - UID exceeds remaining capacity");
+        return;
+    }
+
+    memcpy(currentUniqueId.data + currentUniqueId.len, dynamicAllocation.unique_id.data, dynamicAllocation.unique_id.len);
+    currentUniqueId.len += dynamicAllocation.unique_id.len;
+
+    if (currentUniqueId.len == DNA_UNIQUE_ID_LENGTH)
+    {
+        uint8_t assignedNodeId = dnaLookupOrAssignNode(currentUniqueId.data, requestedNodeId);
+        if (assignedNodeId != 0) {
+            LOG_INFO(CAN, "DNA assigned Node ID: %u to peripheral", assignedNodeId);
+            dnaSendResponse(assignedNodeId, currentUniqueId.data, currentUniqueId.len);
+        }
+        memset(currentUniqueId.data, 0, DNA_UNIQUE_ID_LENGTH);
+        currentUniqueId.len = 0;
+        requestedNodeId = CANARD_BROADCAST_NODE_ID;
+    }
+    else
+    {
+        dnaSendResponse(0, currentUniqueId.data, currentUniqueId.len);
+    }
+
+    lastMessageTimestamp = millis();
+}
+
+static void dnaSendResponse(uint8_t nodeId, const uint8_t *uid, uint8_t uidLen)
+{
+    struct uavcan_protocol_dynamic_node_id_Allocation msg;
+    uint8_t buffer[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_SIZE];
+    static uint8_t transferId;
+    CanardTxTransfer outboundTransfer;
+
+    msg.node_id = nodeId;
+    msg.first_part_of_unique_id = 0;
+    memcpy(msg.unique_id.data, uid, uidLen);
+    msg.unique_id.len = uidLen;
+
+    uint32_t len = uavcan_protocol_dynamic_node_id_Allocation_encode(&msg, buffer);
+
+    canardInitTxTransfer(&outboundTransfer);
+    outboundTransfer.data_type_signature = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE;
+    outboundTransfer.data_type_id        = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID;
+    outboundTransfer.inout_transfer_id   = &transferId;
+    outboundTransfer.priority            = CANARD_TRANSFER_PRIORITY_LOW;
+    outboundTransfer.payload             = buffer;
+    outboundTransfer.payload_len         = (uint16_t)len;
+
+    const int16_t res = canardBroadcastObj(&canard, &outboundTransfer);
+    if (res < 0) {
+        LOG_WARNING(CAN, "DNA: response broadcast failed: %d", res);
+    }
+}
+
+static bool isNodeAvailable(uint8_t assignedNodeId)
+{
+    if (assignedNodeId == canard.node_id)
+        return false;  // Already assigned to this node
+
+    for (uint8_t i = 0; i < DRONECAN_MAX_NODES; i++) {
+        if (assignedNodeId == dnaServerData()->entries[i].nodeId) {
+            return false;  // Already assigned to another node in the allocation table
+        }
+    }
+    // the live node table is only populated from NodeStatus broadcasts (1 Hz), so at 
+    // power-up the allocator may not have heard from all static-ID nodes yet. This is 
+    // a known limitation of non-redundant allocators and is accepted in the spec — 
+    // the downward-from-125 assignment strategy naturally reduces collisions with 
+    // manually assigned low IDs.
+    for (uint8_t i = 0; i < dronecanGetNodeCount(); i++) {
+          const dronecanNodeInfo_t *node = dronecanGetNode(i);
+          if (node && node->nodeID == assignedNodeId)
+              return false;  // Already in use on the network
+      }
+
+    return true;
+}
+/*
+    Search the allocation table for an existing entry matching the given unique
+    identifier. If found, return the previously assigned node ID. If not found,
+    find the first unused node ID (skipping our own) and record a new allocation.
+    Returns 0 if the table is full.
+*/
+static uint8_t dnaLookupOrAssignNode(const uint8_t *uid, uint8_t requestedNodeId)
+{
+    uint8_t  assignedNodeId = CANARD_BROADCAST_NODE_ID;
+    int8_t   conflictIdx = -1;
+    int8_t   writeIdx;
+    uint8_t  storedId;
+    bool     inUse;
+
+    for (int i = 0; i < DRONECAN_MAX_NODES; i++) {
+        if (dnaServerData()->entries[i].nodeId != 0 &&
+                memcmp(dnaServerData()->entries[i].uniqueId, uid, DNA_UNIQUE_ID_LENGTH) == 0) {
+            LOG_DEBUG(CAN, "DNA: found existing allocation for node %u", dnaServerData()->entries[i].nodeId);
+            storedId = dnaServerData()->entries[i].nodeId;
+            inUse = (storedId == canard.node_id);
+            for (uint8_t j = 0; !inUse && j < dronecanGetNodeCount(); j++) {
+                const dronecanNodeInfo_t *node = dronecanGetNode(j);
+                if (node && node->nodeID == storedId)
+                    inUse = true;
+            }
+            if (!inUse)
+                return storedId;
+            LOG_WARNING(CAN, "DNA: stored node ID %u for UID is already in use — re-assigning", storedId);
+            conflictIdx = i;
+            break;
+        }
+    }
+    if (requestedNodeId >= CANARD_MIN_NODE_ID && requestedNodeId <= DRONECAN_DNA_MAX_NODE_ID) {
+        /* Search upward from preferred ID first (per UAVCAN spec) */
+        for (uint8_t id = requestedNodeId; id <= DRONECAN_DNA_MAX_NODE_ID; id++) {
+            if (isNodeAvailable(id)) {
+                assignedNodeId = id;
+                break;
+            }
+        }
+        /* If nothing found upward, search downward from preferred - 1 */
+        if (assignedNodeId == CANARD_BROADCAST_NODE_ID && requestedNodeId > CANARD_MIN_NODE_ID) {
+            for (uint8_t id = requestedNodeId - 1; id >= CANARD_MIN_NODE_ID; id--) {
+                if (isNodeAvailable(id)) {
+                    assignedNodeId = id;
+                    break;
+                }
+            }
+        }
+    }
+    /* No preference or preferred range exhausted → top-down from 125 */
+    if (assignedNodeId == CANARD_BROADCAST_NODE_ID) {
+        for (assignedNodeId = DRONECAN_DNA_MAX_NODE_ID; assignedNodeId >= CANARD_MIN_NODE_ID; assignedNodeId--) {
+            if (isNodeAvailable(assignedNodeId))
+                break;
+        }
+    }
+    if (assignedNodeId < CANARD_MIN_NODE_ID) {
+        LOG_ERROR(CAN, "DNA: no free node IDs available");
+        return 0;
+    }
+    writeIdx = conflictIdx; // Overwrite if already in table
+    if(writeIdx < 0) {
+        for (int i = 0; i < DRONECAN_MAX_NODES; i++) {
+            if (dnaServerData()->entries[i].nodeId == 0) {
+                writeIdx = i;
+                break;
+            }
+        }
+    }
+    if (writeIdx < 0) { 
+        LOG_ERROR(CAN, "DNA: allocation table full");
+        return 0;
+    }
+    memcpy(dnaServerDataMutable()->entries[writeIdx].uniqueId, uid, DNA_UNIQUE_ID_LENGTH);
+    dnaServerDataMutable()->entries[writeIdx].nodeId = assignedNodeId;
+    LOG_INFO(CAN, "DNA added node %u (UID index %u)", assignedNodeId, writeIdx);
+    saveConfig();
+    return assignedNodeId;    
+}
+
+/*
+    Determine which stage of the three-part allocation handshake we expect
+    next, based on how many bytes of the unique identifier have been accumulated.
+*/
+static int8_t getExpectedStage(uint8_t currentUniqueIdLength)
+{
+    if (currentUniqueIdLength == 0)
+        return DNA_STAGE_1;
+    if (currentUniqueIdLength >= (UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST * 2))
+        return DNA_STAGE_3;
+    if (currentUniqueIdLength >= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
+        return DNA_STAGE_2;
+    return DNA_INVALID_STAGE;
+}
+
+/*
+    Classify an incoming allocation request by its stage based on the
+    first_part_of_unique_id flag and the unique_id payload length.
+    Returns DNA_STAGE_1, DNA_STAGE_2, DNA_STAGE_3, or DNA_INVALID_STAGE.
+*/
+static int8_t detectRequestStage(struct uavcan_protocol_dynamic_node_id_Allocation *msg)
+{
+    if ((msg->unique_id.len != UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) &&
+        (msg->unique_id.len != (uint8_t)DNA_STAGE3_UID_LEN) &&
+        (msg->unique_id.len != DNA_UNIQUE_ID_LENGTH))
+    {
+        return DNA_INVALID_STAGE;
+    }
+    if (msg->first_part_of_unique_id)
+    {
+        return DNA_STAGE_1;
+    }
+    if (msg->unique_id.len == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
+    {
+        return DNA_STAGE_2;
+    }
+    if (msg->unique_id.len == (uint8_t)DNA_STAGE3_UID_LEN)
+    {
+        return DNA_STAGE_3;
+    }
+    return DNA_INVALID_STAGE;
+}
+
+#endif // USE_DRONECAN
diff --git a/src/main/drivers/dronecan/dronecan_dna_server.h b/src/main/drivers/dronecan/dronecan_dna_server.h
new file mode 100644
index 00000000000..5eff4843f54
--- /dev/null
+++ b/src/main/drivers/dronecan/dronecan_dna_server.h
@@ -0,0 +1,27 @@
+#pragma once
+
+#include <stdint.h>
+#include "config/parameter_group.h"
+#include "libcanard/canard.h"
+
+#ifdef USE_DRONECAN
+
+extern CanardInstance canard;
+
+#define DNA_UNIQUE_ID_LENGTH       16
+#define DRONECAN_DNA_MAX_NODE_ID  125  /* 126-127 reserved for network maintenance tools */
+
+typedef struct {
+    uint8_t uniqueId[DNA_UNIQUE_ID_LENGTH];
+    uint8_t nodeId;
+} dnaAllocationEntry_t;
+
+typedef struct {
+    dnaAllocationEntry_t entries[DRONECAN_MAX_NODES];
+} dnaServerData_t;
+
+PG_DECLARE(dnaServerData_t, dnaServerData);
+
+void dronecanDnaHandleAllocation(CanardInstance *ins, CanardRxTransfer *transfer);
+
+#endif // USE_DRONECAN
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index db86f748f34..ed0cb32c930 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -4793,7 +4793,67 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
             *ret = MSP_RESULT_ACK;
         }
         break;
-#endif
+
+    case MSP2_INAV_DRONECAN_ASYNC_RESULT:
+        {
+            sbufWriteU8(dst, (uint8_t)dronecanAsyncSlot.state);
+            sbufWriteU8(dst, dronecanAsyncSlot.seq);
+            sbufWriteU16(dst, dronecanAsyncSlot.service_id);
+            sbufWriteU8(dst, dronecanAsyncSlot.node_id);
+
+            if (dronecanAsyncSlot.state == DRONECAN_ASYNC_READY) {
+                switch (dronecanAsyncSlot.service_id) {
+                    case DRONECAN_SERVICE_GETNODEINFO: {
+                        const dronecanGetNodeInfoResult_t *r = &dronecanAsyncSlot.result.node_info;
+                        sbufWriteU8(dst, r->name_len);
+                        sbufWriteDataSafe(dst, r->name, r->name_len);
+                        sbufWriteU8(dst,  r->sw_major);
+                        sbufWriteU8(dst,  r->sw_minor);
+                        sbufWriteU8(dst,  r->sw_optional_field_flags);
+                        sbufWriteU32(dst, r->sw_vcs_commit);
+                        sbufWriteU8(dst,  r->hw_major);
+                        sbufWriteU8(dst,  r->hw_minor);
+                        sbufWriteDataSafe(dst, r->hw_unique_id, 16);
+                        break;
+                    }
+                    case DRONECAN_SERVICE_PARAM_GETSET: {
+                        const dronecanParamResult_t *r = &dronecanAsyncSlot.result.param;
+                        sbufWriteU8(dst, r->name_len);
+                        sbufWriteDataSafe(dst, r->name, r->name_len);
+                        sbufWriteU8(dst, r->type);
+                        switch (r->type) {
+                            case DRONECAN_PARAM_TYPE_INT: {
+                                uint32_t lo = (uint32_t)(r->value_int & 0xFFFFFFFF);
+                                uint32_t hi = (uint32_t)((r->value_int >> 32) & 0xFFFFFFFF);
+                                sbufWriteU32(dst, lo);
+                                sbufWriteU32(dst, hi);
+                                break;
+                            }
+                            case DRONECAN_PARAM_TYPE_FLOAT: {
+                                uint32_t raw;
+                                memcpy(&raw, &r->value_float, 4);
+                                sbufWriteU32(dst, raw);
+                                break;
+                            }
+                            case DRONECAN_PARAM_TYPE_BOOL:
+                                sbufWriteU8(dst, r->value_bool);
+                                break;
+                            case DRONECAN_PARAM_TYPE_STRING:
+                                sbufWriteU8(dst, r->value_str_len);
+                                sbufWriteDataSafe(dst, r->value_str, r->value_str_len);
+                                break;
+                            default:
+                                break;
+                        }
+                        break;
+                    }
+                }
+                dronecanAsyncSlot.state = DRONECAN_ASYNC_IDLE;
+            }
+            *ret = MSP_RESULT_ACK;
+        }
+        break;
+#endif  
 
 #if defined(USE_FLASHFS)
     case MSP_DATAFLASH_READ:
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 74ad48e8d58..0760fdc0306 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4643,3 +4643,8 @@ groups:
         field: bitRateKbps
         table: dronecan_bitrate_table
         type: uint8_t
+      - name: dronecan_use_dna_server
+        description: "Enable the DNA server to manage plug and play dronecan devices"
+        type: bool
+        default_value: ON
+        field: dronecanUseDNAServer
diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt
index 80bd535875c..4d66e70b7c2 100644
--- a/src/test/unit/CMakeLists.txt
+++ b/src/test/unit/CMakeLists.txt
@@ -72,6 +72,7 @@ set_property(SOURCE dronecan_getnodeinfo_unittest.cc PROPERTY definitions USE_DR
 set_property(SOURCE dronecan_application_unittest.cc PROPERTY depends
     "drivers/dronecan/dronecan.c"
     "drivers/dronecan/dronecan_async.c"
+    "drivers/dronecan/dronecan_dna_server.c"
     "drivers/dronecan/libcanard/canard.c")
 set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_sources
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.NodeStatus.c"
@@ -94,13 +95,25 @@ set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_sources
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.RestartNode_res.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.Value.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.NumericValue.c"
-    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.Empty.c")
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.Empty.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.dynamic_node_id.Allocation.c")
 set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_includes
     "../../lib/main/Dronecan/dsdlc_generated/include")
 set_property(SOURCE dronecan_application_unittest.cc PROPERTY definitions
     USE_DRONECAN CANARD_ENABLE_TAO_OPTION=0
     FC_VERSION_MAJOR=10 FC_VERSION_MINOR=0 FC_VERSION_PATCH_LEVEL=0)
 
+# DroneCAN DNA server tests
+set_property(SOURCE dronecan_dna_server_unittest.cc PROPERTY depends
+    "drivers/dronecan/dronecan_dna_server.c"
+    "drivers/dronecan/libcanard/canard.c")
+set_property(SOURCE dronecan_dna_server_unittest.cc PROPERTY extra_sources
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.dynamic_node_id.Allocation.c")
+set_property(SOURCE dronecan_dna_server_unittest.cc PROPERTY extra_includes
+    "../../lib/main/Dronecan/dsdlc_generated/include")
+set_property(SOURCE dronecan_dna_server_unittest.cc PROPERTY definitions
+    USE_DRONECAN CANARD_ENABLE_TAO_OPTION=0)
+
 # CAN bit-timing solver tests - links the real, HAL-free timing core shared
 # by the F7 (bxCAN) and H7 (FDCAN) drivers, so there is nothing to keep in sync
 set_property(SOURCE bxcan_timing_unittest.cc PROPERTY depends
diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc
index 41ab67b1ffd..b27cfa0d1da 100644
--- a/src/test/unit/dronecan_application_unittest.cc
+++ b/src/test/unit/dronecan_application_unittest.cc
@@ -102,6 +102,8 @@ int32_t canardSTM32GetRxFifoFillLevel(void) { return 0; }
 void    canardSTM32RecoverFromBusOff(void) {}
 void    canardSTM32GetUniqueID(uint8_t id[16]) { memset(id, 0, 16); }
 
+void saveConfig(void) {}
+
 /* Version strings declared in build/version.h */
 const char* const shortGitRevision = "00000000";
 const char* const compilerVersion  = "test";
diff --git a/src/test/unit/dronecan_dna_server_unittest.cc b/src/test/unit/dronecan_dna_server_unittest.cc
new file mode 100644
index 00000000000..6cae6618037
--- /dev/null
+++ b/src/test/unit/dronecan_dna_server_unittest.cc
@@ -0,0 +1,539 @@
+/**
+ * DroneCAN DNA Server Unit Tests
+ *
+ * Tests the three-part UID accumulation, node ID assignment, and allocation
+ * table management in dronecan_dna_server.c compiled against real DSDL
+ * encode/decode and INAV stubs.
+ *
+ * Coverage:
+ *   DNA-1  Full 3-stage handshake → entry stored with correct UID and node ID
+ *   DNA-2  Same UID on second handshake → same node ID returned, no new entry
+ *   DNA-3  Two different UIDs → different node IDs assigned
+ *   DNA-4  Table full (DRONECAN_MAX_NODES entries) → no allocation for next node
+ *   DNA-5  Non-broadcast source node ID → transfer silently rejected
+ *   DNA-6  Stage 2 without prior Stage 1 → stage mismatch, rejected
+ *   DNA-7  Followup timeout mid-handshake → accumulator reset, Stage 2 rejected
+ *   DNA-8  FC's own node ID is never assigned to a peripheral
+ *   DNA-9  Peripheral requests specific node ID → honoured when available
+ *   DNA-10 First sequential assignment is 125 (top-down per spec)
+ *   DNA-11 Preferred ID in reserved range (126-127) → falls back to sequential
+ *   DNA-12 Preferred ID already taken → falls back to sequential
+ *   DNA-13 Stored ID in use on live network → reassigned, table entry updated
+ */
+
+#include "gtest/gtest.h"
+
+extern "C" {
+#include <stdint.h>
+#include <string.h>
+#include <stdbool.h>
+
+#include "platform.h"
+
+#include "uavcan.protocol.dynamic_node_id.Allocation.h"
+#include "drivers/dronecan/libcanard/canard.h"
+#include "drivers/dronecan/dronecan.h"
+#include "drivers/dronecan/dronecan_dna_server.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+/* Global canard instance — extern-declared in dronecan_dna_server.h.
+   PG_REGISTER in dronecan_dna_server.c already emits dnaServerData_System
+   and dnaServerData_Copy, so we must NOT define them here. */
+CanardInstance canard;
+
+/* Controllable time source */
+static uint32_t mock_time_ms = 0;
+uint32_t millis(void) { return mock_time_ms; }
+
+/* saveConfig — called when a new allocation is persisted */
+void saveConfig(void) {}
+
+/* Controllable live node table — populated by tests that need it (DNA-13).
+   All other tests leave mock_node_count = 0 so the allocator sees no live nodes. */
+static dronecanNodeInfo_t mock_node_table[DRONECAN_MAX_NODES];
+static uint8_t            mock_node_count = 0;
+
+uint8_t dronecanGetNodeCount(void) { return mock_node_count; }
+const dronecanNodeInfo_t *dronecanGetNode(uint8_t index) {
+    if (index >= mock_node_count) return NULL;
+    return &mock_node_table[index];
+}
+
+} /* extern "C" */
+
+/* =========================================================================
+ * Constants
+ * ========================================================================= */
+
+#define FC_NODE_ID  5u
+#define MAX_LEN     UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST
+#define FOLLOWUP_MS UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_FOLLOWUP_TIMEOUT_MS
+
+/* =========================================================================
+ * Helpers
+ * ========================================================================= */
+
+/* Build a broadcast Allocation transfer from an anonymous peripheral */
+static CanardRxTransfer makeAllocationTransfer(
+        uint8_t        requested_node_id,
+        bool           first_part,
+        const uint8_t *uid,
+        uint8_t        uid_len,
+        uint8_t       *buf)
+{
+    struct uavcan_protocol_dynamic_node_id_Allocation msg;
+    memset(&msg, 0, sizeof(msg));
+    msg.node_id                 = requested_node_id;
+    msg.first_part_of_unique_id = first_part;
+    msg.unique_id.len           = uid_len;
+    memcpy(msg.unique_id.data, uid, uid_len);
+
+    uint32_t len = uavcan_protocol_dynamic_node_id_Allocation_encode(&msg, buf);
+
+    CanardRxTransfer xfer;
+    memset(&xfer, 0, sizeof(xfer));
+    xfer.transfer_type  = CanardTransferTypeBroadcast;
+    xfer.data_type_id   = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID;
+    xfer.source_node_id = CANARD_BROADCAST_NODE_ID;
+    xfer.payload_head   = buf;
+    xfer.payload_len    = (uint16_t)len;
+    return xfer;
+}
+
+/* Run a complete 3-stage handshake for the given 16-byte UID.
+   Stages are sent 100 ms apart (well within the 500 ms followup timeout).
+   Returns the node_id stored in the PG allocation table for this UID,
+   or 0 if no entry was created. */
+static uint8_t runHandshake(const uint8_t uid[16], uint8_t requested_node_id = 0)
+{
+    uint8_t buf[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_SIZE + 4];
+    CanardRxTransfer xfer;
+
+    /* Stage 1: first MAX_LEN bytes, first_part=true */
+    xfer = makeAllocationTransfer(requested_node_id, true, uid, MAX_LEN, buf);
+    dronecanDnaHandleAllocation(NULL, &xfer);
+    mock_time_ms += 100;
+
+    /* Stage 2: next MAX_LEN bytes, first_part=false */
+    xfer = makeAllocationTransfer(0, false, uid + MAX_LEN, MAX_LEN, buf);
+    dronecanDnaHandleAllocation(NULL, &xfer);
+    mock_time_ms += 100;
+
+    /* Stage 3: remaining bytes, first_part=false */
+    xfer = makeAllocationTransfer(0, false, uid + MAX_LEN * 2,
+                                  (uint8_t)(16 - MAX_LEN * 2), buf);
+    dronecanDnaHandleAllocation(NULL, &xfer);
+    mock_time_ms += 100;
+
+    /* Look up the result in the PG allocation table */
+    for (int i = 0; i < DRONECAN_MAX_NODES; i++) {
+        if (dnaServerData()->entries[i].nodeId != 0 &&
+            memcmp(dnaServerData()->entries[i].uniqueId, uid, 16) == 0) {
+            return dnaServerData()->entries[i].nodeId;
+        }
+    }
+    return 0;
+}
+
+/* =========================================================================
+ * Fixture
+ * ========================================================================= */
+
+class DroneCANDnaServerTest : public ::testing::Test {
+protected:
+    static uint32_t s_base_ms;
+    uint8_t canard_pool[512];
+
+    void SetUp() override {
+        /* Jump 10 seconds forward each test so the DNA server's followup
+           timeout fires on the very first call, resetting any accumulated
+           UID state left by the previous test. */
+        s_base_ms   += 10000;
+        mock_time_ms = s_base_ms;
+
+        memset(dnaServerDataMutable(), 0, sizeof(dnaServerData_t));
+        memset(mock_node_table, 0, sizeof(mock_node_table));
+        mock_node_count = 0;
+
+        canardInit(&canard, canard_pool, sizeof(canard_pool),
+                   NULL, NULL, NULL);
+        canardSetLocalNodeID(&canard, FC_NODE_ID);
+    }
+};
+uint32_t DroneCANDnaServerTest::s_base_ms = 0;
+
+/* =========================================================================
+ * DNA-1: Full 3-stage handshake stores entry with correct UID and node ID
+ * ========================================================================= */
+TEST_F(DroneCANDnaServerTest, FullHandshakeAssignsNodeId)
+{
+    const uint8_t uid[16] = {
+        0x01,0x02,0x03,0x04,0x05,0x06,
+        0x07,0x08,0x09,0x0A,0x0B,0x0C,
+        0x0D,0x0E,0x0F,0x10
+    };
+
+    uint8_t assigned = runHandshake(uid);
+
+    EXPECT_NE(assigned, 0u);
+    EXPECT_NE(assigned, FC_NODE_ID);
+    EXPECT_LT(assigned, (uint8_t)CANARD_MAX_NODE_ID);
+
+    /* Verify the table entry contains the correct UID */
+    bool found = false;
+    for (int i = 0; i < DRONECAN_MAX_NODES; i++) {
+        if (dnaServerData()->entries[i].nodeId == assigned) {
+            EXPECT_EQ(0, memcmp(dnaServerData()->entries[i].uniqueId, uid, 16));
+            found = true;
+            break;
+        }
+    }
+    EXPECT_TRUE(found) << "Assigned ID not present in allocation table";
+}
+
+/* =========================================================================
+ * DNA-2: Same UID on second handshake → same node ID, no new table entry
+ * ========================================================================= */
+TEST_F(DroneCANDnaServerTest, SameUidReturnsSameNodeId)
+{
+    const uint8_t uid[16] = {
+        0xAA,0xBB,0xCC,0xDD,0xEE,0xFF,
+        0x11,0x22,0x33,0x44,0x55,0x66,
+        0x77,0x88,0x99,0x00
+    };
+
+    uint8_t first = runHandshake(uid);
+    ASSERT_NE(first, 0u);
+
+    s_base_ms   += 10000;
+    mock_time_ms = s_base_ms;
+
+    uint8_t second = runHandshake(uid);
+
+    EXPECT_EQ(first, second) << "Same UID must return same node ID on re-negotiation";
+
+    int count = 0;
+    for (int i = 0; i < DRONECAN_MAX_NODES; i++) {
+        if (dnaServerData()->entries[i].nodeId != 0 &&
+            memcmp(dnaServerData()->entries[i].uniqueId, uid, 16) == 0) {
+            count++;
+        }
+    }
+    EXPECT_EQ(count, 1) << "Re-negotiation must not create a second table entry";
+}
+
+/* =========================================================================
+ * DNA-3: Two different UIDs receive different node IDs
+ * ========================================================================= */
+TEST_F(DroneCANDnaServerTest, TwoDifferentUidsGetDifferentNodeIds)
+{
+    const uint8_t uid1[16] = {
+        0x01,0x01,0x01,0x01,0x01,0x01,
+        0x01,0x01,0x01,0x01,0x01,0x01,
+        0x01,0x01,0x01,0x01
+    };
+    const uint8_t uid2[16] = {
+        0x02,0x02,0x02,0x02,0x02,0x02,
+        0x02,0x02,0x02,0x02,0x02,0x02,
+        0x02,0x02,0x02,0x02
+    };
+
+    uint8_t id1 = runHandshake(uid1);
+    ASSERT_NE(id1, 0u);
+
+    s_base_ms   += 10000;
+    mock_time_ms = s_base_ms;
+
+    uint8_t id2 = runHandshake(uid2);
+    ASSERT_NE(id2, 0u);
+
+    EXPECT_NE(id1, id2);
+}
+
+/* =========================================================================
+ * DNA-4: Table full → the next peripheral receives no allocation
+ * ========================================================================= */
+TEST_F(DroneCANDnaServerTest, TableFullReturnsNoAllocation)
+{
+    /* Fill every slot with distinct fake UIDs and valid node IDs,
+       skipping FC_NODE_ID in the assigned sequence. */
+    uint8_t nextId = CANARD_MIN_NODE_ID;
+    for (int i = 0; i < DRONECAN_MAX_NODES; i++) {
+        if (nextId == FC_NODE_ID) nextId++;
+        dnaServerDataMutable()->entries[i].nodeId = nextId++;
+        memset(dnaServerDataMutable()->entries[i].uniqueId, (uint8_t)(i + 1), 16);
+    }
+
+    const uint8_t uid[16] = {
+        0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+        0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
+        0xFF,0xFF,0xFF,0xFF
+    };
+
+    uint8_t assigned = runHandshake(uid);
+    EXPECT_EQ(assigned, 0u) << "Full table must not produce an allocation";
+
+    for (int i = 0; i < DRONECAN_MAX_NODES; i++) {
+        EXPECT_NE(0, memcmp(dnaServerData()->entries[i].uniqueId, uid, 16))
+            << "Full table must not write a new entry at slot " << i;
+    }
+}
+
+/* =========================================================================
+ * DNA-5: Non-broadcast source node ID → rejected before any processing
+ * ========================================================================= */
+TEST_F(DroneCANDnaServerTest, NonBroadcastSourceRejected)
+{
+    uint8_t buf[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_SIZE + 4];
+    const uint8_t uid[16] = {
+        0xDE,0xAD,0xBE,0xEF,0xDE,0xAD,
+        0xBE,0xEF,0xDE,0xAD,0xBE,0xEF,
+        0xDE,0xAD,0xBE,0xEF
+    };
+
+    CanardRxTransfer xfer = makeAllocationTransfer(0, true, uid, MAX_LEN, buf);
+    xfer.source_node_id = 42;   /* non-anonymous — must be rejected */
+    dronecanDnaHandleAllocation(NULL, &xfer);
+
+    for (int i = 0; i < DRONECAN_MAX_NODES; i++) {
+        EXPECT_EQ(dnaServerData()->entries[i].nodeId, 0u)
+            << "Non-broadcast source must not create a table entry";
+    }
+}
+
+/* =========================================================================
+ * DNA-6: Stage 2 without a preceding Stage 1 → stage mismatch, rejected
+ * ========================================================================= */
+TEST_F(DroneCANDnaServerTest, Stage2WithoutStage1Rejected)
+{
+    uint8_t buf[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_SIZE + 4];
+    const uint8_t uid[16] = {
+        0xCA,0xFE,0xBA,0xBE,0xCA,0xFE,
+        0xBA,0xBE,0xCA,0xFE,0xBA,0xBE,
+        0xCA,0xFE,0xBA,0xBE
+    };
+
+    /* first_part=false and len=MAX_LEN is a Stage 2 message */
+    CanardRxTransfer xfer = makeAllocationTransfer(0, false, uid + MAX_LEN, MAX_LEN, buf);
+    dronecanDnaHandleAllocation(NULL, &xfer);
+
+    for (int i = 0; i < DRONECAN_MAX_NODES; i++) {
+        EXPECT_EQ(dnaServerData()->entries[i].nodeId, 0u)
+            << "Out-of-sequence Stage 2 must not create a table entry";
+    }
+}
+
+/* =========================================================================
+ * DNA-7: Followup timeout mid-handshake resets the UID accumulator
+ * ========================================================================= */
+TEST_F(DroneCANDnaServerTest, FollowupTimeoutResetsAccumulator)
+{
+    uint8_t buf[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_SIZE + 4];
+    const uint8_t uid[16] = {
+        0x10,0x20,0x30,0x40,0x50,0x60,
+        0x70,0x80,0x90,0xA0,0xB0,0xC0,
+        0xD0,0xE0,0xF0,0x00
+    };
+
+    /* Stage 1 */
+    CanardRxTransfer s1 = makeAllocationTransfer(0, true, uid, MAX_LEN, buf);
+    dronecanDnaHandleAllocation(NULL, &s1);
+
+    /* Advance past the followup timeout — server must reset accumulator */
+    mock_time_ms += FOLLOWUP_MS + 100;
+
+    /* Stage 2 — now arrives as if no Stage 1 preceded it */
+    CanardRxTransfer s2 = makeAllocationTransfer(0, false, uid + MAX_LEN, MAX_LEN, buf);
+    dronecanDnaHandleAllocation(NULL, &s2);
+
+    for (int i = 0; i < DRONECAN_MAX_NODES; i++) {
+        EXPECT_EQ(dnaServerData()->entries[i].nodeId, 0u)
+            << "Stage 2 after timeout must not create a table entry";
+    }
+}
+
+/* =========================================================================
+ * DNA-8: FC's own node ID is never assigned to a peripheral
+ *
+ * Top-down sequential assignment starts at 125 and DRONECAN_MAX_NODES (32)
+ * caps how many allocation-table + live-node-table entries can ever be
+ * occupied at once (64 combined at most) — nowhere near enough to force the
+ * top-down scan down to FC_NODE_ID (5) by filling slots. Requesting
+ * FC_NODE_ID as the *preferred* ID instead exercises the exact
+ * isNodeAvailable(canard.node_id) guard directly and deterministically:
+ * the preferred-ID search starts at the requested value and scans upward,
+ * so if 5 (FC's own ID) is correctly skipped, the peripheral must land on
+ * 6, the next available ID above it.
+ * ========================================================================= */
+TEST_F(DroneCANDnaServerTest, FcNodeIdNeverAssigned)
+{
+    const uint8_t uid[16] = {
+        0xFC,0xFC,0xFC,0xFC,0xFC,0xFC,
+        0xFC,0xFC,0xFC,0xFC,0xFC,0xFC,
+        0xFC,0xFC,0xFC,0xFC
+    };
+
+    uint8_t assigned = runHandshake(uid, FC_NODE_ID);
+
+    ASSERT_NE(assigned, 0u) << "Requesting the FC's own node ID must not block allocation";
+    EXPECT_NE(assigned, FC_NODE_ID)
+        << "FC node ID " << (int)FC_NODE_ID << " was incorrectly assigned to a peripheral";
+    EXPECT_EQ(assigned, FC_NODE_ID + 1)
+        << "Preferred-ID search starts at the requested value and scans upward; "
+        << "skipping " << (int)FC_NODE_ID << " (FC's own ID) on a clean table "
+        << "must land on " << (int)(FC_NODE_ID + 1);
+}
+
+/* =========================================================================
+ * DNA-9: Peripheral's preferred node ID is honoured
+ *
+ * The UAVCAN spec allows a peripheral to include a preferred node_id in its
+ * Stage-1 request. The allocator assigns that ID if it is in the valid
+ * dynamic range (1-125) and not already taken, falling back to sequential
+ * assignment otherwise.
+ * ========================================================================= */
+TEST_F(DroneCANDnaServerTest, RequestedNodeIdIsHonoured)
+{
+    const uint8_t uid[16] = {
+        0x55,0x55,0x55,0x55,0x55,0x55,
+        0x55,0x55,0x55,0x55,0x55,0x55,
+        0x55,0x55,0x55,0x55
+    };
+    const uint8_t requested = 42u;
+
+    uint8_t assigned = runHandshake(uid, requested);
+
+    EXPECT_EQ(assigned, requested)
+        << "Peripheral requested node ID " << (int)requested
+        << " but was assigned " << (int)assigned;
+}
+
+/* =========================================================================
+ * DNA-10: First sequential (no preferred ID) assignment is 125
+ *
+ * The UAVCAN spec requires allocators to assign from the top of the dynamic
+ * range downward so that manually assigned low IDs are least likely to
+ * collide. DRONECAN_DNA_MAX_NODE_ID = 125 (126-127 reserved).
+ * ========================================================================= */
+TEST_F(DroneCANDnaServerTest, SequentialAssignmentStartsAtTop)
+{
+    const uint8_t uid[16] = {
+        0xA0,0xA0,0xA0,0xA0,0xA0,0xA0,
+        0xA0,0xA0,0xA0,0xA0,0xA0,0xA0,
+        0xA0,0xA0,0xA0,0xA0
+    };
+
+    uint8_t assigned = runHandshake(uid);
+
+    EXPECT_EQ(assigned, DRONECAN_DNA_MAX_NODE_ID)
+        << "First sequential assignment must be " << (int)DRONECAN_DNA_MAX_NODE_ID
+        << " (top of dynamic range), got " << (int)assigned;
+}
+
+/* =========================================================================
+ * DNA-11: Preferred ID in reserved range (126 or 127) → falls back to 125
+ *
+ * Node IDs 126 and 127 are reserved for network maintenance tools and must
+ * never be assigned dynamically. A request for these must be silently
+ * ignored and a valid ID assigned instead.
+ * ========================================================================= */
+TEST_F(DroneCANDnaServerTest, ReservedPreferredIdFallsBackToSequential)
+{
+    const uint8_t uid[16] = {
+        0xB0,0xB0,0xB0,0xB0,0xB0,0xB0,
+        0xB0,0xB0,0xB0,0xB0,0xB0,0xB0,
+        0xB0,0xB0,0xB0,0xB0
+    };
+
+    uint8_t assigned = runHandshake(uid, 126u);
+
+    EXPECT_NE(assigned, 0u)   << "Reserved preferred ID must not block allocation";
+    EXPECT_NE(assigned, 126u) << "Reserved node ID 126 must not be assigned";
+    EXPECT_NE(assigned, 127u) << "Reserved node ID 127 must not be assigned";
+    EXPECT_LE(assigned, (uint8_t)DRONECAN_DNA_MAX_NODE_ID)
+        << "Assigned ID must be within the dynamic range";
+}
+
+/* =========================================================================
+ * DNA-12: Preferred ID already taken → falls back to sequential
+ *
+ * If the requested node ID is valid but already in the allocation table,
+ * the allocator must assign the next free ID rather than failing.
+ * ========================================================================= */
+TEST_F(DroneCANDnaServerTest, TakenPreferredIdFallsBackToSequential)
+{
+    const uint8_t uid1[16] = {
+        0xC1,0xC1,0xC1,0xC1,0xC1,0xC1,
+        0xC1,0xC1,0xC1,0xC1,0xC1,0xC1,
+        0xC1,0xC1,0xC1,0xC1
+    };
+    const uint8_t uid2[16] = {
+        0xC2,0xC2,0xC2,0xC2,0xC2,0xC2,
+        0xC2,0xC2,0xC2,0xC2,0xC2,0xC2,
+        0xC2,0xC2,0xC2,0xC2
+    };
+    const uint8_t preferred = 60u;
+
+    /* First peripheral claims the preferred ID */
+    uint8_t first = runHandshake(uid1, preferred);
+    ASSERT_EQ(first, preferred) << "Setup: first peripheral should get its preferred ID";
+
+    s_base_ms   += 10000;
+    mock_time_ms = s_base_ms;
+
+    /* Second peripheral requests the same ID — must get the next upward free ID */
+    uint8_t second = runHandshake(uid2, preferred);
+
+    EXPECT_EQ(second, preferred + 1)
+        << "Upward search from taken preferred ID must yield preferred+1";
+    EXPECT_LE(second, (uint8_t)DRONECAN_DNA_MAX_NODE_ID);
+}
+
+/* =========================================================================
+ * DNA-13: Stored ID in use on live network → reassigned, table entry updated
+ *
+ * If a peripheral re-negotiates and its previously stored node ID is now
+ * claimed by a live static-ID node (visible in the NodeStatus table), the
+ * allocator must assign a new ID and overwrite the old table entry rather
+ * than creating a duplicate.
+ * ========================================================================= */
+TEST_F(DroneCANDnaServerTest, ConflictingLiveNodeCausesReassignment)
+{
+    const uint8_t uid[16] = {
+        0xD0,0xD0,0xD0,0xD0,0xD0,0xD0,
+        0xD0,0xD0,0xD0,0xD0,0xD0,0xD0,
+        0xD0,0xD0,0xD0,0xD0
+    };
+
+    /* Initial allocation — gets DRONECAN_DNA_MAX_NODE_ID (125) */
+    uint8_t first = runHandshake(uid);
+    ASSERT_EQ(first, (uint8_t)DRONECAN_DNA_MAX_NODE_ID);
+
+    s_base_ms   += 10000;
+    mock_time_ms = s_base_ms;
+
+    /* Simulate a static-ID node claiming that ID on the live network */
+    mock_node_table[0].nodeID = first;
+    mock_node_count = 1;
+
+    /* Re-negotiate — server must detect the conflict and assign a new ID */
+    uint8_t second = runHandshake(uid);
+
+    EXPECT_NE(second, 0u)   << "Conflict must not prevent allocation";
+    EXPECT_NE(second, first) << "Conflicted ID must not be re-assigned";
+    EXPECT_LE(second, (uint8_t)DRONECAN_DNA_MAX_NODE_ID);
+
+    /* Table must have exactly one entry for this UID with the new ID */
+    int count = 0;
+    uint8_t tableId = 0;
+    for (int i = 0; i < DRONECAN_MAX_NODES; i++) {
+        if (memcmp(dnaServerData()->entries[i].uniqueId, uid, 16) == 0 &&
+            dnaServerData()->entries[i].nodeId != 0) {
+            count++;
+            tableId = dnaServerData()->entries[i].nodeId;
+        }
+    }
+    EXPECT_EQ(count, 1)      << "Must be exactly one table entry for this UID";
+    EXPECT_EQ(tableId, second) << "Table entry must reflect the newly assigned ID";
+}

From acfa63207f61c3885ef76ee045e9dbc930042187 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Sun, 16 Aug 2026 20:23:14 -0700
Subject: [PATCH 61/67] fix(dronecan): accept single-frame full-UID stage-1
 delivery for DNA allocation

The DNA stage-detection logic (detectRequestStage()) assumed a
peripheral's 16-byte unique ID always arrives split across the
standard three-frame handshake (stage 1: first partial chunk, stage 2:
middle chunk, stage 3: final chunk). Some peripherals - and CAN FD
frames, which have enough payload capacity - deliver the complete
16-byte UID in a single stage-1 frame instead. That single-frame case
was being rejected as a malformed/out-of-sequence request rather than
accepted as a valid one-shot allocation, so those peripherals could
never complete DNA allocation at all.

Regression test (differential: fails against the pre-fix logic, passes
against the fix) added alongside the DNA-1..9 suite.

Also removes a duplicate MSP2_INAV_DRONECAN_ASYNC_RESULT case
statement found while touching this code, and stubs _logf in the DNA
server unit test (needed once the fix's added logging pulled the
symbol into that test binary's link).
---
 docs/DroneCAN-Driver.md                       |   6 +-
 .../drivers/dronecan/dronecan_dna_server.c    |  12 +-
 src/main/fc/fc_msp.c                          |  62 +--------
 src/test/unit/dronecan_dna_server_unittest.cc | 125 ++++++++++++++++++
 4 files changed, 141 insertions(+), 64 deletions(-)

diff --git a/docs/DroneCAN-Driver.md b/docs/DroneCAN-Driver.md
index b1fb9317aa6..ccf508c1827 100644
--- a/docs/DroneCAN-Driver.md
+++ b/docs/DroneCAN-Driver.md
@@ -560,7 +560,9 @@ A peripheral that has no node ID broadcasts anonymous Allocation messages carryi
 
 Each stage must arrive within 500 ms (`FOLLOWUP_TIMEOUT_MS`) of the previous one, or the accumulator resets and the peripheral must start over from Stage 1.
 
-After Stage 3 the server has the full 16-byte UID and calls `dnaLookupOrAssignNode()`.
+A transport capable of a larger single frame (e.g. CAN-FD) may instead send the full 16-byte UID in one message with `first_part_of_unique_id = true` and `unique_id.len = 16`; the server accepts this as Stage 1 and completes the allocation immediately without waiting for Stage 2/3.
+
+After the full 16-byte UID has been assembled — whether over three stages or in one single-frame message — the server calls `dnaLookupOrAssignNode()`.
 
 ### Node ID assignment
 
@@ -599,7 +601,7 @@ case UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID:
 
 ### Unit tests
 
-Thirteen tests in `src/test/unit/dronecan_dna_server_unittest.cc` cover the full handshake, UID re-use, table-full rejection, non-broadcast source rejection, stage ordering, timeout reset, FC node ID exclusion, preferred node ID honouring, top-down sequential assignment, reserved-range and taken-preferred-ID fallback, and live-network reassignment of a stored ID (DNA-1 through DNA-13).
+Sixteen tests in `src/test/unit/dronecan_dna_server_unittest.cc` cover the full handshake, UID re-use, table-full rejection, non-broadcast source rejection, stage ordering, timeout reset, FC node ID exclusion, preferred node ID honouring, top-down sequential assignment, reserved-range and taken-preferred-ID fallback, live-network reassignment of a stored ID, single-frame (e.g. CAN-FD) full-UID delivery, and rejection of a malformed 4-byte stage-1 message without poisoning a legitimate follow-up handshake (DNA-1 through DNA-16).
 
 ---
 
diff --git a/src/main/drivers/dronecan/dronecan_dna_server.c b/src/main/drivers/dronecan/dronecan_dna_server.c
index 58c07fe83eb..cac03aee858 100644
--- a/src/main/drivers/dronecan/dronecan_dna_server.c
+++ b/src/main/drivers/dronecan/dronecan_dna_server.c
@@ -271,7 +271,17 @@ static int8_t detectRequestStage(struct uavcan_protocol_dynamic_node_id_Allocati
     }
     if (msg->first_part_of_unique_id)
     {
-        return DNA_STAGE_1;
+        // Stage 1 is either the classic 6-byte first chunk of a multi-frame
+        // handshake, or the full 16-byte UID delivered in a single message
+        // (e.g. CAN-FD, which can carry the whole UID in one frame). The
+        // 4-byte DNA_STAGE3_UID_LEN length is not a valid stage-1 length
+        // under either scheme.
+        if (msg->unique_id.len == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST ||
+            msg->unique_id.len == DNA_UNIQUE_ID_LENGTH)
+        {
+            return DNA_STAGE_1;
+        }
+        return DNA_INVALID_STAGE;
     }
     if (msg->unique_id.len == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST)
     {
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index ed0cb32c930..db86f748f34 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -4793,67 +4793,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
             *ret = MSP_RESULT_ACK;
         }
         break;
-
-    case MSP2_INAV_DRONECAN_ASYNC_RESULT:
-        {
-            sbufWriteU8(dst, (uint8_t)dronecanAsyncSlot.state);
-            sbufWriteU8(dst, dronecanAsyncSlot.seq);
-            sbufWriteU16(dst, dronecanAsyncSlot.service_id);
-            sbufWriteU8(dst, dronecanAsyncSlot.node_id);
-
-            if (dronecanAsyncSlot.state == DRONECAN_ASYNC_READY) {
-                switch (dronecanAsyncSlot.service_id) {
-                    case DRONECAN_SERVICE_GETNODEINFO: {
-                        const dronecanGetNodeInfoResult_t *r = &dronecanAsyncSlot.result.node_info;
-                        sbufWriteU8(dst, r->name_len);
-                        sbufWriteDataSafe(dst, r->name, r->name_len);
-                        sbufWriteU8(dst,  r->sw_major);
-                        sbufWriteU8(dst,  r->sw_minor);
-                        sbufWriteU8(dst,  r->sw_optional_field_flags);
-                        sbufWriteU32(dst, r->sw_vcs_commit);
-                        sbufWriteU8(dst,  r->hw_major);
-                        sbufWriteU8(dst,  r->hw_minor);
-                        sbufWriteDataSafe(dst, r->hw_unique_id, 16);
-                        break;
-                    }
-                    case DRONECAN_SERVICE_PARAM_GETSET: {
-                        const dronecanParamResult_t *r = &dronecanAsyncSlot.result.param;
-                        sbufWriteU8(dst, r->name_len);
-                        sbufWriteDataSafe(dst, r->name, r->name_len);
-                        sbufWriteU8(dst, r->type);
-                        switch (r->type) {
-                            case DRONECAN_PARAM_TYPE_INT: {
-                                uint32_t lo = (uint32_t)(r->value_int & 0xFFFFFFFF);
-                                uint32_t hi = (uint32_t)((r->value_int >> 32) & 0xFFFFFFFF);
-                                sbufWriteU32(dst, lo);
-                                sbufWriteU32(dst, hi);
-                                break;
-                            }
-                            case DRONECAN_PARAM_TYPE_FLOAT: {
-                                uint32_t raw;
-                                memcpy(&raw, &r->value_float, 4);
-                                sbufWriteU32(dst, raw);
-                                break;
-                            }
-                            case DRONECAN_PARAM_TYPE_BOOL:
-                                sbufWriteU8(dst, r->value_bool);
-                                break;
-                            case DRONECAN_PARAM_TYPE_STRING:
-                                sbufWriteU8(dst, r->value_str_len);
-                                sbufWriteDataSafe(dst, r->value_str, r->value_str_len);
-                                break;
-                            default:
-                                break;
-                        }
-                        break;
-                    }
-                }
-                dronecanAsyncSlot.state = DRONECAN_ASYNC_IDLE;
-            }
-            *ret = MSP_RESULT_ACK;
-        }
-        break;
-#endif  
+#endif
 
 #if defined(USE_FLASHFS)
     case MSP_DATAFLASH_READ:
diff --git a/src/test/unit/dronecan_dna_server_unittest.cc b/src/test/unit/dronecan_dna_server_unittest.cc
index 6cae6618037..0803375d117 100644
--- a/src/test/unit/dronecan_dna_server_unittest.cc
+++ b/src/test/unit/dronecan_dna_server_unittest.cc
@@ -19,6 +19,9 @@
  *   DNA-11 Preferred ID in reserved range (126-127) → falls back to sequential
  *   DNA-12 Preferred ID already taken → falls back to sequential
  *   DNA-13 Stored ID in use on live network → reassigned, table entry updated
+ *   DNA-14 Full 16-byte UID in a single stage-1 message → completes immediately
+ *   DNA-15 Full-length message without the stage-1 flag → rejected
+ *   DNA-16 Malformed 4-byte stage-1 message rejected, doesn't block real handshake
  */
 
 #include "gtest/gtest.h"
@@ -36,6 +39,7 @@ extern "C" {
 #include "drivers/dronecan/dronecan_dna_server.h"
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
+#include "common/log.h"
 
 /* Global canard instance — extern-declared in dronecan_dna_server.h.
    PG_REGISTER in dronecan_dna_server.c already emits dnaServerData_System
@@ -49,6 +53,13 @@ uint32_t millis(void) { return mock_time_ms; }
 /* saveConfig — called when a new allocation is persisted */
 void saveConfig(void) {}
 
+/* Logging — USE_LOG is unconditionally defined by target/common.h (pulled in
+   via platform.h), so LOG_ERROR/LOG_WARNING/LOG_DEBUG in dronecan_dna_server.c
+   expand to real _logf() calls. Stubbed as a no-op rather than linking
+   common/log.c, which would pull in unrelated production dependencies
+   (serial.h, msp.h, msp_serial.h, fc/config.h, config/feature.h). */
+void _logf(logTopic_e topic, unsigned level, const char *fmt, ...) { (void)topic; (void)level; (void)fmt; }
+
 /* Controllable live node table — populated by tests that need it (DNA-13).
    All other tests leave mock_node_count = 0 so the allocator sees no live nodes. */
 static dronecanNodeInfo_t mock_node_table[DRONECAN_MAX_NODES];
@@ -537,3 +548,117 @@ TEST_F(DroneCANDnaServerTest, ConflictingLiveNodeCausesReassignment)
     EXPECT_EQ(count, 1)      << "Must be exactly one table entry for this UID";
     EXPECT_EQ(tableId, second) << "Table entry must reflect the newly assigned ID";
 }
+
+/* =========================================================================
+ * DNA-14: Full 16-byte UID delivered in a single stage-1 message
+ *
+ * A transport capable of a larger single frame (e.g. CAN-FD) can carry the
+ * entire 16-byte unique ID in one message instead of the classic 6+6+4
+ * split. detectRequestStage() must accept first_part_of_unique_id=true with
+ * unique_id.len=16 as a valid Stage 1, and the handler must complete the
+ * allocation immediately without waiting for Stage 2/3 messages.
+ * ========================================================================= */
+TEST_F(DroneCANDnaServerTest, SingleFrameFullUidCompletesImmediately)
+{
+    const uint8_t uid[16] = {
+        0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,
+        0xE0,0xE0,0xE0,0xE0,0xE0,0xE0,
+        0xE0,0xE0,0xE0,0xE0
+    };
+    uint8_t buf[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_SIZE + 4];
+
+    CanardRxTransfer xfer = makeAllocationTransfer(0, true, uid, 16, buf);
+    dronecanDnaHandleAllocation(NULL, &xfer);
+
+    uint8_t assigned = 0;
+    for (int i = 0; i < DRONECAN_MAX_NODES; i++) {
+        if (dnaServerData()->entries[i].nodeId != 0 &&
+            memcmp(dnaServerData()->entries[i].uniqueId, uid, 16) == 0) {
+            assigned = dnaServerData()->entries[i].nodeId;
+            break;
+        }
+    }
+
+    EXPECT_NE(assigned, 0u)
+        << "A single-frame 16-byte stage-1 message must complete allocation "
+        << "without needing follow-up Stage 2/3 messages";
+}
+
+/* =========================================================================
+ * DNA-15: Full-length message not marked as stage 1 is rejected
+ *
+ * A 16-byte unique_id.len is only a valid length when paired with
+ * first_part_of_unique_id=true (DNA-14). The same length with the flag
+ * false must be rejected as DNA_INVALID_STAGE rather than silently
+ * accepted as some other stage.
+ * ========================================================================= */
+TEST_F(DroneCANDnaServerTest, NonFirstPartFullLengthRejected)
+{
+    const uint8_t uid[16] = {
+        0xE1,0xE1,0xE1,0xE1,0xE1,0xE1,
+        0xE1,0xE1,0xE1,0xE1,0xE1,0xE1,
+        0xE1,0xE1,0xE1,0xE1
+    };
+    uint8_t buf[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_SIZE + 4];
+
+    CanardRxTransfer xfer = makeAllocationTransfer(0, false, uid, 16, buf);
+    dronecanDnaHandleAllocation(NULL, &xfer);
+
+    for (int i = 0; i < DRONECAN_MAX_NODES; i++) {
+        EXPECT_NE(0, memcmp(dnaServerData()->entries[i].uniqueId, uid, 16))
+            << "A rejected (non-first-part, full-length) message must not "
+            << "create a table entry at slot " << i;
+    }
+}
+
+/* =========================================================================
+ * DNA-16: Malformed 4-byte stage-1 message is rejected and does not poison
+ * the accumulator against a legitimate follow-up handshake
+ *
+ * Before this fix, first_part_of_unique_id=true with unique_id.len=4 (the
+ * DNA_STAGE3_UID_LEN length) was accepted as a valid Stage 1, since the old
+ * detectRequestStage() only checked the first_part flag, not the length,
+ * once the length passed the top-level {4,6,16} whitelist. That set
+ * currentUniqueId.len=4, and getExpectedStage(4) falls through to
+ * DNA_INVALID_STAGE (4 is neither 0, nor >=12, nor >=6) — so *any*
+ * legitimate follow-up message arriving within the 500ms followup window
+ * was rejected as a stage mismatch, silently blocking real allocation
+ * until the timeout reset the accumulator. This is the actual regression
+ * this fix guards against; DNA-14/DNA-15 do not exercise it since both of
+ * their (length, first_part) combinations were already handled correctly
+ * before this fix.
+ * ========================================================================= */
+TEST_F(DroneCANDnaServerTest, MalformedFourByteStage1DoesNotBlockRealHandshake)
+{
+    const uint8_t bogus_uid[4] = {0xBA, 0xD0, 0xBA, 0xD0};
+    uint8_t buf[UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_SIZE + 4];
+
+    /* Malformed message: first_part=true with the stage-3-only 4-byte length.
+       Must be rejected outright and must not touch the accumulator at all. */
+    CanardRxTransfer bogus = makeAllocationTransfer(0, true, bogus_uid, 4, buf);
+    dronecanDnaHandleAllocation(NULL, &bogus);
+
+    /* Immediately after, a legitimate single-frame 16-byte stage-1 handshake
+       (well within the 500ms followup window) must still succeed. */
+    const uint8_t real_uid[16] = {
+        0xE2,0xE2,0xE2,0xE2,0xE2,0xE2,
+        0xE2,0xE2,0xE2,0xE2,0xE2,0xE2,
+        0xE2,0xE2,0xE2,0xE2
+    };
+    mock_time_ms += 10;
+    CanardRxTransfer real = makeAllocationTransfer(0, true, real_uid, 16, buf);
+    dronecanDnaHandleAllocation(NULL, &real);
+
+    uint8_t assigned = 0;
+    for (int i = 0; i < DRONECAN_MAX_NODES; i++) {
+        if (dnaServerData()->entries[i].nodeId != 0 &&
+            memcmp(dnaServerData()->entries[i].uniqueId, real_uid, 16) == 0) {
+            assigned = dnaServerData()->entries[i].nodeId;
+            break;
+        }
+    }
+
+    EXPECT_NE(assigned, 0u)
+        << "A malformed 4-byte stage-1 message must not poison the accumulator "
+        << "and block a legitimate handshake that follows it";
+}

From a795eadb1de288796c300fde17b6814c4966ae0f Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Sun, 16 Aug 2026 06:42:39 -0700
Subject: [PATCH 62/67] refactor(dronecan): extract NodeStatus tracking module

Split the node presence/heartbeat concern out of dronecan.c into
dronecan_node_status.c/.h: the live node table (activeNodeCount/
nodeTable[], consumed by dronecan_dna_server.c's isNodeAvailable() via
dronecanGetNodeCount()/dronecanGetNode() to avoid handing out node IDs
already seen on the bus), the incoming NodeStatus broadcast handler
(renamed handle_NodeStatus -> dronecanNodeStatusHandleBroadcast for
external linkage), and our own 1Hz NodeStatus heartbeat send.
dronecan.c's handle_GetNodeInfo() now calls the new
dronecanGetOwnNodeStatus() accessor instead of touching the node_status
struct directly, and process1HzTasks() delegates to
dronecanNodeStatusUpdate() after its own stale-transfer cleanup.

Also drops a dead duplicate stale-node pruning loop: process1HzTasks()
had two back-to-back loops removing nodes past their last-seen
timeout, one using DRONECAN_NODE_STALE_TIMEOUT_MS (10000) and one
hardcoding the same value - the second could never find anything left
to prune since the first already removed it.

Cherry-picked from feature/dronecan-actuator-control (original commit
34aefbc00) directly onto feature/dronecan-dna-server's existing tip:
this is general dronecan.c restructuring in NodeStatus/node-table
territory - driven by dna_server.c's own need for the
dronecanGetNodeCount()/dronecanGetNode() accessors this introduces -
not actuator-control-specific, so it belongs here rather than riding
along with unrelated actuator-output work. Applied as a plain cherry-
pick on top of dna-server's unmodified history (not a full rebase of
its 34 commits onto the updated param-getset branch) to avoid
replaying real pre-existing history irregularities found partway
through an initial rebase attempt.

Note: the include-alphabetization part of the original commit's title
wasn't carried over - dna-server's current header block already has a
different structure (several includes above the USE_DRONECAN guard,
predating this change) than what the original diff assumed, so only
the new dronecan_node_status.h include was added without reordering
the rest.

Full unit test suite (dronecan_application_unittest 29/29,
dronecan_dna_server_unittest 16/16, no failures suite-wide) passes.
SITL builds clean with -Werror.
---
 src/main/CMakeLists.txt                       |   2 +
 src/main/drivers/dronecan/dronecan.c          | 139 +--------------
 .../drivers/dronecan/dronecan_node_status.c   | 167 ++++++++++++++++++
 .../drivers/dronecan/dronecan_node_status.h   |  26 +++
 src/test/unit/CMakeLists.txt                  |   1 +
 .../unit/dronecan_application_unittest.cc     |  24 +--
 6 files changed, 214 insertions(+), 145 deletions(-)
 create mode 100644 src/main/drivers/dronecan/dronecan_node_status.c
 create mode 100644 src/main/drivers/dronecan/dronecan_node_status.h

diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 7c91ad1f12f..2689c60a04b 100755
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -175,6 +175,8 @@ main_sources(COMMON_SRC
     drivers/dronecan/dronecan_async.h
     drivers/dronecan/dronecan_dna_server.c
     drivers/dronecan/dronecan_dna_server.h
+    drivers/dronecan/dronecan_node_status.c
+    drivers/dronecan/dronecan_node_status.h
     drivers/dronecan/dronecan.h
      
     drivers/display.c
diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 425e015b6da..7e60e7fd8dd 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -28,12 +28,12 @@
 #include <dronecan_msgs.h>
 #include "dronecan_async.h"
 #include "dronecan_dna_server.h"
+#include "dronecan_node_status.h"
 
 /* Private variables ---------------------------------------------------------*/
 
 CanardInstance canard; /* non-static: dronecan_async.c needs extern access */
 static uint8_t memory_pool[1024];
-static struct uavcan_protocol_NodeStatus node_status;
 
 PG_REGISTER_WITH_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig, PG_DRONECAN_CONFIG, 0);
 
@@ -46,15 +46,11 @@ PG_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig,
 static dronecanState_e dronecanState = STATE_DRONECAN_INIT;
 
 #ifdef UNIT_TEST
-uint8_t activeNodeCount = 0;
-dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES];
-static volatile uint32_t txErrCount = 0;
 static uint32_t busOffCount = 0;
-#else
-static uint8_t activeNodeCount = 0;
-static dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES];
 static volatile uint32_t txErrCount = 0;
+#else
 static uint32_t busOffCount = 0;
+static volatile uint32_t txErrCount = 0;
 #endif
 
 /* Forward declarations ------------------------------------------------------*/
@@ -63,7 +59,6 @@ static void processCanardTxQueueSafe(void);
 static void process1HzTasks(timeUs_t timestamp_usec);
 #ifdef UNIT_TEST
 bool shouldAcceptTransfer(const CanardInstance *ins, uint64_t *out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, uint8_t source_node_id);
-void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer);
 void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer);
 #else
 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);
@@ -233,11 +228,6 @@ dronecanState_e dronecanGetState(void)
     return dronecanState;
 }
 
-uint8_t dronecanGetNodeCount(void)
-{
-    return activeNodeCount;
-}
-
 uint32_t dronecanGetBitrateKbps(void)
 {
     switch (dronecanConfig()->bitRateKbps){
@@ -259,11 +249,6 @@ uint32_t dronecanGetBitrateKbps(void)
     }
 }
 
-const dronecanNodeInfo_t *dronecanGetNode(uint8_t index) {
-    if (index < activeNodeCount) return &nodeTable[index];
-    return NULL;
-}
-
 uint32_t dronecanGetBusOffCount(void)
 {
     return busOffCount;
@@ -345,66 +330,8 @@ static void processCanardTxQueueSafe(void) {
 // 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
 
-static dronecanNodeInfo_t *findNodeByID(uint8_t nodeID) {
-    for (uint8_t i = 0; i < activeNodeCount; i++) {
-        if (nodeTable[i].nodeID == nodeID) {
-            return &nodeTable[i];
-        }
-    }
-    return NULL;
-}
-
-const dronecanNodeInfo_t *dronecanGetNodeByID(uint8_t nodeID) {
-    return findNodeByID(nodeID);
-}
-
 // Canard Handlers and 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
- */
-static void send_NodeStatus(void) {
-    uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE];
-
-    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 = (uint16_t)(armingFlags & 0xFFFF);  /* field is 16-bit by UAVCAN spec; bits 16-30 of armingFlags are not transmitted */
-
-    uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer);
-
-    // we need a static variable for the transfer ID. This is
-    // incremented on each transfer, allowing for detection of packet
-    // loss
-    static uint8_t transfer_id;
-
-    int16_t bc_res;
-    ATOMIC_BLOCK(NVIC_PRIO_CAN) {
-        bc_res = canardBroadcast(&canard,
-                        UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
-                        UAVCAN_PROTOCOL_NODESTATUS_ID,
-                        &transfer_id,
-                        CANARD_TRANSFER_PRIORITY_LOW,
-                        buffer,
-                        len);
-    }
-    if (bc_res < 0) {
-        LOG_DEBUG(CAN, "NodeStatus broadcast failed: %d", bc_res);
-    }
-
-}
-
 /*
   This function is called at 1 Hz rate from the main loop.
 */
@@ -417,20 +344,7 @@ static void process1HzTasks(timeUs_t timestamp_usec)
         canardCleanupStaleTransfers(&canard, timestamp_usec);
     }
 
-    // Remove nodes that have stopped broadcasting NodeStatus
-    for (uint8_t i = 0; i < activeNodeCount; ) {
-        if (millis() - nodeTable[i].last_seen_ms > DRONECAN_NODE_STALE_TIMEOUT_MS) {
-            nodeTable[i] = nodeTable[activeNodeCount - 1];
-            activeNodeCount--;
-        } else {
-            i++;
-        }
-    }
-
-    /*
-      Transmit the node status message
-    */
-    send_NodeStatus();
+    dronecanNodeStatusUpdate(timestamp_usec);
 }
 
 /*
@@ -507,46 +421,6 @@ static bool shouldAcceptTransfer(const CanardInstance *ins,
 
 // Canard Handlers ( Many have code copied from libcanard esc_node example: https://github.com/dronecan/libcanard/blob/master/examples/ESCNode/esc_node.c )
 
-#ifdef UNIT_TEST
-void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) {
-#else
-static void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer) {
-#endif
-    UNUSED(ins);
-
-    struct uavcan_protocol_NodeStatus nodeStatus;
-
-	if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) {
-		LOG_WARNING(CAN, "NodeStatus decode failed");
-		return;
-	}
-
-	uint8_t nodeID = transfer->source_node_id;
-    dronecanNodeInfo_t *node = findNodeByID(nodeID);
-    if (node) {
-        node->health = nodeStatus.health;
-        node->mode = nodeStatus.mode;
-        node->uptime_sec = nodeStatus.uptime_sec;
-        node->vendor_status_code = nodeStatus.vendor_specific_status_code;
-        node->last_seen_ms = millis();
-        return;
-    }
-    // new node
-    if (activeNodeCount < DRONECAN_MAX_NODES) {
-        memset(&nodeTable[activeNodeCount], 0, sizeof(dronecanNodeInfo_t));
-        nodeTable[activeNodeCount].nodeID = nodeID;
-        nodeTable[activeNodeCount].health = nodeStatus.health;
-        nodeTable[activeNodeCount].mode = nodeStatus.mode;
-        nodeTable[activeNodeCount].uptime_sec = nodeStatus.uptime_sec;
-        nodeTable[activeNodeCount].vendor_status_code = nodeStatus.vendor_specific_status_code;
-        nodeTable[activeNodeCount].last_seen_ms = millis();
-        activeNodeCount++;
-
-    } else {
-        LOG_WARNING(CAN, "DroneCAN: node table full (%u nodes), ignoring node %u", DRONECAN_MAX_NODES, nodeID);
-    }
-}
-
 static void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer) {
 	UNUSED(ins);
     if (gpsConfig()->provider != GPS_DRONECAN) return;
@@ -616,8 +490,7 @@ static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer)
 
 	memset(&pkt, 0, sizeof(pkt));
 
-	node_status.uptime_sec = millis() / 1000ULL;
-	pkt.status = node_status;
+	pkt.status = dronecanGetOwnNodeStatus();
 
 	// fill in your major and minor firmware version
 	pkt.software_version.major = FC_VERSION_MAJOR;
@@ -682,7 +555,7 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
 		switch (transfer->data_type_id) {
 
             case UAVCAN_PROTOCOL_NODESTATUS_ID:
-                handle_NodeStatus(ins, transfer);
+                dronecanNodeStatusHandleBroadcast(ins, transfer);
                 break;
 
             case UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID:
diff --git a/src/main/drivers/dronecan/dronecan_node_status.c b/src/main/drivers/dronecan/dronecan_node_status.c
new file mode 100644
index 00000000000..35eee0244c7
--- /dev/null
+++ b/src/main/drivers/dronecan/dronecan_node_status.c
@@ -0,0 +1,167 @@
+#include "platform.h"
+#if defined(USE_DRONECAN)
+
+#include <stdint.h>
+#include <string.h>
+
+#include "build/atomic.h"
+
+#include "common/log.h"
+#include "common/time.h"
+#include "common/utils.h"
+
+#include "drivers/nvic.h"
+#include "drivers/time.h"
+
+#include "fc/runtime_config.h"
+
+#include "libcanard/canard.h"
+
+#include "sensors/diagnostics.h"
+
+#include <dronecan_msgs.h>
+
+#include "dronecan.h"
+#include "dronecan_node_status.h"
+
+extern CanardInstance canard; /* the FC's own canard instance, owned by dronecan.c */
+
+static struct uavcan_protocol_NodeStatus node_status;
+
+#ifdef UNIT_TEST
+uint8_t activeNodeCount = 0;
+dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES];
+#else
+static uint8_t activeNodeCount = 0;
+static dronecanNodeInfo_t nodeTable[DRONECAN_MAX_NODES];
+#endif
+
+static dronecanNodeInfo_t *findNodeByID(uint8_t nodeID)
+{
+    for (uint8_t i = 0; i < activeNodeCount; i++) {
+        if (nodeTable[i].nodeID == nodeID) {
+            return &nodeTable[i];
+        }
+    }
+    return NULL;
+}
+
+uint8_t dronecanGetNodeCount(void)
+{
+    return activeNodeCount;
+}
+
+const dronecanNodeInfo_t *dronecanGetNode(uint8_t index)
+{
+    if (index < activeNodeCount) return &nodeTable[index];
+    return NULL;
+}
+
+const dronecanNodeInfo_t *dronecanGetNodeByID(uint8_t nodeID)
+{
+    return findNodeByID(nodeID);
+}
+
+void dronecanNodeStatusHandleBroadcast(CanardInstance *ins, CanardRxTransfer *transfer)
+{
+    UNUSED(ins);
+
+    struct uavcan_protocol_NodeStatus nodeStatus;
+
+    if (uavcan_protocol_NodeStatus_decode(transfer, &nodeStatus)) {
+        LOG_WARNING(CAN, "NodeStatus decode failed");
+        return;
+    }
+
+    uint8_t nodeID = transfer->source_node_id;
+    dronecanNodeInfo_t *node = findNodeByID(nodeID);
+    if (node) {
+        node->health = nodeStatus.health;
+        node->mode = nodeStatus.mode;
+        node->uptime_sec = nodeStatus.uptime_sec;
+        node->vendor_status_code = nodeStatus.vendor_specific_status_code;
+        node->last_seen_ms = millis();
+        return;
+    }
+    // new node
+    if (activeNodeCount < DRONECAN_MAX_NODES) {
+        memset(&nodeTable[activeNodeCount], 0, sizeof(dronecanNodeInfo_t));
+        nodeTable[activeNodeCount].nodeID = nodeID;
+        nodeTable[activeNodeCount].health = nodeStatus.health;
+        nodeTable[activeNodeCount].mode = nodeStatus.mode;
+        nodeTable[activeNodeCount].uptime_sec = nodeStatus.uptime_sec;
+        nodeTable[activeNodeCount].vendor_status_code = nodeStatus.vendor_specific_status_code;
+        nodeTable[activeNodeCount].last_seen_ms = millis();
+        activeNodeCount++;
+    } else {
+        LOG_WARNING(CAN, "DroneCAN: node table full (%u nodes), ignoring node %u", DRONECAN_MAX_NODES, nodeID);
+    }
+}
+
+struct uavcan_protocol_NodeStatus dronecanGetOwnNodeStatus(void)
+{
+    node_status.uptime_sec = millis() / 1000ULL;
+    return 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];
+
+    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 = (uint16_t)(armingFlags & 0xFFFF);  /* field is 16-bit by UAVCAN spec; bits 16-30 of armingFlags are not transmitted */
+
+    uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer);
+
+    // we need a static variable for the transfer ID. This is
+    // incremented on each transfer, allowing for detection of packet
+    // loss
+    static uint8_t transfer_id;
+
+    int16_t bc_res;
+    ATOMIC_BLOCK(NVIC_PRIO_CAN) {
+        bc_res = canardBroadcast(&canard,
+                        UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE,
+                        UAVCAN_PROTOCOL_NODESTATUS_ID,
+                        &transfer_id,
+                        CANARD_TRANSFER_PRIORITY_LOW,
+                        buffer,
+                        len);
+    }
+    if (bc_res < 0) {
+        LOG_WARNING(CAN, "NodeStatus broadcast failed: %d", bc_res);
+    }
+}
+
+void dronecanNodeStatusUpdate(timeUs_t timestamp_usec)
+{
+    UNUSED(timestamp_usec);
+
+    // Remove nodes that have stopped broadcasting NodeStatus
+    for (uint8_t i = 0; i < activeNodeCount; ) {
+        if (millis() - nodeTable[i].last_seen_ms > DRONECAN_NODE_STALE_TIMEOUT_MS) {
+            nodeTable[i] = nodeTable[activeNodeCount - 1];
+            activeNodeCount--;
+        } else {
+            i++;
+        }
+    }
+
+    send_NodeStatus();
+}
+
+#endif // USE_DRONECAN
diff --git a/src/main/drivers/dronecan/dronecan_node_status.h b/src/main/drivers/dronecan/dronecan_node_status.h
new file mode 100644
index 00000000000..c0c4cf84255
--- /dev/null
+++ b/src/main/drivers/dronecan/dronecan_node_status.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include "common/time.h"
+#include "libcanard/canard.h"
+
+#ifdef USE_DRONECAN
+
+#include <dronecan_msgs.h>
+
+/* Dispatch target for incoming NodeStatus broadcasts from other nodes -
+   called from dronecan.c's onTransferReceived(). Updates the live node
+   table also consulted by dronecan_dna_server.c (via dronecanGetNode(),
+   declared in dronecan.h) to avoid handing out node IDs already seen on
+   the bus. */
+void dronecanNodeStatusHandleBroadcast(CanardInstance *ins, CanardRxTransfer *transfer);
+
+/* Broadcasts our own NodeStatus heartbeat and prunes nodes we haven't
+   heard a NodeStatus from in DRONECAN_NODE_STALE_TIMEOUT_MS. Called once
+   per second from dronecan.c's process1HzTasks(). */
+void dronecanNodeStatusUpdate(timeUs_t timestamp_usec);
+
+/* Our own current NodeStatus (uptime refreshed on each call), for
+   dronecan.c's handle_GetNodeInfo() to embed in a GetNodeInfo response. */
+struct uavcan_protocol_NodeStatus dronecanGetOwnNodeStatus(void);
+
+#endif // USE_DRONECAN
diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt
index 4d66e70b7c2..e09c9c660cf 100644
--- a/src/test/unit/CMakeLists.txt
+++ b/src/test/unit/CMakeLists.txt
@@ -72,6 +72,7 @@ set_property(SOURCE dronecan_getnodeinfo_unittest.cc PROPERTY definitions USE_DR
 set_property(SOURCE dronecan_application_unittest.cc PROPERTY depends
     "drivers/dronecan/dronecan.c"
     "drivers/dronecan/dronecan_async.c"
+    "drivers/dronecan/dronecan_node_status.c"
     "drivers/dronecan/dronecan_dna_server.c"
     "drivers/dronecan/libcanard/canard.c")
 set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_sources
diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc
index b27cfa0d1da..463ffb0a68c 100644
--- a/src/test/unit/dronecan_application_unittest.cc
+++ b/src/test/unit/dronecan_application_unittest.cc
@@ -51,7 +51,7 @@ extern uint8_t activeNodeCount;
 extern dronecanNodeInfo_t nodeTable[];
 
 /* Private functions not exposed in dronecan.h */
-void handle_NodeStatus(CanardInstance *ins, CanardRxTransfer *transfer);
+void dronecanNodeStatusHandleBroadcast(CanardInstance *ins, CanardRxTransfer *transfer);
 bool shouldAcceptTransfer(const CanardInstance *ins,
                           uint64_t *out_data_type_signature,
                           uint16_t data_type_id,
@@ -233,7 +233,7 @@ TEST_F(DroneCANNodeTableTest, NewNodeAddedOnFirstStatus)
             UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK,
             UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL,
             0xABCD, buf);
-    handle_NodeStatus(&ins, &xfer);
+    dronecanNodeStatusHandleBroadcast(&ins, &xfer);
 
     EXPECT_EQ(dronecanGetNodeCount(), 1u);
 
@@ -250,9 +250,9 @@ TEST_F(DroneCANNodeTableTest, NewNodeAddedOnFirstStatus)
 TEST_F(DroneCANNodeTableTest, TwoDistinctNodesStoredSeparately)
 {
     CanardRxTransfer x1 = makeNodeStatusTransfer(10, 100, 0, 0, 0, buf);
-    handle_NodeStatus(&ins, &x1);
+    dronecanNodeStatusHandleBroadcast(&ins, &x1);
     CanardRxTransfer x2 = makeNodeStatusTransfer(20, 200, 0, 0, 0, buf);
-    handle_NodeStatus(&ins, &x2);
+    dronecanNodeStatusHandleBroadcast(&ins, &x2);
 
     EXPECT_EQ(dronecanGetNodeCount(), 2u);
     EXPECT_EQ(dronecanGetNode(0)->nodeID, 10u);
@@ -267,7 +267,7 @@ TEST_F(DroneCANNodeTableTest, ExistingNodeUpdatedInPlace)
             UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK,
             UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL,
             0x0000, buf);
-    handle_NodeStatus(&ins, &x1);
+    dronecanNodeStatusHandleBroadcast(&ins, &x1);
     ASSERT_EQ(dronecanGetNodeCount(), 1u);
 
     CanardRxTransfer x2 = makeNodeStatusTransfer(
@@ -275,7 +275,7 @@ TEST_F(DroneCANNodeTableTest, ExistingNodeUpdatedInPlace)
             UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING,
             UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE,
             0xBEEF, buf);
-    handle_NodeStatus(&ins, &x2);
+    dronecanNodeStatusHandleBroadcast(&ins, &x2);
 
     EXPECT_EQ(dronecanGetNodeCount(), 1u);   /* still one node */
 
@@ -292,7 +292,7 @@ TEST_F(DroneCANNodeTableTest, LastSeenMsFollowsMillis)
 {
     mock_time_ms = 1000;
     CanardRxTransfer x1 = makeNodeStatusTransfer(20, 10, 0, 0, 0, buf);
-    handle_NodeStatus(&ins, &x1);
+    dronecanNodeStatusHandleBroadcast(&ins, &x1);
 
     const dronecanNodeInfo_t *node = dronecanGetNode(0);
     ASSERT_NE(node, nullptr);
@@ -300,7 +300,7 @@ TEST_F(DroneCANNodeTableTest, LastSeenMsFollowsMillis)
 
     mock_time_ms = 2500;
     CanardRxTransfer x2 = makeNodeStatusTransfer(20, 20, 0, 0, 0, buf);
-    handle_NodeStatus(&ins, &x2);
+    dronecanNodeStatusHandleBroadcast(&ins, &x2);
 
     EXPECT_EQ(node->last_seen_ms, 2500u);
 }
@@ -310,7 +310,7 @@ TEST_F(DroneCANNodeTableTest, LastSeenMsSetOnInsert)
 {
     mock_time_ms = 9999;
     CanardRxTransfer xfer = makeNodeStatusTransfer(5, 0, 0, 0, 0, buf);
-    handle_NodeStatus(&ins, &xfer);
+    dronecanNodeStatusHandleBroadcast(&ins, &xfer);
 
     const dronecanNodeInfo_t *node = dronecanGetNode(0);
     ASSERT_NE(node, nullptr);
@@ -323,13 +323,13 @@ TEST_F(DroneCANNodeTableTest, TableFullNodeRejected)
 {
     for (uint8_t i = 1; i <= DRONECAN_MAX_NODES; i++) {
         CanardRxTransfer xfer = makeNodeStatusTransfer(i, 0, 0, 0, 0, buf);
-        handle_NodeStatus(&ins, &xfer);
+        dronecanNodeStatusHandleBroadcast(&ins, &xfer);
     }
     ASSERT_EQ(dronecanGetNodeCount(), (uint8_t)DRONECAN_MAX_NODES);
 
     /* Try to add a 33rd node (ID 100, not in 1..32) */
     CanardRxTransfer overflow = makeNodeStatusTransfer(100, 0, 0, 0, 0, buf);
-    handle_NodeStatus(&ins, &overflow);
+    dronecanNodeStatusHandleBroadcast(&ins, &overflow);
 
     EXPECT_EQ(dronecanGetNodeCount(), (uint8_t)DRONECAN_MAX_NODES);
 
@@ -493,7 +493,7 @@ TEST_F(DroneCANDispatchTest, GetNodeInfoResponsePopulatesAsyncSlot)
     /* Pre-insert node 42 via a NodeStatus (node table is independent of async slot) */
     uint8_t ns_buf[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE + 4];
     CanardRxTransfer ns_xfer = makeNodeStatusTransfer(42, 10, 0, 0, 0, ns_buf);
-    handle_NodeStatus(&ins, &ns_xfer);
+    dronecanNodeStatusHandleBroadcast(&ins, &ns_xfer);
     ASSERT_EQ(dronecanGetNodeCount(), 1u);
 
     /* Prime the async slot — handle_AsyncServiceResponse guards on state, service_id,

From 97a0368f41af1cc4da545e842350854c0a6234c6 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Mon, 17 Aug 2026 16:06:26 -0700
Subject: [PATCH 63/67] feat(dronecan): add GPS node-ID and battery-ID
 filtering with health guard

Add dronecan_gps_node_id and dronecan_battery_id settings so users with
multiple identical DroneCAN sensors (e.g. two batteries, two GPS units)
can pin the FC to a specific source node instead of accepting whichever
one happens to broadcast.

GPS side (new dronecanGpsAcceptSource() gate in gps_dronecan.c, shared
by the Fix2 and Auxiliary receive paths):
- With dronecan_gps_node_id unset (0), the first node to report Fix2
  data locks in as the active source ("first-over-fence"); all other
  nodes are rejected until the active one is evicted from the node
  table (stale NodeStatus timeout) or the filter is set.
- With dronecan_gps_node_id set, only that node's data is accepted,
  bypassing the lock so reconfiguring at runtime to a different node
  takes effect immediately rather than getting stuck on the old lock.
- A node reporting NodeStatus health >= ERROR is rejected even before
  it would otherwise lock in.
- dronecanGpsIsHealthy() (wired into io/gps.c's isGPSHealthy() for the
  GPS_DRONECAN provider) reports whether the currently-selected node
  (locked-in or statically filtered) is present and healthy, so
  arming/OSD reflect a degraded or absent CAN GPS.
- dronecanGpsOnNodeEvicted(), called from the stale-node purge in
  dronecan_node_status.c's dronecanNodeStatusUpdate(), releases the
  lock when the active node drops off the bus.
- No automatic failover to a second GPS node if the locked-in one
  degrades but keeps broadcasting NodeStatus: INAV has no general
  redundant-sensor story, and picking a winner between two live nodes
  needs its own design. Deliberately out of scope here (documented in
  dronecanGpsAcceptSource()'s comment).

Battery side: handle_BatteryInfo in dronecan.c now drops BatteryInfo
messages whose battery_id doesn't match dronecan_battery_id (0 = any).

dronecanConfig_t grows two fields (batteryId, gpsNodeId); PG version
bumped 0->1 per the struct-layout-change rule in
docs/development/settings/versioning-rules.md.

dronecanState and dronecanUpdate()'s 1Hz scheduler deadline
(next_1hz_service_at) move from function-local/static to file-scope,
exposed non-static under UNIT_TEST alongside the existing
activeNodeCount/nodeTable pattern - needed so
gps_dronecan_unittest.cc's end-to-end eviction test can reset them in
SetUp() for deterministic sequencing regardless of test order.

New gps_dronecan_unittest.cc links the real gps_dronecan.c against the
real dronecan.c (unlike dronecan_application_unittest.cc, which stubs
GNSS receive as no-ops) to test the filtering/lock/health-guard logic
end-to-end, including through the real dronecanUpdate() 1Hz task path.
15 tests (GPS-1..8, GPS-14..20 in the file's numbering - GPS-9..13,
covering parseGnssTime(), land in a separate commit alongside the
covariance/time-parsing fix they test).

Reconstructed against the current dronecan.c (which already has the
async-client, NodeStatus, and actuator-output extractions applied)
rather than cherry-picked, since the original commits' diffs were
obscured by dronecan_gps-health-guard's divergent copy of that
extraction work. Full relevant suite passes: gps_dronecan_unittest
15/15, dronecan_application_unittest 29/29, dronecan_dna_server_unittest
16/16, full suite 0 failures.
---
 src/main/CMakeLists.txt                       |   1 +
 src/main/drivers/dronecan/dronecan.c          |  21 +-
 src/main/drivers/dronecan/dronecan.h          |   2 +
 .../drivers/dronecan/dronecan_node_status.c   |   3 +
 src/main/fc/settings.yaml                     |  14 +
 src/main/io/gps.c                             |   6 +
 src/main/io/gps.h                             |   2 -
 src/main/io/gps_dronecan.c                    |  83 +++-
 src/main/io/gps_dronecan.h                    |  38 ++
 src/main/io/gps_private.h                     |   2 +
 src/test/unit/CMakeLists.txt                  |  39 ++
 .../unit/dronecan_application_unittest.cc     |   6 +-
 src/test/unit/gps_dronecan_unittest.cc        | 414 ++++++++++++++++++
 13 files changed, 618 insertions(+), 13 deletions(-)
 create mode 100644 src/main/io/gps_dronecan.h
 create mode 100644 src/test/unit/gps_dronecan_unittest.cc

diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 2689c60a04b..fc6dbf3efde 100755
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -577,6 +577,7 @@ main_sources(COMMON_SRC
     io/gps_msp.c
     io/gps_crsf.c
     io/gps_dronecan.c
+    io/gps_dronecan.h
     io/gps_fake.c
     io/gps_private.h
     io/ledstrip.c
diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 7e60e7fd8dd..0f83028d907 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -13,6 +13,7 @@
 #if defined(USE_DRONECAN)
 
 #include "io/gps.h"
+#include "io/gps_dronecan.h"
 #include "sensors/battery_sensor_dronecan.h"
 
 #include "config/parameter_group.h"
@@ -35,22 +36,26 @@
 CanardInstance canard; /* non-static: dronecan_async.c needs extern access */
 static uint8_t memory_pool[1024];
 
-PG_REGISTER_WITH_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig, PG_DRONECAN_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig, PG_DRONECAN_CONFIG, 1);
 
 PG_RESET_TEMPLATE(dronecanConfig_t, dronecanConfig,
     .nodeID = SETTING_DRONECAN_NODE_ID_DEFAULT,
     .bitRateKbps = SETTING_DRONECAN_BITRATE_KBPS_DEFAULT,
-    .dronecanUseDNAServer = SETTING_DRONECAN_USE_DNA_SERVER_DEFAULT
+    .dronecanUseDNAServer = SETTING_DRONECAN_USE_DNA_SERVER_DEFAULT,
+    .batteryId = SETTING_DRONECAN_BATTERY_ID_DEFAULT,
+    .gpsNodeId = SETTING_DRONECAN_GPS_NODE_ID_DEFAULT
 );
 
-static dronecanState_e dronecanState = STATE_DRONECAN_INIT;
-
 #ifdef UNIT_TEST
 static uint32_t busOffCount = 0;
 static volatile uint32_t txErrCount = 0;
+dronecanState_e dronecanState = STATE_DRONECAN_INIT;
+timeUs_t next_1hz_service_at = 0;
 #else
 static uint32_t busOffCount = 0;
 static volatile uint32_t txErrCount = 0;
+static dronecanState_e dronecanState = STATE_DRONECAN_INIT;
+static timeUs_t next_1hz_service_at = 0;
 #endif
 
 /* Forward declarations ------------------------------------------------------*/
@@ -125,7 +130,6 @@ void dronecanInit(void)
 
 void dronecanUpdate(timeUs_t currentTimeUs)
 {
-    static timeUs_t next_1hz_service_at = 0;
     static timeUs_t busoffTimeUs = 0;
     CanardCANFrame rx_frame;
     int numMessagesToProcess = 0;
@@ -430,7 +434,7 @@ static void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer
 		LOG_WARNING(CAN, "GNSSAuxiliary decode failed");
 		return;
 	}
-    dronecanGPSReceiveGNSSAuxiliary(&gnssAuxiliary);
+    dronecanGPSReceiveGNSSAuxiliary(&gnssAuxiliary, transfer->source_node_id);
 }
 
 static void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) {
@@ -454,7 +458,7 @@ static void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) {
 		LOG_WARNING(CAN, "GNSSFix2 decode failed");
 		return;
 	}
-    dronecanGPSReceiveGNSSFix2(&gnssFix2);
+    dronecanGPSReceiveGNSSFix2(&gnssFix2, transfer->source_node_id);
 }
 
 static void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) {
@@ -476,6 +480,9 @@ static void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer)
 		LOG_WARNING(CAN, "BatteryInfo decode failed");
 		return;
 	}
+    if (batteryInfo.battery_id != dronecanConfig()->batteryId) {
+        return;
+    }
     dronecanBatterySensorReceiveInfo(&batteryInfo);
 }
 
diff --git a/src/main/drivers/dronecan/dronecan.h b/src/main/drivers/dronecan/dronecan.h
index 7af7e0f392b..1558d010f46 100644
--- a/src/main/drivers/dronecan/dronecan.h
+++ b/src/main/drivers/dronecan/dronecan.h
@@ -26,6 +26,8 @@ typedef struct dronecanConfig_s {
     uint8_t nodeID;
     dronecanBitrate_e bitRateKbps;
     bool dronecanUseDNAServer;
+    uint8_t batteryId;
+    uint8_t gpsNodeId;
 } dronecanConfig_t;
 
 typedef struct dronecanNodeInfo_s {
diff --git a/src/main/drivers/dronecan/dronecan_node_status.c b/src/main/drivers/dronecan/dronecan_node_status.c
index 35eee0244c7..800ffe399f3 100644
--- a/src/main/drivers/dronecan/dronecan_node_status.c
+++ b/src/main/drivers/dronecan/dronecan_node_status.c
@@ -21,6 +21,8 @@
 
 #include <dronecan_msgs.h>
 
+#include "io/gps_dronecan.h"
+
 #include "dronecan.h"
 #include "dronecan_node_status.h"
 
@@ -154,6 +156,7 @@ void dronecanNodeStatusUpdate(timeUs_t timestamp_usec)
     // Remove nodes that have stopped broadcasting NodeStatus
     for (uint8_t i = 0; i < activeNodeCount; ) {
         if (millis() - nodeTable[i].last_seen_ms > DRONECAN_NODE_STALE_TIMEOUT_MS) {
+            dronecanGpsOnNodeEvicted(nodeTable[i].nodeID);
             nodeTable[i] = nodeTable[activeNodeCount - 1];
             activeNodeCount--;
         } else {
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 0760fdc0306..f0ea3462c72 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4648,3 +4648,17 @@ groups:
         type: bool
         default_value: ON
         field: dronecanUseDNAServer
+      - name: dronecan_battery_id
+        description: "Only accept BatteryInfo messages whose battery_id field (battery slot) matches this value. Every value 0-255 is a valid battery_id on the wire, so there is no wildcard; the default (0) matches the conventional primary-battery ID used by most peripherals."
+        default_value: 0
+        field: batteryId
+        min: 0
+        max: 255
+        type: uint8_t
+      - name: dronecan_gps_node_id
+        description: "Filter GPS messages by source Node ID. 0 = use any node."
+        default_value: 0
+        field: gpsNodeId
+        min: 0
+        max: 127
+        type: uint8_t
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index 3d2afe754b4..8e394645ae9 100755
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -56,6 +56,7 @@
 #include "io/gps.h"
 #include "io/gps_private.h"
 #include "io/gps_ublox.h"
+#include "io/gps_dronecan.h"
 
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
@@ -667,6 +668,11 @@ void updateGpsIndicator(timeUs_t currentTimeUs)
 
 bool isGPSHealthy(void)
 {
+#if defined(USE_DRONECAN)
+    if (gpsConfig()->provider == GPS_DRONECAN) {
+        return dronecanGpsIsHealthy();
+    }
+#endif
     return true;
 }
 
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index f02d9d8cc78..131f786bf68 100755
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -181,8 +181,6 @@ struct serialPort_s;
 void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort);
 void mspGPSReceiveNewData(const uint8_t * bufferPtr);
 void dronecanGPSReceiveGNSSFix(const struct uavcan_equipment_gnss_Fix * pgnssFix);
-void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 * pgnssFix2);
-void dronecanGPSReceiveGNSSAuxiliary(const struct uavcan_equipment_gnss_Auxiliary * pgnssAux);
 
 const char *getGpsHwVersion(void);
 uint8_t getGpsProtoMajorVersion(void);
diff --git a/src/main/io/gps_dronecan.c b/src/main/io/gps_dronecan.c
index 2c689d88724..43523bd8c90 100644
--- a/src/main/io/gps_dronecan.c
+++ b/src/main/io/gps_dronecan.c
@@ -50,12 +50,18 @@
 
 #include "io/gps.h"
 #include "io/gps_private.h"
+#include "io/gps_dronecan.h"
 #include "drivers/dronecan/dronecan.h"
 
 #include <dronecan_msgs.h>
 
 static bool newDataReady;
 static uint16_t lastHDOP = 9999;
+#ifdef UNIT_TEST
+uint8_t activeGpsNodeId = 0;
+#else
+static uint8_t activeGpsNodeId = 0;
+#endif
 
 void gpsRestartDronecan(void)
 {
@@ -129,8 +135,58 @@ void dronecanGPSReceiveGNSSFix(const struct uavcan_equipment_gnss_Fix * pgnssFix
     newDataReady = true;
 }
 
-void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 * pgnssFix2)
+/* Shared acceptance gate for GNSS Fix2/Auxiliary messages: rejects
+ * unhealthy nodes and non-matching gpsNodeId, then applies the
+ * first-over-fence lock so two GPS nodes can't race to write gpsSolDRV.
+ *
+ * No automatic failover to a second GPS node: if the locked-in node degrades
+ * to ERROR/CRITICAL health but keeps broadcasting NodeStatus (so it's never
+ * evicted by process1HzTasks()'s stale-node purge), its data - and any other
+ * node's data - stays rejected until it recovers or eventually goes silent
+ * long enough to be evicted. dronecanGpsIsHealthy() correctly reports
+ * unhealthy in this state, so arming/OSD reflect it, but there's no
+ * redundant-sensor handoff. INAV doesn't have a general redundant-sensor
+ * story, and picking one node over another when both are transmitting isn't
+ * something to improvise here - it needs its own design (how to detect which
+ * is actually reliable, how it's configured, etc). Deliberately out of scope
+ * for this health guard. */
+static bool dronecanGpsAcceptSource(uint8_t sourceNodeId)
 {
+    const dronecanNodeInfo_t *node = dronecanGetNodeByID(sourceNodeId);
+    // node == NULL means no NodeStatus has been received yet for this ID - deliberately
+    // NOT rejected here, unlike dronecanGpsIsHealthy() below, which returns false in that
+    // same case. This function decides whether to use incoming GPS data right now, so it
+    // fails open on unknown health (don't block real position updates just because
+    // NodeStatus, which may broadcast on a different cadence than Fix2, hasn't arrived
+    // yet). dronecanGpsIsHealthy() feeds arming/OSD/telemetry status, so it fails closed
+    // instead (don't report healthy without evidence). The asymmetry is intentional.
+    if (node && node->health >= UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR) {
+        return false;
+    }
+    if (dronecanConfig()->gpsNodeId != 0) {
+        // A configured static filter already uniquely selects the source,
+        // so skip the first-over-fence lock entirely - otherwise
+        // reconfiguring gpsNodeId at runtime to a node other than the one
+        // currently locked in would leave GPS stuck rejecting it forever.
+        if (sourceNodeId != dronecanConfig()->gpsNodeId) {
+            return false;
+        }
+        activeGpsNodeId = sourceNodeId;
+        return true;
+    }
+    if (activeGpsNodeId == 0) {
+        activeGpsNodeId = sourceNodeId;
+    } else if (sourceNodeId != activeGpsNodeId) {
+        return false;
+    }
+    return true;
+}
+
+void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 * pgnssFix2, uint8_t sourceNodeId)
+{
+    if (!dronecanGpsAcceptSource(sourceNodeId)) {
+        return;
+    }
     gpsSolDRV.fixType   = gpsMapFixType(pgnssFix2->status);
     gpsSolDRV.numSat    = pgnssFix2->sats_used;
     gpsSolDRV.llh.lon   = pgnssFix2->longitude_deg_1e8 / 10; // convert to deg_1e7
@@ -178,12 +234,35 @@ void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 * pgnssF
     newDataReady = true;
 }
 
-void dronecanGPSReceiveGNSSAuxiliary(const struct uavcan_equipment_gnss_Auxiliary * pgnssAux)
+void dronecanGPSReceiveGNSSAuxiliary(const struct uavcan_equipment_gnss_Auxiliary * pgnssAux, uint8_t sourceNodeId)
 {
+    if (!dronecanGpsAcceptSource(sourceNodeId)) {
+        return;
+    }
     // DroneCAN float16 optional fields encode NaN when unpopulated; guard before use.
     // gpsConstrainHDOP clamps to 9999 preventing uint16_t overflow for extreme DOP values.
     if (!isnan(pgnssAux->hdop)) {
         lastHDOP = gpsConstrainHDOP((uint32_t)(pgnssAux->hdop * 100));
     }
 }
+
+void dronecanGpsOnNodeEvicted(uint8_t nodeID)
+{
+    if (activeGpsNodeId == nodeID) {
+        activeGpsNodeId = 0;
+    }
+}
+
+bool dronecanGpsIsHealthy(void)
+{
+    uint8_t nodeId = (dronecanConfig()->gpsNodeId != 0) ? dronecanConfig()->gpsNodeId : activeGpsNodeId;
+    if (nodeId == 0) {
+        return false;
+    }
+    const dronecanNodeInfo_t *node = dronecanGetNodeByID(nodeId);
+    if (node == NULL) {
+        return false;
+    }
+    return node->health < UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR;
+}
 #endif
\ No newline at end of file
diff --git a/src/main/io/gps_dronecan.h b/src/main/io/gps_dronecan.h
new file mode 100644
index 00000000000..30cd36a1865
--- /dev/null
+++ b/src/main/io/gps_dronecan.h
@@ -0,0 +1,38 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#ifdef USE_DRONECAN
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <dronecan_msgs.h>
+
+void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 *pgnssFix2, uint8_t sourceNodeId);
+void dronecanGPSReceiveGNSSAuxiliary(const struct uavcan_equipment_gnss_Auxiliary *pgnssAux, uint8_t sourceNodeId);
+bool dronecanGpsIsHealthy(void);
+void dronecanGpsOnNodeEvicted(uint8_t nodeID);
+
+#endif
diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h
index cb46d6f784a..7c9e50a8ccf 100755
--- a/src/main/io/gps_private.h
+++ b/src/main/io/gps_private.h
@@ -96,8 +96,10 @@ extern void gpsFakeRestart(void);
 extern void gpsFakeHandle(void);
 #endif
 
+#ifdef USE_DRONECAN
 void gpsRestartDronecan(void);
 void gpsHandleDronecan(void);
+#endif
 
 
 #endif
diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt
index e09c9c660cf..8415f0c715a 100644
--- a/src/test/unit/CMakeLists.txt
+++ b/src/test/unit/CMakeLists.txt
@@ -115,6 +115,45 @@ set_property(SOURCE dronecan_dna_server_unittest.cc PROPERTY extra_includes
 set_property(SOURCE dronecan_dna_server_unittest.cc PROPERTY definitions
     USE_DRONECAN CANARD_ENABLE_TAO_OPTION=0)
 
+# DroneCAN GPS health guard tests - links the real gps_dronecan.c against the
+# real dronecan.c (node table / health lookups), unlike
+# dronecan_application_unittest.cc which stubs GNSS receive as no-ops.
+set_property(SOURCE gps_dronecan_unittest.cc PROPERTY depends
+    "drivers/dronecan/dronecan.c"
+    "drivers/dronecan/dronecan_async.c"
+    "drivers/dronecan/dronecan_node_status.c"
+    "drivers/dronecan/dronecan_dna_server.c"
+    "drivers/dronecan/libcanard/canard.c"
+    "io/gps_dronecan.c"
+    "common/maths.c")
+set_property(SOURCE gps_dronecan_unittest.cc PROPERTY extra_sources
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.NodeStatus.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.GetNodeInfo_res.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.GetNodeInfo_req.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.SoftwareVersion.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.HardwareVersion.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Fix.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Fix2.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Auxiliary.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.RTCMStream.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.power.BatteryInfo.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.Timestamp.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.GetSet_req.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.GetSet_res.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.ExecuteOpcode_req.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.ExecuteOpcode_res.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.RestartNode_req.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.RestartNode_res.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.Value.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.NumericValue.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.Empty.c"
+    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.dynamic_node_id.Allocation.c")
+set_property(SOURCE gps_dronecan_unittest.cc PROPERTY extra_includes
+    "../../lib/main/Dronecan/dsdlc_generated/include")
+set_property(SOURCE gps_dronecan_unittest.cc PROPERTY definitions
+    USE_DRONECAN CANARD_ENABLE_TAO_OPTION=0
+    FC_VERSION_MAJOR=10 FC_VERSION_MINOR=0 FC_VERSION_PATCH_LEVEL=0)
+
 # CAN bit-timing solver tests - links the real, HAL-free timing core shared
 # by the F7 (bxCAN) and H7 (FDCAN) drivers, so there is nothing to keep in sync
 set_property(SOURCE bxcan_timing_unittest.cc PROPERTY depends
diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc
index 463ffb0a68c..94261ab2391 100644
--- a/src/test/unit/dronecan_application_unittest.cc
+++ b/src/test/unit/dronecan_application_unittest.cc
@@ -37,6 +37,7 @@ extern "C" {
 /* INAV headers pulled in by dronecan.c — included here so the types are
    available when we define stub globals below. */
 #include "io/gps.h"
+#include "io/gps_dronecan.h"
 #include "sensors/battery_sensor_dronecan.h"
 #include "fc/runtime_config.h"
 #include "sensors/diagnostics.h"
@@ -88,8 +89,9 @@ void _logf(logTopic_e topic, unsigned level, const char *fmt, ...) { (void)topic
 
 /* GPS and battery DroneCAN receive stubs */
 void dronecanGPSReceiveGNSSFix(const struct uavcan_equipment_gnss_Fix *p) { (void)p; }
-void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 *p) { (void)p; }
-void dronecanGPSReceiveGNSSAuxiliary(const struct uavcan_equipment_gnss_Auxiliary *p) { (void)p; }
+void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 *p, uint8_t sourceNodeId) { (void)p; (void)sourceNodeId; }
+void dronecanGPSReceiveGNSSAuxiliary(const struct uavcan_equipment_gnss_Auxiliary *p, uint8_t sourceNodeId) { (void)p; (void)sourceNodeId; }
+void dronecanGpsOnNodeEvicted(uint8_t nodeID) { (void)nodeID; }
 void dronecanBatterySensorReceiveInfo(struct uavcan_equipment_power_BatteryInfo *p) { (void)p; }
 
 /* STM32 CAN driver stubs */
diff --git a/src/test/unit/gps_dronecan_unittest.cc b/src/test/unit/gps_dronecan_unittest.cc
new file mode 100644
index 00000000000..28009b4637a
--- /dev/null
+++ b/src/test/unit/gps_dronecan_unittest.cc
@@ -0,0 +1,414 @@
+/**
+ * DroneCAN GPS Health Guard Unit Tests
+ *
+ * Tests the real gps_dronecan.c compiled against the real dronecan.c (for
+ * the node table / health lookups) and INAV stubs. Exercises the parts of
+ * the "single active GPS node" guard that dronecan_application_unittest.cc
+ * cannot reach, because that file stubs out dronecanGPSReceiveGNSSFix2/
+ * Auxiliary as no-ops instead of linking the real gps_dronecan.c.
+ *
+ * Coverage:
+ *   GPS-1   First node to report Fix2 data locks in as the active node
+ *   GPS-2   A second node's Fix2 data is ignored while the first is active
+ *   GPS-3   dronecanGpsOnNodeEvicted() on the active node releases the lock,
+ *           allowing a different node's data through afterward
+ *   GPS-4   dronecanGpsOnNodeEvicted() on an unrelated node ID is a no-op —
+ *           the active lock is untouched
+ *   GPS-5   Data from a node whose NodeStatus health is ERROR is rejected
+ *           even before any node has locked in
+ *   GPS-6   dronecan_gps_node_id static filter rejects non-matching sources
+ *   GPS-7   dronecan_gps_node_id static filter accepts the configured source
+ *   GPS-8   Reconfiguring dronecan_gps_node_id at runtime to a different node
+ *           than the one currently locked in takes effect immediately
+ *   GPS-14  End-to-end: a stale node purged by process1HzTasks()'s real 1Hz
+ *           task releases the GPS lock, not just dronecanGpsOnNodeEvicted()
+ *           called directly
+ *   GPS-15  dronecanGpsIsHealthy(): no filter, no active node -> false
+ *   GPS-16  dronecanGpsIsHealthy(): no filter, healthy active node -> true
+ *   GPS-17  dronecanGpsIsHealthy(): no filter, active node evicted -> false
+ *   GPS-18  dronecanGpsIsHealthy(): filter configured, healthy node -> true
+ *   GPS-19  dronecanGpsIsHealthy(): filter configured, ERROR-health node -> false
+ *   GPS-20  dronecanGpsIsHealthy(): filter configured, node never seen -> false
+ *
+ * parseGnssTime() coverage (GPS-9 .. GPS-13) lives in a separate commit
+ * alongside the covariance/time-parsing fix it tests.
+ */
+
+#include "gtest/gtest.h"
+
+extern "C" {
+#include <stdint.h>
+#include <string.h>
+#include <stdbool.h>
+#include <time.h>
+
+#include "platform.h"
+
+/* DSDL types used by dronecan.c / gps_dronecan.c handlers */
+#include "uavcan.protocol.NodeStatus.h"
+#include "uavcan.protocol.GetNodeInfo.h"
+#include "uavcan.protocol.param.GetSet_res.h"
+#include "uavcan.protocol.param.ExecuteOpcode_res.h"
+#include "uavcan.protocol.RestartNode_res.h"
+#include "uavcan.equipment.gnss.Fix2.h"
+#include "uavcan.equipment.gnss.Auxiliary.h"
+
+/* Canard core and STM32 driver declarations */
+#include "drivers/dronecan/libcanard/canard.h"
+#include "drivers/dronecan/libcanard/canard_stm32_driver.h"
+
+/* INAV headers pulled in by dronecan.c / gps_dronecan.c — included here so
+   the types are available when we define stub globals below. */
+#include "io/gps.h"
+#include "io/gps_private.h"
+#include "io/gps_dronecan.h"
+#include "sensors/battery_sensor_dronecan.h"
+#include "fc/runtime_config.h"
+#include "sensors/diagnostics.h"
+#include "build/version.h"
+#include "common/log.h"
+
+/* Public API we test against */
+#include "drivers/dronecan/dronecan.h"
+
+/* Private state made non-static in UNIT_TEST builds */
+extern uint8_t activeNodeCount;
+extern dronecanNodeInfo_t nodeTable[];
+extern uint8_t activeGpsNodeId;
+extern dronecanState_e dronecanState;
+extern timeUs_t next_1hz_service_at;
+
+/* Private functions not exposed in dronecan.h, needed to populate node health */
+void dronecanNodeStatusHandleBroadcast(CanardInstance *ins, CanardRxTransfer *transfer);
+bool shouldAcceptTransfer(const CanardInstance *ins,
+                          uint64_t *out_data_type_signature,
+                          uint16_t data_type_id,
+                          CanardTransferType transfer_type,
+                          uint8_t source_node_id);
+void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer);
+
+/* =========================================================================
+ * Stubs — provide every symbol dronecan.c/gps_dronecan.c reference that
+ * isn't supplied by the compiled dependencies (dronecan.c, gps_dronecan.c,
+ * canard.c, common/maths.c, DSDL .c files).
+ * ========================================================================= */
+
+static uint32_t mock_time_ms = 0;
+uint32_t millis(void) { return mock_time_ms; }
+
+uint32_t armingFlags = 0;
+
+gpsConfig_t gpsConfig_System;
+gpsConfig_t gpsConfig_Copy;
+
+bool isHardwareHealthy(void) { return true; }
+
+/* GPS core is not under test here; gps_dronecan.c only needs a place to
+   write its solution and a couple of clamp helpers. Real gps.c is not
+   linked in to avoid pulling in the whole GPS subsystem. */
+gpsSolutionData_t gpsSolDRV;
+void gpsProcessNewDriverData(void) {}
+void gpsProcessNewSolutionData(bool timeout) { (void)timeout; }
+uint16_t gpsConstrainHDOP(uint32_t hdop) { return (uint16_t)(hdop > 9999 ? 9999 : hdop); }
+uint16_t gpsConstrainEPE(uint32_t epe)   { return (uint16_t)(epe > 9999 ? 9999 : epe); }
+
+/* Battery receive is not under test here; dronecan.c's handle_BatteryInfo
+   still needs a symbol to link against. */
+void dronecanBatterySensorReceiveInfo(struct uavcan_equipment_power_BatteryInfo *p) { (void)p; }
+
+/* Logging and config-save are no-ops; tests don't assert on either. */
+void _logf(logTopic_e topic, unsigned level, const char *fmt, ...) { (void)topic; (void)level; (void)fmt; }
+void saveConfig(void) {}
+
+/* STM32 CAN driver stubs */
+int16_t canardSTM32CAN1_Init(uint32_t b) { (void)b; return CANARD_OK; }
+int16_t canardSTM32Receive(CanardCANFrame *f) { (void)f; return 0; }
+uint32_t canardSTM32GetAndClearRxDropCount(void) { return 0; }
+int16_t canardSTM32Transmit(const CanardCANFrame *f) { (void)f; return 1; }
+void    canardSTM32GetProtocolStatus(canardProtocolStatus_t *s) { memset(s, 0, sizeof(*s)); }
+int32_t canardSTM32GetRxFifoFillLevel(void) { return 0; }
+void    canardSTM32RecoverFromBusOff(void) {}
+void    canardSTM32GetUniqueID(uint8_t id[16]) { memset(id, 0, 16); }
+
+const char* const shortGitRevision = "00000000";
+const char* const compilerVersion  = "test";
+const char* const targetName       = "TEST";
+const char* const buildDate        = "Jan 01 2026";
+const char* const buildTime        = "00:00:00";
+
+} /* extern "C" */
+
+/* =========================================================================
+ * Helpers
+ * ========================================================================= */
+
+static CanardRxTransfer makeNodeStatusTransfer(
+        uint8_t nodeId, uint8_t health, uint8_t *buf)
+{
+    struct uavcan_protocol_NodeStatus ns;
+    memset(&ns, 0, sizeof(ns));
+    ns.health = health;
+    ns.mode   = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
+
+    uint32_t len = uavcan_protocol_NodeStatus_encode(&ns, buf);
+
+    CanardRxTransfer xfer;
+    memset(&xfer, 0, sizeof(xfer));
+    xfer.transfer_type  = CanardTransferTypeBroadcast;
+    xfer.data_type_id   = UAVCAN_PROTOCOL_NODESTATUS_ID;
+    xfer.source_node_id = nodeId;
+    xfer.payload_head   = buf;
+    xfer.payload_len    = (uint16_t)len;
+    return xfer;
+}
+
+/* Builds a minimal Fix2 message whose latitude uniquely identifies which
+   call produced the resulting gpsSolDRV state. */
+static void makeFix2(int32_t latitude_deg_1e8, struct uavcan_equipment_gnss_Fix2 *out)
+{
+    memset(out, 0, sizeof(*out));
+    out->latitude_deg_1e8  = latitude_deg_1e8;
+    out->longitude_deg_1e8 = 0;
+    out->status            = 3; /* 3D fix */
+    out->sats_used          = 8;
+    out->gnss_timestamp.usec = UAVCAN_TIMESTAMP_UNKNOWN;
+}
+
+class DroneCANGpsHealthGuardTest : public ::testing::Test {
+protected:
+    uint8_t buf[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE + 4];
+
+    void SetUp() override {
+        activeNodeCount  = 0;
+        activeGpsNodeId  = 0;
+        memset(nodeTable, 0, sizeof(dronecanNodeInfo_t) * DRONECAN_MAX_NODES);
+        memset(&gpsSolDRV, 0, sizeof(gpsSolDRV));
+        dronecanConfigMutable()->gpsNodeId = 0;
+        mock_time_ms = 0;
+        dronecanState = STATE_DRONECAN_INIT;
+        next_1hz_service_at = 0;
+    }
+
+    /* CanardInstance is unused by dronecanNodeStatusHandleBroadcast; pass NULL like the
+       existing dronecan_application_unittest.cc does. */
+    void setNodeHealth(uint8_t nodeId, uint8_t health) {
+        CanardRxTransfer xfer = makeNodeStatusTransfer(nodeId, health, buf);
+        dronecanNodeStatusHandleBroadcast(nullptr, &xfer);
+    }
+
+    void sendFix2(uint8_t sourceNodeId, int32_t latitude_deg_1e8) {
+        struct uavcan_equipment_gnss_Fix2 fix;
+        makeFix2(latitude_deg_1e8, &fix);
+        dronecanGPSReceiveGNSSFix2(&fix, sourceNodeId);
+    }
+};
+
+/* GPS-1: First node to report locks in as the active node */
+TEST_F(DroneCANGpsHealthGuardTest, FirstNodeLocksIn)
+{
+    sendFix2(10, 400000000); /* 40.0 deg */
+
+    EXPECT_EQ(gpsSolDRV.llh.lat, 40000000);
+}
+
+/* GPS-2: A second node's data is ignored while the first is locked in */
+TEST_F(DroneCANGpsHealthGuardTest, SecondNodeRejectedWhileFirstActive)
+{
+    sendFix2(10, 400000000); /* locks node 10 in */
+    sendFix2(20, 500000000); /* should be ignored */
+
+    EXPECT_EQ(gpsSolDRV.llh.lat, 40000000) << "node 20's data must not overwrite node 10's fix";
+}
+
+/* GPS-3: Evicting the active node releases the lock for the next reporter */
+TEST_F(DroneCANGpsHealthGuardTest, EvictionReleasesLockForNewNode)
+{
+    sendFix2(10, 400000000);
+    sendFix2(20, 500000000); /* rejected, node 10 still locked */
+    ASSERT_EQ(gpsSolDRV.llh.lat, 40000000);
+
+    dronecanGpsOnNodeEvicted(10); /* node 10 dropped off the bus */
+    sendFix2(20, 500000000);      /* now accepted */
+
+    EXPECT_EQ(gpsSolDRV.llh.lat, 50000000);
+}
+
+/* GPS-4: Evicting a node that is not the active GPS node is a no-op —
+   directly covers the "what if it's not a GPS node that's evicted?"
+   case: dronecanGpsOnNodeEvicted must self-filter and leave the lock alone. */
+TEST_F(DroneCANGpsHealthGuardTest, EvictingUnrelatedNodeDoesNotUnlock)
+{
+    sendFix2(10, 400000000); /* node 10 locked in */
+
+    dronecanGpsOnNodeEvicted(99); /* some other (e.g. battery) node evicted */
+    sendFix2(20, 500000000);      /* node 10 is still locked; must be rejected */
+
+    EXPECT_EQ(gpsSolDRV.llh.lat, 40000000);
+}
+
+/* GPS-5: A node reporting HEALTH_ERROR is rejected even before any node
+   has locked in as the active GPS node. */
+TEST_F(DroneCANGpsHealthGuardTest, UnhealthyNodeRejectedBeforeLockingIn)
+{
+    setNodeHealth(30, UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR);
+
+    sendFix2(30, 400000000);
+
+    EXPECT_EQ(gpsSolDRV.llh.lat, 0) << "unhealthy node must not lock in or update gpsSolDRV";
+
+    /* A healthy node afterward must still be able to lock in normally —
+       proves the rejection above didn't leave activeGpsNodeId stuck. */
+    sendFix2(10, 500000000);
+    EXPECT_EQ(gpsSolDRV.llh.lat, 50000000);
+}
+
+/* GPS-6: dronecan_gps_node_id static filter rejects non-matching sources,
+   even for the first message seen (checked before the first-over-fence
+   lock logic). */
+TEST_F(DroneCANGpsHealthGuardTest, StaticNodeIdFilterRejectsNonMatchingSource)
+{
+    dronecanConfigMutable()->gpsNodeId = 15;
+
+    sendFix2(10, 400000000);
+
+    EXPECT_EQ(gpsSolDRV.llh.lat, 0) << "node 10 does not match the configured static gpsNodeId 15";
+}
+
+/* GPS-7: dronecan_gps_node_id static filter accepts the configured source */
+TEST_F(DroneCANGpsHealthGuardTest, StaticNodeIdFilterAcceptsMatchingSource)
+{
+    dronecanConfigMutable()->gpsNodeId = 15;
+
+    sendFix2(15, 400000000);
+
+    EXPECT_EQ(gpsSolDRV.llh.lat, 40000000);
+}
+
+/* GPS-8: Reconfiguring dronecan_gps_node_id at runtime to a node other than
+ * the one currently locked in by first-over-fence must take effect
+ * immediately, not get stuck rejecting the newly configured node forever.
+ * Node 10 locks in under accept-any (gpsNodeId=0), then the user
+ * reconfigures to node 20 without rebooting. */
+TEST_F(DroneCANGpsHealthGuardTest, ReconfiguringNodeIdWhileLockedToOtherNode)
+{
+    sendFix2(10, 400000000); /* locks in under accept-any (gpsNodeId=0) */
+    ASSERT_EQ(gpsSolDRV.llh.lat, 40000000);
+
+    dronecanConfigMutable()->gpsNodeId = 20; /* reconfigure without reboot */
+    sendFix2(20, 500000000);
+
+    EXPECT_EQ(gpsSolDRV.llh.lat, 50000000)
+        << "node 20 matches the newly configured gpsNodeId but is stuck rejected "
+           "because activeGpsNodeId was never released";
+}
+
+/* GPS-14: End-to-end wiring check. GPS-3 above calls dronecanGpsOnNodeEvicted()
+ * directly and only proves that function works in isolation - it would still
+ * pass even if process1HzTasks() never called it. This test drives the real
+ * path: a node goes stale in the node table, dronecanUpdate() runs its 1Hz
+ * task, and *that* eviction is what must release the GPS lock.
+ *
+ * dronecanState and the 1Hz scheduler's next_1hz_service_at are reset in
+ * SetUp() (exposed non-static under UNIT_TEST), so this test's sequencing
+ * from STATE_DRONECAN_INIT / next_1hz_service_at=0 holds regardless of test
+ * order, shuffling, or repetition.
+ */
+TEST_F(DroneCANGpsHealthGuardTest, StaleNodeEvictionThroughOneHzTaskReleasesGpsLock)
+{
+    dronecanInit(); /* sets up the real canard instance process1HzTasks uses */
+
+    setNodeHealth(10, UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK); /* last_seen_ms = 0 */
+    sendFix2(10, 400000000);
+    ASSERT_EQ(gpsSolDRV.llh.lat, 40000000);
+
+    dronecanUpdate(0); /* STATE_INIT -> STATE_NORMAL; schedules next 1Hz tick at 1,000,000us */
+
+    mock_time_ms = DRONECAN_NODE_STALE_TIMEOUT_MS + 1; /* node 10 is now stale */
+    dronecanUpdate(1000001); /* crosses the 1Hz boundary -> process1HzTasks() purges node 10 */
+
+    ASSERT_EQ(dronecanGetNodeCount(), 0u) << "stale node 10 must have been purged from the table";
+
+    sendFix2(20, 500000000); /* must be accepted now that node 10's eviction released the lock */
+
+    EXPECT_EQ(gpsSolDRV.llh.lat, 50000000)
+        << "process1HzTasks() evicted node 10 but never told the GPS layer, "
+           "so the lock was never released";
+}
+
+/* =========================================================================
+ * dronecanGpsIsHealthy() coverage (GPS-15 … GPS-20)
+ *
+ * None of the tests above ever call dronecanGpsIsHealthy() directly - the
+ * function the "health guard" is named for had no direct coverage at all.
+ * These pin its behavior across every filter-on/off x node
+ * present/absent/unhealthy/evicted combination.
+ * ========================================================================= */
+
+/* GPS-15: no filter, no node has ever locked in -> unhealthy */
+TEST_F(DroneCANGpsHealthGuardTest, IsHealthy_NoFilterNoActiveNode_ReturnsFalse)
+{
+    EXPECT_FALSE(dronecanGpsIsHealthy());
+}
+
+/* GPS-16: no filter, active node is healthy -> healthy */
+TEST_F(DroneCANGpsHealthGuardTest, IsHealthy_NoFilterHealthyActiveNode_ReturnsTrue)
+{
+    setNodeHealth(10, UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK);
+    sendFix2(10, 400000000); /* locks activeGpsNodeId = 10 */
+
+    EXPECT_TRUE(dronecanGpsIsHealthy());
+}
+
+/* GPS-17: no filter, active node evicted -> unhealthy again */
+TEST_F(DroneCANGpsHealthGuardTest, IsHealthy_NoFilterActiveNodeEvicted_ReturnsFalse)
+{
+    setNodeHealth(10, UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK);
+    sendFix2(10, 400000000);
+    ASSERT_TRUE(dronecanGpsIsHealthy());
+
+    dronecanGpsOnNodeEvicted(10);
+
+    EXPECT_FALSE(dronecanGpsIsHealthy());
+}
+
+/* GPS-18: static filter configured, configured node is healthy in the table
+ * -> healthy, regardless of whether any Fix2/Auxiliary message has been
+ * processed yet (dronecanGpsIsHealthy() checks the configured gpsNodeId
+ * directly, not activeGpsNodeId, when a filter is set). */
+TEST_F(DroneCANGpsHealthGuardTest, IsHealthy_FilterConfiguredHealthyNode_ReturnsTrue)
+{
+    dronecanConfigMutable()->gpsNodeId = 15;
+    setNodeHealth(15, UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK);
+
+    EXPECT_TRUE(dronecanGpsIsHealthy());
+}
+
+/* GPS-19: static filter configured, configured node reports ERROR health -> unhealthy.
+ * A different, healthy node (20) is planted as activeGpsNodeId first so this
+ * only passes if the configured gpsNodeId (15) is actually being consulted -
+ * if the code incorrectly fell back to activeGpsNodeId, it would see node 20
+ * (healthy) and wrongly return true instead of false. */
+TEST_F(DroneCANGpsHealthGuardTest, IsHealthy_FilterConfiguredErrorHealthNode_ReturnsFalse)
+{
+    setNodeHealth(20, UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK);
+    sendFix2(20, 100000000); /* locks activeGpsNodeId = 20, a healthy decoy */
+
+    dronecanConfigMutable()->gpsNodeId = 15;
+    setNodeHealth(15, UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR);
+
+    EXPECT_FALSE(dronecanGpsIsHealthy());
+}
+
+/* GPS-20: static filter configured, configured node has never sent NodeStatus
+ * -> unhealthy (fails conservative: no info means "can't confirm healthy",
+ * not "assume healthy"). Same healthy-decoy-on-activeGpsNodeId setup as
+ * GPS-19, so this can't pass merely because activeGpsNodeId defaults to 0. */
+TEST_F(DroneCANGpsHealthGuardTest, IsHealthy_FilterConfiguredNodeNeverSeen_ReturnsFalse)
+{
+    setNodeHealth(20, UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK);
+    sendFix2(20, 100000000); /* locks activeGpsNodeId = 20, a healthy decoy */
+
+    dronecanConfigMutable()->gpsNodeId = 15; /* node 15 never added to nodeTable */
+
+    EXPECT_FALSE(dronecanGpsIsHealthy());
+}

From ca6f3d7326eb80cc3b6f4cd7cd4370ff9095b3eb Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Mon, 17 Aug 2026 16:07:40 -0700
Subject: [PATCH 64/67] feat(dronecan): log node health-state transitions

Add logNodeHealth(), called from dronecanNodeStatusHandleBroadcast()
whenever a node's health changes (OK/WARNING/ERROR/CRITICAL, mapped to
LOG_INFO/WARNING/ERROR/ERROR) and once when a node is first seen. Pure
observability - no behavior change to node tracking, GPS filtering, or
anything else; the health value written to nodeTable is unchanged.

Full relevant suite passes unchanged: dronecan_application_unittest
29/29, gps_dronecan_unittest 15/15, dronecan_dna_server_unittest 16/16.
---
 .../drivers/dronecan/dronecan_node_status.c   | 25 +++++++++++++++++++
 1 file changed, 25 insertions(+)

diff --git a/src/main/drivers/dronecan/dronecan_node_status.c b/src/main/drivers/dronecan/dronecan_node_status.c
index 800ffe399f3..c8f672cd30f 100644
--- a/src/main/drivers/dronecan/dronecan_node_status.c
+++ b/src/main/drivers/dronecan/dronecan_node_status.c
@@ -64,6 +64,27 @@ const dronecanNodeInfo_t *dronecanGetNodeByID(uint8_t nodeID)
     return findNodeByID(nodeID);
 }
 
+static void logNodeHealth(uint8_t nodeID, uint8_t health)
+{
+    UNUSED(nodeID); // only referenced inside LOG_* macros, which expand to nothing when USE_LOG is undefined
+    switch (health) {
+    case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK:
+        LOG_INFO(CAN, "Node %d health: OK", nodeID);
+        break;
+    case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_WARNING:
+        LOG_WARNING(CAN, "Node %d health: WARNING", nodeID);
+        break;
+    case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR:
+        LOG_ERROR(CAN, "Node %d health: ERROR", nodeID);
+        break;
+    case UAVCAN_PROTOCOL_NODESTATUS_HEALTH_CRITICAL:
+        LOG_ERROR(CAN, "Node %d health: CRITICAL", nodeID);
+        break;
+    default:
+        break;
+    }
+}
+
 void dronecanNodeStatusHandleBroadcast(CanardInstance *ins, CanardRxTransfer *transfer)
 {
     UNUSED(ins);
@@ -78,6 +99,9 @@ void dronecanNodeStatusHandleBroadcast(CanardInstance *ins, CanardRxTransfer *tr
     uint8_t nodeID = transfer->source_node_id;
     dronecanNodeInfo_t *node = findNodeByID(nodeID);
     if (node) {
+        if (node->health != nodeStatus.health) {
+            logNodeHealth(nodeID, nodeStatus.health);
+        }
         node->health = nodeStatus.health;
         node->mode = nodeStatus.mode;
         node->uptime_sec = nodeStatus.uptime_sec;
@@ -94,6 +118,7 @@ void dronecanNodeStatusHandleBroadcast(CanardInstance *ins, CanardRxTransfer *tr
         nodeTable[activeNodeCount].uptime_sec = nodeStatus.uptime_sec;
         nodeTable[activeNodeCount].vendor_status_code = nodeStatus.vendor_specific_status_code;
         nodeTable[activeNodeCount].last_seen_ms = millis();
+        logNodeHealth(nodeID, nodeStatus.health);
         activeNodeCount++;
     } else {
         LOG_WARNING(CAN, "DroneCAN: node table full (%u nodes), ignoring node %u", DRONECAN_MAX_NODES, nodeID);

From c64221a92e17204abb39cf81769c2fe4a6645adf Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Mon, 17 Aug 2026 16:12:15 -0700
Subject: [PATCH 65/67] fix(dronecan): correct GNSS Fix2 covariance parsing,
 add GNSS time parsing
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Two independent data-quality fixes to dronecanGPSReceiveGNSSFix2(),
unrelated to node filtering:

Covariance (EPH/EPV): the DSDL doesn't specify Fix2's covariance array
layout. The old code treated it as a 6-element upper-triangular
position covariance matrix (indices [0]/[2]/[5] as x/y/z variance,
summing x+y for horizontal). AP_Periph - the dominant DroneCAN
peripheral firmware - actually packs it as
[0]=[1]=hacc², [2]=vacc², [3]=[4]=[5]=sacc² (verified against live
hardware data: [0]==[1] and [3]==[4]==[5] as expected from AP_Periph's
source). Read index [0] directly for horizontal accuracy and [2] for
vertical, and lower the length guard from >=6 to >=3 to match.

GNSS time: gpsSolDRV.flags.validTime was hardcoded to 0 with the
actual field-population code commented out as dead TODOs. Add
parseGnssTime(), which converts the DSDL's gnss_timestamp (UTC/GPS/TAI
standard, microseconds since each standard's own epoch) to
gpsSolDRV.time via gmtime(). GPS and TAI standards need
num_leap_seconds to compute the UTC offset (GPS: UTC = GPS -
leap_seconds + 9; TAI: UTC = TAI - leap_seconds - 10, per the DSDL
comment) and reject rather than guess when num_leap_seconds is
UNKNOWN; UAVCAN_TIMESTAMP_UNKNOWN rejects immediately regardless of
standard.

Adds GPS-9..13 to gps_dronecan_unittest.cc: UTC/GPS/TAI conversion,
both leap-second-unknown rejection paths, and the unknown-timestamp
rejection - independently deriving expected calendar fields via
gmtime() on the same epoch formula rather than hand-transcribing
dates, so a rederivation error can't accidentally match a matching bug
in parseGnssTime() itself. Full suite: gps_dronecan_unittest 20/20
(all of GPS-1..20 now present), dronecan_application_unittest 29/29,
dronecan_dna_server_unittest 16/16, 0 failures suite-wide.
---
 src/main/io/gps_dronecan.c             |  80 +++++++++++++-----
 src/test/unit/gps_dronecan_unittest.cc | 107 ++++++++++++++++++++++++-
 2 files changed, 165 insertions(+), 22 deletions(-)

diff --git a/src/main/io/gps_dronecan.c b/src/main/io/gps_dronecan.c
index 43523bd8c90..721ab45ca7d 100644
--- a/src/main/io/gps_dronecan.c
+++ b/src/main/io/gps_dronecan.c
@@ -27,6 +27,7 @@
 #include <ctype.h>
 #include <string.h>
 #include <math.h>
+#include <time.h>
 
 #include "platform.h"
 #include "build/build_config.h"
@@ -55,6 +56,8 @@
 
 #include <dronecan_msgs.h>
 
+static void parseGnssTime(uint64_t usec, uint8_t time_standard, uint8_t num_leap_seconds);
+
 static bool newDataReady;
 static uint16_t lastHDOP = 9999;
 #ifdef UNIT_TEST
@@ -201,8 +204,7 @@ void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 * pgnssF
         groundCourse += 2 * M_PIf;
     }
     gpsSolDRV.groundCourse = RADIANS_TO_DECIDEGREES(groundCourse);
-    // TODO where to get EPH gpsSolDRV.eph = gpsConstrainEPE(pgnssFix-> / 10);
-    // TODO where to get EPV gpsSolDRV.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);
+
     if (pgnssFix2->pdop > 0){
         gpsSolDRV.hdop = gpsConstrainHDOP(pgnssFix2->pdop * 100); // Only update if valid.
     } else if((9999 > lastHDOP) && (lastHDOP > 0)) {
@@ -211,24 +213,17 @@ void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 * pgnssF
     gpsSolDRV.flags.validVelNE = true;
     gpsSolDRV.flags.validVelD = true;
     gpsSolDRV.flags.validEPE = false;  // assume invalid unless the covariance is filled in.
-    if (pgnssFix2->covariance.len >= 6) {
-        float var_x = pgnssFix2->covariance.data[0];  // meters²
-        float var_y = pgnssFix2->covariance.data[2];  // meters²
-        float var_z = pgnssFix2->covariance.data[5];  // meters²
-
-        gpsSolDRV.eph = gpsConstrainEPE((uint32_t)(sqrtf(var_x + var_y) * 100));  // cm
-        gpsSolDRV.epv = gpsConstrainEPE((uint32_t)(sqrtf(var_z) * 100));          // cm
+    // Fix2 covariance layout is not specified by the DSDL. AP_Periph (the dominant peripheral
+    // firmware) packs: [0]=[1]=hacc², [2]=vacc², [3]=[4]=[5]=sacc²  (m² / (m/s)²).
+    // Verified against live data: [0]==[1] and [3]==[4]==[5] as expected from AP_Periph source.
+    if (pgnssFix2->covariance.len >= 3) {
+        gpsSolDRV.eph = gpsConstrainEPE((uint32_t)(sqrtf(pgnssFix2->covariance.data[0]) * 100));
+        gpsSolDRV.epv = gpsConstrainEPE((uint32_t)(sqrtf(pgnssFix2->covariance.data[2]) * 100));
         gpsSolDRV.flags.validEPE = true;
-    } 
-    // gpsSolDRV.time.year   = pkt->year;
-    // gpsSolDRV.time.month  = pkt->month;
-    // gpsSolDRV.time.day    = pkt->day;
-    // gpsSolDRV.time.hours  = pkt->hour;
-    // gpsSolDRV.time.minutes = pkt->min;
-    // gpsSolDRV.time.seconds = pkt->sec;
-    // gpsSolDRV.time.millis  = 0;
-
-    gpsSolDRV.flags.validTime = 0; //(pkt->fixType >= 3);
+    }
+    parseGnssTime(pgnssFix2->gnss_timestamp.usec,
+                  pgnssFix2->gnss_time_standard,
+                  pgnssFix2->num_leap_seconds);
 
     gpsProcessNewDriverData();
     newDataReady = true;
@@ -265,4 +260,51 @@ bool dronecanGpsIsHealthy(void)
     }
     return node->health < UAVCAN_PROTOCOL_NODESTATUS_HEALTH_ERROR;
 }
+
+static void parseGnssTime(uint64_t usec, uint8_t time_standard, uint8_t num_leap_seconds)
+{
+    if (usec == UAVCAN_TIMESTAMP_UNKNOWN) {
+        gpsSolDRV.flags.validTime = false;
+        return;
+    }
+    time_t unix_s;
+    switch (time_standard) {
+    case UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_UTC:
+        unix_s = (time_t)(usec / 1000000ULL);
+        break;
+    case UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_GPS:
+        if (num_leap_seconds == UAVCAN_EQUIPMENT_GNSS_FIX2_NUM_LEAP_SECONDS_UNKNOWN) {
+            gpsSolDRV.flags.validTime = false;
+            return;
+        }
+        // DSDL: GPS epoch is µs since GPS time at UTC 1970-01-01; UTC = GPS - leap_seconds + 9
+        unix_s = (time_t)(usec / 1000000ULL) - num_leap_seconds + 9;
+        break;
+    case UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_TAI:
+        if (num_leap_seconds == UAVCAN_EQUIPMENT_GNSS_FIX2_NUM_LEAP_SECONDS_UNKNOWN) {
+            gpsSolDRV.flags.validTime = false;
+            return;
+        }
+        // DSDL: TAI epoch is µs since TAI time at UTC 1970-01-01; UTC = TAI - leap_seconds - 10
+        unix_s = (time_t)(usec / 1000000ULL) - num_leap_seconds - 10;
+        break;
+    default:
+        gpsSolDRV.flags.validTime = false;
+        return;
+    }
+    struct tm *t = gmtime(&unix_s);
+    if (!t) {
+        gpsSolDRV.flags.validTime = false;
+        return;
+    }
+    gpsSolDRV.time.year    = (uint16_t)(t->tm_year + 1900);
+    gpsSolDRV.time.month   = (uint8_t)(t->tm_mon + 1);
+    gpsSolDRV.time.day     = (uint8_t)t->tm_mday;
+    gpsSolDRV.time.hours   = (uint8_t)t->tm_hour;
+    gpsSolDRV.time.minutes = (uint8_t)t->tm_min;
+    gpsSolDRV.time.seconds = (uint8_t)t->tm_sec;
+    gpsSolDRV.time.millis  = (uint16_t)((usec % 1000000ULL) / 1000ULL);
+    gpsSolDRV.flags.validTime = true;
+}
+
 #endif
\ No newline at end of file
diff --git a/src/test/unit/gps_dronecan_unittest.cc b/src/test/unit/gps_dronecan_unittest.cc
index 28009b4637a..fed92931de6 100644
--- a/src/test/unit/gps_dronecan_unittest.cc
+++ b/src/test/unit/gps_dronecan_unittest.cc
@@ -20,6 +20,11 @@
  *   GPS-7   dronecan_gps_node_id static filter accepts the configured source
  *   GPS-8   Reconfiguring dronecan_gps_node_id at runtime to a different node
  *           than the one currently locked in takes effect immediately
+ *   GPS-9   parseGnssTime(): UTC standard conversion
+ *   GPS-10  parseGnssTime(): GPS standard leap-second offset
+ *   GPS-11  parseGnssTime(): TAI standard leap-second offset
+ *   GPS-12  parseGnssTime(): GPS/TAI standards reject NUM_LEAP_SECONDS_UNKNOWN
+ *   GPS-13  parseGnssTime(): UAVCAN_TIMESTAMP_UNKNOWN rejects immediately
  *   GPS-14  End-to-end: a stale node purged by process1HzTasks()'s real 1Hz
  *           task releases the GPS lock, not just dronecanGpsOnNodeEvicted()
  *           called directly
@@ -29,9 +34,6 @@
  *   GPS-18  dronecanGpsIsHealthy(): filter configured, healthy node -> true
  *   GPS-19  dronecanGpsIsHealthy(): filter configured, ERROR-health node -> false
  *   GPS-20  dronecanGpsIsHealthy(): filter configured, node never seen -> false
- *
- * parseGnssTime() coverage (GPS-9 .. GPS-13) lives in a separate commit
- * alongside the covariance/time-parsing fix it tests.
  */
 
 #include "gtest/gtest.h"
@@ -302,6 +304,105 @@ TEST_F(DroneCANGpsHealthGuardTest, ReconfiguringNodeIdWhileLockedToOtherNode)
            "because activeGpsNodeId was never released";
 }
 
+/* =========================================================================
+ * parseGnssTime coverage (GPS-9 … GPS-13)
+ *
+ * makeFix2() defaults gnss_timestamp.usec to UAVCAN_TIMESTAMP_UNKNOWN, so
+ * none of the tests above ever exercise the UTC/GPS/TAI conversion branches
+ * or the leap-second guards. These call dronecanGPSReceiveGNSSFix2()
+ * directly with a real timestamp and independently derive the expected
+ * calendar fields via gmtime() on the same epoch-conversion formula
+ * documented in the Fix2 DSDL, rather than hand-transcribing dates.
+ * ========================================================================= */
+
+static void expectGpsSolTimeMatches(time_t expected_unix_s)
+{
+    struct tm *t = gmtime(&expected_unix_s);
+    ASSERT_NE(t, nullptr);
+    EXPECT_TRUE(gpsSolDRV.flags.validTime);
+    EXPECT_EQ(gpsSolDRV.time.year,    (uint16_t)(t->tm_year + 1900));
+    EXPECT_EQ(gpsSolDRV.time.month,   (uint8_t)(t->tm_mon + 1));
+    EXPECT_EQ(gpsSolDRV.time.day,     (uint8_t)t->tm_mday);
+    EXPECT_EQ(gpsSolDRV.time.hours,   (uint8_t)t->tm_hour);
+    EXPECT_EQ(gpsSolDRV.time.minutes, (uint8_t)t->tm_min);
+    EXPECT_EQ(gpsSolDRV.time.seconds, (uint8_t)t->tm_sec);
+}
+
+/* GPS-9: UTC standard converts usec directly to unix seconds, no leap-second offset */
+TEST_F(DroneCANGpsHealthGuardTest, GnssTimeUtcStandardPopulatesDate)
+{
+    const uint64_t epoch_s = 1700000000ULL;
+    struct uavcan_equipment_gnss_Fix2 fix;
+    makeFix2(400000000, &fix);
+    fix.gnss_timestamp.usec = epoch_s * 1000000ULL;
+    fix.gnss_time_standard  = UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_UTC;
+    fix.num_leap_seconds    = UAVCAN_EQUIPMENT_GNSS_FIX2_NUM_LEAP_SECONDS_UNKNOWN; /* UTC ignores this field */
+
+    dronecanGPSReceiveGNSSFix2(&fix, 10);
+
+    expectGpsSolTimeMatches((time_t)epoch_s);
+}
+
+/* GPS-10: GPS standard applies UTC = GPS - leap_seconds + 9 per the DSDL comment */
+TEST_F(DroneCANGpsHealthGuardTest, GnssTimeGpsStandardAppliesLeapSecondOffset)
+{
+    const uint64_t gps_epoch_s = 1700000000ULL;
+    const uint8_t  leap_seconds = 18;
+    struct uavcan_equipment_gnss_Fix2 fix;
+    makeFix2(400000000, &fix);
+    fix.gnss_timestamp.usec = gps_epoch_s * 1000000ULL;
+    fix.gnss_time_standard  = UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_GPS;
+    fix.num_leap_seconds    = leap_seconds;
+
+    dronecanGPSReceiveGNSSFix2(&fix, 10);
+
+    expectGpsSolTimeMatches((time_t)(gps_epoch_s - leap_seconds + 9));
+}
+
+/* GPS-11: TAI standard applies UTC = TAI - leap_seconds - 10 per the DSDL comment */
+TEST_F(DroneCANGpsHealthGuardTest, GnssTimeTaiStandardAppliesLeapSecondOffset)
+{
+    const uint64_t tai_epoch_s = 1700000000ULL;
+    const uint8_t  leap_seconds = 18;
+    struct uavcan_equipment_gnss_Fix2 fix;
+    makeFix2(400000000, &fix);
+    fix.gnss_timestamp.usec = tai_epoch_s * 1000000ULL;
+    fix.gnss_time_standard  = UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_TAI;
+    fix.num_leap_seconds    = leap_seconds;
+
+    dronecanGPSReceiveGNSSFix2(&fix, 10);
+
+    expectGpsSolTimeMatches((time_t)(tai_epoch_s - leap_seconds - 10));
+}
+
+/* GPS-12: GPS/TAI standards require num_leap_seconds; UNKNOWN must reject rather
+ * than silently compute a wrong offset. */
+TEST_F(DroneCANGpsHealthGuardTest, GnssTimeGpsStandardWithUnknownLeapSecondsRejected)
+{
+    struct uavcan_equipment_gnss_Fix2 fix;
+    makeFix2(400000000, &fix);
+    fix.gnss_timestamp.usec = 1700000000ULL * 1000000ULL;
+    fix.gnss_time_standard  = UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_GPS;
+    fix.num_leap_seconds    = UAVCAN_EQUIPMENT_GNSS_FIX2_NUM_LEAP_SECONDS_UNKNOWN;
+
+    dronecanGPSReceiveGNSSFix2(&fix, 10);
+
+    EXPECT_FALSE(gpsSolDRV.flags.validTime);
+}
+
+/* GPS-13: usec == UAVCAN_TIMESTAMP_UNKNOWN rejects immediately, regardless of
+ * time_standard - this is the default makeFix2() takes in every other test,
+ * so pin it explicitly here rather than leaving it only implicit. */
+TEST_F(DroneCANGpsHealthGuardTest, GnssTimeUnknownTimestampRejected)
+{
+    struct uavcan_equipment_gnss_Fix2 fix;
+    makeFix2(400000000, &fix); /* gnss_timestamp.usec defaults to UAVCAN_TIMESTAMP_UNKNOWN */
+
+    dronecanGPSReceiveGNSSFix2(&fix, 10);
+
+    EXPECT_FALSE(gpsSolDRV.flags.validTime);
+}
+
 /* GPS-14: End-to-end wiring check. GPS-3 above calls dronecanGpsOnNodeEvicted()
  * directly and only proves that function works in isolation - it would still
  * pass even if process1HzTasks() never called it. This test drives the real

From 6ab50f866467ce6e40f7a3e67c8ed23779d8c336 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Mon, 17 Aug 2026 19:05:33 -0700
Subject: [PATCH 66/67] refactor(dronecan): drop legacy GNSS Fix support and
 dead RTCMStream handler

Legacy uavcan.equipment.gnss.Fix is superseded by Fix2, which every node
we support already sends. Rather than maintaining two parallel decode
paths, drop Fix entirely: keep accepting the transfer (so a node still
sending it doesn't trip an unhandled-transfer error) but log a one-time
deprecation warning instead of decoding it. dronecanGPSReceiveGNSSFix()
and its stale covariance/time-TODO code are removed along with it.

Also drop the uavcan.equipment.gnss.RTCMStream handler. It decoded
inbound RTCM correction data but never did anything with it - the FC
transmits RTCM corrections to GPS nodes, it does not receive them, so
the handler was dead code from the start.

Removing both handlers means dronecan.c and gps_dronecan.c no longer
reference the Fix/RTCMStream DSDL codecs, so the corresponding
extra_sources entries come out of the two unit test targets that link
dronecan.c/gps_dronecan.c directly (dronecan_messages_unittest and
dronecan_getnodeinfo_unittest still need them - they exercise the DSDL
codec itself, independent of dronecan.c).
---
 src/main/drivers/dronecan/dronecan.c          | 40 +++------------
 src/main/io/gps.h                             |  2 -
 src/main/io/gps_dronecan.c                    | 50 -------------------
 src/test/unit/CMakeLists.txt                  |  4 --
 .../unit/dronecan_application_unittest.cc     |  1 -
 5 files changed, 8 insertions(+), 89 deletions(-)

diff --git a/src/main/drivers/dronecan/dronecan.c b/src/main/drivers/dronecan/dronecan.c
index 0f83028d907..112f09533e5 100644
--- a/src/main/drivers/dronecan/dronecan.c
+++ b/src/main/drivers/dronecan/dronecan.c
@@ -412,9 +412,6 @@ static bool shouldAcceptTransfer(const CanardInstance *ins,
         case UAVCAN_EQUIPMENT_GNSS_FIX2_ID:
             *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE;
             return true;
-        case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID:
-            *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_SIGNATURE;
-            return true;
         case UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID:
             *out_data_type_signature = UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE;
             return true;
@@ -437,18 +434,6 @@ static void handle_GNSSAuxiliary(CanardInstance *ins, CanardRxTransfer *transfer
     dronecanGPSReceiveGNSSAuxiliary(&gnssAuxiliary, transfer->source_node_id);
 }
 
-static void handle_GNSSFix(CanardInstance *ins, CanardRxTransfer *transfer) {
-	UNUSED(ins);
-    if (gpsConfig()->provider != GPS_DRONECAN) return;
-    struct uavcan_equipment_gnss_Fix gnssFix;
-
-	if (uavcan_equipment_gnss_Fix_decode(transfer, &gnssFix)) {
-		LOG_WARNING(CAN, "GNSSFix decode failed");
-		return;
-	}
-    dronecanGPSReceiveGNSSFix(&gnssFix);
-}
-
 static void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) {
 	UNUSED(ins);
     if (gpsConfig()->provider != GPS_DRONECAN) return;
@@ -461,17 +446,6 @@ static void handle_GNSSFix2(CanardInstance *ins, CanardRxTransfer *transfer) {
     dronecanGPSReceiveGNSSFix2(&gnssFix2, transfer->source_node_id);
 }
 
-static void handle_GNSSRCTMStream(CanardInstance *ins, CanardRxTransfer *transfer) {
-	UNUSED(ins);
-    if (gpsConfig()->provider != GPS_DRONECAN) return;
-    struct uavcan_equipment_gnss_RTCMStream gnssRTCMStream;
-
-	if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &gnssRTCMStream)) {
-		LOG_WARNING(CAN, "RTCMStream decode failed");
-		return;
-	}
-}
-
 static void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
 	UNUSED(ins);
     struct uavcan_equipment_power_BatteryInfo batteryInfo;
@@ -577,18 +551,20 @@ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
                 handle_GNSSAuxiliary(ins, transfer);
                 break;
 
-            case UAVCAN_EQUIPMENT_GNSS_FIX_ID:
-                handle_GNSSFix(ins, transfer);
+            case UAVCAN_EQUIPMENT_GNSS_FIX_ID: {
+                static bool warned = false;
+                if (!warned) {
+                    LOG_WARNING(CAN, "Node %d: Fix (deprecated) ignored, node must send Fix2",
+                                transfer->source_node_id);
+                    warned = true;
+                }
                 break;
+            }
 
             case UAVCAN_EQUIPMENT_GNSS_FIX2_ID:
                 handle_GNSSFix2(ins, transfer);
                 break;
 
-            case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID:
-                handle_GNSSRCTMStream(ins, transfer);
-                break;
-
             case UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID:
                 handle_BatteryInfo(ins, transfer);
                 break;
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index 131f786bf68..c29217a0508 100755
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -21,7 +21,6 @@
 #include <time.h>
 
 #include "config/parameter_group.h"
-#include <dronecan_msgs.h>
 
 #include "common/time.h"
 
@@ -180,7 +179,6 @@ bool isGPSHeadingValid(void);
 struct serialPort_s;
 void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort);
 void mspGPSReceiveNewData(const uint8_t * bufferPtr);
-void dronecanGPSReceiveGNSSFix(const struct uavcan_equipment_gnss_Fix * pgnssFix);
 
 const char *getGpsHwVersion(void);
 uint8_t getGpsProtoMajorVersion(void);
diff --git a/src/main/io/gps_dronecan.c b/src/main/io/gps_dronecan.c
index 721ab45ca7d..941232a63f8 100644
--- a/src/main/io/gps_dronecan.c
+++ b/src/main/io/gps_dronecan.c
@@ -88,56 +88,6 @@ static uint8_t gpsMapFixType(uint8_t dronecanFixType)
     return GPS_NO_FIX;
 }
 
-void dronecanGPSReceiveGNSSFix(const struct uavcan_equipment_gnss_Fix * pgnssFix)
-{
-    gpsSolDRV.fixType   = gpsMapFixType(pgnssFix->status);
-    gpsSolDRV.numSat    = pgnssFix->sats_used;
-    gpsSolDRV.llh.lon   = pgnssFix->longitude_deg_1e8 / 10; // convert to deg_1e7
-    gpsSolDRV.llh.lat   = pgnssFix->latitude_deg_1e8 / 10; // convert to deg_1e7
-    gpsSolDRV.llh.alt   = pgnssFix->height_msl_mm / 10; // convert to cm
-    gpsSolDRV.velNED[X] = pgnssFix->ned_velocity[0] * 100; // Dronecan is North, East, Down
-    gpsSolDRV.velNED[Y] = pgnssFix->ned_velocity[1] * 100;
-    gpsSolDRV.velNED[Z] = pgnssFix->ned_velocity[2] * 100;
-    gpsSolDRV.groundSpeed = calc_length_pythagorean_2D((float)pgnssFix->ned_velocity[0], (float)pgnssFix->ned_velocity[1]) * 100;
-    float groundCourse = atan2_approx(pgnssFix->ned_velocity[1], pgnssFix->ned_velocity[0]); // atan2 returns [-M_PI, M_PI], with 0 indicating the vector points in the X direction
-    if (groundCourse < 0) {
-        groundCourse += 2 * M_PIf;
-    }
-    gpsSolDRV.groundCourse = RADIANS_TO_DECIDEGREES(groundCourse);
-    // TODO where to get EPH gpsSolDRV.eph = gpsConstrainEPE(pgnssFix-> / 10);
-    // TODO where to get EPV gpsSolDRV.epv = gpsConstrainEPE(pkt->verticalPosAccuracy / 10);
-    if(pgnssFix->pdop > 0){
-        gpsSolDRV.hdop = gpsConstrainHDOP(pgnssFix->pdop * 100);  // Only update if populated
-    } else if((9999 > lastHDOP) && (lastHDOP> 0)) {
-        gpsSolDRV.hdop = lastHDOP;
-    }
-    gpsSolDRV.flags.validVelNE = true;
-    gpsSolDRV.flags.validVelD = true;
-    gpsSolDRV.flags.validEPE = false;  // assume invalid unless the covariance is filled in.
-    if (pgnssFix->position_covariance.len >= 6) {
-        float var_x = pgnssFix->position_covariance.data[0];  // meters²
-        float var_y = pgnssFix->position_covariance.data[2];  // meters²
-        float var_z = pgnssFix->position_covariance.data[5];  // meters²
-
-        gpsSolDRV.eph = gpsConstrainEPE((uint32_t)(sqrtf(var_x + var_y) * 100));  // cm
-        gpsSolDRV.epv = gpsConstrainEPE((uint32_t)(sqrtf(var_z) * 100));          // cm
-        gpsSolDRV.flags.validEPE = true;
-    } 
-
-    // gpsSolDRV.time.year   = pkt->year;
-    // gpsSolDRV.time.month  = pkt->month;
-    // gpsSolDRV.time.day    = pkt->day;
-    // gpsSolDRV.time.hours  = pkt->hour;
-    // gpsSolDRV.time.minutes = pkt->min;
-    // gpsSolDRV.time.seconds = pkt->sec;
-    // gpsSolDRV.time.millis  = 0;
-
-    gpsSolDRV.flags.validTime = 0; //(pkt->fixType >= 3);
-
-    gpsProcessNewDriverData();
-    newDataReady = true;
-}
-
 /* Shared acceptance gate for GNSS Fix2/Auxiliary messages: rejects
  * unhealthy nodes and non-matching gpsNodeId, then applies the
  * first-over-fence lock so two GPS nodes can't race to write gpsSolDRV.
diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt
index 8415f0c715a..20f3590bfed 100644
--- a/src/test/unit/CMakeLists.txt
+++ b/src/test/unit/CMakeLists.txt
@@ -82,10 +82,8 @@ set_property(SOURCE dronecan_application_unittest.cc PROPERTY extra_sources
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.SoftwareVersion.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.HardwareVersion.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Fix2.c"
-    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Fix.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Auxiliary.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.power.BatteryInfo.c"
-    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.RTCMStream.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.Timestamp.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.ECEFPositionVelocity.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.GetSet_req.c"
@@ -132,10 +130,8 @@ set_property(SOURCE gps_dronecan_unittest.cc PROPERTY extra_sources
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.GetNodeInfo_req.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.SoftwareVersion.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.HardwareVersion.c"
-    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Fix.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Fix2.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.Auxiliary.c"
-    "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.gnss.RTCMStream.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.equipment.power.BatteryInfo.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.Timestamp.c"
     "../../lib/main/Dronecan/dsdlc_generated/src/uavcan.protocol.param.GetSet_req.c"
diff --git a/src/test/unit/dronecan_application_unittest.cc b/src/test/unit/dronecan_application_unittest.cc
index 94261ab2391..75918b023d5 100644
--- a/src/test/unit/dronecan_application_unittest.cc
+++ b/src/test/unit/dronecan_application_unittest.cc
@@ -88,7 +88,6 @@ bool isHardwareHealthy(void) { return true; }
 void _logf(logTopic_e topic, unsigned level, const char *fmt, ...) { (void)topic; (void)level; (void)fmt; }
 
 /* GPS and battery DroneCAN receive stubs */
-void dronecanGPSReceiveGNSSFix(const struct uavcan_equipment_gnss_Fix *p) { (void)p; }
 void dronecanGPSReceiveGNSSFix2(const struct uavcan_equipment_gnss_Fix2 *p, uint8_t sourceNodeId) { (void)p; (void)sourceNodeId; }
 void dronecanGPSReceiveGNSSAuxiliary(const struct uavcan_equipment_gnss_Auxiliary *p, uint8_t sourceNodeId) { (void)p; (void)sourceNodeId; }
 void dronecanGpsOnNodeEvicted(uint8_t nodeID) { (void)nodeID; }

From 03a1ae9b9f7cc9c0b6354946f885aad8f14bd783 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Mon, 17 Aug 2026 20:26:14 -0700
Subject: [PATCH 67/67] docs(dronecan): add missing
 dronecan_battery_id/dronecan_gps_node_id entries

Settings.md was never regenerated after these two settings were added,
so the settings_md CI check (which diffs a fresh update_cli_docs.py run
against the committed file) was failing on this PR.
---
 docs/Settings.md | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/docs/Settings.md b/docs/Settings.md
index 970636e5673..2ef20d8155a 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -882,6 +882,16 @@ Re-purpose the craft name field for messages.
 
 ---
 
+### dronecan_battery_id
+
+Only accept BatteryInfo messages whose battery_id field (battery slot) matches this value. Every value 0-255 is a valid battery_id on the wire, so there is no wildcard; the default (0) matches the conventional primary-battery ID used by most peripherals.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 255 |
+
+---
+
 ### dronecan_bitrate_kbps
 
 The speed of the CANbus network in kbps. Set all devices to the same speed. 
@@ -895,6 +905,16 @@ The speed of the CANbus network in kbps. Set all devices to the same speed.
 
 ---
 
+### dronecan_gps_node_id
+
+Filter GPS messages by source Node ID. 0 = use any node.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 127 |
+
+---
+
 ### dronecan_node_id
 
 Unique identifier for this device. Valid values are 1 to 127. 126 and 127 are reserved for diagnostic tools.