AP_HAL_ChibiOS: add init method to take in separate canfd bitrate

libraries/AP_HAL/CANIface.h

@@ -163,7 +163,7 @@ public:
         }
     };
 
-    virtual bool init(const uint32_t bitrate, const OperatingMode mode, const uint32_t fdbitrate) {
+    virtual bool init(const uint32_t bitrate, const uint32_t fdbitrate, const OperatingMode mode) {
         return init(bitrate, mode);
     }
 

libraries/AP_HAL_ChibiOS/CANFDIface.cpp

@@ -554,7 +554,7 @@ uint16_t CANIface::getNumFilters() const
 }
 
 bool CANIface::clock_init_ = false;
-bool CANIface::init(const uint32_t bitrate, const OperatingMode mode)
+bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const OperatingMode mode)
 {
     Debug("Bitrate %lu mode %d", static_cast<unsigned long>(bitrate), static_cast<int>(mode));
     if (self_index_ > HAL_NUM_CAN_IFACES) {
@@ -669,23 +669,24 @@ bool CANIface::init(const uint32_t bitrate, const OperatingMode mode)
                   (timings.bs2 << FDCAN_NBTP_NTSEG2_Pos)  |
                   (timings.prescaler << FDCAN_NBTP_NBRP_Pos));
 
-    if (!computeTimings(bitrate*8, timings)) { // Do 8x fast Data transmission for CAN FD frames
+    if (fdbitrate) {
-        can_->CCCR &= ~FDCAN_CCCR_INIT;
+        if (!computeTimings(fdbitrate, timings)) { // Do 8x fast Data transmission for CAN FD frames
-        uint32_t while_start_ms = AP_HAL::millis();
+            can_->CCCR &= ~FDCAN_CCCR_INIT;
-        while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) {
+            uint32_t while_start_ms = AP_HAL::millis();
-            if ((AP_HAL::millis() - while_start_ms) > REG_SET_TIMEOUT) {
+            while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) {
-                return false;
+                if ((AP_HAL::millis() - while_start_ms) > REG_SET_TIMEOUT) {
+                    return false;
+                }
             }
+            return false;
         }
-        return false;
+        Debug("CANFD Timings: presc=%u bs1=%u bs2=%u\n",
+              unsigned(timings.prescaler), unsigned(timings.bs1), unsigned(timings.bs2));
+        can_->DBTP = ((timings.bs1 << FDCAN_DBTP_DTSEG1_Pos) |
+                     (timings.bs2 << FDCAN_DBTP_DTSEG2_Pos)  |
+                     (timings.prescaler << FDCAN_DBTP_DBRP_Pos));
     }
-    Debug("CANFD Timings: presc=%u bs1=%u bs2=%u\n",
-          unsigned(timings.prescaler), unsigned(timings.bs1), unsigned(timings.bs2));
 
-    can_->DBTP = ((timings.bs1 << FDCAN_DBTP_DTSEG1_Pos) |
-                  (timings.bs2 << FDCAN_DBTP_DTSEG2_Pos)  |
-                  (timings.prescaler << FDCAN_DBTP_DBRP_Pos));
-    
     //RX Config
 #if defined(STM32H7)
     can_->RXESC = 0; //Set for 8Byte Frames

libraries/AP_HAL_ChibiOS/CANFDIface.h

@@ -178,7 +178,11 @@ public:
     static uint8_t next_interface;
 
     // Initialise CAN Peripheral
-    bool init(const uint32_t bitrate, const OperatingMode mode) override;
+    bool init(const uint32_t bitrate, const OperatingMode mode) override {
+        return init(bitrate, 0, mode);
+    }
+
+    bool init(const uint32_t bitrate, const uint32_t fdbitrate, const OperatingMode mode) override;
 
     // Put frame into Tx FIFO returns negative on error, 0 on buffer full, 
     // 1 on successfully pushing a frame into FIFO