HAL_Linux: implement BinarySemaphore

libraries/AP_HAL_Linux/Semaphores.cpp

@@ -44,3 +44,56 @@ bool Semaphore::take_nonblocking()
     return pthread_mutex_trylock(&_lock) == 0;
 }
 
+/*
+  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) {
+        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);
+    }
+}

libraries/AP_HAL_Linux/Semaphores.h

@@ -10,6 +10,7 @@ namespace Linux {
 
 class Semaphore : public AP_HAL::Semaphore {
 public:
+    friend class BinarySemaphore;
     Semaphore();
     bool give() override;
     bool take(uint32_t timeout_ms) override;
@@ -18,4 +19,19 @@ protected:
     pthread_mutex_t _lock;
 };
 
+
+class BinarySemaphore : public AP_HAL::BinarySemaphore {
+public:
+    BinarySemaphore(bool initial_state=false);
+
+    bool wait(uint32_t timeout_us) override;
+    bool wait_blocking(void) override;
+    void signal(void) override;
+
+protected:
+    Semaphore mtx;
+    pthread_cond_t cond;
+    bool pending;
+};
+    
 }