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

HAL_ChibiOS: modify hal can drivers for use by AP_Periph

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
extern const AP_HAL::HAL& hal;
extern AP_HAL::HAL& hal;
static_assert(STM32_FDCANCLK <= 80U*1000U*1000U, "FDCAN clock must be max 80MHz");
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)
#else
#define Debug(fmt, args...)
#endif
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)
{
if (iface_index >= HAL_NUM_CAN_IFACES) {
return false;
}
if (hal.can[iface_index] == nullptr) {
if (can_ifaces[iface_index] == nullptr) {
return false;
}
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) ||
(CANIface::Can[iface_index]->IR & 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) ||
(CANIface::Can[iface_index]->IR & 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 {
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) {
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)) {
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;
CANIface::CANIface(uint8_t 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) {
AP_HAL::panic("Bad CANIface index.");
@ -451,6 +457,18 @@ uint16_t CANIface::getNumFilters() const
bool CANIface::clock_init_ = false;
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
//Doing it second time will reset the previously initialised bus
if (!clock_init_) {
@ -604,7 +622,9 @@ void CANIface::handleTxCompleteInterrupt(const uint64_t timestamp_us)
}
if (event_handle_ != nullptr) {
stats.num_events++;
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
evt_src_.signalI(1 << self_index_);
#endif
}
}
}
@ -703,7 +723,9 @@ void CANIface::handleRxInterrupt(uint8_t fifo_index)
}
if (event_handle_ != nullptr) {
stats.num_events++;
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
evt_src_.signalI(1 << self_index_);
#endif
}
}
@ -766,6 +788,7 @@ uint32_t CANIface::getErrorCount() const
stats.tx_timedout;
}
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
ChibiOS::EventSource CANIface::evt_src_;
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_);
return event_handle_->register_event(1 << self_index_);
}
#endif
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
}
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
uint32_t CANIface::get_stats(char* data, uint32_t max_size)
{
if (data == nullptr) {
@ -886,7 +911,7 @@ uint32_t CANIface::get_stats(char* data, uint32_t max_size)
stats.num_events);
return ret;
}
#endif
/*
* 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_;
CanType* can_;
CanRxItem rx_buffer[HAL_CAN_RX_QUEUE_SIZE];
ByteBuffer rx_bytebuffer_;
ObjectBuffer<CanRxItem> rx_queue_;
CanTxItem pending_tx_[NumTxMailboxes];
uint8_t peak_tx_mailbox_index_;
@ -113,7 +116,9 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
bool initialised_;
bool had_activity_;
AP_HAL::EventHandle* event_handle_;
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
static ChibiOS::EventSource evt_src_;
#endif
const uint8_t self_index_;
bool computeTimings(uint32_t target_bitrate, Timings& out_timings);
@ -208,13 +213,14 @@ public:
const AP_HAL::CANFrame* const pending_tx,
uint64_t blocking_deadline) override;
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
// setup event handle for waiting on events
bool set_event_handle(AP_HAL::EventHandle* handle) override;
// fetch stats text and return the size of the same,
// results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt
uint32_t get_stats(char* data, uint32_t max_size) override;
#endif
/************************************
* 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];
ChibiOS::bxcan::CanType* can_;
uint64_t error_cnt_;
CanRxItem rx_buffer[HAL_CAN_RX_QUEUE_SIZE];
ByteBuffer rx_bytebuffer_;
ObjectBuffer<CanRxItem> rx_queue_;
CanTxItem pending_tx_[NumTxMailboxes];
bool irq_init_;
bool initialised_;
bool had_activity_;
bool irq_init_:1;
bool initialised_:1;
bool had_activity_:1;
#ifndef HAL_BUILD_AP_PERIPH
AP_HAL::EventHandle* event_handle_;
static ChibiOS::EventSource evt_src_;
#endif
const uint8_t self_index_;
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);
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
struct {
uint32_t tx_requests;
uint32_t tx_rejected;
@ -145,6 +149,7 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
uint32_t num_busoff_err;
uint32_t num_events;
} stats;
#endif
public:
/******************************************
@ -165,10 +170,11 @@ public:
int16_t receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us,
CanIOFlags& out_flags) override;
#if !defined(HAL_BOOTLOADER_BUILD)
// Set Filters to ignore frames not to be handled by us
bool configureFilters(const CanFilterConfig* filter_configs,
uint16_t num_configs) override;
#endif
// In BxCAN the Busoff error is cleared automatically,
// so always return false
bool is_busoff() const override
@ -185,7 +191,9 @@ public:
}
// Get total number of Errors discovered
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
uint32_t getErrorCount() const override;
#endif
// returns true if init was successfully called
bool is_initialized() const override
@ -201,13 +209,14 @@ public:
const AP_HAL::CANFrame* const pending_tx,
uint64_t blocking_deadline) override;
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
// setup event handle for waiting on events
bool set_event_handle(AP_HAL::EventHandle* handle) override;
// fetch stats text and return the size of the same,
// results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt
uint32_t get_stats(char* data, uint32_t max_size) override;
#endif
/************************************
* Methods used inside interrupt *
************************************/

134
libraries/AP_HAL_ChibiOS/CanIface.cpp
View File

@ -50,13 +50,6 @@
# if !defined(STM32H7XX)
#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. */
#if defined(STM32F3XX)
#define RCC_APB1ENR_CAN1EN RCC_APB1ENR_CANEN
@ -64,18 +57,37 @@
#define CAN1_TX_IRQn CAN_TX_IRQn
#define CAN1_RX0_IRQn CAN_RX0_IRQn
#define CAN1_RX1_IRQn CAN_RX1_IRQn
#define CAN1_TX_IRQHandler CAN_TX_IRQHandler
#define CAN1_RX0_IRQHandler CAN_RX0_IRQHandler
#define CAN1_RX1_IRQHandler CAN_RX1_IRQHandler
#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
#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
#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;
constexpr bxcan::CanType* const CANIface::Can[];
static ChibiOS::CANIface* can_ifaces[HAL_NUM_CAN_IFACES] = {nullptr};
static inline void handleTxInterrupt(uint8_t iface_index)
{
@ -86,8 +98,8 @@ static inline void handleTxInterrupt(uint8_t iface_index)
if (precise_time > 0) {
precise_time--;
}
if (hal.can[iface_index] != nullptr) {
((ChibiOS::CANIface*)hal.can[iface_index])->handleTxInterrupt(precise_time);
if (can_ifaces[iface_index] != nullptr) {
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) {
precise_time--;
}
if (hal.can[iface_index] != UAVCAN_NULLPTR) {
((CANIface*)hal.can[iface_index])->handleRxInterrupt(fifo_index, precise_time);
if (can_ifaces[iface_index] != nullptr) {
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) :
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) {
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)
{
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) {
txmailbox = 2;
} else {
stats.tx_rejected++;
PERF_STATS(stats.tx_rejected);
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;
}
#if !defined(HAL_BOOTLOADER_BUILD)
bool CANIface::configureFilters(const CanFilterConfig* filter_configs,
uint16_t num_configs)
{
@ -418,6 +431,7 @@ bool CANIface::configureFilters(const CanFilterConfig* filter_configs,
return false;
}
#endif
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.timestamp_us = timestamp_us;
rx_item.flags = AP_HAL::CANIface::Loopback;
stats.tx_loopback++;
PERF_STATS(stats.tx_loopback);
rx_queue_.push(rx_item);
}
if (txok && !txi.pushed) {
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);
}
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
if (event_handle_ != nullptr) {
stats.num_events++;
PERF_STATS(stats.num_events);
evt_src_.signalI(1 << self_index_);
}
#endif
pollErrorFlagsFromISR();
}
@ -494,7 +510,7 @@ void CANIface::handleRxInterrupt(uint8_t fifo_index, uint64_t timestamp_us)
* Register overflow as a hardware error
*/
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.flags = 0;
if (rx_queue_.push(rx_item)) {
stats.rx_received++;
PERF_STATS(stats.rx_received);
} else {
stats.rx_overflow++;
PERF_STATS(stats.rx_overflow);
}
had_activity_ = true;
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
if (event_handle_ != nullptr) {
stats.num_events++;
PERF_STATS(stats.num_events);
evt_src_.signalI(1 << self_index_);
}
#endif
pollErrorFlagsFromISR();
}
@ -555,7 +572,6 @@ void CANIface::pollErrorFlagsFromISR()
const uint8_t lec = uint8_t((can_->ESR & bxcan::ESR_LEC_MASK) >> bxcan::ESR_LEC_SHIFT);
if (lec != 0) {
can_->ESR = 0;
error_cnt_++;
// Serving abort requests
for (int i = 0; i < NumTxMailboxes; i++) {
@ -563,7 +579,7 @@ void CANIface::pollErrorFlagsFromISR()
if (txi.aborted && txi.abort_on_error) {
can_->TSR = TSR_ABRQx[i];
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) {
can_->TSR = TSR_ABRQx[i]; // Goodnight sweet transmission
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;
}
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
uint32_t CANIface::getErrorCount() const
{
CriticalSectionLocker lock;
@ -657,6 +674,8 @@ bool CANIface::set_event_handle(AP_HAL::EventHandle* handle)
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
{
write = false;
@ -687,6 +706,9 @@ bool CANIface::select(bool &read, bool &write,
if ((read && in_read) || (write && in_write)) {
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) {
if (event_handle_ == nullptr) {
break;
@ -698,6 +720,7 @@ bool CANIface::select(bool &read, bool &write,
}
time = AP_HAL::micros();
}
#endif // #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
return true;
}
@ -750,20 +773,26 @@ bool CANIface::init(const uint32_t bitrate, const CANIface::OperatingMode mode)
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
}
if (hal.can[0] == nullptr) {
const_cast <AP_HAL::HAL&> (hal).can[0] = new CANIface(0);
if (can_ifaces[0] == nullptr) {
can_ifaces[0] = new CANIface(0);
Debug("Failed to allocate CAN iface 0");
if (hal.can[0] == nullptr) {
if (can_ifaces[0] == nullptr) {
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("Enabling CAN iface 0");
((CANIface*)hal.can[0])->initOnce(false);
can_ifaces[0]->initOnce(false);
Debug("Initing iface 0...");
if (!hal.can[0]->init(bitrate, mode)) {
if (!can_ifaces[0]->init(bitrate, mode)) {
Debug("Iface 0 init failed");
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
*/
rx_queue_.clear();
error_cnt_ = 0;
for (uint32_t i=0; i < NumTxMailboxes; i++) {
pending_tx_[i] = CanTxItem();
@ -867,6 +895,7 @@ bool CANIface::init(const uint32_t bitrate, const CANIface::OperatingMode mode)
return true;
}
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
uint32_t CANIface::get_stats(char* data, uint32_t max_size)
{
if (data == nullptr) {
@ -897,6 +926,7 @@ uint32_t CANIface::get_stats(char* data, uint32_t max_size)
memset(&stats, 0, sizeof(stats));
return ret;
}
#endif
/*
* Interrupt handlers
@ -904,24 +934,24 @@ uint32_t CANIface::get_stats(char* data, uint32_t max_size)
extern "C"
{
CH_IRQ_HANDLER(CAN1_TX_IRQHandler);
CH_IRQ_HANDLER(CAN1_TX_IRQHandler)
CH_IRQ_HANDLER(CAN1_TX_IRQ_Handler);
CH_IRQ_HANDLER(CAN1_TX_IRQ_Handler)
{
CH_IRQ_PROLOGUE();
handleTxInterrupt(0);
CH_IRQ_EPILOGUE();
}
CH_IRQ_HANDLER(CAN1_RX0_IRQHandler);
CH_IRQ_HANDLER(CAN1_RX0_IRQHandler)
CH_IRQ_HANDLER(CAN1_RX0_IRQ_Handler);
CH_IRQ_HANDLER(CAN1_RX0_IRQ_Handler)
{
CH_IRQ_PROLOGUE();
handleRxInterrupt(0, 0);
CH_IRQ_EPILOGUE();
}
CH_IRQ_HANDLER(CAN1_RX1_IRQHandler);
CH_IRQ_HANDLER(CAN1_RX1_IRQHandler)
CH_IRQ_HANDLER(CAN1_RX1_IRQ_Handler);
CH_IRQ_HANDLER(CAN1_RX1_IRQ_Handler)
{
CH_IRQ_PROLOGUE();
handleRxInterrupt(0, 1);
@ -930,36 +960,36 @@ extern "C"
#if HAL_NUM_CAN_IFACES > 1
#if !defined(CAN2_TX_IRQHandler)
#if !defined(CAN2_TX_IRQ_Handler)
# error "Misconfigured build1"
#endif
#if !defined(CAN2_RX0_IRQHandler)
#if !defined(CAN2_RX0_IRQ_Handler)
# error "Misconfigured build2"
#endif
#if !defined(CAN2_RX1_IRQHandler)
#if !defined(CAN2_RX1_IRQ_Handler)
# error "Misconfigured build3"
#endif
CH_IRQ_HANDLER(CAN2_TX_IRQHandler);
CH_IRQ_HANDLER(CAN2_TX_IRQHandler)
CH_IRQ_HANDLER(CAN2_TX_IRQ_Handler);
CH_IRQ_HANDLER(CAN2_TX_IRQ_Handler)
{
CH_IRQ_PROLOGUE();
handleTxInterrupt(1);
CH_IRQ_EPILOGUE();
}
CH_IRQ_HANDLER(CAN2_RX0_IRQHandler);
CH_IRQ_HANDLER(CAN2_RX0_IRQHandler)
CH_IRQ_HANDLER(CAN2_RX0_IRQ_Handler);
CH_IRQ_HANDLER(CAN2_RX0_IRQ_Handler)
{
CH_IRQ_PROLOGUE();
handleRxInterrupt(1, 0);
CH_IRQ_EPILOGUE();
}
CH_IRQ_HANDLER(CAN2_RX1_IRQHandler);
CH_IRQ_HANDLER(CAN2_RX1_IRQHandler)
CH_IRQ_HANDLER(CAN2_RX1_IRQ_Handler);
CH_IRQ_HANDLER(CAN2_RX1_IRQ_Handler)
{
CH_IRQ_PROLOGUE();
handleRxInterrupt(1, 1);

6
libraries/AP_HAL_ChibiOS/EventSource.cpp
View File

@ -1,5 +1,6 @@
#include "EventSource.h"
#include <AP_HAL/AP_HAL.h>
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
using namespace ChibiOS;
@ -31,4 +32,5 @@ void EventSource::signalI(uint32_t evt_mask)
ch_evt_src_.broadcastFlagsI(evt_mask);
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
#include <AP_HAL/AP_HAL.h>
#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
#include <stdint.h>
#include <AP_HAL/AP_HAL_Boards.h>
#include <AP_HAL/AP_HAL_Macros.h>
@ -22,4 +24,5 @@ public:
// Wait on an Event handle, method for internal use by EventHandle
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"
#endif
#ifndef HAL_BOOTLOADER_BUILD
extern const AP_HAL::HAL& hal;
#endif
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;
va_start (arg, fmt);
#ifdef HAL_BOOTLOADER_BUILD
done = chvsnprintf(str, size, fmt, arg);
#else
done = hal.util->vsnprintf(str, size, fmt, arg);
#endif
va_end (arg);
return done;
@ -85,7 +91,7 @@ int __wrap_vprintf(const char *fmt, va_list arg)
{
#ifdef HAL_STDOUT_SERIAL
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();
return chvprintf((BaseSequentialStream*)&SDU1, fmt, arg);
#else

7
libraries/AP_HAL_ChibiOS/system.cpp
View File

@ -217,6 +217,7 @@ void init()
void panic(const char *errormsg, ...)
{
#ifndef HAL_BOOTLOADER_BUILD
va_list ap;
va_start(ap, errormsg);
@ -228,6 +229,12 @@ void panic(const char *errormsg, ...)
vprintf(errormsg, ap);
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()