diff --git a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h index 9d559a561c..4b62fcb928 100644 --- a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h +++ b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h @@ -17,7 +17,7 @@ namespace ChibiOS { class RCOutput; class Scheduler; class Semaphore; - class EventSource; + class BinarySemaphore; class SPIBus; class SPIDesc; class SPIDevice; diff --git a/libraries/AP_HAL_ChibiOS/EventSource.cpp b/libraries/AP_HAL_ChibiOS/EventSource.cpp deleted file mode 100644 index f1821bca00..0000000000 --- a/libraries/AP_HAL_ChibiOS/EventSource.cpp +++ /dev/null @@ -1,35 +0,0 @@ -#include "EventSource.h" -#include - -using namespace ChibiOS; - -#if CH_CFG_USE_EVENTS == TRUE - -bool EventSource::wait(uint16_t duration_us, AP_HAL::EventHandle *evt_handle) -{ - chibios_rt::EventListener evt_listener; - eventmask_t evt_mask = evt_handle->get_evt_mask(); - msg_t ret = msg_t(); - ch_evt_src_.registerMask(&evt_listener, evt_mask); - if (duration_us == 0) { - ret = chEvtWaitAnyTimeout(evt_mask, TIME_IMMEDIATE); - } else { - const sysinterval_t wait_us = MIN(TIME_MAX_INTERVAL, MAX(CH_CFG_ST_TIMEDELTA, chTimeUS2I(duration_us))); - ret = chEvtWaitAnyTimeout(evt_mask, wait_us); - } - ch_evt_src_.unregister(&evt_listener); - return ret == MSG_OK; -} - -void EventSource::signal(uint32_t evt_mask) -{ - ch_evt_src_.broadcastFlags(evt_mask); -} - -__RAMFUNC__ void EventSource::signalI(uint32_t evt_mask) -{ - chSysLockFromISR(); - ch_evt_src_.broadcastFlagsI(evt_mask); - chSysUnlockFromISR(); -} -#endif //#if CH_CFG_USE_EVENTS == TRUE diff --git a/libraries/AP_HAL_ChibiOS/EventSource.h b/libraries/AP_HAL_ChibiOS/EventSource.h deleted file mode 100644 index a1dda78b2e..0000000000 --- a/libraries/AP_HAL_ChibiOS/EventSource.h +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once -#include - -#include -#include -#include -#include -#include "AP_HAL_ChibiOS_Namespace.h" -#include - -#if CH_CFG_USE_EVENTS == TRUE -class ChibiOS::EventSource : public AP_HAL::EventSource { - // Single event source to be shared across multiple users - chibios_rt::EventSource ch_evt_src_; - -public: - // generate event from thread context - void signal(uint32_t evt_mask) override; - - // generate event from interrupt context - void signalI(uint32_t evt_mask) override; - - // Wait on an Event handle, method for internal use by EventHandle - bool wait(uint16_t duration_us, AP_HAL::EventHandle* evt_handle) override; -}; -#endif //#if CH_CFG_USE_EVENTS == TRUE diff --git a/libraries/AP_HAL_ChibiOS/Semaphores.cpp b/libraries/AP_HAL_ChibiOS/Semaphores.cpp index 4949339977..47e24348af 100644 --- a/libraries/AP_HAL_ChibiOS/Semaphores.cpp +++ b/libraries/AP_HAL_ChibiOS/Semaphores.cpp @@ -18,7 +18,9 @@ #include #include "Semaphores.h" #include +#include #include "AP_HAL_ChibiOS.h" +#include #if CH_CFG_USE_MUTEXES == TRUE @@ -78,4 +80,57 @@ void Semaphore::assert_owner(void) osalDbgAssert(check_owner(), "owner"); } +#if CH_CFG_USE_SEMAPHORES == TRUE +BinarySemaphore::BinarySemaphore(bool initial_state) : + AP_HAL::BinarySemaphore(initial_state) +{ + static_assert(sizeof(_lock) >= sizeof(semaphore_t), "invalid semaphore_t size"); + auto *sem = (binary_semaphore_t *)_lock; + /* + the initial_state in ChibiOS binary semaphores is 'taken', so we + need to negate it for the ArduPilot semantics + */ + chBSemObjectInit(sem, !initial_state); +} + +bool BinarySemaphore::wait(uint32_t timeout_us) +{ + auto *sem = (binary_semaphore_t *)_lock; + if (timeout_us == 0) { + return chBSemWaitTimeout(sem, TIME_IMMEDIATE) == MSG_OK; + } + // loop waiting for 60ms at a time. This ensures we can wait for + // any amount of time even on systems with a 16 bit timer + while (timeout_us > 0) { + const uint32_t us = MIN(timeout_us, 60000U); + if (chBSemWaitTimeout(sem, TIME_US2I(us)) == MSG_OK) { + return true; + } + timeout_us -= us; + } + return false; +} + +bool BinarySemaphore::wait_blocking(void) +{ + auto *sem = (binary_semaphore_t *)_lock; + return chBSemWait(sem) == MSG_OK; +} + +void BinarySemaphore::signal(void) +{ + auto *sem = (binary_semaphore_t *)_lock; + chBSemSignal(sem); +} + +void BinarySemaphore::signal_ISR(void) +{ + auto *sem = (binary_semaphore_t *)_lock; + chSysLockFromISR(); + chBSemSignalI(sem); + chSysUnlockFromISR(); +} + +#endif // CH_CFG_USE_SEMAPHORES == TRUE + #endif // CH_CFG_USE_MUTEXES diff --git a/libraries/AP_HAL_ChibiOS/Semaphores.h b/libraries/AP_HAL_ChibiOS/Semaphores.h index 484570d086..49f258366b 100644 --- a/libraries/AP_HAL_ChibiOS/Semaphores.h +++ b/libraries/AP_HAL_ChibiOS/Semaphores.h @@ -37,3 +37,21 @@ protected: // we declare the lock as a uint32_t array, and cast inside the cpp file uint32_t _lock[5]; }; + +/* + BinarySemaphore implementation + */ +class ChibiOS::BinarySemaphore : public AP_HAL::BinarySemaphore { +public: + BinarySemaphore(bool initial_state=false); + + CLASS_NO_COPY(BinarySemaphore); + + bool wait(uint32_t timeout_us) override; + bool wait_blocking(void) override; + void signal(void) override; + void signal_ISR(void) override; + +protected: + uint32_t _lock[5]; +};