HAL_SITL: added semaphore support

libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h

@@ -13,6 +13,7 @@ class SITLRCOutput;
 class ADCSource;
 class RCInput;
 class SITLUtil;
+class Semaphore;
 }
 
 #endif // __AP_HAL_SITL_NAMESPACE_H__

libraries/AP_HAL_SITL/AP_HAL_SITL_Private.h

@@ -7,6 +7,7 @@
 #include "Storage.h"
 #include "UARTDriver.h"
 #include "SITL_State.h"
+#include "Semaphores.h"
 
 #endif // __AP_HAL_SITL_PRIVATE_H__
 

libraries/AP_HAL_SITL/Semaphores.cpp

@@ -0,0 +1,39 @@
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+
+#include "Semaphores.h"
+
+extern const AP_HAL::HAL& hal;
+
+using namespace HALSITL;
+
+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_SITL/Semaphores.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include <AP_HAL/AP_HAL_Boards.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include "AP_HAL_SITL.h"
+#include <pthread.h>
+
+class HALSITL::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_SITL/Util.h

@@ -4,6 +4,7 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include "AP_HAL_SITL_Namespace.h"
+#include "Semaphores.h"
 
 class HALSITL::SITLUtil : public AP_HAL::Util {
 public:
@@ -18,6 +19,9 @@ public:
         // SITL is assumed to always have plenty of memory. Return 128k for now
         return 0x20000;
     }
+
+    // create a new semaphore
+    AP_HAL::Semaphore *new_semaphore(void) override { return new HALSITL::Semaphore; }
 };
 
 #endif // __AP_HAL_SITL_UTIL_H__