PX4 Firmware
PX4 Autopilot Software http://px4.io
|
Single CAN iface. More...
#include <can.hpp>
Classes | |
class | RxQueue |
struct | Timings |
struct | TxItem |
Public Types | |
enum | { MaxRxQueueCapacity = 254 } |
enum | OperatingMode { NormalMode, SilentMode } |
Public Member Functions | |
CanIface (bxcan::CanType *can, BusEvent &update_event, uavcan::uint8_t self_index, CanRxItem *rx_queue_buffer, uavcan::uint8_t rx_queue_capacity) | |
int | init (const uavcan::uint32_t bitrate, const OperatingMode mode) |
Initializes the hardware CAN controller. More... | |
void | handleTxInterrupt (uavcan::uint64_t utc_usec) |
void | handleRxInterrupt (uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec) |
void | pollErrorFlagsFromISR () |
This method is used to count errors and abort transmission on error if necessary. More... | |
void | discardTimedOutTxMailboxes (uavcan::MonotonicTime current_time) |
bool | canAcceptNewTxFrame (const uavcan::CanFrame &frame) const |
bool | isRxBufferEmpty () const |
uavcan::uint32_t | getRxQueueOverflowCount () const |
Number of RX frames lost due to queue overflow. More... | |
virtual uavcan::uint64_t | getErrorCount () const |
Total number of hardware failures and other kinds of errors (e.g. More... | |
uavcan::uint32_t | getVoluntaryTxAbortCount () const |
Number of times the driver exercised library's requirement to abort transmission on first error. More... | |
unsigned | getRxQueueLength () const |
Returns the number of frames pending in the RX queue. More... | |
bool | hadActivity () |
Whether this iface had at least one successful IO since the previous call of this method. More... | |
uavcan::uint8_t | getPeakNumTxMailboxesUsed () const |
Peak number of TX mailboxes used concurrently since initialization. More... | |
Private Types | |
enum | { NumTxMailboxes = 3 } |
enum | { NumFilters = 14 } |
Private Member Functions | |
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 | handleTxMailboxInterrupt (uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec) |
bool | waitMsrINakBitStateChange (bool target_state) |
Private Attributes | |
RxQueue | rx_queue_ |
bxcan::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_ |
Static Private Attributes | |
static const uavcan::uint32_t | TSR_ABRQx [NumTxMailboxes] |
Single CAN iface.
The application shall not use this directly.
|
inline |
Definition at line 147 of file can.hpp.
References uavcan_stm32::CanRxItem::frame, and ToneAlarmInterface::init().
bool uavcan_stm32::CanIface::canAcceptNewTxFrame | ( | const uavcan::CanFrame & | frame | ) | const |
Definition at line 733 of file uc_stm32_can.cpp.
References can_, uavcan_stm32::CanIface::TxItem::frame, MicroBenchHRT::lock(), NumTxMailboxes, pending_tx_, uavcan_stm32::bxcan::CanType::TSR, uavcan_stm32::bxcan::TSR_TME0, uavcan_stm32::bxcan::TSR_TME1, and uavcan_stm32::bxcan::TSR_TME2.
|
private |
Definition at line 159 of file uc_stm32_can.cpp.
References uavcan_stm32::CanIface::Timings::bs1, uavcan_stm32::CanIface::Timings::bs2, uavcan_stm32::ErrInvalidBitRate, uavcan_stm32::ErrLogic, uavcan_stm32::CanIface::Timings::prescaler, uavcan_stm32::CanIface::Timings::sjw, and UAVCAN_STM32_LOG.
Referenced by init().
|
privatevirtual |
Definition at line 400 of file uc_stm32_can.cpp.
References can_, uavcan_stm32::ErrFilterNumConfigs, uavcan_stm32::bxcan::CanType::FA1R, uavcan_stm32::bxcan::CanType::FFA1R, uavcan_stm32::bxcan::CanType::FilterRegister, uavcan_stm32::bxcan::CanType::FM1R, uavcan_stm32::bxcan::CanType::FMR, uavcan_stm32::bxcan::FMR_FINIT, uavcan_stm32::bxcan::FilterRegisterType::FR1, uavcan_stm32::bxcan::FilterRegisterType::FR2, uavcan_stm32::bxcan::CanType::FS1R, MicroBenchHRT::lock(), NumFilters, uavcan_stm32::bxcan::RIR_IDE, uavcan_stm32::bxcan::RIR_RTR, and self_index_.
void uavcan_stm32::CanIface::discardTimedOutTxMailboxes | ( | uavcan::MonotonicTime | current_time | ) |
Definition at line 718 of file uc_stm32_can.cpp.
References can_, uavcan_stm32::CanIface::TxItem::deadline, error_cnt_, MicroBenchHRT::lock(), NumTxMailboxes, uavcan_stm32::CanIface::TxItem::pending, pending_tx_, uavcan_stm32::bxcan::CanType::TSR, and TSR_ABRQx.
|
virtual |
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.
Definition at line 773 of file uc_stm32_can.cpp.
References error_cnt_, uavcan_stm32::CanIface::RxQueue::getOverflowCount(), MicroBenchHRT::lock(), and rx_queue_.
|
inlineprivatevirtual |
Definition at line 133 of file can.hpp.
References uavcan_stm32::CanRxItem::utc_usec.
|
inline |
unsigned uavcan_stm32::CanIface::getRxQueueLength | ( | ) | const |
Returns the number of frames pending in the RX queue.
This is intended for debug use only.
Definition at line 779 of file uc_stm32_can.cpp.
References uavcan_stm32::CanIface::RxQueue::getLength(), MicroBenchHRT::lock(), and rx_queue_.
|
inline |
Number of RX frames lost due to queue overflow.
This is an atomic read, it doesn't require a critical section.
Definition at line 191 of file can.hpp.
References uavcan_stm32::CanIface::RxQueue::getOverflowCount().
|
inline |
bool uavcan_stm32::CanIface::hadActivity | ( | ) |
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.
Definition at line 785 of file uc_stm32_can.cpp.
References had_activity_, and MicroBenchHRT::lock().
void uavcan_stm32::CanIface::handleRxInterrupt | ( | uavcan::uint8_t | fifo_index, |
uavcan::uint64_t | utc_usec | ||
) |
Definition at line 637 of file uc_stm32_can.cpp.
References can_, error_cnt_, had_activity_, pollErrorFlagsFromISR(), uavcan_stm32::CanIface::RxQueue::push(), uavcan_stm32::bxcan::RxMailboxType::RDHR, uavcan_stm32::bxcan::RxMailboxType::RDLR, uavcan_stm32::bxcan::RxMailboxType::RDTR, uavcan_stm32::bxcan::CanType::RF0R, uavcan_stm32::bxcan::CanType::RF1R, uavcan_stm32::bxcan::RFR_FMP_MASK, uavcan_stm32::bxcan::RFR_FOVR, uavcan_stm32::bxcan::RFR_FULL, uavcan_stm32::bxcan::RFR_RFOM, uavcan_stm32::bxcan::RxMailboxType::RIR, uavcan_stm32::bxcan::RIR_IDE, uavcan_stm32::bxcan::RIR_RTR, rx_queue_, uavcan_stm32::bxcan::CanType::RxMailbox, and update_event_.
void uavcan_stm32::CanIface::handleTxInterrupt | ( | uavcan::uint64_t | utc_usec | ) |
Definition at line 610 of file uc_stm32_can.cpp.
References can_, handleTxMailboxInterrupt(), pollErrorFlagsFromISR(), uavcan_stm32::bxcan::CanType::TSR, uavcan_stm32::bxcan::TSR_RQCP0, uavcan_stm32::bxcan::TSR_RQCP1, uavcan_stm32::bxcan::TSR_RQCP2, uavcan_stm32::bxcan::TSR_TXOK0, uavcan_stm32::bxcan::TSR_TXOK1, uavcan_stm32::bxcan::TSR_TXOK2, and update_event_.
|
private |
Definition at line 595 of file uc_stm32_can.cpp.
References uavcan_stm32::CanIface::TxItem::frame, had_activity_, uavcan_stm32::CanIface::TxItem::loopback, NumTxMailboxes, uavcan_stm32::CanIface::TxItem::pending, pending_tx_, uavcan_stm32::CanIface::RxQueue::push(), and rx_queue_.
Referenced by handleTxInterrupt().
int uavcan_stm32::CanIface::init | ( | const uavcan::uint32_t | bitrate, |
const OperatingMode | mode | ||
) |
Initializes the hardware CAN controller.
Assumes:
Definition at line 496 of file uc_stm32_can.cpp.
References uavcan_stm32::CanIface::Timings::bs1, uavcan_stm32::CanIface::Timings::bs2, uavcan_stm32::bxcan::CanType::BTR, uavcan_stm32::bxcan::BTR_SILM, can_, computeTimings(), uavcan_stm32::ErrMsrInakNotCleared, uavcan_stm32::ErrMsrInakNotSet, error_cnt_, uavcan_stm32::bxcan::CanType::FA1R, uavcan_stm32::bxcan::CanType::FFA1R, uavcan_stm32::bxcan::CanType::FilterRegister, uavcan_stm32::bxcan::CanType::FM1R, uavcan_stm32::bxcan::CanType::FMR, uavcan_stm32::bxcan::FMR_FINIT, uavcan_stm32::bxcan::FilterRegisterType::FR1, uavcan_stm32::bxcan::FilterRegisterType::FR2, uavcan_stm32::bxcan::CanType::FS1R, had_activity_, uavcan_stm32::bxcan::CanType::IER, uavcan_stm32::bxcan::IER_FMPIE0, uavcan_stm32::bxcan::IER_FMPIE1, uavcan_stm32::bxcan::IER_TMEIE, MicroBenchHRT::lock(), uavcan_stm32::bxcan::CanType::MCR, uavcan_stm32::bxcan::MCR_ABOM, uavcan_stm32::bxcan::MCR_AWUM, uavcan_stm32::bxcan::MCR_INRQ, uavcan_stm32::bxcan::MCR_RESET, uavcan_stm32::bxcan::MCR_SLEEP, NumFilters, NumTxMailboxes, peak_tx_mailbox_index_, pending_tx_, uavcan_stm32::CanIface::Timings::prescaler, uavcan_stm32::CanIface::RxQueue::reset(), rx_queue_, self_index_, served_aborts_cnt_, SilentMode, uavcan_stm32::CanIface::Timings::sjw, UAVCAN_STM32_LOG, and waitMsrINakBitStateChange().
bool uavcan_stm32::CanIface::isRxBufferEmpty | ( | ) | const |
Definition at line 767 of file uc_stm32_can.cpp.
References uavcan_stm32::CanIface::RxQueue::getLength(), MicroBenchHRT::lock(), and rx_queue_.
Referenced by uavcan_stm32::CanInitHelper< RxQueueCapacity >::init().
void uavcan_stm32::CanIface::pollErrorFlagsFromISR | ( | ) |
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.
Definition at line 697 of file uc_stm32_can.cpp.
References uavcan_stm32::CanIface::TxItem::abort_on_error, can_, error_cnt_, uavcan_stm32::bxcan::CanType::ESR, uavcan_stm32::bxcan::ESR_LEC_MASK, uavcan_stm32::bxcan::ESR_LEC_SHIFT, NumTxMailboxes, uavcan_stm32::CanIface::TxItem::pending, pending_tx_, served_aborts_cnt_, uavcan_stm32::bxcan::CanType::TSR, and TSR_ABRQx.
Referenced by handleRxInterrupt(), and handleTxInterrupt().
|
privatevirtual |
Definition at line 382 of file uc_stm32_can.cpp.
References uavcan_stm32::CanIface::RxQueue::getLength(), uavcan_stm32::clock::getMonotonic(), MicroBenchHRT::lock(), uavcan_stm32::CanIface::RxQueue::pop(), and rx_queue_.
|
privatevirtual |
Definition at line 297 of file uc_stm32_can.cpp.
References uavcan_stm32::CanIface::TxItem::abort_on_error, can_, uavcan_stm32::CanIface::TxItem::deadline, uavcan_stm32::ErrUnsupportedFrame, uavcan_stm32::CanIface::TxItem::frame, MicroBenchHRT::lock(), uavcan_stm32::CanIface::TxItem::loopback, math::max(), peak_tx_mailbox_index_, uavcan_stm32::CanIface::TxItem::pending, pending_tx_, uavcan_stm32::bxcan::TxMailboxType::TDHR, uavcan_stm32::bxcan::TxMailboxType::TDLR, uavcan_stm32::bxcan::TxMailboxType::TDTR, uavcan_stm32::bxcan::TxMailboxType::TIR, uavcan_stm32::bxcan::TIR_IDE, uavcan_stm32::bxcan::TIR_RTR, uavcan_stm32::bxcan::TIR_TXRQ, uavcan_stm32::bxcan::CanType::TSR, uavcan_stm32::bxcan::TSR_TME0, uavcan_stm32::bxcan::TSR_TME1, uavcan_stm32::bxcan::TSR_TME2, and uavcan_stm32::bxcan::CanType::TxMailbox.
|
private |
Definition at line 473 of file uc_stm32_can.cpp.
References can_, uavcan_stm32::bxcan::CanType::MSR, uavcan_stm32::bxcan::MSR_INAK, and state.
Referenced by init().
|
private |
Definition at line 113 of file can.hpp.
Referenced by canAcceptNewTxFrame(), configureFilters(), discardTimedOutTxMailboxes(), handleRxInterrupt(), handleTxInterrupt(), init(), pollErrorFlagsFromISR(), send(), and waitMsrINakBitStateChange().
|
private |
Definition at line 114 of file can.hpp.
Referenced by discardTimedOutTxMailboxes(), getErrorCount(), handleRxInterrupt(), init(), and pollErrorFlagsFromISR().
|
private |
Definition at line 120 of file can.hpp.
Referenced by hadActivity(), handleRxInterrupt(), handleTxMailboxInterrupt(), and init().
|
private |
|
private |
Definition at line 117 of file can.hpp.
Referenced by canAcceptNewTxFrame(), discardTimedOutTxMailboxes(), handleTxMailboxInterrupt(), init(), pollErrorFlagsFromISR(), and send().
|
private |
Definition at line 112 of file can.hpp.
Referenced by getErrorCount(), getRxQueueLength(), handleRxInterrupt(), handleTxMailboxInterrupt(), init(), isRxBufferEmpty(), and receive().
|
private |
Definition at line 119 of file can.hpp.
Referenced by configureFilters(), and init().
|
private |
Definition at line 115 of file can.hpp.
Referenced by init(), and pollErrorFlagsFromISR().
|
staticprivate |
Definition at line 110 of file can.hpp.
Referenced by discardTimedOutTxMailboxes(), pollErrorFlagsFromISR(), and uavcan_stm32::CanIface::RxQueue::reset().
|
private |
Definition at line 116 of file can.hpp.
Referenced by handleRxInterrupt(), handleTxInterrupt(), and uavcan_stm32::CanDriver::select().