diff --git a/libraries/AP_HAL_ChibiOS/CAN.h b/libraries/AP_HAL_ChibiOS/CAN.h index d46075c857..1f11192a70 100644 --- a/libraries/AP_HAL_ChibiOS/CAN.h +++ b/libraries/AP_HAL_ChibiOS/CAN.h @@ -28,7 +28,11 @@ #include "CANThread.h" #include "CANClock.h" +#if defined(STM32H7XX) +#include "CANFDIface.h" +#else #include "CANIface.h" +#endif #define MAX_NUMBER_OF_CAN_INTERFACES 2 #define MAX_NUMBER_OF_CAN_DRIVERS 2 diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp new file mode 100644 index 0000000000..de21f2d8b6 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -0,0 +1,1092 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2014 Pavel Kirienko + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Siddharth Bharat Purohit + */ + +#include "AP_HAL_ChibiOS.h" + +#if HAL_WITH_UAVCAN +#include +#include +#include "CANClock.h" +#include "CANInternal.h" +#include "CANSerialRouter.h" +#include +#include +# include + +# if defined(STM32H7XX) +#include "CANFDIface.h" + +#define FDCAN1_IT0_IRQHandler STM32_FDCAN1_IT0_HANDLER +#define FDCAN1_IT1_IRQHandler STM32_FDCAN1_IT1_HANDLER +#define FDCAN2_IT0_IRQHandler STM32_FDCAN2_IT0_HANDLER +#define FDCAN2_IT1_IRQHandler STM32_FDCAN2_IT1_HANDLER + +#define FDCAN_FRAME_BUFFER_SIZE 4 // Buffer size for 8 bytes data field + +//Message RAM Allocations in Word lengths +#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 MESSAGE_RAM_END_ADDR 0x4000B5FC + +extern const AP_HAL::HAL& hal; + +namespace ChibiOS_CAN +{ +namespace +{ + +CanIface* ifaces[UAVCAN_STM32_NUM_IFACES] = { + UAVCAN_NULLPTR +#if UAVCAN_STM32_NUM_IFACES > 1 + , UAVCAN_NULLPTR +#endif +}; + +inline void handleInterrupt(uavcan::uint8_t iface_index, uavcan::uint8_t line_index) +{ + UAVCAN_ASSERT(iface_index < UAVCAN_STM32_NUM_IFACES); + if (ifaces[iface_index] == UAVCAN_NULLPTR) { + //Just reset all the interrupts and return + ifaces[iface_index]->can_reg()->IR = FDCAN_IR_RF0N; + ifaces[iface_index]->can_reg()->IR = FDCAN_IR_RF1N; + ifaces[iface_index]->can_reg()->IR = FDCAN_IR_TEFN; + UAVCAN_ASSERT(0); + return; + } + if (line_index == 0) { + if ((ifaces[iface_index]->can_reg()->IR & FDCAN_IR_RF0N) || + (ifaces[iface_index]->can_reg()->IR & FDCAN_IR_RF0F)) { + ifaces[iface_index]->can_reg()->IR = FDCAN_IR_RF0N | FDCAN_IR_RF0F; + ifaces[iface_index]->handleRxInterrupt(0); + } + if ((ifaces[iface_index]->can_reg()->IR & FDCAN_IR_RF1N) || + (ifaces[iface_index]->can_reg()->IR & FDCAN_IR_RF1F)) { + ifaces[iface_index]->can_reg()->IR = FDCAN_IR_RF1N | FDCAN_IR_RF1F; + ifaces[iface_index]->handleRxInterrupt(1); + } + } else { + if (ifaces[iface_index]->can_reg()->IR & FDCAN_IR_TC) { + ifaces[iface_index]->can_reg()->IR = FDCAN_IR_TC; + uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); + if (utc_usec > 0) { + utc_usec--; + } + ifaces[iface_index]->handleTxCompleteInterrupt(utc_usec); + } + } + ifaces[iface_index]->pollErrorFlagsFromISR(); +} + +} // namespace + +uint32_t CanIface::FDCANMessageRAMOffset_ = 0; +SLCANRouter *CanIface::_slcan_router = nullptr; + +CanIface::CanIface(fdcan::CanType* can, BusEvent& update_event, uavcan::uint8_t self_index, + CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity) + : rx_queue_(rx_queue_buffer, rx_queue_capacity) + , can_(can) + , error_cnt_(0) + , served_aborts_cnt_(0) + , update_event_(update_event) + , peak_tx_mailbox_index_(0) + , self_index_(self_index) + , had_activity_(false) +{ + UAVCAN_ASSERT(self_index_ < UAVCAN_STM32_NUM_IFACES); + _slcan_router = SLCANRouter::get_singleton(); +} + +/* + * CanIface::RxQueue + */ +void CanIface::RxQueue::registerOverflow() +{ + if (overflow_cnt_ < 0xFFFFFFFF) { + overflow_cnt_++; + } +} + +void CanIface::RxQueue::push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags) +{ + buf_[in_].frame = frame; + buf_[in_].utc_usec = utc_usec; + buf_[in_].flags = flags; + in_++; + if (in_ >= capacity_) { + in_ = 0; + } + len_++; + if (len_ > capacity_) { + len_ = capacity_; + registerOverflow(); + out_++; + if (out_ >= capacity_) { + out_ = 0; + } + } +} + +void CanIface::RxQueue::pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags) +{ + if (len_ > 0) { + out_frame = buf_[out_].frame; + out_utc_usec = buf_[out_].utc_usec; + out_flags = buf_[out_].flags; + out_++; + if (out_ >= capacity_) { + out_ = 0; + } + len_--; + } else { + UAVCAN_ASSERT(0); + } +} + +void CanIface::RxQueue::reset() +{ + in_ = 0; + out_ = 0; + len_ = 0; + overflow_cnt_ = 0; +} + +int CanIface::computeTimings(const uavcan::uint32_t target_bitrate, Timings& out_timings) +{ + if (target_bitrate < 1) { + return -ErrInvalidBitRate; + } + + /* + * Hardware configuration + */ + const uavcan::uint32_t pclk = STM32_PLL1_Q_CK; + + static const int MaxBS1 = 16; + static const int MaxBS2 = 8; + + /* + * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG + * CAN in Automation, 2003 + * + * According to the source, optimal quanta per bit are: + * Bitrate Optimal Maximum + * 1000 kbps 8 10 + * 500 kbps 16 17 + * 250 kbps 16 17 + * 125 kbps 16 17 + */ + const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17; + + UAVCAN_ASSERT(max_quanta_per_bit <= (MaxBS1 + MaxBS2)); + + static const int MaxSamplePointLocation = 900; + + /* + * Computing (prescaler * BS): + * BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2)) -- See the Reference Manual + * BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2)) -- Simplified + * let: + * BS = 1 + BS1 + BS2 -- Number of time quanta per bit + * PRESCALER_BS = PRESCALER * BS + * ==> + * PRESCALER_BS = PCLK / BITRATE + */ + const uavcan::uint32_t prescaler_bs = pclk / target_bitrate; + + /* + * Searching for such prescaler value so that the number of quanta per bit is highest. + */ + uavcan::uint8_t bs1_bs2_sum = uavcan::uint8_t(max_quanta_per_bit - 1); + + while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0) { + if (bs1_bs2_sum <= 2) { + return -ErrInvalidBitRate; // No solution + } + bs1_bs2_sum--; + } + + const uavcan::uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum); + if ((prescaler < 1U) || (prescaler > 1024U)) { + return -ErrInvalidBitRate; // No solution + } + + /* + * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum. + * We need to find the values so that the sample point is as close as possible to the optimal value. + * + * Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2] (* Where 7/8 is 0.875, the recommended sample point location *) + * {{bs2 -> (1 + bs1)/7}} + * + * Hence: + * bs2 = (1 + bs1) / 7 + * bs1 = (7 * bs1_bs2_sum - 1) / 8 + * + * Sample point location can be computed as follows: + * Sample point location = (1 + bs1) / (1 + bs1 + bs2) + * + * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one: + * - With rounding to nearest + * - With rounding to zero + */ + struct BsPair { + uavcan::uint8_t bs1; + uavcan::uint8_t bs2; + uavcan::uint16_t sample_point_permill; + + BsPair() : + bs1(0), + bs2(0), + sample_point_permill(0) + { } + + BsPair(uavcan::uint8_t bs1_bs2_sum, uavcan::uint8_t arg_bs1) : + bs1(arg_bs1), + bs2(uavcan::uint8_t(bs1_bs2_sum - bs1)), + sample_point_permill(uavcan::uint16_t(1000 * (1 + bs1) / (1 + bs1 + bs2))) + { + UAVCAN_ASSERT(bs1_bs2_sum > arg_bs1); + } + + bool isValid() const + { + return (bs1 >= 1) && (bs1 <= MaxBS1) && (bs2 >= 1) && (bs2 <= MaxBS2); + } + }; + + // First attempt with rounding to nearest + BsPair solution(bs1_bs2_sum, uavcan::uint8_t(((7 * bs1_bs2_sum - 1) + 4) / 8)); + + if (solution.sample_point_permill > MaxSamplePointLocation) { + // Second attempt with rounding to zero + solution = BsPair(bs1_bs2_sum, uavcan::uint8_t((7 * bs1_bs2_sum - 1) / 8)); + } + + /* + * Final validation + * Helpful Python: + * def sample_point_from_btr(x): + * assert 0b0011110010000000111111000000000 & x == 0 + * ts2,ts1,brp = (x>>20)&7, (x>>16)&15, x&511 + * return (1+ts1+1)/(1+ts1+1+ts2+1) + * + */ + if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !solution.isValid()) { + UAVCAN_ASSERT(0); + return -ErrLogic; + } + + UAVCAN_STM32_LOG("Timings: quanta/bit: %d, sample point location: %.1f%%", + int(1 + solution.bs1 + solution.bs2), float(solution.sample_point_permill) / 10.F); + + out_timings.prescaler = uavcan::uint16_t(prescaler - 1U); + out_timings.sjw = 0; // Which means one + out_timings.bs1 = uavcan::uint8_t(solution.bs1 - 1); + out_timings.bs2 = uavcan::uint8_t(solution.bs2 - 1); + return 0; +} + +uavcan::int16_t CanIface::send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, + uavcan::CanIOFlags flags) +{ + if (frame.isErrorFrame() || frame.dlc > 8) { + return -ErrUnsupportedFrame; + } + + /* + * Normally we should perform the same check as in @ref canAcceptNewTxFrame(), because + * it is possible that the highest-priority frame between select() and send() could have been + * replaced with a lower priority one due to TX timeout. But we don't do this check because: + * + * - It is a highly unlikely scenario. + * + * - Frames do not timeout on a properly functioning bus. Since frames do not timeout, the new + * frame can only have higher priority, which doesn't break the logic. + * + * - If high-priority frames are timing out in the TX queue, there's probably a lot of other + * issues to take care of before this one becomes relevant. + * + * - It takes CPU time. Not just CPU time, but critical section time, which is expensive. + */ + CriticalSectionLocker lock; + + /* + * Seeking for an empty slot + */ + uavcan::uint8_t index; + + if ((can_->TXFQS & FDCAN_TXFQS_TFQF) != 0) { + return false; //we don't have free space + } + index = ((can_->TXFQS & FDCAN_TXFQS_TFQPI) >> FDCAN_TXFQS_TFQPI_Pos); + + // Copy Frame to RAM + // Calculate Tx element address + uint32_t* buffer = (uint32_t *)(MessageRam_.TxFIFOQSA + (index * FDCAN_FRAME_BUFFER_SIZE * 4)); + + //Setup Frame ID + if (frame.isExtended()) { + buffer[0] = (fdcan::IDE | frame.id); + } else { + buffer[0] = (frame.id << 18); + } + if (frame.isRemoteTransmissionRequest()) { + buffer[0] |= fdcan::RTR; + } + //Write Data Length Code, and Message Marker + buffer[1] = frame.dlc << 16 | index << 24; + + // Write Frame to the message RAM + buffer[2] = (uavcan::uint32_t(frame.data[3]) << 24) | + (uavcan::uint32_t(frame.data[2]) << 16) | + (uavcan::uint32_t(frame.data[1]) << 8) | + (uavcan::uint32_t(frame.data[0]) << 0); + buffer[3] = (uavcan::uint32_t(frame.data[7]) << 24) | + (uavcan::uint32_t(frame.data[6]) << 16) | + (uavcan::uint32_t(frame.data[5]) << 8) | + (uavcan::uint32_t(frame.data[4]) << 0); + //Set Add Request + can_->TXBAR = (1 << index); + + //Registering the pending transmission so we can track its deadline and loopback it as needed + pending_tx_[index].deadline = tx_deadline; + pending_tx_[index].frame = frame; + pending_tx_[index].loopback = (flags & uavcan::CanIOFlagLoopback) != 0; + pending_tx_[index].abort_on_error = (flags & uavcan::CanIOFlagAbortOnError) != 0; + pending_tx_[index].index = index; + return 1; +} + +uavcan::int16_t CanIface::receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, + uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) +{ + out_ts_monotonic = clock::getMonotonic(); // High precision is not required for monotonic timestamps + uavcan::uint64_t utc_usec = 0; + { + CriticalSectionLocker lock; + if (rx_queue_.getLength() == 0) { + return 0; + } + rx_queue_.pop(out_frame, utc_usec, out_flags); + } + out_ts_utc = uavcan::UtcTime::fromUSec(utc_usec); + return 1; +} + +uavcan::int16_t CanIface::configureFilters(const uavcan::CanFilterConfig* filter_configs, + uavcan::uint16_t num_configs) +{ + uint32_t num_extid = 0, num_stdid = 0; + uint32_t total_available_list_size = MAX_FILTER_LIST_SIZE; + uint32_t* filter_ptr; + //count number of frames of each type + for (uint8_t i = 0; i < num_configs; i++) { + const uavcan::CanFilterConfig* const cfg = filter_configs + i; + if ((cfg->id & uavcan::CanFrame::FlagEFF) || !(cfg->mask & uavcan::CanFrame::FlagEFF)) { + num_extid++; + } else { + num_stdid++; + } + } + + CriticalSectionLocker lock; + can_->CCCR |= FDCAN_CCCR_INIT; // Request init + while ((can_->CCCR & FDCAN_CCCR_INIT) == 0) {} + can_->CCCR |= FDCAN_CCCR_CCE; //Enable Config change + + //Allocate Message RAM for Standard ID Filter List + if (num_stdid == 0) { //No Frame with Standard ID is to be accepted + can_->GFC |= 0x2; //Reject All Standard ID Frames + } else if ((num_stdid < total_available_list_size) && (num_stdid <= 128)) { + can_->SIDFC = (FDCANMessageRAMOffset_ << 2) | (num_stdid << 16); + MessageRam_.StandardFilterSA = SRAMCAN_BASE + (FDCANMessageRAMOffset_ * 4U); + FDCANMessageRAMOffset_ += num_stdid; + total_available_list_size -= num_stdid; + can_->GFC |= (0x3U << 4); //Reject non matching Standard frames + } else { //The List is too big, return fail + can_->CCCR &= ~FDCAN_CCCR_INIT; // Leave init mode + return -ErrFilterNumConfigs; + } + + if (num_stdid) { + num_stdid = 0; //reset list count + filter_ptr = (uint32_t*)MessageRam_.StandardFilterSA; + //Run through the filter list and setup standard id filter list + for (uint8_t i = 0; i < num_configs; i++) { + uint32_t id = 0; + uint32_t mask = 0; + const uavcan::CanFilterConfig* const cfg = filter_configs + i; + if (!((cfg->id & uavcan::CanFrame::FlagEFF) || !(cfg->mask & uavcan::CanFrame::FlagEFF))) { + id = (cfg->id & uavcan::CanFrame::MaskStdID); // Regular std frames, nothing fancy. + mask = (cfg->mask & 0x7F); + filter_ptr[num_stdid] = 0x2U << 30 | //Classic CAN Filter + 0x1U << 27 | //Store in Rx FIFO0 if filter matches + id << 16 | + mask; + num_stdid++; + } + } + } + + //Allocate Message RAM for Extended ID Filter List + if (num_extid == 0) { //No Frame with Extended ID is to be accepted + can_->GFC |= 0x1; //Reject All Extended ID Frames + } else if ((num_extid < (total_available_list_size/2)) && (num_extid <= 64)) { + can_->XIDFC = (FDCANMessageRAMOffset_ << 2) | (num_extid << 16); + MessageRam_.ExtendedFilterSA = SRAMCAN_BASE + (FDCANMessageRAMOffset_ * 4U); + FDCANMessageRAMOffset_ += num_extid*2; + can_->GFC = (0x3U << 2); // Reject non matching Extended frames + } else { //The List is too big, return fail + can_->CCCR &= ~FDCAN_CCCR_INIT; // Leave init mode + return -ErrFilterNumConfigs; + } + + if (num_extid) { + num_extid = 0; + filter_ptr = (uint32_t*)MessageRam_.ExtendedFilterSA; + //Run through the filter list and setup extended id filter list + for (uint8_t i = 0; i < num_configs; i++) { + uint32_t id = 0; + uint32_t mask = 0; + const uavcan::CanFilterConfig* const cfg = filter_configs + i; + if ((cfg->id & uavcan::CanFrame::FlagEFF) || !(cfg->mask & uavcan::CanFrame::FlagEFF)) { + id = (cfg->id & uavcan::CanFrame::MaskExtID); + mask = (cfg->mask & uavcan::CanFrame::MaskExtID); + filter_ptr[num_extid*2] = 0x1U << 29 | id; // Classic CAN Filter + filter_ptr[num_extid*2 + 1] = 0x2U << 30 | mask; //Store in Rx FIFO0 if filter matches + num_extid++; + } + } + } + + MessageRam_.EndAddress = SRAMCAN_BASE + (FDCANMessageRAMOffset_ * 4U); + if (MessageRam_.EndAddress > MESSAGE_RAM_END_ADDR) { + //We are overflowing the limit of Allocated Message RAM + AP_HAL::panic("CANFDIface: Message RAM Overflow!"); + } + + can_->CCCR &= ~FDCAN_CCCR_INIT; // Leave init mode + return 0; +} + +uavcan::uint16_t CanIface::getNumFilters() const +{ + return MAX_FILTER_LIST_SIZE; +} + +int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode) +{ + // Setup FDCAN for configuration mode and disable all interrupts + { + CriticalSectionLocker lock; + + can_->CCCR &= ~FDCAN_CCCR_CSR; // Exit sleep mode + while ((can_->CCCR & FDCAN_CCCR_CSA) == FDCAN_CCCR_CSA) {} //Wait for wake up ack + can_->CCCR |= FDCAN_CCCR_INIT; // Request init + while ((can_->CCCR & FDCAN_CCCR_INIT) == 0) {} + can_->CCCR |= FDCAN_CCCR_CCE; //Enable Config change + can_->IE = 0; // Disable interrupts while initialization is in progress + } + + /* + * Object state - interrupts are disabled, so it's safe to modify it now + */ + rx_queue_.reset(); + error_cnt_ = 0; + served_aborts_cnt_ = 0; + uavcan::fill_n(pending_tx_, NumTxMailboxes, TxItem()); + peak_tx_mailbox_index_ = 0; + had_activity_ = false; + + /* + * CAN timings for this bitrate + */ + Timings timings; + const int timings_res = computeTimings(bitrate, timings); + if (timings_res < 0) { + can_->CCCR &= ~FDCAN_CCCR_INIT; + return timings_res; + } + UAVCAN_STM32_LOG("Timings: presc=%u sjw=%u bs1=%u bs2=%u", + 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_TSEG2_Pos) | + (timings.prescaler << FDCAN_NBTP_NBRP_Pos)); + + //RX Config + can_->RXESC = 0; //Set for 8Byte Frames + + //Setup Message RAM + setupMessageRam(); + //Clear all Interrupts + can_->IR = 0x3FFFFFFF; + //Enable Interrupts + can_->IE = FDCAN_IE_TCE | // Transmit Complete interrupt enable + FDCAN_IE_RF0NE | // RX FIFO 0 new message + FDCAN_IE_RF0FE | // Rx FIFO 1 FIFO Full + FDCAN_IE_RF1NE | // RX FIFO 1 new message + FDCAN_IE_RF1FE; // Rx FIFO 1 FIFO Full + can_->ILS = FDCAN_ILS_TCL; //Set Line 1 for Transmit Complete Event Interrupt + can_->TXBTIE = 0xFFFFFFFF; + can_->ILE = 0x3; + + //Leave Init + can_->CCCR &= ~FDCAN_CCCR_INIT; // Leave init mode + return 0; +} + +void CanIface::setupMessageRam() +{ + uint32_t num_elements = 0; + + // Rx FIFO 0 start address and element count + num_elements = MIN((FDCAN_NUM_RXFIFO0_SIZE/FDCAN_FRAME_BUFFER_SIZE), 64U); + if (num_elements) { + can_->RXF0C = (FDCANMessageRAMOffset_ << 2) | (num_elements << 16); + MessageRam_.RxFIFO0SA = SRAMCAN_BASE + (FDCANMessageRAMOffset_ * 4U); + FDCANMessageRAMOffset_ += num_elements*FDCAN_FRAME_BUFFER_SIZE; + } + + // Tx FIFO/queue start address and element count + num_elements = MIN((FDCAN_TX_FIFO_BUFFER_SIZE/FDCAN_FRAME_BUFFER_SIZE), 32U); + if (num_elements) { + can_->TXBC = (FDCANMessageRAMOffset_ << 2) | (num_elements << 24); + can_->TXBC |= 1U << 30; //Set Queue mode + MessageRam_.TxFIFOQSA = SRAMCAN_BASE + (FDCANMessageRAMOffset_ * 4U); + FDCANMessageRAMOffset_ += num_elements*FDCAN_FRAME_BUFFER_SIZE; + } + MessageRam_.EndAddress = SRAMCAN_BASE + (FDCANMessageRAMOffset_ * 4U); + if (MessageRam_.EndAddress > MESSAGE_RAM_END_ADDR) { + //We are overflowing the limit of Allocated Message RAM + AP_HAL::panic("CANFDIface: Message RAM Overflow!"); + return; + } +} + +void CanIface::handleTxCompleteInterrupt(const uavcan::uint64_t utc_usec) +{ + for (uint8_t i = 0; i < NumTxMailboxes; i++) { + if ((can_->TXBTO & (1UL << i))) { + if (pending_tx_[i].loopback && had_activity_) { + rx_queue_.push(pending_tx_[i].frame, utc_usec, uavcan::CanIOFlagLoopback); + } + } + } +} + +bool CanIface::readRxFIFO(uavcan::uint8_t fifo_index) +{ + UAVCAN_ASSERT(fifo_index < 2); + uint32_t *frame_ptr; + uint32_t index; + uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); + if (fifo_index == 0) { + //Check if RAM allocated to RX FIFO + if ((can_->RXF0C & FDCAN_RXF0C_F0S) == 0) { + UAVCAN_ASSERT(0); + return false; + } + //Register Message Lost as a hardware error + if ((can_->RXF0S & FDCAN_RXF0S_RF0L) != 0) { + error_cnt_++; + } + + if ((can_->RXF0S & FDCAN_RXF0S_F0FL) == 0) { + return false; //No More messages in FIFO + } else { + index = ((can_->RXF0S & FDCAN_RXF0S_F0GI) >> 8); + frame_ptr = (uint32_t *)(MessageRam_.RxFIFO0SA + (index * FDCAN_FRAME_BUFFER_SIZE * 4)); + } + } else if (fifo_index == 1) { + //Check if RAM allocated to RX FIFO + if ((can_->RXF1C & FDCAN_RXF1C_F1S) == 0) { + UAVCAN_ASSERT(0); + return false; + } + //Register Message Lost as a hardware error + if ((can_->RXF1S & FDCAN_RXF1S_RF1L) != 0) { + error_cnt_++; + } + + if ((can_->RXF1S & FDCAN_RXF1S_F1FL) == 0) { + return false; + } else { + index = ((can_->RXF1S & FDCAN_RXF1S_F1GI) >> 8); + frame_ptr = (uint32_t *)(MessageRam_.RxFIFO1SA + (index * FDCAN_FRAME_BUFFER_SIZE * 4)); + } + } else { + return false; + } + + // Read the frame contents + uavcan::CanFrame frame; + uint32_t id = frame_ptr[0]; + if ((id & fdcan::IDE) == 0) { + //Standard ID + frame.id = ((id & fdcan::STID_MASK) >> 18) & uavcan::CanFrame::MaskStdID; + } else { + //Extended ID + frame.id = (id & fdcan::EXID_MASK) & uavcan::CanFrame::MaskExtID; + frame.id |= uavcan::CanFrame::FlagEFF; + } + + if ((id & fdcan::RTR) != 0) { + frame.id |= uavcan::CanFrame::FlagRTR; + } + frame.dlc = (frame_ptr[1] & fdcan::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++) { + frame.data[i] = data[i]; + } + + //Acknowledge the FIFO entry we just read + if (fifo_index == 0) { + can_->RXF0A = index; + } else if (fifo_index == 1) { + can_->RXF1A = index; + } + + /* + * Store with timeout into the FIFO buffer and signal update event + */ + rx_queue_.push(frame, utc_usec, 0); +#if !HAL_MINIMIZE_FEATURES + if (_slcan_router != nullptr) { + _slcan_router->route_frame_to_slcan(this, frame, utc_usec); + } +#endif + return true; +} + +void CanIface::handleRxInterrupt(uavcan::uint8_t fifo_index) +{ + while (readRxFIFO(fifo_index)) { + had_activity_ = true; + } + update_event_.signalFromInterrupt(); +} + +void CanIface::pollErrorFlagsFromISR() +{ + const uavcan::uint8_t cel = can_->ECR >> 16; + + if (cel != 0) { + for (int i = 0; i < NumTxMailboxes; i++) { + if (!pending_tx_[i].abort_on_error) { + continue; + } + if (((1 << pending_tx_[i].index) & can_->TXBRP)) { + can_->TXBCR = 1 << pending_tx_[i].index; // Goodnight sweet transmission + error_cnt_++; + //Wait for Cancelation to finish + while (!(can_->TXBCF & (1 << pending_tx_[i].index))) {} + served_aborts_cnt_++; + } + } + } +} + +void CanIface::discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time) +{ + CriticalSectionLocker lock; + for (int i = 0; i < NumTxMailboxes; i++) { + if (((1 << pending_tx_[i].index) & can_->TXBRP) && pending_tx_[i].deadline < current_time) { + can_->TXBCR = 1 << pending_tx_[i].index; // Goodnight sweet transmission + error_cnt_++; + //Wait for Cancelation to finish + while (!(can_->TXBCF & (1 << pending_tx_[i].index))) {} + } + } +} + +bool CanIface::canAcceptNewTxFrame(const uavcan::CanFrame& frame) const +{ + //Check if Tx FIFO is allocated + if ((can_->TXBC & FDCAN_TXBC_TFQS) == 0) { + return false; + } + if ((can_->TXFQS & FDCAN_TXFQS_TFQF) != 0) { + return false; //we don't have free space + } + + return true; +} + +bool CanIface::isRxBufferEmpty() const +{ + CriticalSectionLocker lock; + return rx_queue_.getLength() == 0; +} + +uavcan::uint64_t CanIface::getErrorCount() const +{ + CriticalSectionLocker lock; + return error_cnt_ + rx_queue_.getOverflowCount(); +} + +unsigned CanIface::getRxQueueLength() const +{ + CriticalSectionLocker lock; + return rx_queue_.getLength(); +} + +bool CanIface::hadActivity() +{ + CriticalSectionLocker lock; + const bool ret = had_activity_; + had_activity_ = false; + return ret; +} + +/* + * CanDriver + */ +uavcan::CanSelectMasks CanDriver::makeSelectMasks(const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces]) const +{ + uavcan::CanSelectMasks msk; + + for (uavcan::uint8_t i = 0; i < num_ifaces_; i++) { + CanIface* iface = ifaces[if_int_to_gl_index_[i]]; + msk.read |= (iface->isRxBufferEmpty() ? 0 : 1) << i; + + if (pending_tx[i] != UAVCAN_NULLPTR) { + msk.write |= (iface->canAcceptNewTxFrame(*pending_tx[i]) ? 1 : 0) << i; + } + } + + return msk; +} + +bool CanDriver::hasReadableInterfaces() const +{ + for (uavcan::uint8_t i = 0; i < num_ifaces_; i++) { + if (!ifaces[if_int_to_gl_index_[i]]->isRxBufferEmpty()) { + return true; + } + } + + return false; +} + +uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks, + const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces], + const uavcan::MonotonicTime blocking_deadline) +{ + const uavcan::CanSelectMasks in_masks = inout_masks; + const uavcan::MonotonicTime time = clock::getMonotonic(); + + for (uavcan::uint8_t i = 0; i < num_ifaces_; i++) { + CanIface* iface = ifaces[if_int_to_gl_index_[i]]; + iface->discardTimedOutTxMailboxes(time); // Check TX timeouts - this may release some TX slots + { + CriticalSectionLocker cs_locker; + iface->pollErrorFlagsFromISR(); + } + } + + inout_masks = makeSelectMasks(pending_tx); // Check if we already have some of the requested events + if ((inout_masks.read & in_masks.read) != 0 || + (inout_masks.write & in_masks.write) != 0) { + return 1; + } + + (void)update_event_.wait(blocking_deadline - time); // Block until timeout expires or any iface updates + inout_masks = makeSelectMasks(pending_tx); // Return what we got even if none of the requested events are set + return 1; // Return value doesn't matter as long as it is non-negative +} + +void CanDriver::initOnce() +{ + { + CriticalSectionLocker lock; + RCC->APB1HRSTR |= RCC_APB1HRSTR_FDCANRST; + RCC->APB1HRSTR &= ~RCC_APB1HRSTR_FDCANRST; + RCC->APB1HENR |= RCC_APB1HENR_FDCANEN; + } + + /* + * IRQ + */ + { + CriticalSectionLocker lock; + nvicEnableVector(FDCAN1_IT0_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + nvicEnableVector(FDCAN1_IT1_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); +# if UAVCAN_STM32_NUM_IFACES > 1 + nvicEnableVector(FDCAN2_IT0_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + nvicEnableVector(FDCAN2_IT1_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); +# endif + } +} + +int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode) +{ + int res = 0; + + UAVCAN_STM32_LOG("Bitrate %lu mode %d", static_cast(bitrate), static_cast(mode)); + + static bool initialized_once = false; + if (!initialized_once) { + initialized_once = true; + UAVCAN_STM32_LOG("First initialization"); + initOnce(); + } + + /* + * CAN1 + */ + UAVCAN_STM32_LOG("Initing iface 0..."); + ifaces[0] = &if0_; // This link must be initialized first, + res = if0_.init(bitrate, mode); // otherwise an IRQ may fire while the interface is not linked yet; + if (res < 0) { // a typical race condition. + UAVCAN_STM32_LOG("Iface 0 init failed %i", res); + ifaces[0] = UAVCAN_NULLPTR; + goto fail; + } + + /* + * CAN2 + */ +#if UAVCAN_STM32_NUM_IFACES > 1 + UAVCAN_STM32_LOG("Initing iface 1..."); + ifaces[1] = &if1_; // Same thing here. + res = if1_.init(bitrate, mode); + if (res < 0) { + UAVCAN_STM32_LOG("Iface 1 init failed %i", res); + ifaces[1] = UAVCAN_NULLPTR; + goto fail; + } +#endif + + UAVCAN_STM32_LOG("CAN drv init OK"); + UAVCAN_ASSERT(res >= 0); + return res; + +fail: + UAVCAN_STM32_LOG("CAN drv init failed %i", res); + UAVCAN_ASSERT(res < 0); + return res; +} + +bool CanDriver::clock_init_ = false; +void CanDriver::initOnce(uavcan::uint8_t can_number, bool enable_irqs) +{ + //Only do it once + //Doing it second time will reset the previously initialised bus + if (!clock_init_) { + CriticalSectionLocker lock; + RCC->APB1HENR |= RCC_APB1HENR_FDCANEN; + RCC->APB1HRSTR |= RCC_APB1HRSTR_FDCANRST; + RCC->APB1HRSTR &= ~RCC_APB1HRSTR_FDCANRST; + clock_init_ = true; + } + + if (!enable_irqs) { + return; + } + /* + * IRQ + */ + { + CriticalSectionLocker lock; + if (can_number == 0) { + nvicEnableVector(FDCAN1_IT0_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + nvicEnableVector(FDCAN1_IT1_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + } +# if UAVCAN_STM32_NUM_IFACES > 1 + else if (can_number == 1) { + nvicEnableVector(FDCAN2_IT0_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + nvicEnableVector(FDCAN2_IT1_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + } +# endif + } +} + +int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode, uavcan::uint8_t can_number) +{ + int res = 0; + + UAVCAN_STM32_LOG("Bitrate %lu mode %d", static_cast(bitrate), static_cast(mode)); + if (can_number > UAVCAN_STM32_NUM_IFACES) { + res = -1; + goto fail; + } + static bool initialized_once[UAVCAN_STM32_NUM_IFACES] = {false}; + + if (!initialized_once[can_number]) { + initialized_once[can_number] = true; + initialized_by_me_[can_number] = true; + + if (can_number == 1 && !initialized_once[0]) { + UAVCAN_STM32_LOG("Iface 0 is not initialized yet but we need it for Iface 1, trying to init it"); + UAVCAN_STM32_LOG("Enabling CAN iface 0"); + initOnce(0, false); + UAVCAN_STM32_LOG("Initing iface 0..."); + res = if0_.init(bitrate, mode); + + if (res < 0) { + UAVCAN_STM32_LOG("Iface 0 init failed %i", res); + goto fail; + } + } + + UAVCAN_STM32_LOG("Enabling CAN iface %d", can_number); + initOnce(can_number, true); + } else if (!initialized_by_me_[can_number]) { + UAVCAN_STM32_LOG("CAN iface %d initialized in another CANDriver!", can_number); + res = -2; + goto fail; + } + + if (can_number == 0) { + /* + * CAN1 + */ + UAVCAN_STM32_LOG("Initing iface 0..."); + ifaces[0] = &if0_; // This link must be initialized first, + res = if0_.init(bitrate, mode); // otherwise an IRQ may fire while the interface is not linked yet; + if (res < 0) { // a typical race condition. + UAVCAN_STM32_LOG("Iface 0 init failed %i", res); + ifaces[0] = UAVCAN_NULLPTR; + goto fail; + } + } else if (can_number == 1) { + /* + * CAN2 + */ +#if UAVCAN_STM32_NUM_IFACES > 1 + UAVCAN_STM32_LOG("Initing iface 1..."); + ifaces[1] = &if1_; // Same thing here. + res = if1_.init(bitrate, mode); + if (res < 0) { + UAVCAN_STM32_LOG("Iface 1 init failed %i", res); + ifaces[1] = UAVCAN_NULLPTR; + goto fail; + } +#endif + } + + if_int_to_gl_index_[num_ifaces_++] = can_number; + + UAVCAN_STM32_LOG("CAN drv init OK"); + UAVCAN_ASSERT(res >= 0); + return res; + +fail: + UAVCAN_STM32_LOG("CAN drv init failed %i", res); + UAVCAN_ASSERT(res < 0); + return res; +} + +CanIface* CanDriver::getIface(uavcan::uint8_t iface_index) +{ + if (iface_index < num_ifaces_) { + return ifaces[if_int_to_gl_index_[iface_index]]; + } + return UAVCAN_NULLPTR; +} + +bool CanDriver::hadActivity() +{ + for (uavcan::uint8_t i = 0; i < num_ifaces_; i++) { + if (ifaces[if_int_to_gl_index_[i]]->hadActivity()) { + return true; + } + } + + return false; +} + +} // namespace uavcan_stm32 + +/* + * Interrupt handlers + */ +extern "C" +{ + + UAVCAN_STM32_IRQ_HANDLER(FDCAN1_IT0_IRQHandler); + UAVCAN_STM32_IRQ_HANDLER(FDCAN1_IT0_IRQHandler) + { + UAVCAN_STM32_IRQ_PROLOGUE(); + ChibiOS_CAN::handleInterrupt(0, 0); + UAVCAN_STM32_IRQ_EPILOGUE(); + } + + UAVCAN_STM32_IRQ_HANDLER(FDCAN1_IT1_IRQHandler); + UAVCAN_STM32_IRQ_HANDLER(FDCAN1_IT1_IRQHandler) + { + UAVCAN_STM32_IRQ_PROLOGUE(); + ChibiOS_CAN::handleInterrupt(0, 1); + UAVCAN_STM32_IRQ_EPILOGUE(); + } + + +# if UAVCAN_STM32_NUM_IFACES > 1 + + UAVCAN_STM32_IRQ_HANDLER(FDCAN2_IT0_IRQHandler); + UAVCAN_STM32_IRQ_HANDLER(FDCAN2_IT0_IRQHandler) + { + UAVCAN_STM32_IRQ_PROLOGUE(); + ChibiOS_CAN::handleInterrupt(1, 0); + UAVCAN_STM32_IRQ_EPILOGUE(); + } + + UAVCAN_STM32_IRQ_HANDLER(FDCAN2_IT1_IRQHandler); + UAVCAN_STM32_IRQ_HANDLER(FDCAN2_IT1_IRQHandler) + { + UAVCAN_STM32_IRQ_PROLOGUE(); + ChibiOS_CAN::handleInterrupt(1, 1); + UAVCAN_STM32_IRQ_EPILOGUE(); + } + +# endif + +} // extern "C" + +#endif //defined(STM32H7XX) + +#endif //HAL_WITH_UAVCAN diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h new file mode 100644 index 0000000000..69f6a57d1d --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.h @@ -0,0 +1,448 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2014 Pavel Kirienko + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Siddharth Bharat Purohit + */ + +#pragma once + +#include "AP_HAL_ChibiOS.h" + +#if HAL_WITH_UAVCAN + +#include "CANThread.h" +#include "fdcan.hpp" + +class SLCANRouter; + +namespace ChibiOS_CAN +{ +/** + * Driver error codes. + * These values can be returned from driver functions negated. + */ +//static const uavcan::int16_t ErrUnknown = 1000; ///< Reserved for future use +static const uavcan::int16_t ErrNotImplemented = 1001; ///< Feature not implemented +static const uavcan::int16_t ErrInvalidBitRate = 1002; ///< Bit rate not supported +static const uavcan::int16_t ErrLogic = 1003; ///< Internal logic error +static const uavcan::int16_t ErrUnsupportedFrame = 1004; ///< Frame not supported (e.g. RTR, CAN FD, etc) +static const uavcan::int16_t ErrMsrInakNotSet = 1005; ///< INAK bit of the MSR register is not 1 +static const uavcan::int16_t ErrMsrInakNotCleared = 1006; ///< INAK bit of the MSR register is not 0 +static const uavcan::int16_t ErrBitRateNotDetected = 1007; ///< Auto bit rate detection could not be finished +static const uavcan::int16_t ErrFilterNumConfigs = 1008; ///< Number of filters is more than supported + +/** + * RX queue item. + * The application shall not use this directly. + */ +struct CanRxItem { + uavcan::uint64_t utc_usec; + uavcan::CanFrame frame; + uavcan::CanIOFlags flags; + CanRxItem() + : utc_usec(0) + , flags(0) + { } +}; + +/** + * Single CAN iface. + * The application shall not use this directly. + */ +class CanIface : public uavcan::ICanIface, uavcan::Noncopyable +{ + friend class ::SLCANRouter; + static SLCANRouter *_slcan_router; + class RxQueue + { + CanRxItem* const buf_; + const uavcan::uint8_t capacity_; + uavcan::uint8_t in_; + uavcan::uint8_t out_; + uavcan::uint8_t len_; + uavcan::uint32_t overflow_cnt_; + void registerOverflow(); + + public: + RxQueue(CanRxItem* buf, uavcan::uint8_t capacity) + : buf_(buf) + , capacity_(capacity) + , in_(0) + , out_(0) + , len_(0) + , overflow_cnt_(0) + { } + + void push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags); + void pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags); + + void reset(); + + unsigned getLength() const + { + return len_; + } + + uavcan::uint32_t getOverflowCount() const + { + return overflow_cnt_; + } + }; + + struct MessageRAM { + uavcan::uint32_t StandardFilterSA; + uavcan::uint32_t ExtendedFilterSA; + uavcan::uint32_t RxFIFO0SA; + uavcan::uint32_t RxFIFO1SA; + uavcan::uint32_t TxFIFOQSA; + uavcan::uint32_t EndAddress; + } MessageRam_; + + struct Timings { + uavcan::uint16_t prescaler; + uavcan::uint8_t sjw; + uavcan::uint8_t bs1; + uavcan::uint8_t bs2; + + Timings() + : prescaler(0) + , sjw(0) + , bs1(0) + , bs2(0) + { } + }; + + struct TxItem { + uavcan::MonotonicTime deadline; + uavcan::CanFrame frame; + bool loopback; + bool abort_on_error; + uint8_t index; + TxItem() : + loopback(false), + abort_on_error(false) + { } + }; + + enum { NumTxMailboxes = 32 }; + + static const uavcan::uint32_t TSR_ABRQx[NumTxMailboxes]; + static uint32_t FDCANMessageRAMOffset_; + RxQueue rx_queue_; + fdcan::CanType* const can_; + uavcan::uint64_t error_cnt_; + uavcan::uint32_t served_aborts_cnt_; + BusEvent& update_event_; + TxItem pending_tx_[NumTxMailboxes]; + uavcan::uint8_t peak_tx_mailbox_index_; + const uavcan::uint8_t self_index_; + bool had_activity_; + int computeTimings(uavcan::uint32_t target_bitrate, Timings& out_timings); + + virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, + uavcan::CanIOFlags flags); + + virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, + uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags); + + virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs, + uavcan::uint16_t num_configs); + + virtual uavcan::uint16_t getNumFilters() const; + + void setupMessageRam(void); + + static uint32_t FDCAN2MessageRAMOffset_; +public: + enum { MaxRxQueueCapacity = 254 }; + + enum OperatingMode { + NormalMode, + SilentMode + }; + + CanIface(fdcan::CanType* can, BusEvent& update_event, uavcan::uint8_t self_index, + CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity); + + /** + * Initializes the hardware CAN controller. + * Assumes: + * - Iface clock is enabled + * - Iface has been resetted via RCC + * - Caller will configure NVIC by itself + */ + int init(const uavcan::uint32_t bitrate, const OperatingMode mode); + + void handleTxCompleteInterrupt(uavcan::uint64_t utc_usec); + void handleRxInterrupt(uavcan::uint8_t fifo_index); + + bool readRxFIFO(uavcan::uint8_t fifo_index); + /** + * This method is used to count errors and abort transmission on error if necessary. + * This functionality used to be implemented in the SCE interrupt handler, but that approach was + * generating too much processing overhead, especially on disconnected interfaces. + * + * Should be called from RX ISR, TX ISR, and select(); interrupts must be enabled. + */ + void pollErrorFlagsFromISR(); + + void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time); + + bool canAcceptNewTxFrame(const uavcan::CanFrame& frame) const; + bool isRxBufferEmpty() const; + + /** + * Number of RX frames lost due to queue overflow. + * This is an atomic read, it doesn't require a critical section. + */ + uavcan::uint32_t getRxQueueOverflowCount() const + { + return rx_queue_.getOverflowCount(); + } + + /** + * Total number of hardware failures and other kinds of errors (e.g. queue overruns). + * May increase continuously if the interface is not connected to the bus. + */ + virtual uavcan::uint64_t getErrorCount() const; + + /** + * Number of times the driver exercised library's requirement to abort transmission on first error. + * This is an atomic read, it doesn't require a critical section. + * See @ref uavcan::CanIOFlagAbortOnError. + */ + uavcan::uint32_t getVoluntaryTxAbortCount() const + { + return served_aborts_cnt_; + } + + /** + * Returns the number of frames pending in the RX queue. + * This is intended for debug use only. + */ + unsigned getRxQueueLength() const; + + /** + * Whether this iface had at least one successful IO since the previous call of this method. + * This is designed for use with iface activity LEDs. + */ + bool hadActivity(); + + /** + * Peak number of TX mailboxes used concurrently since initialization. + * Range is [1, 3]. + * Value of 3 suggests that priority inversion could be taking place. + */ + uavcan::uint8_t getPeakNumTxMailboxesUsed() const + { + return uavcan::uint8_t(peak_tx_mailbox_index_ + 1); + } + + fdcan::CanType* can_reg(void) + { + return can_; + } +}; + +/** + * CAN driver, incorporates all available CAN ifaces. + * Please avoid direct use, prefer @ref CanInitHelper instead. + */ +class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable +{ + BusEvent update_event_; + static bool clock_init_; + CanIface if0_; +#if UAVCAN_STM32_NUM_IFACES > 1 + CanIface if1_; +#endif + bool initialized_by_me_[UAVCAN_STM32_NUM_IFACES]; + uavcan::uint8_t num_ifaces_; + uavcan::uint8_t if_int_to_gl_index_[UAVCAN_STM32_NUM_IFACES]; + + + virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, + const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces], + uavcan::MonotonicTime blocking_deadline); + + static void initOnce(); + static void initOnce(uavcan::uint8_t can_number, bool enable_irqs); + +public: + template + CanDriver(CanRxItem(&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity]) + : update_event_(*this) + , if0_(fdcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity) +#if UAVCAN_STM32_NUM_IFACES > 1 + , if1_(fdcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity) +#endif + { + uavcan::StaticAssert<(RxQueueCapacity <= CanIface::MaxRxQueueCapacity)>::check(); + } + + /** + * This function returns select masks indicating which interfaces are available for read/write. + */ + uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces]) const; + + BusEvent* getUpdateEvent() + { + return &update_event_; + } + /** + * Whether there's at least one interface where receive() would return a frame. + */ + bool hasReadableInterfaces() const; + + /** + * Returns zero if OK. + * Returns negative value if failed (e.g. invalid bitrate). + */ + int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode); + int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode, uavcan::uint8_t can_number); + + virtual CanIface* getIface(uavcan::uint8_t iface_index); + + virtual uavcan::uint8_t getNumIfaces() const + { + return num_ifaces_; + } + + /** + * Whether at least one iface had at least one successful IO since previous call of this method. + * This is designed for use with iface activity LEDs. + */ + bool hadActivity(); +}; + +/** + * Helper class. + * Normally only this class should be used by the application. + * 145 usec per Extended CAN frame @ 1 Mbps, e.g. 32 RX slots * 145 usec --> 4.6 msec before RX queue overruns. + */ +template +class CanInitHelper +{ + CanRxItem queue_storage_[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity]; + +public: + enum { BitRateAutoDetect = 0 }; + + CanDriver driver; + + CanInitHelper() : + driver(queue_storage_) + { } + + /** + * This overload simply configures the provided bitrate. + * Auto bit rate detection will not be performed. + * Bitrate value must be positive. + * @return Negative value on error; non-negative on success. Refer to constants Err*. + */ + int init(uavcan::uint32_t bitrate) + { + return driver.init(bitrate, CanIface::NormalMode); + } + + int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode, uavcan::uint8_t can_number) + { + return driver.init(bitrate, mode, can_number); + } + + /** + * This function can either initialize the driver at a fixed bit rate, or it can perform + * automatic bit rate detection. For theory please refer to the CiA application note #801. + * + * @param delay_callable A callable entity that suspends execution for strictly more than one second. + * The callable entity will be invoked without arguments. + * @ref getRecommendedListeningDelay(). + * + * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. + * If auto detection was used, the function will update the argument + * with established bit rate. In case of an error the value will be undefined. + * + * @return Negative value on error; non-negative on success. Refer to constants Err*. + */ + template + int init(DelayCallable delay_callable, uavcan::uint32_t& inout_bitrate = BitRateAutoDetect) + { + if (inout_bitrate > 0) { + return driver.init(inout_bitrate, CanIface::NormalMode); + } else { + static const uavcan::uint32_t StandardBitRates[] = { + 1000000, + 500000, + 250000, + 125000 + }; + + for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) { + inout_bitrate = StandardBitRates[br]; + + const int res = driver.init(inout_bitrate, CanIface::SilentMode); + + delay_callable(); + + if (res >= 0) { + for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) { + if (!driver.getIface(iface)->isRxBufferEmpty()) { + // Re-initializing in normal mode + return driver.init(inout_bitrate, CanIface::NormalMode); + } + } + } + } + + return -ErrBitRateNotDetected; + } + } + + /** + * Use this value for listening delay during automatic bit rate detection. + */ + static uavcan::MonotonicDuration getRecommendedListeningDelay() + { + return uavcan::MonotonicDuration::fromMSec(1050); + } +}; + +} + +#include "CANSerialRouter.h" + +#endif //HAL_WITH_UAVCAN diff --git a/libraries/AP_HAL_ChibiOS/CANIface.h b/libraries/AP_HAL_ChibiOS/CANIface.h index 761c57d0ac..6fe8d848db 100644 --- a/libraries/AP_HAL_ChibiOS/CANIface.h +++ b/libraries/AP_HAL_ChibiOS/CANIface.h @@ -43,7 +43,9 @@ #include "AP_HAL_ChibiOS.h" #if HAL_WITH_UAVCAN - +# if defined(STM32H7XX) +#include "CANFDIface.h" +#else #include "CANThread.h" #include "CANIface.h" #include "bxcan.hpp" @@ -444,5 +446,5 @@ public: } #include "CANSerialRouter.h" - +#endif // defined(STM32H7XX) #endif //HAL_WITH_UAVCAN diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp index d5d32a8790..9b06e156a6 100644 --- a/libraries/AP_HAL_ChibiOS/CanIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp @@ -43,13 +43,15 @@ #if HAL_WITH_UAVCAN #include #include -#include "CANIface.h" #include "CANClock.h" #include "CANInternal.h" #include "CANSerialRouter.h" #include # include +# if !defined(STM32H7XX) +#include "CANIface.h" + #if CH_KERNEL_MAJOR == 2 # if !(defined(STM32F10X_CL) || defined(STM32F2XX) || defined(STM32F3XX) || defined(STM32F4XX)) // IRQ numbers @@ -1253,4 +1255,6 @@ UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler) } // extern "C" +#endif //!defined(STM32H7XX) + #endif //HAL_WITH_UAVCAN diff --git a/libraries/AP_HAL_ChibiOS/bxcan.hpp b/libraries/AP_HAL_ChibiOS/bxcan.hpp index 334009e9e4..ba255b60a0 100644 --- a/libraries/AP_HAL_ChibiOS/bxcan.hpp +++ b/libraries/AP_HAL_ChibiOS/bxcan.hpp @@ -44,7 +44,7 @@ #if HAL_WITH_UAVCAN -#include +# if !defined(STM32H7XX) #include #include @@ -326,4 +326,5 @@ constexpr unsigned long FMR_FINIT = (1U << 0); /* Bit 0: Filter Init #if UAVCAN_CPP_VERSION < UAVCAN_CPP11 # undef constexpr #endif +#endif //!defined(STM32H7XX) #endif //HAL_WITH_UAVCAN \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/fdcan.hpp b/libraries/AP_HAL_ChibiOS/fdcan.hpp new file mode 100644 index 0000000000..67165833cd --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/fdcan.hpp @@ -0,0 +1,90 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2014 Pavel Kirienko + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Siddharth Bharat Purohit + */ + +#pragma once + +#include "AP_HAL_ChibiOS.h" + +#if HAL_WITH_UAVCAN + +#if defined(STM32H7XX) + +#include +#include + +#ifndef UAVCAN_CPP_VERSION +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION < UAVCAN_CPP11 +// #undef'ed at the end of this file +# define constexpr const +#endif + +namespace ChibiOS_CAN +{ +namespace fdcan +{ +typedef FDCAN_GlobalTypeDef CanType; + +constexpr unsigned long IDE = (0x40000000U); // Identifier Extension +constexpr unsigned long STID_MASK = (0x1FFC0000U); // Standard Identifier Mask +constexpr unsigned long EXID_MASK = (0x1FFFFFFFU); // Extended Identifier Mask +constexpr unsigned long RTR = (0x20000000U); // Remote Transmission Request +constexpr unsigned long DLC_MASK = (0x000F0000U); // Data Length Code + +/** + * CANx register sets + */ +CanType* const Can[UAVCAN_STM32_NUM_IFACES] = { + reinterpret_cast(FDCAN1_BASE) +#if UAVCAN_STM32_NUM_IFACES > 1 + , + reinterpret_cast(FDCAN2_BASE) +#endif +}; +} +} + +#if UAVCAN_CPP_VERSION < UAVCAN_CPP11 +# undef constexpr +#endif +#endif //defined(STM32H7XX) +#endif //HAL_WITH_UAVCAN \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat index 8175c9f77f..4633234702 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat @@ -81,8 +81,8 @@ PB2 BOOT1 INPUT PB3 FMU_SW0 INPUT # This defines the pins for the 2nd CAN interface, if available. -#PB6 CAN2_TX CAN2 -#PB12 CAN2_RX CAN2 +PB6 CAN2_TX CAN2 +PB12 CAN2_RX CAN2 # Now the first I2C bus. The pin speeds are automatically setup # correctly, but can be overridden here if needed. @@ -135,8 +135,8 @@ PD7 BARO_CS CS PE4 MPU_EXT_CS CS # the first CAN bus -#PD0 CAN1_RX CAN1 -#PD1 CAN1_TX CAN1 +PD0 CAN1_RX CAN1 +PD1 CAN1_TX CAN1 # Another USART, this one for telem1. This one has RTS and CTS lines. # USART2 serial2 telem1