diff --git a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h index 5a360cbf59..f5924ebd8d 100644 --- a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h +++ b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h @@ -15,6 +15,7 @@ class ADCSource; class RCInput; class Util; class Semaphore; +class BinarySemaphore; class GPIO; class DigitalSource; class DSP; diff --git a/libraries/AP_HAL_SITL/Semaphores.cpp b/libraries/AP_HAL_SITL/Semaphores.cpp index 216c1d98ee..0067fd9c14 100644 --- a/libraries/AP_HAL_SITL/Semaphores.cpp +++ b/libraries/AP_HAL_SITL/Semaphores.cpp @@ -76,4 +76,77 @@ bool Semaphore::take_nonblocking() return false; } + +/* + binary semaphore using pthread condition variables + */ + +BinarySemaphore::BinarySemaphore(bool initial_state) : + AP_HAL::BinarySemaphore(initial_state) +{ + pthread_cond_init(&cond, NULL); + pending = initial_state; +} + +bool BinarySemaphore::wait(uint32_t timeout_us) +{ + WITH_SEMAPHORE(mtx); + if (!pending) { + if (hal.scheduler->in_main_thread() || + Scheduler::from(hal.scheduler)->semaphore_wait_hack_required()) { + /* + when in the main thread we need to do a busy wait to ensure + the clock advances + */ + uint64_t end_us = AP_HAL::micros64() + timeout_us; + struct timespec ts {}; + do { + if (pthread_cond_timedwait(&cond, &mtx._lock, &ts) == 0) { + pending = false; + return true; + } + hal.scheduler->delay_microseconds(10); + } while (AP_HAL::micros64() < end_us); + return false; + } + + struct timespec ts; + if (clock_gettime(CLOCK_REALTIME, &ts) != 0) { + return false; + } + ts.tv_sec += timeout_us/1000000UL; + ts.tv_nsec += (timeout_us % 1000000U) * 1000UL; + if (ts.tv_nsec >= 1000000000L) { + ts.tv_sec++; + ts.tv_nsec -= 1000000000L; + } + if (pthread_cond_timedwait(&cond, &mtx._lock, &ts) != 0) { + return false; + } + } + pending = false; + return true; +} + +bool BinarySemaphore::wait_blocking(void) +{ + WITH_SEMAPHORE(mtx); + if (!pending) { + if (pthread_cond_wait(&cond, &mtx._lock) != 0) { + return false; + } + } + pending = false; + return true; +} + +void BinarySemaphore::signal(void) +{ + WITH_SEMAPHORE(mtx); + if (!pending) { + pending = true; + pthread_cond_signal(&cond); + } +} + #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_SITL/Semaphores.h b/libraries/AP_HAL_SITL/Semaphores.h index 63aa57f2f5..c88d76854f 100644 --- a/libraries/AP_HAL_SITL/Semaphores.h +++ b/libraries/AP_HAL_SITL/Semaphores.h @@ -9,6 +9,7 @@ class HALSITL::Semaphore : public AP_HAL::Semaphore { public: + friend class HALSITL::BinarySemaphore; Semaphore(); bool give() override; bool take(uint32_t timeout_ms) override; @@ -24,3 +25,20 @@ protected: // semaphore once we're done with it uint8_t take_count; }; + + +class HALSITL::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; + +private: + HALSITL::Semaphore mtx; + pthread_cond_t cond; + bool pending; +};