Canfd

acanfd-stm32/src/ACANFD-STM32-fixed-ram-sections.h

@@ -170,6 +170,19 @@ class ACANFD_STM32 {
 //  Bit 4 : bus off
   public: uint32_t statusFlags (void) const ;
 
+//--- Bus-off recovery. On bus-off, M_CAN sets CCCR.INIT and stays off the bus
+//    until software clears it (ISO 11898-1 requires software-controlled
+//    recovery). Clearing INIT starts the recovery sequence (129 occurrences
+//    of 11 consecutive recessive bits). Returns true if recovery was started.
+  public: inline bool recoverFromBusOff (void) {
+    if (((mPeripheralPtr->PSR & FDCAN_PSR_BO) != 0)
+     && ((mPeripheralPtr->CCCR & FDCAN_CCCR_INIT) != 0)) {
+      mPeripheralPtr->CCCR &= ~FDCAN_CCCR_INIT ;
+      return true ;
+    }
+    return false ;
+  }
+
 //--- Bus Status
   public: inline BusStatus getStatus (void) const { return BusStatus (mPeripheralPtr) ; }
 

acanfd-stm32/src/ACANFD-STM32-programmable-ram-sections.h

@@ -191,6 +191,19 @@ class ACANFD_STM32 {
 //  Bit 4 : bus off
   public: uint32_t statusFlags (void) const ;
 
+//--- Bus-off recovery. On bus-off, M_CAN sets CCCR.INIT and stays off the bus
+//    until software clears it (ISO 11898-1 requires software-controlled
+//    recovery). Clearing INIT starts the recovery sequence (129 occurrences
+//    of 11 consecutive recessive bits). Returns true if recovery was started.
+  public: inline bool recoverFromBusOff (void) {
+    if (((mPeripheralPtr->PSR & FDCAN_PSR_BO) != 0)
+     && ((mPeripheralPtr->CCCR & FDCAN_CCCR_INIT) != 0)) {
+      mPeripheralPtr->CCCR &= ~FDCAN_CCCR_INIT ;
+      return true ;
+    }
+    return false ;
+  }
+
 //--- Bus Status
   public: inline BusStatus getStatus (void) const { return BusStatus (mPeripheralPtr) ; }
 

acanfd-stm32/src/ACANFD_STM32_NUCLEO_H723ZG-settings.h

@@ -20,10 +20,11 @@ inline uint32_t fdcanClock (void) {
   //--- Reset CAN peripherals
     __HAL_RCC_FDCAN_FORCE_RESET () ;
     __HAL_RCC_FDCAN_RELEASE_RESET () ;
-  //--- Select CAN clock
-    LL_RCC_SetFDCANClockSource (LL_RCC_FDCAN_CLKSOURCE_PLL1Q) ;
+  //--- Select CAN clock: PLL2Q (96 MHz on MicroNodePlus). PLL1Q is 137.5 MHz,
+  //    which cannot divide to an exact 4 Mbps CAN-FD data phase.
+    LL_RCC_SetFDCANClockSource (LL_RCC_FDCAN_CLKSOURCE_PLL2Q) ;
   }
-  return HAL_RCC_GetPCLK1Freq () ;
+  return LL_RCC_GetFDCANClockFreq (LL_RCC_FDCAN_CLKSOURCE) ;
 }
 
 //------------------------------------------------------------------------------

acanfd-stm32/src/ACANFD_STM32_NUCLEO_H743ZI2-objects.h

@@ -24,8 +24,9 @@ const std::initializer_list <ACANFD_STM32::PinPort> fdcan1_tx_pin_array{
 
 const std::initializer_list <ACANFD_STM32::PinPort> fdcan1_rx_pin_array {
   ACANFD_STM32::PinPort (PD_0,  9), // Rx Pin: PD_0, AF9
-  ACANFD_STM32::PinPort (PB_8,  9), // Rx Pin: PB_9, AF9
-  ACANFD_STM32::PinPort (PA_11, 9)  // Rx Pin: PA_11, AF9
+  ACANFD_STM32::PinPort (PB_8,  9), // Rx Pin: PB_8, AF9
+  ACANFD_STM32::PinPort (PA_11, 9), // Rx Pin: PA_11, AF9
+  ACANFD_STM32::PinPort (PH_14, 9)  // Rx Pin: PH_14, AF9 (CoreNode CAN1 connector)
 } ;
 
 //------------------------------------------------------------------------------

acanfd-stm32/src/ACANFD_STM32_NUCLEO_H743ZI2-settings.h

@@ -20,10 +20,12 @@ inline uint32_t fdcanClock (void) {
   //--- Reset CAN peripherals
     __HAL_RCC_FDCAN_FORCE_RESET () ;
     __HAL_RCC_FDCAN_RELEASE_RESET () ;
-  //--- Select CAN clock
-    LL_RCC_SetFDCANClockSource (LL_RCC_FDCAN_CLKSOURCE_PLL1Q) ;
+  //--- Select CAN clock: PLL2Q (96 MHz on CoreNode). 96 MHz divides into
+  //    1/2/4/8 Mbps with prescaler 1, which CAN-FD's TDC needs; PLL1Q (120 MHz)
+  //    forces BRP=2 at 2 Mbps. Matches MicroNodePlus (also PLL2Q).
+    LL_RCC_SetFDCANClockSource (LL_RCC_FDCAN_CLKSOURCE_PLL2Q) ;
   }
-  return HAL_RCC_GetPCLK1Freq () ;
+  return LL_RCC_GetFDCANClockFreq (LL_RCC_FDCAN_CLKSOURCE) ;
 }
 
 //------------------------------------------------------------------------------

acanfd-stm32/src/ACANFD_STM32_Settings.cpp

@@ -123,10 +123,13 @@ mDataBitRateFactor (inDataBitRateFactor) {
 //--- Set RJW to PS2
   mDataSJW = mDataPhaseSegment2 ;
 //-------------------------- Set TDCO
-  mTransceiverDelayCompensation = (dataBitRate <= 1'000'000)
-    ? 0
-    : ((mBitRatePrescaler * bestDataTQCount) / 2)
-  ;
+//--- ST/Bosch recommendation: place the secondary sample point at the data
+//    phase sample point, i.e. TDCO = DBRP * (1 + DTSEG1), in kernel clock periods
+  uint32_t tdco = mBitRatePrescaler * (1 + mDataPhaseSegment1) ;
+  if (tdco > MAX_TRANSCEIVER_DELAY_COMPENSATION) {
+    tdco = MAX_TRANSCEIVER_DELAY_COMPENSATION ;
+  }
+  mTransceiverDelayCompensation = (dataBitRate <= 1'000'000) ? 0 : tdco ;
 //-------------------------- Set Arbitration segment lengthes
   const uint32_t bestArbitrationTQCount = bestDataTQCount * uint32_t (inDataBitRateFactor) ;
 //--- Compute PS1

src/canH723.cpp

@@ -32,6 +32,13 @@ static const PortConfig kPortConfig[2] = {
     { &fdcan2, PB_5, PB_6 }, // PORT2 — FDCAN2_RX=PB5, FDCAN2_TX=PB6 (AF9)
 };
 
+// Data-phase multiplier → ACANFD factor enum (whose values are the multiplier).
+// Out-of-range values fall back to x4, the DroneCAN-on-FD common default.
+static DataBitRateFactor data_factor_from(uint8_t factor) {
+    if (factor < 1 || factor > 10) factor = 4;
+    return DataBitRateFactor(factor);
+}
+
 static uint32_t bitrate_to_hz(BITRATE bitrate) {
     switch (bitrate) {
         case CAN_50KBPS:   return  50000;
@@ -44,10 +51,22 @@ static uint32_t bitrate_to_hz(BITRATE bitrate) {
     return 1000000;
 }
 
+// Wrapper-level begin_status: the FDCAN kernel clock cannot hit the requested
+// bitrates within tolerance (settings.mBitSettingOk). Distinct from the
+// driver's beginFD() error flags, which never reach bit 31.
+static const uint32_t kBitTimingOutOfTolerance = 1UL << 31;
+
 static bool init_port(uint8_t idx, ACANFD_STM32_Settings &settings) {
+    gPorts[idx].drv = kPortConfig[idx].drv;
+    if (!settings.mBitSettingOk) {
+        // Refuse to start with off-frequency bit timing: it passes the driver's
+        // range checks but produces bit errors / bus-off on a real bus.
+        gPorts[idx].begin_status = kBitTimingOutOfTolerance;
+        gPorts[idx].initialized  = false;
+        return false;
+    }
     settings.mRxPin = kPortConfig[idx].rx_pin;
     settings.mTxPin = kPortConfig[idx].tx_pin;
-    gPorts[idx].drv = kPortConfig[idx].drv;
     gPorts[idx].begin_status = gPorts[idx].drv->beginFD(settings);
     gPorts[idx].initialized  = (gPorts[idx].begin_status == 0);
     return gPorts[idx].initialized;
@@ -56,8 +75,8 @@ static bool init_port(uint8_t idx, ACANFD_STM32_Settings &settings) {
 // "Classic" mode: peripheral runs NORMAL_FD at x4 data rate so it can *receive*
 // FD frames from other nodes (e.g. ArduPilot with UC_OPTION +4). The Classic/FD
 // distinction only affects the per-frame TX canfd flag, not the peripheral config.
-bool CANInit(BITRATE bitrate, uint8_t port) {
-    ACANFD_STM32_Settings settings(bitrate_to_hz(bitrate), DataBitRateFactor::x4);
+bool CANInit(BITRATE bitrate, uint8_t port, uint8_t data_factor) {
+    ACANFD_STM32_Settings settings(bitrate_to_hz(bitrate), data_factor_from(data_factor));
     settings.mModuleMode = ACANFD_STM32_Settings::NORMAL_FD;
     if (port == CAN_PORT_BOTH) {
         return init_port(0, settings) & init_port(1, settings);
@@ -66,8 +85,8 @@ bool CANInit(BITRATE bitrate, uint8_t port) {
     return init_port(port, settings);
 }
 
-bool CANInit_fd(BITRATE bitrate, uint8_t port) {
-    ACANFD_STM32_Settings settings(bitrate_to_hz(bitrate), DataBitRateFactor::x4);
+bool CANInit_fd(BITRATE bitrate, uint8_t port, uint8_t data_factor) {
+    ACANFD_STM32_Settings settings(bitrate_to_hz(bitrate), data_factor_from(data_factor));
     settings.mModuleMode = ACANFD_STM32_Settings::NORMAL_FD;
     if (port == CAN_PORT_BOTH) {
         return init_port(0, settings) & init_port(1, settings);
@@ -85,9 +104,6 @@ static bool send_on(PortState &p, const CanardCANFrame *tx_msg) {
     message.len = tx_msg->data_len;
     memcpy(message.data, tx_msg->data, tx_msg->data_len);
 #if CANARD_ENABLE_CANFD
-    // Temporarily forcing NO_BIT_RATE_SWITCH while diagnosing the 4 Mbps BRS
-    // bus-off issue. FD framing intact, payload stays at nominal bitrate.
-    // Restore to CANFD_WITH_BIT_RATE_SWITCH once the BRS issue is resolved.
     message.type = tx_msg->canfd
                  ? CANFDMessage::CANFD_WITH_BIT_RATE_SWITCH
                  : CANFDMessage::CAN_DATA;
@@ -149,6 +165,7 @@ void CANReceive(CanardCANFrame *rx_msg, uint8_t port) {
 // Prints once when a port enters bus-off and once when it recovers — no spam.
 static void check_bus_off_once(uint8_t idx) {
     static bool was_bus_off[2] = { false, false };
+    static uint32_t last_recover_ms[2] = { 0, 0 };
     if (!gPorts[idx].initialized || !gPorts[idx].drv) return;
     const bool bus_off = (gPorts[idx].drv->statusFlags() & (1U << 4)) != 0;
     if (bus_off != was_bus_off[idx]) {
@@ -158,6 +175,13 @@ static void check_bus_off_once(uint8_t idx) {
         }
         was_bus_off[idx] = bus_off;
     }
+    // M_CAN parks in INIT after bus-off and stays off the bus until software
+    // restarts it. Retry at 1 Hz so a transient bus fault doesn't take the
+    // node offline until reboot, without storming a bus whose fault persists.
+    if (bus_off && (millis() - last_recover_ms[idx]) >= 1000U) {
+        gPorts[idx].drv->recoverFromBusOff();
+        last_recover_ms[idx] = millis();
+    }
 }
 
 uint8_t CANMsgAvail(uint8_t port) {

src/canH723.h

@@ -22,8 +22,12 @@ static constexpr uint8_t CAN_PORT1      = 0;
 static constexpr uint8_t CAN_PORT2      = 1;
 static constexpr uint8_t CAN_PORT_BOTH  = 2;
 
-bool    CANInit(BITRATE bitrate, uint8_t port = CAN_PORT1);
-bool    CANInit_fd(BITRATE bitrate, uint8_t port = CAN_PORT1);
+// data_factor = data-phase bitrate multiplier over the arbitration bitrate
+// (matches ArduPilot CAN_Px_FDBITRATE: 4 -> 4 Mbps, 8 -> 8 Mbps at 1 Mbps arb).
+// CANInit also takes it because the peripheral always runs NORMAL_FD: the
+// data timing must match the bus to receive other nodes' FD frames.
+bool    CANInit(BITRATE bitrate, uint8_t port = CAN_PORT1, uint8_t data_factor = 4);
+bool    CANInit_fd(BITRATE bitrate, uint8_t port = CAN_PORT1, uint8_t data_factor = 4);
 // Returns true if the frame was accepted by the peripheral (TX FIFO had room).
 // Returns false if the driver could not accept the frame (TX FIFO full, bus-off,
 // uninitialised port). Callers should not pop the libcanard TX queue when this

src/canH743.cpp

@@ -12,25 +12,31 @@
 
 #define CAN_EXT_ID_MASK 0x1FFFFFFFU
 
-// Per-port driver state. Index 0 = PORT1 (FDCAN2 on CoreNode), index 1 = PORT2 (FDCAN1).
+// Per-port driver state. Index 0 = PORT1 (FDCAN1 = physical CAN1),
+// index 1 = PORT2 (FDCAN2 = physical CAN2). Matches MicroNodePlus, where
+// PORT1 is also FDCAN1.
 struct PortState {
     ACANFD_STM32 *drv          = nullptr;
     uint32_t      begin_status = 0xFFFF;
     bool          initialized  = false;
 };
 static PortState gPorts[2];
 
-// Map port index to driver instance + pin pair.
-// PORT1: FDCAN2 at PB_5 (RX) / PB_6 (TX) — today's production wiring on CoreNode.
-// PORT2: FDCAN1 at PB_8 (RX) / PB_9 (TX) — placeholder; confirm from board schematic.
+// Map port index to driver instance + pin pair. Port number tracks the silk
+// label: PORT1 = CoreNode connector "CAN1", PORT2 = "CAN2".
+// PORT1: FDCAN1 at PH_14 (RX) / PD_1 (TX) — physical CAN1 connector.
+//   (Earlier PB_8/PB_9 placeholders were wrong — FDCAN1 routes to PD1/PH14 here,
+//    verified on hardware; PH_14 was added to fdcan1_rx_pin_array in the H743
+//    objects header.)
+// PORT2: FDCAN2 at PB_5 (RX) / PB_6 (TX) — physical CAN2 connector.
 struct PortConfig {
     ACANFD_STM32 *drv;
     uint32_t      rx_pin;
     uint32_t      tx_pin;
 };
 static const PortConfig kPortConfig[2] = {
-    { &fdcan2, PB_5, PB_6 }, // PORT1
-    { &fdcan1, PB_8, PB_9 }, // PORT2 (placeholder pins — verify with schematic)
+    { &fdcan1, PH_14, PD_1 }, // PORT1 — physical CAN1 (FDCAN1)
+    { &fdcan2, PB_5,  PB_6 }, // PORT2 — physical CAN2 (FDCAN2)
 };
 
 static uint32_t bitrate_to_hz(BITRATE bitrate) {
@@ -45,10 +51,22 @@ static uint32_t bitrate_to_hz(BITRATE bitrate) {
     return 1000000;
 }
 
+// Wrapper-level begin_status: the FDCAN kernel clock cannot hit the requested
+// bitrates within tolerance (settings.mBitSettingOk). Distinct from the
+// driver's beginFD() error flags, which never reach bit 31.
+static const uint32_t kBitTimingOutOfTolerance = 1UL << 31;
+
 static bool init_port(uint8_t idx, ACANFD_STM32_Settings &settings) {
+    gPorts[idx].drv = kPortConfig[idx].drv;
+    if (!settings.mBitSettingOk) {
+        // Refuse to start with off-frequency bit timing: it passes the driver's
+        // range checks but produces bit errors / bus-off on a real bus.
+        gPorts[idx].begin_status = kBitTimingOutOfTolerance;
+        gPorts[idx].initialized  = false;
+        return false;
+    }
     settings.mRxPin = kPortConfig[idx].rx_pin;
     settings.mTxPin = kPortConfig[idx].tx_pin;
-    gPorts[idx].drv = kPortConfig[idx].drv;
     gPorts[idx].begin_status = gPorts[idx].drv->beginFD(settings);
     gPorts[idx].initialized  = (gPorts[idx].begin_status == 0);
     return gPorts[idx].initialized;
@@ -63,8 +81,9 @@ bool CANInit(BITRATE bitrate, uint8_t port) {
     return init_port(port, settings);
 }
 
-bool CANInit_fd(BITRATE bitrate, uint8_t port) {
-    ACANFD_STM32_Settings settings(bitrate_to_hz(bitrate), DataBitRateFactor::x4);
+bool CANInit_fd(BITRATE bitrate, uint8_t port, uint8_t data_factor) {
+    if (data_factor < 1 || data_factor > 10) data_factor = 4;
+    ACANFD_STM32_Settings settings(bitrate_to_hz(bitrate), DataBitRateFactor(data_factor));
     settings.mModuleMode = ACANFD_STM32_Settings::NORMAL_FD;
     if (port == CAN_PORT_BOTH) {
         return init_port(0, settings) & init_port(1, settings);
@@ -138,14 +157,42 @@ void CANReceive(CanardCANFrame *rx_msg, uint8_t port) {
     }
 }
 
+// Edge-triggered bus-off detector + recovery. Called from CANMsgAvail() so it's
+// polled every main-loop iteration. ACANFD_STM32::statusFlags() bit 4 = PSR.BO.
+// Prints once when a port enters bus-off and once when it recovers — no spam.
+// M_CAN parks in INIT after bus-off and stays off the bus until software
+// restarts it; retry recovery at 1 Hz so a transient bus fault doesn't take the
+// node offline until reboot, without storming a bus whose fault persists.
+// (Ported from canH723.cpp.)
+static void check_bus_off_once(uint8_t idx) {
+    static bool was_bus_off[2] = { false, false };
+    static uint32_t last_recover_ms[2] = { 0, 0 };
+    if (!gPorts[idx].initialized || !gPorts[idx].drv) return;
+    const bool bus_off = (gPorts[idx].drv->statusFlags() & (1U << 4)) != 0;
+    if (bus_off != was_bus_off[idx]) {
+        if (Serial) {
+            Serial.print(bus_off ? "CANFD BUS-OFF on PORT" : "CANFD recovered on PORT");
+            Serial.println(idx + 1);
+        }
+        was_bus_off[idx] = bus_off;
+    }
+    if (bus_off && (millis() - last_recover_ms[idx]) >= 1000U) {
+        gPorts[idx].drv->recoverFromBusOff();
+        last_recover_ms[idx] = millis();
+    }
+}
+
 uint8_t CANMsgAvail(uint8_t port) {
     if (port == CAN_PORT_BOTH) {
+        check_bus_off_once(0);
+        check_bus_off_once(1);
         uint8_t n = 0;
         if (gPorts[0].initialized && gPorts[0].drv) n += gPorts[0].drv->driverReceiveFIFO0Count();
         if (gPorts[1].initialized && gPorts[1].drv) n += gPorts[1].drv->driverReceiveFIFO0Count();
         return n;
     }
     if (port > 1 || !gPorts[port].initialized || !gPorts[port].drv) return 0;
+    check_bus_off_once(port);
     return gPorts[port].drv->driverReceiveFIFO0Count();
 }
 

src/canH743.h

@@ -25,8 +25,10 @@ static constexpr uint8_t CAN_PORT_BOTH = 2;
 // Classic CAN init for the given port.
 bool CANInit(BITRATE bitrate, uint8_t port = CAN_PORT1);
 
-// CAN-FD init (NORMAL_FD, 4x data rate) for the given port.
-bool CANInit_fd(BITRATE bitrate, uint8_t port = CAN_PORT1);
+// CAN-FD init (NORMAL_FD) for the given port. data_factor = data-phase bitrate
+// multiplier over the arbitration bitrate (matches ArduPilot CAN_Px_FDBITRATE:
+// 4 -> 4 Mbps, 8 -> 8 Mbps at 1 Mbps arb).
+bool CANInit_fd(BITRATE bitrate, uint8_t port = CAN_PORT1, uint8_t data_factor = 4);
 
 // Send a frame on the given port. BOTH fans out to both peripherals.
 // Returns true if the peripheral accepted the frame (TX FIFO had room). Returns

src/dronecan.cpp

@@ -7,14 +7,17 @@ static bool dronecan_can_init(DroneCAN::CanMode mode, uint8_t port)
 #ifdef CANL431
     (void)port; // L431 has one fixed CAN port wired at remap=2 (PB8/PB9)
 #if CANARD_ENABLE_CANFD
-    if (mode == DroneCAN::CanMode::FD) return CANInit_fd(CAN_1000KBPS, 2);
+    if (mode != DroneCAN::CanMode::Classic) return CANInit_fd(CAN_1000KBPS, 2);
 #else
     (void)mode;
 #endif
     return CANInit(CAN_1000KBPS, 2);
 #else
 #if CANARD_ENABLE_CANFD
-    if (mode == DroneCAN::CanMode::FD) return CANInit_fd(CAN_1000KBPS, port);
+    // CanMode enumerator values encode the data-phase bitrate multiplier.
+    if (mode != DroneCAN::CanMode::Classic) {
+        return CANInit_fd(CAN_1000KBPS, port, (uint8_t)mode);
+    }
 #else
     (void)mode;
 #endif
@@ -37,7 +40,7 @@ void DroneCAN::init(CanardOnTransferReception onTransferReceived,
     storage_page_ = (storage_page >= 0)
                   ? (uint32_t)storage_page
                   : DroneCAN_Storage::default_page((uint8_t)port);
-    canfd_default_ = (mode == CanMode::FD);
+    canfd_default_ = (mode != CanMode::Classic);
     dronecan_can_init(mode, (uint8_t)port);
 
     strncpy(this->node_name, name, sizeof(this->node_name));
@@ -112,7 +115,7 @@ void DroneCAN::init(const std::vector<parameter> &param_list,
     storage_page_ = (storage_page >= 0)
                   ? (uint32_t)storage_page
                   : DroneCAN_Storage::default_page((uint8_t)port);
-    canfd_default_ = (mode == CanMode::FD);
+    canfd_default_ = (mode != CanMode::Classic);
     dronecan_can_init(mode, (uint8_t)port);
 
     strncpy(this->node_name, name, sizeof(this->node_name));
@@ -166,7 +169,7 @@ void DroneCAN::init(CanardOnTransferReception onTransferReceived,
 {
     port_          = port;
     storage_page_  = DroneCAN_Storage::default_page((uint8_t)port);
-    canfd_default_ = (mode == CanMode::FD);
+    canfd_default_ = (mode != CanMode::Classic);
     dronecan_can_init(mode, (uint8_t)port);
 
     strncpy(this->node_name, name, sizeof(this->node_name));