diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index 0c7aadea15..6bd9a13d3e 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -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(bitrate), static_cast(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 diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h index 6b9918a566..3bad00138e 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.h +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.h @@ -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 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 * ************************************/ diff --git a/libraries/AP_HAL_ChibiOS/CANIface.h b/libraries/AP_HAL_ChibiOS/CANIface.h index 4719514226..188ddbe88d 100644 --- a/libraries/AP_HAL_ChibiOS/CANIface.h +++ b/libraries/AP_HAL_ChibiOS/CANIface.h @@ -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 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 * ************************************/ diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp index 18138112dc..dea9f4b89b 100644 --- a/libraries/AP_HAL_ChibiOS/CanIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp @@ -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 (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); diff --git a/libraries/AP_HAL_ChibiOS/EventSource.cpp b/libraries/AP_HAL_ChibiOS/EventSource.cpp index 7409baba0f..e530c8245f 100644 --- a/libraries/AP_HAL_ChibiOS/EventSource.cpp +++ b/libraries/AP_HAL_ChibiOS/EventSource.cpp @@ -1,5 +1,6 @@ #include "EventSource.h" -#include + +#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) \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/EventSource.h b/libraries/AP_HAL_ChibiOS/EventSource.h index a3b497b931..ad5de8649d 100644 --- a/libraries/AP_HAL_ChibiOS/EventSource.h +++ b/libraries/AP_HAL_ChibiOS/EventSource.h @@ -1,5 +1,7 @@ #pragma once +#include +#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD) #include #include #include @@ -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) diff --git a/libraries/AP_HAL_ChibiOS/stdio.cpp b/libraries/AP_HAL_ChibiOS/stdio.cpp index dce772306d..d7f263d0b2 100644 --- a/libraries/AP_HAL_ChibiOS/stdio.cpp +++ b/libraries/AP_HAL_ChibiOS/stdio.cpp @@ -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 diff --git a/libraries/AP_HAL_ChibiOS/system.cpp b/libraries/AP_HAL_ChibiOS/system.cpp index 0e19a3856e..45c5ec96b0 100644 --- a/libraries/AP_HAL_ChibiOS/system.cpp +++ b/libraries/AP_HAL_ChibiOS/system.cpp @@ -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()