HAL_PX4: added Semaphore support

libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h

@@ -16,6 +16,7 @@ namespace PX4 {
         class NSHShellStream;
         class PX4I2CDriver;
         class PX4_I2C;
+        class Semaphore;
 }
 
 #endif //__AP_HAL_PX4_NAMESPACE_H__

libraries/AP_HAL_PX4/Semaphores.cpp

@@ -0,0 +1,39 @@
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+
+#include "Semaphores.h"
+
+extern const AP_HAL::HAL& hal;
+
+using namespace PX4;
+
+bool Semaphore::give() 
+{
+    return pthread_mutex_unlock(&_lock) == 0;
+}
+
+bool Semaphore::take(uint32_t timeout_ms) 
+{
+    if (timeout_ms == 0) {
+        return pthread_mutex_lock(&_lock) == 0;
+    }
+    if (take_nonblocking()) {
+        return true;
+    }
+    uint64_t start = AP_HAL::micros64();
+    do {
+        hal.scheduler->delay_microseconds(200);
+        if (take_nonblocking()) {
+            return true;
+        }
+    } while ((AP_HAL::micros64() - start) < timeout_ms*1000);
+    return false;
+}
+
+bool Semaphore::take_nonblocking() 
+{
+    return pthread_mutex_trylock(&_lock) == 0;
+}
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_HAL_PX4/Semaphores.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include <AP_HAL/AP_HAL_Boards.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#include "AP_HAL_PX4.h"
+#include <pthread.h>
+
+class PX4::Semaphore : public AP_HAL::Semaphore {
+public:
+    Semaphore() {
+        pthread_mutex_init(&_lock, NULL);
+    }
+    bool give();
+    bool take(uint32_t timeout_ms);
+    bool take_nonblocking();
+private:
+    pthread_mutex_t _lock;
+};
+#endif // CONFIG_HAL_BOARD

libraries/AP_HAL_PX4/Util.h

@@ -4,6 +4,7 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include "AP_HAL_PX4_Namespace.h"
+#include "Semaphores.h"
 
 class PX4::NSHShellStream : public AP_HAL::Stream {
 public:
@@ -56,6 +57,9 @@ public:
     void perf_end(perf_counter_t) override;
     void perf_count(perf_counter_t) override;
     
+    // create a new semaphore
+    AP_HAL::Semaphore *new_semaphore(void) override { return new PX4::Semaphore; }
+
 private:
     int _safety_handle;
     PX4::NSHShellStream _shell_stream;