Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_ChibiOS: modify hal can drivers for use by AP_Periph

Browse Source
This commit is contained in:
Siddharth Purohit 2020-07-30 23:20:57 +05:30 committed by Peter Barker
parent c7d69e4d13
commit 36ffcdae30
8 changed files with 161 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
libraries/AP_HAL_ChibiOS/CANFDIface.cpp
View File

@ -63,22 +63,27 @@
#define MESSAGE_RAM_END_ADDR 0x4000B5FC #define MESSAGE_RAM_END_ADDR 0x4000B5FC
extern const AP_HAL::HAL& hal; extern AP_HAL::HAL& hal;
static_assert(STM32_FDCANCLK <= 80U*1000U*1000U, "FDCAN clock must be max 80MHz"); static_assert(STM32_FDCANCLK <= 80U*1000U*1000U, "FDCAN clock must be max 80MHz");
using namespace ChibiOS; using namespace ChibiOS;
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
#define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANFDIface", fmt, ##args); } while (0) #define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANFDIface", fmt, ##args); } while (0)
#else
#define Debug(fmt, args...)
#endif
constexpr CANIface::CanType* const CANIface::Can[]; constexpr CANIface::CanType* const CANIface::Can[];
static ChibiOS::CANIface* can_ifaces[HAL_NUM_CAN_IFACES] = {nullptr};
static inline bool driver_initialised(uint8_t iface_index) static inline bool driver_initialised(uint8_t iface_index)
{ {
if (iface_index >= HAL_NUM_CAN_IFACES) { if (iface_index >= HAL_NUM_CAN_IFACES) {
return false; return false;
} }
if (hal.can[iface_index] == nullptr) { if (can_ifaces[iface_index] == nullptr) {
return false; return false;
} }
return true; return true;
@ -97,12 +102,12 @@ static inline void handleCANInterrupt(uint8_t iface_index, uint8_t line_index)
if ((CANIface::Can[iface_index]->IR & FDCAN_IR_RF0N) || if ((CANIface::Can[iface_index]->IR & FDCAN_IR_RF0N) ||
(CANIface::Can[iface_index]->IR & FDCAN_IR_RF0F)) { (CANIface::Can[iface_index]->IR & FDCAN_IR_RF0F)) {
CANIface::Can[iface_index]->IR = FDCAN_IR_RF0N | FDCAN_IR_RF0F; CANIface::Can[iface_index]->IR = FDCAN_IR_RF0N | FDCAN_IR_RF0F;
((ChibiOS::CANIface*)hal.can[iface_index])->handleRxInterrupt(0); can_ifaces[iface_index]->handleRxInterrupt(0);
} }
if ((CANIface::Can[iface_index]->IR & FDCAN_IR_RF1N) || if ((CANIface::Can[iface_index]->IR & FDCAN_IR_RF1N) ||
(CANIface::Can[iface_index]->IR & FDCAN_IR_RF1F)) { (CANIface::Can[iface_index]->IR & FDCAN_IR_RF1F)) {
CANIface::Can[iface_index]->IR = FDCAN_IR_RF1N | FDCAN_IR_RF1F; CANIface::Can[iface_index]->IR = FDCAN_IR_RF1N | FDCAN_IR_RF1F;
((ChibiOS::CANIface*)hal.can[iface_index])->handleRxInterrupt(1); can_ifaces[iface_index]->handleRxInterrupt(1);
} }
} else { } else {
if (CANIface::Can[iface_index]->IR & FDCAN_IR_TC) { if (CANIface::Can[iface_index]->IR & FDCAN_IR_TC) {
@ -111,22 +116,23 @@ static inline void handleCANInterrupt(uint8_t iface_index, uint8_t line_index)
if (timestamp_us > 0) { if (timestamp_us > 0) {
timestamp_us--; timestamp_us--;
} }
((ChibiOS::CANIface*)hal.can[iface_index])->handleTxCompleteInterrupt(timestamp_us); can_ifaces[iface_index]->handleTxCompleteInterrupt(timestamp_us);
} }
if ((CANIface::Can[iface_index]->IR & FDCAN_IR_BO)) { if ((CANIface::Can[iface_index]->IR & FDCAN_IR_BO)) {
CANIface::Can[iface_index]->IR = FDCAN_IR_BO; CANIface::Can[iface_index]->IR = FDCAN_IR_BO;
((ChibiOS::CANIface*)hal.can[iface_index])->handleBusOffInterrupt(); can_ifaces[iface_index]->handleBusOffInterrupt();
} }
} }
((ChibiOS::CANIface*)hal.can[iface_index])->pollErrorFlagsFromISR(); can_ifaces[iface_index]->pollErrorFlagsFromISR();
} }
uint32_t CANIface::FDCANMessageRAMOffset_ = 0; uint32_t CANIface::FDCANMessageRAMOffset_ = 0;
CANIface::CANIface(uint8_t index) : CANIface::CANIface(uint8_t index) :
self_index_(index), self_index_(index),
rx_queue_(HAL_CAN_RX_QUEUE_SIZE) rx_bytebuffer_((uint8_t*)rx_buffer, sizeof(rx_buffer)),
rx_queue_(&rx_bytebuffer_)
{ {
if (index >= HAL_NUM_CAN_IFACES) { if (index >= HAL_NUM_CAN_IFACES) {
AP_HAL::panic("Bad CANIface index."); AP_HAL::panic("Bad CANIface index.");
@ -451,6 +457,18 @@ uint16_t CANIface::getNumFilters() const
bool CANIface::clock_init_ = false; bool CANIface::clock_init_ = false;
bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) bool CANIface::init(const uint32_t bitrate, const OperatingMode mode)
{ {
Debug("Bitrate %lu mode %d", static_cast<unsigned long>(bitrate), static_cast<int>(mode));
if (self_index_ > HAL_NUM_CAN_IFACES) {
Debug("CAN drv init failed");
return false;
}
if (can_ifaces[self_index_] == nullptr) {
can_ifaces[self_index_] = this;
#if !defined(HAL_BOOTLOADER_BUILD)
hal.can[self_index_] = this;
#endif
}
//Only do it once //Only do it once
//Doing it second time will reset the previously initialised bus //Doing it second time will reset the previously initialised bus
if (!clock_init_) { if (!clock_init_) {
@ -604,7 +622,9 @@ void CANIface::handleTxCompleteInterrupt(const uint64_t timestamp_us)
} }
if (event_handle_ != nullptr) { if (event_handle_ != nullptr) {
stats.num_events++; stats.num_events++;
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
evt_src_.signalI(1 << self_index_); evt_src_.signalI(1 << self_index_);
#endif
} }
} }
} }
@ -703,7 +723,9 @@ void CANIface::handleRxInterrupt(uint8_t fifo_index)
} }
if (event_handle_ != nullptr) { if (event_handle_ != nullptr) {
stats.num_events++; stats.num_events++;
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
evt_src_.signalI(1 << self_index_); evt_src_.signalI(1 << self_index_);
#endif
} }
} }
@ -766,6 +788,7 @@ uint32_t CANIface::getErrorCount() const
stats.tx_timedout; stats.tx_timedout;
} }
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
ChibiOS::EventSource CANIface::evt_src_; ChibiOS::EventSource CANIface::evt_src_;
bool CANIface::set_event_handle(AP_HAL::EventHandle* handle) bool CANIface::set_event_handle(AP_HAL::EventHandle* handle)
{ {
@ -774,6 +797,7 @@ bool CANIface::set_event_handle(AP_HAL::EventHandle* handle)
event_handle_->set_source(&evt_src_); event_handle_->set_source(&evt_src_);
return event_handle_->register_event(1 << self_index_); return event_handle_->register_event(1 << self_index_);
} }
#endif
bool CANIface::isRxBufferEmpty() const bool CANIface::isRxBufferEmpty() const
{ {
@ -857,6 +881,7 @@ bool CANIface::select(bool &read, bool &write,
return true; // Return value doesn't matter as long as it is non-negative return true; // Return value doesn't matter as long as it is non-negative
} }
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
uint32_t CANIface::get_stats(char* data, uint32_t max_size) uint32_t CANIface::get_stats(char* data, uint32_t max_size)
{ {
if (data == nullptr) { if (data == nullptr) {
@ -886,7 +911,7 @@ uint32_t CANIface::get_stats(char* data, uint32_t max_size)
stats.num_events); stats.num_events);
return ret; return ret;
} }
#endif
/* /*
* Interrupt handlers * Interrupt handlers

8
libraries/AP_HAL_ChibiOS/CANFDIface.h
View File

@ -106,6 +106,9 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
static uint32_t FDCANMessageRAMOffset_; static uint32_t FDCANMessageRAMOffset_;
CanType* can_; CanType* can_;
CanRxItem rx_buffer[HAL_CAN_RX_QUEUE_SIZE];
ByteBuffer rx_bytebuffer_;
ObjectBuffer<CanRxItem> rx_queue_; ObjectBuffer<CanRxItem> rx_queue_;
CanTxItem pending_tx_[NumTxMailboxes]; CanTxItem pending_tx_[NumTxMailboxes];
uint8_t peak_tx_mailbox_index_; uint8_t peak_tx_mailbox_index_;
@ -113,7 +116,9 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
bool initialised_; bool initialised_;
bool had_activity_; bool had_activity_;
AP_HAL::EventHandle* event_handle_; AP_HAL::EventHandle* event_handle_;
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
static ChibiOS::EventSource evt_src_; static ChibiOS::EventSource evt_src_;
#endif
const uint8_t self_index_; const uint8_t self_index_;
bool computeTimings(uint32_t target_bitrate, Timings& out_timings); bool computeTimings(uint32_t target_bitrate, Timings& out_timings);
@ -208,13 +213,14 @@ public:
const AP_HAL::CANFrame* const pending_tx, const AP_HAL::CANFrame* const pending_tx,
uint64_t blocking_deadline) override; uint64_t blocking_deadline) override;
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
// setup event handle for waiting on events // setup event handle for waiting on events
bool set_event_handle(AP_HAL::EventHandle* handle) override; bool set_event_handle(AP_HAL::EventHandle* handle) override;
// fetch stats text and return the size of the same, // fetch stats text and return the size of the same,
// results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt // results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt
uint32_t get_stats(char* data, uint32_t max_size) override; uint32_t get_stats(char* data, uint32_t max_size) override;
#endif
/************************************ /************************************
* Methods used inside interrupt * * Methods used inside interrupt *
************************************/ ************************************/

23
libraries/AP_HAL_ChibiOS/CANIface.h
View File

@ -96,15 +96,18 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
static const uint32_t TSR_ABRQx[NumTxMailboxes]; static const uint32_t TSR_ABRQx[NumTxMailboxes];
ChibiOS::bxcan::CanType* can_; ChibiOS::bxcan::CanType* can_;
uint64_t error_cnt_;
CanRxItem rx_buffer[HAL_CAN_RX_QUEUE_SIZE];
ByteBuffer rx_bytebuffer_;
ObjectBuffer<CanRxItem> rx_queue_; ObjectBuffer<CanRxItem> rx_queue_;
CanTxItem pending_tx_[NumTxMailboxes]; CanTxItem pending_tx_[NumTxMailboxes];
bool irq_init_; bool irq_init_:1;
bool initialised_; bool initialised_:1;
bool had_activity_; bool had_activity_:1;
#ifndef HAL_BUILD_AP_PERIPH
AP_HAL::EventHandle* event_handle_; AP_HAL::EventHandle* event_handle_;
static ChibiOS::EventSource evt_src_; static ChibiOS::EventSource evt_src_;
#endif
const uint8_t self_index_; const uint8_t self_index_;
bool computeTimings(uint32_t target_bitrate, Timings& out_timings); bool computeTimings(uint32_t target_bitrate, Timings& out_timings);
@ -132,6 +135,7 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
void initOnce(bool enable_irq); void initOnce(bool enable_irq);
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
struct { struct {
uint32_t tx_requests; uint32_t tx_requests;
uint32_t tx_rejected; uint32_t tx_rejected;
@ -145,6 +149,7 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
uint32_t num_busoff_err; uint32_t num_busoff_err;
uint32_t num_events; uint32_t num_events;
} stats; } stats;
#endif
public: public:
/****************************************** /******************************************
@ -165,10 +170,11 @@ public:
int16_t receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, int16_t receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us,
CanIOFlags& out_flags) override; CanIOFlags& out_flags) override;
#if !defined(HAL_BOOTLOADER_BUILD)
// Set Filters to ignore frames not to be handled by us // Set Filters to ignore frames not to be handled by us
bool configureFilters(const CanFilterConfig* filter_configs, bool configureFilters(const CanFilterConfig* filter_configs,
uint16_t num_configs) override; uint16_t num_configs) override;
#endif
// In BxCAN the Busoff error is cleared automatically, // In BxCAN the Busoff error is cleared automatically,
// so always return false // so always return false
bool is_busoff() const override bool is_busoff() const override
@ -185,7 +191,9 @@ public:
} }
// Get total number of Errors discovered // Get total number of Errors discovered
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
uint32_t getErrorCount() const override; uint32_t getErrorCount() const override;
#endif
// returns true if init was successfully called // returns true if init was successfully called
bool is_initialized() const override bool is_initialized() const override
@ -201,13 +209,14 @@ public:
const AP_HAL::CANFrame* const pending_tx, const AP_HAL::CANFrame* const pending_tx,
uint64_t blocking_deadline) override; uint64_t blocking_deadline) override;
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
// setup event handle for waiting on events // setup event handle for waiting on events
bool set_event_handle(AP_HAL::EventHandle* handle) override; bool set_event_handle(AP_HAL::EventHandle* handle) override;
// fetch stats text and return the size of the same, // fetch stats text and return the size of the same,
// results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt // results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt
uint32_t get_stats(char* data, uint32_t max_size) override; uint32_t get_stats(char* data, uint32_t max_size) override;
#endif
/************************************ /************************************
* Methods used inside interrupt * * Methods used inside interrupt *
************************************/ ************************************/

134
libraries/AP_HAL_ChibiOS/CanIface.cpp
View File

@ -50,13 +50,6 @@
# if !defined(STM32H7XX) # if !defined(STM32H7XX)
#include "CANIface.h" #include "CANIface.h"
#define CAN1_TX_IRQHandler STM32_CAN1_TX_HANDLER
#define CAN1_RX0_IRQHandler STM32_CAN1_RX0_HANDLER
#define CAN1_RX1_IRQHandler STM32_CAN1_RX1_HANDLER
#define CAN2_TX_IRQHandler STM32_CAN2_TX_HANDLER
#define CAN2_RX0_IRQHandler STM32_CAN2_RX0_HANDLER
#define CAN2_RX1_IRQHandler STM32_CAN2_RX1_HANDLER
/* STM32F3's only CAN inteface does not have a number. */ /* STM32F3's only CAN inteface does not have a number. */
#if defined(STM32F3XX) #if defined(STM32F3XX)
#define RCC_APB1ENR_CAN1EN RCC_APB1ENR_CANEN #define RCC_APB1ENR_CAN1EN RCC_APB1ENR_CANEN
@ -64,18 +57,37 @@
#define CAN1_TX_IRQn CAN_TX_IRQn #define CAN1_TX_IRQn CAN_TX_IRQn
#define CAN1_RX0_IRQn CAN_RX0_IRQn #define CAN1_RX0_IRQn CAN_RX0_IRQn
#define CAN1_RX1_IRQn CAN_RX1_IRQn #define CAN1_RX1_IRQn CAN_RX1_IRQn
#define CAN1_TX_IRQHandler CAN_TX_IRQHandler #define CAN1_TX_IRQ_Handler STM32_CAN1_TX_HANDLER
#define CAN1_RX0_IRQHandler CAN_RX0_IRQHandler #define CAN1_RX0_IRQ_Handler STM32_CAN1_RX0_HANDLER
#define CAN1_RX1_IRQHandler CAN_RX1_IRQHandler #define CAN1_RX1_IRQ_Handler STM32_CAN1_RX1_HANDLER
#else
#define CAN1_TX_IRQ_Handler STM32_CAN1_TX_HANDLER
#define CAN1_RX0_IRQ_Handler STM32_CAN1_RX0_HANDLER
#define CAN1_RX1_IRQ_Handler STM32_CAN1_RX1_HANDLER
#define CAN2_TX_IRQ_Handler STM32_CAN2_TX_HANDLER
#define CAN2_RX0_IRQ_Handler STM32_CAN2_RX0_HANDLER
#define CAN2_RX1_IRQ_Handler STM32_CAN2_RX1_HANDLER
#endif // #if defined(STM32F3XX)
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
#define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANIface", fmt, ##args); } while (0)
#else
#define Debug(fmt, args...)
#endif #endif
#define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANIface", fmt, ##args); } while (0) #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
#define PERF_STATS(x) (x++)
#else
#define PERF_STATS(x)
#endif
extern const AP_HAL::HAL& hal;
extern AP_HAL::HAL& hal;
using namespace ChibiOS; using namespace ChibiOS;
constexpr bxcan::CanType* const CANIface::Can[]; constexpr bxcan::CanType* const CANIface::Can[];
static ChibiOS::CANIface* can_ifaces[HAL_NUM_CAN_IFACES] = {nullptr};
static inline void handleTxInterrupt(uint8_t iface_index) static inline void handleTxInterrupt(uint8_t iface_index)
{ {
@ -86,8 +98,8 @@ static inline void handleTxInterrupt(uint8_t iface_index)
if (precise_time > 0) { if (precise_time > 0) {
precise_time--; precise_time--;
} }
if (hal.can[iface_index] != nullptr) { if (can_ifaces[iface_index] != nullptr) {
((ChibiOS::CANIface*)hal.can[iface_index])->handleTxInterrupt(precise_time); can_ifaces[iface_index]->handleTxInterrupt(precise_time);
} }
} }
@ -100,8 +112,8 @@ static inline void handleRxInterrupt(uint8_t iface_index, uint8_t fifo_index)
if (precise_time > 0) { if (precise_time > 0) {
precise_time--; precise_time--;
} }
if (hal.can[iface_index] != UAVCAN_NULLPTR) { if (can_ifaces[iface_index] != nullptr) {
((CANIface*)hal.can[iface_index])->handleRxInterrupt(fifo_index, precise_time); can_ifaces[iface_index]->handleRxInterrupt(fifo_index, precise_time);
} }
} }
@ -117,7 +129,8 @@ const uint32_t CANIface::TSR_ABRQx[CANIface::NumTxMailboxes] = {
CANIface::CANIface(uint8_t index) : CANIface::CANIface(uint8_t index) :
self_index_(index), self_index_(index),
rx_queue_(HAL_CAN_RX_QUEUE_SIZE) rx_bytebuffer_((uint8_t*)rx_buffer, sizeof(rx_buffer)),
rx_queue_(&rx_bytebuffer_)
{ {
if (index >= HAL_NUM_CAN_IFACES) { if (index >= HAL_NUM_CAN_IFACES) {
AP_HAL::panic("Bad CANIface index."); AP_HAL::panic("Bad CANIface index.");
@ -126,7 +139,6 @@ CANIface::CANIface(uint8_t index) :
} }
} }
bool CANIface::computeTimings(uint32_t target_bitrate, Timings& out_timings) bool CANIface::computeTimings(uint32_t target_bitrate, Timings& out_timings)
{ {
if (target_bitrate < 1) { if (target_bitrate < 1) {
@ -293,7 +305,7 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
} else if ((can_->TSR & bxcan::TSR_TME2) == bxcan::TSR_TME2) { } else if ((can_->TSR & bxcan::TSR_TME2) == bxcan::TSR_TME2) {
txmailbox = 2; txmailbox = 2;
} else { } else {
stats.tx_rejected++; PERF_STATS(stats.tx_rejected);
return 0; // No transmission for you. return 0; // No transmission for you.
} }
@ -350,6 +362,7 @@ int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_u
return 1; return 1;
} }
#if !defined(HAL_BOOTLOADER_BUILD)
bool CANIface::configureFilters(const CanFilterConfig* filter_configs, bool CANIface::configureFilters(const CanFilterConfig* filter_configs,
uint16_t num_configs) uint16_t num_configs)
{ {
@ -418,6 +431,7 @@ bool CANIface::configureFilters(const CanFilterConfig* filter_configs,
return false; return false;
} }
#endif
bool CANIface::waitMsrINakBitStateChange(bool target_state) bool CANIface::waitMsrINakBitStateChange(bool target_state)
{ {
@ -447,13 +461,13 @@ void CANIface::handleTxMailboxInterrupt(uint8_t mailbox_index, bool txok, const
rx_item.frame = txi.frame; rx_item.frame = txi.frame;
rx_item.timestamp_us = timestamp_us; rx_item.timestamp_us = timestamp_us;
rx_item.flags = AP_HAL::CANIface::Loopback; rx_item.flags = AP_HAL::CANIface::Loopback;
stats.tx_loopback++; PERF_STATS(stats.tx_loopback);
rx_queue_.push(rx_item); rx_queue_.push(rx_item);
} }
if (txok && !txi.pushed) { if (txok && !txi.pushed) {
txi.pushed = true; txi.pushed = true;
stats.tx_success++; PERF_STATS(stats.tx_success);
} }
} }
@ -476,10 +490,12 @@ void CANIface::handleTxInterrupt(const uint64_t utc_usec)
handleTxMailboxInterrupt(2, txok, utc_usec); handleTxMailboxInterrupt(2, txok, utc_usec);
} }
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
if (event_handle_ != nullptr) { if (event_handle_ != nullptr) {
stats.num_events++; PERF_STATS(stats.num_events);
evt_src_.signalI(1 << self_index_); evt_src_.signalI(1 << self_index_);
} }
#endif
pollErrorFlagsFromISR(); pollErrorFlagsFromISR();
} }
@ -494,7 +510,7 @@ void CANIface::handleRxInterrupt(uint8_t fifo_index, uint64_t timestamp_us)
* Register overflow as a hardware error * Register overflow as a hardware error
*/ */
if ((*rfr_reg & bxcan::RFR_FOVR) != 0) { if ((*rfr_reg & bxcan::RFR_FOVR) != 0) {
stats.rx_errors++; PERF_STATS(stats.rx_errors);
} }
/* /*
@ -535,18 +551,19 @@ void CANIface::handleRxInterrupt(uint8_t fifo_index, uint64_t timestamp_us)
rx_item.timestamp_us = timestamp_us; rx_item.timestamp_us = timestamp_us;
rx_item.flags = 0; rx_item.flags = 0;
if (rx_queue_.push(rx_item)) { if (rx_queue_.push(rx_item)) {
stats.rx_received++; PERF_STATS(stats.rx_received);
} else { } else {
stats.rx_overflow++; PERF_STATS(stats.rx_overflow);
} }
had_activity_ = true; had_activity_ = true;
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
if (event_handle_ != nullptr) { if (event_handle_ != nullptr) {
stats.num_events++; PERF_STATS(stats.num_events);
evt_src_.signalI(1 << self_index_); evt_src_.signalI(1 << self_index_);
} }
#endif
pollErrorFlagsFromISR(); pollErrorFlagsFromISR();
} }
@ -555,7 +572,6 @@ void CANIface::pollErrorFlagsFromISR()
const uint8_t lec = uint8_t((can_->ESR & bxcan::ESR_LEC_MASK) >> bxcan::ESR_LEC_SHIFT); const uint8_t lec = uint8_t((can_->ESR & bxcan::ESR_LEC_MASK) >> bxcan::ESR_LEC_SHIFT);
if (lec != 0) { if (lec != 0) {
can_->ESR = 0; can_->ESR = 0;
error_cnt_++;
// Serving abort requests // Serving abort requests
for (int i = 0; i < NumTxMailboxes; i++) { for (int i = 0; i < NumTxMailboxes; i++) {
@ -563,7 +579,7 @@ void CANIface::pollErrorFlagsFromISR()
if (txi.aborted && txi.abort_on_error) { if (txi.aborted && txi.abort_on_error) {
can_->TSR = TSR_ABRQx[i]; can_->TSR = TSR_ABRQx[i];
txi.aborted = true; txi.aborted = true;
stats.tx_abort++; PERF_STATS(stats.tx_abort);
} }
} }
} }
@ -580,7 +596,7 @@ void CANIface::discardTimedOutTxMailboxes(uint64_t current_time)
if (txi.deadline < current_time) { if (txi.deadline < current_time) {
can_->TSR = TSR_ABRQx[i]; // Goodnight sweet transmission can_->TSR = TSR_ABRQx[i]; // Goodnight sweet transmission
pending_tx_[i].aborted = true; pending_tx_[i].aborted = true;
stats.tx_timedout++; PERF_STATS(stats.tx_timedout);
} }
} }
} }
@ -637,6 +653,7 @@ bool CANIface::isRxBufferEmpty() const
return rx_queue_.available() == 0; return rx_queue_.available() == 0;
} }
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
uint32_t CANIface::getErrorCount() const uint32_t CANIface::getErrorCount() const
{ {
CriticalSectionLocker lock; CriticalSectionLocker lock;
@ -657,6 +674,8 @@ bool CANIface::set_event_handle(AP_HAL::EventHandle* handle)
return event_handle_->register_event(1 << self_index_); return event_handle_->register_event(1 << self_index_);
} }
#endif // #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
void CANIface::checkAvailable(bool& read, bool& write, const AP_HAL::CANFrame* pending_tx) const void CANIface::checkAvailable(bool& read, bool& write, const AP_HAL::CANFrame* pending_tx) const
{ {
write = false; write = false;
@ -687,6 +706,9 @@ bool CANIface::select(bool &read, bool &write,
if ((read && in_read) || (write && in_write)) { if ((read && in_read) || (write && in_write)) {
return true; return true;
} }
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
// we don't support blocking select in AP_Periph and bootloader
while (time < blocking_deadline) { while (time < blocking_deadline) {
if (event_handle_ == nullptr) { if (event_handle_ == nullptr) {
break; break;
@ -698,6 +720,7 @@ bool CANIface::select(bool &read, bool &write,
} }
time = AP_HAL::micros(); time = AP_HAL::micros();
} }
#endif // #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
return true; return true;
} }
@ -750,20 +773,26 @@ bool CANIface::init(const uint32_t bitrate, const CANIface::OperatingMode mode)
Debug("CAN drv init failed"); Debug("CAN drv init failed");
return false; return false;
} }
if (can_ifaces[self_index_] == nullptr) {
can_ifaces[self_index_] = this;
#if !defined(HAL_BOOTLOADER_BUILD)
hal.can[self_index_] = this;
#endif
}
if (hal.can[0] == nullptr) { if (can_ifaces[0] == nullptr) {
const_cast <AP_HAL::HAL&> (hal).can[0] = new CANIface(0); can_ifaces[0] = new CANIface(0);
Debug("Failed to allocate CAN iface 0"); Debug("Failed to allocate CAN iface 0");
if (hal.can[0] == nullptr) { if (can_ifaces[0] == nullptr) {
return false; return false;
} }
} }
if (self_index_ == 1 && !hal.can[0]->is_initialized()) { if (self_index_ == 1 && !can_ifaces[0]->is_initialized()) {
Debug("Iface 0 is not initialized yet but we need it for Iface 1, trying to init it"); Debug("Iface 0 is not initialized yet but we need it for Iface 1, trying to init it");
Debug("Enabling CAN iface 0"); Debug("Enabling CAN iface 0");
((CANIface*)hal.can[0])->initOnce(false); can_ifaces[0]->initOnce(false);
Debug("Initing iface 0..."); Debug("Initing iface 0...");
if (!hal.can[0]->init(bitrate, mode)) { if (!can_ifaces[0]->init(bitrate, mode)) {
Debug("Iface 0 init failed"); Debug("Iface 0 init failed");
return false;; return false;;
} }
@ -793,7 +822,6 @@ bool CANIface::init(const uint32_t bitrate, const CANIface::OperatingMode mode)
* Object state - interrupts are disabled, so it's safe to modify it now * Object state - interrupts are disabled, so it's safe to modify it now
*/ */
rx_queue_.clear(); rx_queue_.clear();
error_cnt_ = 0;
for (uint32_t i=0; i < NumTxMailboxes; i++) { for (uint32_t i=0; i < NumTxMailboxes; i++) {
pending_tx_[i] = CanTxItem(); pending_tx_[i] = CanTxItem();
@ -867,6 +895,7 @@ bool CANIface::init(const uint32_t bitrate, const CANIface::OperatingMode mode)
return true; return true;
} }
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
uint32_t CANIface::get_stats(char* data, uint32_t max_size) uint32_t CANIface::get_stats(char* data, uint32_t max_size)
{ {
if (data == nullptr) { if (data == nullptr) {
@ -897,6 +926,7 @@ uint32_t CANIface::get_stats(char* data, uint32_t max_size)
memset(&stats, 0, sizeof(stats)); memset(&stats, 0, sizeof(stats));
return ret; return ret;
} }
#endif
/* /*
* Interrupt handlers * Interrupt handlers
@ -904,24 +934,24 @@ uint32_t CANIface::get_stats(char* data, uint32_t max_size)
extern "C" extern "C"
{ {
CH_IRQ_HANDLER(CAN1_TX_IRQHandler); CH_IRQ_HANDLER(CAN1_TX_IRQ_Handler);
CH_IRQ_HANDLER(CAN1_TX_IRQHandler) CH_IRQ_HANDLER(CAN1_TX_IRQ_Handler)
{ {
CH_IRQ_PROLOGUE(); CH_IRQ_PROLOGUE();
handleTxInterrupt(0); handleTxInterrupt(0);
CH_IRQ_EPILOGUE(); CH_IRQ_EPILOGUE();
} }
CH_IRQ_HANDLER(CAN1_RX0_IRQHandler); CH_IRQ_HANDLER(CAN1_RX0_IRQ_Handler);
CH_IRQ_HANDLER(CAN1_RX0_IRQHandler) CH_IRQ_HANDLER(CAN1_RX0_IRQ_Handler)
{ {
CH_IRQ_PROLOGUE(); CH_IRQ_PROLOGUE();
handleRxInterrupt(0, 0); handleRxInterrupt(0, 0);
CH_IRQ_EPILOGUE(); CH_IRQ_EPILOGUE();
} }
CH_IRQ_HANDLER(CAN1_RX1_IRQHandler); CH_IRQ_HANDLER(CAN1_RX1_IRQ_Handler);
CH_IRQ_HANDLER(CAN1_RX1_IRQHandler) CH_IRQ_HANDLER(CAN1_RX1_IRQ_Handler)
{ {
CH_IRQ_PROLOGUE(); CH_IRQ_PROLOGUE();
handleRxInterrupt(0, 1); handleRxInterrupt(0, 1);
@ -930,36 +960,36 @@ extern "C"
#if HAL_NUM_CAN_IFACES > 1 #if HAL_NUM_CAN_IFACES > 1
#if !defined(CAN2_TX_IRQHandler) #if !defined(CAN2_TX_IRQ_Handler)
# error "Misconfigured build1" # error "Misconfigured build1"
#endif #endif
#if !defined(CAN2_RX0_IRQHandler) #if !defined(CAN2_RX0_IRQ_Handler)
# error "Misconfigured build2" # error "Misconfigured build2"
#endif #endif
#if !defined(CAN2_RX1_IRQHandler) #if !defined(CAN2_RX1_IRQ_Handler)
# error "Misconfigured build3" # error "Misconfigured build3"
#endif #endif
CH_IRQ_HANDLER(CAN2_TX_IRQHandler); CH_IRQ_HANDLER(CAN2_TX_IRQ_Handler);
CH_IRQ_HANDLER(CAN2_TX_IRQHandler) CH_IRQ_HANDLER(CAN2_TX_IRQ_Handler)
{ {
CH_IRQ_PROLOGUE(); CH_IRQ_PROLOGUE();
handleTxInterrupt(1); handleTxInterrupt(1);
CH_IRQ_EPILOGUE(); CH_IRQ_EPILOGUE();
} }
CH_IRQ_HANDLER(CAN2_RX0_IRQHandler); CH_IRQ_HANDLER(CAN2_RX0_IRQ_Handler);
CH_IRQ_HANDLER(CAN2_RX0_IRQHandler) CH_IRQ_HANDLER(CAN2_RX0_IRQ_Handler)
{ {
CH_IRQ_PROLOGUE(); CH_IRQ_PROLOGUE();
handleRxInterrupt(1, 0); handleRxInterrupt(1, 0);
CH_IRQ_EPILOGUE(); CH_IRQ_EPILOGUE();
} }
CH_IRQ_HANDLER(CAN2_RX1_IRQHandler); CH_IRQ_HANDLER(CAN2_RX1_IRQ_Handler);
CH_IRQ_HANDLER(CAN2_RX1_IRQHandler) CH_IRQ_HANDLER(CAN2_RX1_IRQ_Handler)
{ {
CH_IRQ_PROLOGUE(); CH_IRQ_PROLOGUE();
handleRxInterrupt(1, 1); handleRxInterrupt(1, 1);

6
libraries/AP_HAL_ChibiOS/EventSource.cpp
View File

@ -1,5 +1,6 @@
#include "EventSource.h" #include "EventSource.h"
#include <AP_HAL/AP_HAL.h>
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
using namespace ChibiOS; using namespace ChibiOS;
@ -31,4 +32,5 @@ void EventSource::signalI(uint32_t evt_mask)
ch_evt_src_.broadcastFlagsI(evt_mask); ch_evt_src_.broadcastFlagsI(evt_mask);
chSysUnlockFromISR(); chSysUnlockFromISR();
} }
#endif #endif //#if CH_CFG_USE_EVENTS == TRUE
#endif //#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)

5
libraries/AP_HAL_ChibiOS/EventSource.h
View File

@ -1,5 +1,7 @@
#pragma once #pragma once
#include <AP_HAL/AP_HAL.h>
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
#include <stdint.h> #include <stdint.h>
#include <AP_HAL/AP_HAL_Boards.h> #include <AP_HAL/AP_HAL_Boards.h>
#include <AP_HAL/AP_HAL_Macros.h> #include <AP_HAL/AP_HAL_Macros.h>
@ -22,4 +24,5 @@ public:
// Wait on an Event handle, method for internal use by EventHandle // Wait on an Event handle, method for internal use by EventHandle
bool wait(uint64_t duration, AP_HAL::EventHandle* evt_handle) override; bool wait(uint64_t duration, AP_HAL::EventHandle* evt_handle) override;
}; };
#endif #endif //#if CH_CFG_USE_EVENTS == TRUE
#endif //#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)

8
libraries/AP_HAL_ChibiOS/stdio.cpp
View File

@ -34,7 +34,9 @@
#include "UARTDriver.h" #include "UARTDriver.h"
#endif #endif
#ifndef HAL_BOOTLOADER_BUILD
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
#endif
int __wrap_snprintf(char *str, size_t size, const char *fmt, ...) int __wrap_snprintf(char *str, size_t size, const char *fmt, ...)
{ {
@ -42,7 +44,11 @@ int __wrap_snprintf(char *str, size_t size, const char *fmt, ...)
int done; int done;
va_start (arg, fmt); va_start (arg, fmt);
#ifdef HAL_BOOTLOADER_BUILD
done = chvsnprintf(str, size, fmt, arg);
#else
done = hal.util->vsnprintf(str, size, fmt, arg); done = hal.util->vsnprintf(str, size, fmt, arg);
#endif
va_end (arg); va_end (arg);
return done; return done;
@ -85,7 +91,7 @@ int __wrap_vprintf(const char *fmt, va_list arg)
{ {
#ifdef HAL_STDOUT_SERIAL #ifdef HAL_STDOUT_SERIAL
return chvprintf((BaseSequentialStream*)&HAL_STDOUT_SERIAL, fmt, arg); return chvprintf((BaseSequentialStream*)&HAL_STDOUT_SERIAL, fmt, arg);
#elif HAL_USE_SERIAL_USB == TRUE #elif HAL_USE_SERIAL_USB == TRUE && !defined(HAL_BOOTLOADER_BUILD)
usb_initialise(); usb_initialise();
return chvprintf((BaseSequentialStream*)&SDU1, fmt, arg); return chvprintf((BaseSequentialStream*)&SDU1, fmt, arg);
#else #else

7
libraries/AP_HAL_ChibiOS/system.cpp
View File

@ -217,6 +217,7 @@ void init()
void panic(const char *errormsg, ...) void panic(const char *errormsg, ...)
{ {
#ifndef HAL_BOOTLOADER_BUILD
va_list ap; va_list ap;
va_start(ap, errormsg); va_start(ap, errormsg);
@ -228,6 +229,12 @@ void panic(const char *errormsg, ...)
vprintf(errormsg, ap); vprintf(errormsg, ap);
hal.scheduler->delay(500); hal.scheduler->delay(500);
} }
#else
// we don't support variable args in bootlaoder
chSysHalt(errormsg);
// we will never get here, this just to silence a warning
while (1) {}
#endif
} }
uint32_t micros() uint32_t micros()