AP_HAL_ChibiOS: set canfd bitrates using CANFD_SUPPORTED opt

libraries/AP_HAL_ChibiOS/CANFDIface.cpp

@@ -309,6 +309,7 @@ bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timing
         return false;
     }
 
+    out_timings.sample_point_permill = solution.sample_point_permill;
     out_timings.prescaler = uint16_t(prescaler - 1U);
     out_timings.sjw = 0;                                        // Which means one
     out_timings.bs1 = uint8_t(solution.bs1 - 1);
@@ -647,8 +648,6 @@ bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const Oper
     /*
      * CAN timings for this bitrate
      */
-    Timings timings;
-
     if (!computeTimings(bitrate, timings)) {
         can_->CCCR &= ~FDCAN_CCCR_INIT;
         uint32_t while_start_ms = AP_HAL::millis();
@@ -659,18 +658,18 @@ bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const Oper
         }
         return false;
     }
+    _bitrate = bitrate;
     Debug("Timings: presc=%u sjw=%u bs1=%u bs2=%u\n",
           unsigned(timings.prescaler), unsigned(timings.sjw), unsigned(timings.bs1), unsigned(timings.bs2));
 
     //setup timing register
-    //TODO: Do timing calculations for FDCAN
     can_->NBTP = ((timings.sjw << FDCAN_NBTP_NSJW_Pos)   |
                   (timings.bs1 << FDCAN_NBTP_NTSEG1_Pos) |
                   (timings.bs2 << FDCAN_NBTP_NTSEG2_Pos)  |
                   (timings.prescaler << FDCAN_NBTP_NBRP_Pos));
 
     if (fdbitrate) {
-        if (!computeTimings(fdbitrate, timings)) { // Do 8x fast Data transmission for CAN FD frames
+        if (!computeTimings(fdbitrate, fdtimings)) {
             can_->CCCR &= ~FDCAN_CCCR_INIT;
             uint32_t while_start_ms = AP_HAL::millis();
             while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) {
@@ -680,11 +679,12 @@ bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const Oper
             }
             return false;
         }
+        _fdbitrate = fdbitrate;
         Debug("CANFD Timings: presc=%u bs1=%u bs2=%u\n",
-              unsigned(timings.prescaler), unsigned(timings.bs1), unsigned(timings.bs2));
+              unsigned(fdtimings.prescaler), unsigned(fdtimings.bs1), unsigned(fdtimings.bs2));
-        can_->DBTP = ((timings.bs1 << FDCAN_DBTP_DTSEG1_Pos) |
+        can_->DBTP = ((fdtimings.bs1 << FDCAN_DBTP_DTSEG1_Pos) |
-                     (timings.bs2 << FDCAN_DBTP_DTSEG2_Pos)  |
+                     (fdtimings.bs2 << FDCAN_DBTP_DTSEG2_Pos)  |
-                     (timings.prescaler << FDCAN_DBTP_DBRP_Pos));
+                     (fdtimings.prescaler << FDCAN_DBTP_DBRP_Pos));
     }
 
     //RX Config
@@ -1086,7 +1086,14 @@ bool CANIface::select(bool &read, bool &write,
 void CANIface::get_stats(ExpandingString &str)
 {
     CriticalSectionLocker lock;
-    str.printf("tx_requests:    %lu\n"
+    str.printf("------- Clock Config -------\n"
+               "CAN_CLK_FREQ:   %luMHz\n"
+               "Std Timings: bitrate=%lu presc=%u\n"
+               "sjw=%u bs1=%u bs2=%u sample_point=%f%%\n"
+               "FD Timings:  bitrate=%lu presc=%u\n"
+               "sjw=%u bs1=%u bs2=%u sample_point=%f%%\n"
+               "------- CAN Interface Stats -------\n"
+               "tx_requests:    %lu\n"
                "tx_rejected:    %lu\n"
                "tx_overflow:    %lu\n"
                "tx_success:     %lu\n"
@@ -1101,6 +1108,13 @@ void CANIface::get_stats(ExpandingString &str)
                "fdf_rx:         %lu\n"
                "fdf_tx_req:     %lu\n"
                "fdf_tx:         %lu\n",
+               STM32_FDCANCLK/1000000UL,
+               _bitrate, unsigned(timings.prescaler),
+               unsigned(timings.sjw), unsigned(timings.bs1),
+               unsigned(timings.bs2), timings.sample_point_permill/10.0f,
+               _fdbitrate, unsigned(fdtimings.prescaler),
+               unsigned(fdtimings.sjw), unsigned(fdtimings.bs1),
+               unsigned(fdtimings.bs2), timings.sample_point_permill/10.0f,
                stats.tx_requests,
                stats.tx_rejected,
                stats.tx_overflow,

libraries/AP_HAL_ChibiOS/CANFDIface.h

@@ -91,13 +91,15 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
     } MessageRam_;
 
     struct Timings {
+        uint16_t sample_point_permill;
         uint16_t prescaler;
         uint8_t sjw;
         uint8_t bs1;
         uint8_t bs2;
 
         Timings()
-            : prescaler(0)
+            : sample_point_permill(0)
+            , prescaler(0)
             , sjw(0)
             , bs1(0)
             , bs2(0)
@@ -150,7 +152,8 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
     static bool clock_init_;
 
     bool _detected_bus_off;
-
+    Timings timings, fdtimings;
+    uint32_t _bitrate, _fdbitrate;
     struct {
         uint32_t tx_requests;
         uint32_t tx_rejected;

libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat

@@ -18,7 +18,8 @@ APJ_BOARD_ID 140
 
 FLASH_SIZE_KB 2048
 
-CANFD_SUPPORTED 1
+# supports upto 8MBits/s
+CANFD_SUPPORTED 8
 
 # with 2M flash we can afford to optimize for speed
 env OPTIMIZE -O2

libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py

@@ -689,14 +689,16 @@ def enable_can(f):
     if 'CAN' in bytype and mcu_type.startswith("STM32F3"):
         f.write('#define CAN1_BASE CAN_BASE\n')
     env_vars['HAL_NUM_CAN_IFACES'] = str(len(base_list))
-    canfd_supported = get_config('CANFD_SUPPORTED', 0, required=False)
-    if canfd_supported:
-        f.write('#define HAL_CANFD_SUPPORTED 1\n')
-        env_vars['HAL_CANFD_SUPPORTED'] = 1
-    else:
-        f.write('#define HAL_CANFD_SUPPORTED 0\n')
-        env_vars['HAL_CANFD_SUPPORTED'] = 0
 
+    if mcu_series.startswith("STM32H7") and not args.bootloader:
+        # set maximum supported canfd bit rate in MBits/sec
+        canfd_supported = int(get_config('CANFD_SUPPORTED', 0, default=4, required=False))
+        f.write('#define HAL_CANFD_SUPPORTED %d\n' % canfd_supported)
+        env_vars['HAL_CANFD_SUPPORTED'] = canfd_supported
+    else:
+        canfd_supported = int(get_config('CANFD_SUPPORTED', 0, default=0, required=False))
+        f.write('#define HAL_CANFD_SUPPORTED %d\n' % canfd_supported)
+        env_vars['HAL_CANFD_SUPPORTED'] = canfd_supported
 
 def has_sdcard_spi():
     '''check for sdcard connected to spi bus'''