From faf0c0d7c33201fd148c2f5c24309b31dc07b2b1 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 1 Mar 2022 14:45:20 +0530 Subject: [PATCH] AP_HAL_ChibiOS: set canfd bitrates using CANFD_SUPPORTED opt --- libraries/AP_HAL_ChibiOS/CANFDIface.cpp | 32 +++++++++++++------ libraries/AP_HAL_ChibiOS/CANFDIface.h | 7 ++-- .../AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat | 3 +- .../hwdef/scripts/chibios_hwdef.py | 16 ++++++---- 4 files changed, 39 insertions(+), 19 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index d648ee5fa8..84837325f6 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/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)); - can_->DBTP = ((timings.bs1 << FDCAN_DBTP_DTSEG1_Pos) | - (timings.bs2 << FDCAN_DBTP_DTSEG2_Pos) | - (timings.prescaler << FDCAN_DBTP_DBRP_Pos)); + unsigned(fdtimings.prescaler), unsigned(fdtimings.bs1), unsigned(fdtimings.bs2)); + can_->DBTP = ((fdtimings.bs1 << FDCAN_DBTP_DTSEG1_Pos) | + (fdtimings.bs2 << FDCAN_DBTP_DTSEG2_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, diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h index 67c5acc9c8..6e92fe395b 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.h +++ b/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; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat index aac0e9b47e..aedc8dd099 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat +++ b/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 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 697bcdcb37..226e874f1d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/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'''