From 8aa047d8769b1fb7443adfdda782c8465adddf6f Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Mon, 3 May 2021 18:24:50 +0530 Subject: [PATCH] AP_HAL_ChibiOS: add support for CANFD for H7 and G4 --- libraries/AP_HAL_ChibiOS/CANFDIface.cpp | 98 +++++++++++++++++++------ libraries/AP_HAL_ChibiOS/CANFDIface.h | 7 ++ 2 files changed, 84 insertions(+), 21 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index 9510c6161f..2258f0f040 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -57,20 +57,16 @@ #define FDCAN2_IT0_IRQHandler STM32_FDCAN2_IT0_HANDLER #define FDCAN2_IT1_IRQHandler STM32_FDCAN2_IT1_HANDLER -#if defined(STM32G4) -// on G4 FIFO elements are spaced at 18 words +// FIFO elements are spaced at 18 words #define FDCAN_FRAME_BUFFER_SIZE 18 -#else -// on H7 they are spaced at 4 words -#define FDCAN_FRAME_BUFFER_SIZE 4 -#endif + //Message RAM Allocations in Word lengths #if defined(STM32H7) -#define MAX_FILTER_LIST_SIZE 80U //80 element Standard Filter List elements or 40 element Extended Filter List -#define FDCAN_NUM_RXFIFO0_SIZE 104U //26 Frames -#define FDCAN_TX_FIFO_BUFFER_SIZE 128U //32 Frames +#define MAX_FILTER_LIST_SIZE 78U //78 element Standard Filter List elements or 40 element Extended Filter List +#define FDCAN_NUM_RXFIFO0_SIZE 108U //6 Frames +#define FDCAN_TX_FIFO_BUFFER_SIZE 126U //7 Frames #define MESSAGE_RAM_END_ADDR 0x4000B5FC #elif defined(STM32G4) @@ -243,6 +239,7 @@ bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timing const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum); if ((prescaler < 1U) || (prescaler > 1024U)) { + Debug("Timings: No Solution found\n"); return false; // No solution } @@ -295,6 +292,9 @@ bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timing solution = BsPair(bs1_bs2_sum, uint8_t((7 * bs1_bs2_sum - 1) / 8)); } + Debug("Timings: quanta/bit: %d, sample point location: %.1f%%\n", + int(1 + solution.bs1 + solution.bs2), float(solution.sample_point_permill) / 10.F); + /* * Final validation * Helpful Python: @@ -305,12 +305,10 @@ bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timing * */ if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !solution.isValid()) { + Debug("Timings: Invalid Solution %lu %lu %d %d %lu \n", pclk, prescaler, int(solution.bs1), int(solution.bs2), (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))); return false; } - Debug("Timings: quanta/bit: %d, sample point location: %.1f%%\n", - int(1 + solution.bs1 + solution.bs2), float(solution.sample_point_permill) / 10.F); - out_timings.prescaler = uint16_t(prescaler - 1U); out_timings.sjw = 0; // Which means one out_timings.bs1 = uint8_t(solution.bs1 - 1); @@ -318,11 +316,23 @@ bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timing return true; } +static uint32_t bytes_to_word(const uint8_t data[]) { + return (uint32_t(data[3]) << 24) | + (uint32_t(data[2]) << 16) | + (uint32_t(data[1]) << 8) | + (uint32_t(data[0]) << 0); +} + int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline, CanIOFlags flags) { + if (!initialised_) { + return -1; + } + stats.tx_requests++; - if (frame.isErrorFrame() || frame.dlc > 8 || !initialised_) { + if (frame.isErrorFrame() || (frame.dlc > 8 && !frame.isCanFDFrame()) || + frame.dlc > 15) { stats.tx_rejected++; return -1; } @@ -336,7 +346,7 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline, uint8_t index; if ((can_->TXFQS & FDCAN_TXFQS_TFQF) != 0) { - stats.tx_rejected++; + stats.tx_overflow++; return 0; //we don't have free space } index = ((can_->TXFQS & FDCAN_TXFQS_TFQPI) >> FDCAN_TXFQS_TFQPI_Pos); @@ -357,9 +367,19 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline, //Write Data Length Code, and Message Marker buffer[1] = frame.dlc << 16 | index << 24; + if (frame.isCanFDFrame()) { + buffer[1] |= FDF | BRS; // do CAN FD transfer and bit rate switching + stats.fdf_tx_requests++; + pending_tx_[index].canfd_frame = true; + } else { + pending_tx_[index].canfd_frame = false; + } + // Write Frame to the message RAM - buffer[2] = frame.data_32[0]; - buffer[3] = frame.data_32[1]; + const uint8_t data_length = AP_HAL::CANFrame::dlcToDataLength(frame.dlc); + for (uint8_t i = 0, l = 2; i < data_length; i+=4, l++) { + buffer[l] = bytes_to_word(&frame.data[i]); + } //Set Add Request can_->TXBAR = (1 << index); @@ -649,6 +669,19 @@ 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 + can_->CCCR &= ~FDCAN_CCCR_INIT; + uint32_t while_start_ms = AP_HAL::millis(); + while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) { + if ((AP_HAL::millis() - while_start_ms) > REG_SET_TIMEOUT) { + 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)); @@ -684,6 +717,8 @@ bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) #endif can_->ILE = 0x3; + can_->CCCR |= FDCAN_CCCR_FDOE | FDCAN_CCCR_BRSE; // enable sending CAN FD frames, and Bitrate switching + // If mode is Filtered then we finish the initialisation in configureFilter method // otherwise we finish here if (mode != FilteredMode) { @@ -722,6 +757,8 @@ void CANIface::setupMessageRam() #else uint32_t num_elements = 0; + can_->RXESC = 0x777; //Support upto 64byte long frames + can_->TXESC = 0x7; //Support upto 64byte long frames // Rx FIFO 0 start address and element count num_elements = MIN((FDCAN_NUM_RXFIFO0_SIZE/FDCAN_FRAME_BUFFER_SIZE), 64U); if (num_elements) { @@ -753,6 +790,9 @@ void CANIface::handleTxCompleteInterrupt(const uint64_t timestamp_us) if (!pending_tx_[i].pushed) { stats.tx_success++; + if (pending_tx_[i].canfd_frame) { + stats.fdf_tx_success++; + } pending_tx_[i].pushed = true; } else { continue; @@ -821,7 +861,7 @@ bool CANIface::readRxFIFO(uint8_t fifo_index) } // Read the frame contents - AP_HAL::CANFrame frame; + AP_HAL::CANFrame frame {}; uint32_t id = frame_ptr[0]; if ((id & IDE) == 0) { //Standard ID @@ -835,10 +875,18 @@ bool CANIface::readRxFIFO(uint8_t fifo_index) if ((id & RTR) != 0) { frame.id |= AP_HAL::CANFrame::FlagRTR; } + + if (frame_ptr[1] & FDF) { + frame.setCanFD(true); + stats.fdf_rx_received++; + } else { + frame.setCanFD(false); + } + frame.dlc = (frame_ptr[1] & DLC_MASK) >> 16; uint8_t *data = (uint8_t*)&frame_ptr[2]; - //We only handle Data Length of 8 Bytes for now - for (uint8_t i = 0; i < 8; i++) { + + for (uint8_t i = 0; i < AP_HAL::CANFrame::MaxDataLen; i++) { frame.data[i] = data[i]; } @@ -1039,6 +1087,7 @@ void CANIface::get_stats(ExpandingString &str) CriticalSectionLocker lock; str.printf("tx_requests: %lu\n" "tx_rejected: %lu\n" + "tx_overflow: %lu\n" "tx_success: %lu\n" "tx_timedout: %lu\n" "tx_abort: %lu\n" @@ -1047,9 +1096,13 @@ void CANIface::get_stats(ExpandingString &str) "rx_errors: %lu\n" "num_busoff_err: %lu\n" "num_events: %lu\n" - "ECR: %lx\n", + "ECR: %lx\n" + "fdf_rx: %lu\n" + "fdf_tx_req: %lu\n" + "fdf_tx: %lu\n", stats.tx_requests, stats.tx_rejected, + stats.tx_overflow, stats.tx_success, stats.tx_timedout, stats.tx_abort, @@ -1058,7 +1111,10 @@ void CANIface::get_stats(ExpandingString &str) stats.rx_errors, stats.num_busoff_err, stats.num_events, - stats.ecr); + stats.ecr, + stats.fdf_rx_received, + stats.fdf_tx_requests, + stats.fdf_tx_success); } #endif diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h index b133c4cc26..4d1dcb2f55 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.h +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.h @@ -61,6 +61,9 @@ class ChibiOS::CANIface : public AP_HAL::CANIface static constexpr unsigned long EXID_MASK = (0x1FFFFFFFU); // Extended Identifier Mask static constexpr unsigned long RTR = (0x20000000U); // Remote Transmission Request static constexpr unsigned long DLC_MASK = (0x000F0000U); // Data Length Code + static constexpr unsigned long FDF = (0x00200000U); // CAN FD Frame + static constexpr unsigned long BRS = (0x00100000U); // Bit Rate Switching + /** * CANx register sets @@ -151,6 +154,7 @@ class ChibiOS::CANIface : public AP_HAL::CANIface struct { uint32_t tx_requests; uint32_t tx_rejected; + uint32_t tx_overflow; uint32_t tx_success; uint32_t tx_timedout; uint32_t tx_abort; @@ -160,6 +164,9 @@ class ChibiOS::CANIface : public AP_HAL::CANIface uint32_t num_busoff_err; uint32_t num_events; uint32_t ecr; + uint32_t fdf_tx_requests; + uint32_t fdf_tx_success; + uint32_t fdf_rx_received; } stats; public: