HAL_Linux: use pthread mutexes for semaphores

libraries/AP_HAL_Linux/Semaphores.cpp

@@ -1,26 +1,28 @@
 
 #include "Semaphores.h"
 
+extern const AP_HAL::HAL& hal;
+
 using namespace Linux;
 
-bool LinuxSemaphore::give() {
-    if (_taken) {
-        _taken = false;
-        return true;
-    } else {
-        return false;
-    }
+bool LinuxSemaphore::give() 
+{
+    return pthread_mutex_unlock(&_lock) == 0;
 }
 
-bool LinuxSemaphore::take(uint32_t timeout_ms) {
+bool LinuxSemaphore::take(uint32_t timeout_ms) 
+{
+    if (timeout_ms == 0) {
+        return pthread_mutex_lock(&_lock) == 0;
+    }
+    while (timeout_ms-- > 0) {
+        if (take_nonblocking()) return true;
+        hal.scheduler->delay(1);
+    }
     return take_nonblocking();
 }
 
-bool LinuxSemaphore::take_nonblocking() {
-    /* we need pthread semaphores here */
-    if (!_taken) {
-        _taken = true;
-        return true;
-    }
-    return false;
+bool LinuxSemaphore::take_nonblocking() 
+{
+    return pthread_mutex_trylock(&_lock) == 0;
 }

libraries/AP_HAL_Linux/Semaphores.h

@@ -3,15 +3,18 @@
 #define __AP_HAL_LINUX_SEMAPHORE_H__
 
 #include <AP_HAL_Linux.h>
+#include <pthread.h>
 
 class Linux::LinuxSemaphore : public AP_HAL::Semaphore {
 public:
-    LinuxSemaphore() : _taken(false) {}
+    LinuxSemaphore() {
+        pthread_mutex_init(&_lock, NULL);
+    }
     bool give();
     bool take(uint32_t timeout_ms);
     bool take_nonblocking();
 private:
-    volatile bool _taken;
+    pthread_mutex_t _lock;
 };
 
 #endif // __AP_HAL_LINUX_SEMAPHORE_H__