DroneCAN: Fix H7 FDCAN and F7 bxCAN driver configuration

docs/Settings.md

@@ -5946,6 +5946,7 @@ Selection of pitot hardware. VIRTUAL only works if a GPS is enabled.
 | FAKE |  |
 | MSP |  |
 | DLVR-L10D |  |
+| MS5525 |  |
 
 ---
 

src/main/drivers/dronecan/dronecan.c

@@ -85,49 +85,49 @@ 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)) {
 		LOG_DEBUG(CAN, "GNSSAuxiliary decode failed");
 		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) {
 	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);
-    LOG_DEBUG(CAN, "GNSS Fix received");
 }
 
 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);
-    LOG_DEBUG(CAN, "GNSS Fix2 received");
 }
 
 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;
 	}
-    LOG_DEBUG(CAN, "GNSS RTCM");
 }
 
 void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
@@ -139,7 +139,6 @@ void handle_BatteryInfo(CanardInstance *ins, CanardRxTransfer *transfer) {
 		return;
 	}
     dronecanBatterySensorReceiveInfo(&batteryInfo);
-    LOG_DEBUG(CAN, "Battery Info");
 }
 
 /*
@@ -148,8 +147,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;
 
@@ -196,7 +193,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;
@@ -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) {
@@ -346,7 +335,6 @@ void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) {
                 break;
 
             case UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID:
-                LOG_DEBUG(CAN, "Battery Info");
                 handle_BatteryInfo(ins, transfer);
                 break;
         }
@@ -364,7 +352,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
@@ -392,7 +379,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){
@@ -420,7 +406,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;
     }  
     /*
@@ -463,8 +449,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);
 
@@ -485,26 +469,35 @@ 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);
+                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 + 100000)) { // Wait 100 mS
+            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;
-
+
+        case STATE_DRONECAN_FAILED:
+            break;
+
+        case STATE_DRONECAN_COUNT:
+            break;
+
     }
 
 }

src/main/drivers/dronecan/dronecan.h

@@ -14,7 +14,9 @@ typedef enum {
 typedef enum {
     STATE_DRONECAN_INIT,
     STATE_DRONECAN_NORMAL,
-    STATE_DRONECAN_BUS_OFF
+    STATE_DRONECAN_BUS_OFF,
+    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.

src/main/drivers/dronecan/libcanard/canard_stm32f7xx_driver.c

@@ -164,13 +164,10 @@ 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;
 	}
 
-	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;
 }
 
@@ -183,14 +180,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;
@@ -207,43 +200,29 @@ 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;
 
-    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;
     hcan1.Init.TimeSeg1     = (uint32_t)out_timings.bs1 << CAN_BTR_TS1_Pos;
     hcan1.Init.TimeSeg2     = (uint32_t)out_timings.bs2 << CAN_BTR_TS2_Pos;
     LOG_DEBUG(CAN, "Prescaler: %d, SJW: %d, BS1: %d, BS2: %d", out_timings.prescaler, out_timings.sjw, out_timings.bs1, out_timings.bs2);
 
-    // hcan1.Init.StdFiltersNbr = 0;
-    // hcan1.Init.ExtFiltersNbr = 1;
-    // hcan1.Init.TxFifoQueueElmtsNbr = 32;
-    // LOG_DEBUG(CAN, "In CAN Init");
-
-    /** Initializes the peripherals clock
-    */
-    // PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_FDCAN;
-    // PeriphClkInitStruct.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL;
-    // if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
-    // {
-    //   LOG_DEBUG(CAN, "Unable to configure peripheral clock");
-    // }
-
-    canardSTM32GPIO_Init();  // Set up the pins for CAN and optional listen only mode
-
-    // LOG_DEBUG(CAN, "System Clock Speed: %lu", HAL_RCC_GetSysClockFreq());
-    // LOG_DEBUG(CAN, "PClk1 Clock Speed: %lu", HAL_RCC_GetPCLK1Freq());
+    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;
@@ -278,9 +257,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
 
 
@@ -289,7 +268,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
 }
@@ -320,9 +299,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 +312,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 +328,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 +378,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;  // 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.
 
@@ -419,17 +390,17 @@ 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){
     return rxBufferNumMessages(&RxBuffer);
 }
 
 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
+    // 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.
 }
 
 /*