diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index eaefd34324..08c532e46d 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -812,11 +812,9 @@ void CANIface::handleTxCompleteInterrupt(const uint64_t timestamp_us) rx_item.flags = AP_HAL::CANIface::Loopback; add_to_rx_queue(rx_item); } - if (event_handle_ != nullptr) { - stats.num_events++; -#if CH_CFG_USE_EVENTS == TRUE - evt_src_.signalI(1 << self_index_); -#endif + stats.num_events++; + if (sem_handle != nullptr) { + sem_handle->signal_ISR(); } } } @@ -925,11 +923,9 @@ void CANIface::handleRxInterrupt(uint8_t fifo_index) while (readRxFIFO(fifo_index)) { had_activity_ = true; } - if (event_handle_ != nullptr) { - stats.num_events++; -#if CH_CFG_USE_EVENTS == TRUE - evt_src_.signalI(1 << self_index_); -#endif + stats.num_events++; + if (sem_handle != nullptr) { + sem_handle->signal_ISR(); } } @@ -995,16 +991,11 @@ uint32_t CANIface::getErrorCount() const stats.tx_timedout; } -#if CH_CFG_USE_EVENTS == TRUE -ChibiOS::EventSource CANIface::evt_src_; -bool CANIface::set_event_handle(AP_HAL::EventHandle* handle) +bool CANIface::set_event_handle(AP_HAL::BinarySemaphore *handle) { - CriticalSectionLocker lock; - event_handle_ = handle; - event_handle_->set_source(&evt_src_); - return event_handle_->register_event(1 << self_index_); + sem_handle = handle; + return true; } -#endif bool CANIface::isRxBufferEmpty() const { @@ -1075,10 +1066,10 @@ bool CANIface::select(bool &read, bool &write, return true; } while (time < blocking_deadline) { - if (event_handle_ == nullptr) { + if (sem_handle == nullptr) { break; } - event_handle_->wait(blocking_deadline - time); // Block until timeout expires or any iface updates + IGNORE_RETURN(sem_handle->wait(blocking_deadline - time)); // Block until timeout expires or any iface updates checkAvailable(read, write, pending_tx); // Check what we got if ((read && in_read) || (write && in_write)) { return true; diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h index 81815fdaa0..f7ff133392 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.h +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.h @@ -41,7 +41,6 @@ #pragma once #include "AP_HAL_ChibiOS.h" -#include "EventSource.h" #if HAL_NUM_CAN_IFACES @@ -120,10 +119,8 @@ class ChibiOS::CANIface : public AP_HAL::CANIface bool irq_init_; bool initialised_; bool had_activity_; - AP_HAL::EventHandle* event_handle_; -#if CH_CFG_USE_EVENTS == TRUE - static ChibiOS::EventSource evt_src_; -#endif + AP_HAL::BinarySemaphore *sem_handle; + const uint8_t self_index_; bool computeTimings(uint32_t target_bitrate, Timings& out_timings); @@ -224,10 +221,8 @@ public: const AP_HAL::CANFrame* const pending_tx, uint64_t blocking_deadline) override; -#if CH_CFG_USE_EVENTS == TRUE // setup event handle for waiting on events - bool set_event_handle(AP_HAL::EventHandle* handle) override; -#endif + bool set_event_handle(AP_HAL::BinarySemaphore *handle) override; #if !defined(HAL_BOOTLOADER_BUILD) // fetch stats text and return the size of the same, diff --git a/libraries/AP_HAL_ChibiOS/CANIface.h b/libraries/AP_HAL_ChibiOS/CANIface.h index b85ed4e538..363c002d49 100644 --- a/libraries/AP_HAL_ChibiOS/CANIface.h +++ b/libraries/AP_HAL_ChibiOS/CANIface.h @@ -46,7 +46,6 @@ # else #if HAL_NUM_CAN_IFACES #include "bxcan.hpp" -#include "EventSource.h" #ifndef HAL_CAN_RX_QUEUE_SIZE #define HAL_CAN_RX_QUEUE_SIZE 128 @@ -109,10 +108,8 @@ class ChibiOS::CANIface : public AP_HAL::CANIface bool irq_init_:1; bool initialised_:1; bool had_activity_:1; -#if CH_CFG_USE_EVENTS == TRUE - AP_HAL::EventHandle* event_handle_; - static ChibiOS::EventSource evt_src_; -#endif + AP_HAL::BinarySemaphore *sem_handle; + const uint8_t self_index_; bool computeTimings(uint32_t target_bitrate, Timings& out_timings); @@ -210,10 +207,9 @@ public: const AP_HAL::CANFrame* const pending_tx, uint64_t blocking_deadline) override; -#if CH_CFG_USE_EVENTS == TRUE // setup event handle for waiting on events - bool set_event_handle(AP_HAL::EventHandle* handle) override; -#endif + bool set_event_handle(AP_HAL::BinarySemaphore *handle) override; + #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD) // fetch stats text and return the size of the same, // results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp index 97cd8acf2a..f6704b6438 100644 --- a/libraries/AP_HAL_ChibiOS/CanIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp @@ -522,12 +522,11 @@ void CANIface::handleTxInterrupt(const uint64_t utc_usec) handleTxMailboxInterrupt(2, txok, utc_usec); } -#if CH_CFG_USE_EVENTS == TRUE - if (event_handle_ != nullptr) { + if (sem_handle != nullptr) { PERF_STATS(stats.num_events); - evt_src_.signalI(1 << self_index_); + sem_handle->signal_ISR(); } -#endif + pollErrorFlagsFromISR(); } @@ -590,12 +589,11 @@ void CANIface::handleRxInterrupt(uint8_t fifo_index, uint64_t timestamp_us) had_activity_ = true; -#if CH_CFG_USE_EVENTS == TRUE - if (event_handle_ != nullptr) { + if (sem_handle != nullptr) { PERF_STATS(stats.num_events); - evt_src_.signalI(1 << self_index_); + sem_handle->signal_ISR(); } -#endif + pollErrorFlagsFromISR(); } @@ -702,17 +700,12 @@ uint32_t CANIface::getErrorCount() const #endif // #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD) -#if CH_CFG_USE_EVENTS == TRUE -ChibiOS::EventSource CANIface::evt_src_; -bool CANIface::set_event_handle(AP_HAL::EventHandle* handle) +bool CANIface::set_event_handle(AP_HAL::BinarySemaphore *handle) { - CriticalSectionLocker lock; - event_handle_ = handle; - event_handle_->set_source(&evt_src_); - return event_handle_->register_event(1 << self_index_); + sem_handle = handle; + return true; } -#endif // #if CH_CFG_USE_EVENTS == TRUE void CANIface::checkAvailable(bool& read, bool& write, const AP_HAL::CANFrame* pending_tx) const { @@ -745,13 +738,13 @@ bool CANIface::select(bool &read, bool &write, return true; } -#if CH_CFG_USE_EVENTS == 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) { + if (sem_handle == nullptr) { break; } - event_handle_->wait(blocking_deadline - time); // Block until timeout expires or any iface updates + IGNORE_RETURN(sem_handle->wait(blocking_deadline - time)); // Block until timeout expires or any iface updates checkAvailable(read, write, pending_tx); // Check what we got if ((read && in_read) || (write && in_write)) { return true;