Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_ChibiOS: add init method to take in separate canfd bitrate

This commit is contained in:
bugobliterator 2022-02-10 13:10:05 +05:30 committed by Andrew Tridgell
parent cb1bc4e613
commit 47dd964e63
3 changed files with 21 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_HAL/CANIface.h
View File

@ -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);
}

29
libraries/AP_HAL_ChibiOS/CANFDIface.cpp
View File

@ -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
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;
if (fdbitrate) {
if (!computeTimings(fdbitrate, 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;
}
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

6
libraries/AP_HAL_ChibiOS/CANFDIface.h
View File

@ -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