HAL_SITL: support HAL_Semaphore_Recursive

libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h

@@ -12,6 +12,7 @@ class ADCSource;
 class RCInput;
 class Util;
 class Semaphore;
+class Semaphore_Recursive;
 class GPIO;
 class DigitalSource;
 class HALSITLCAN;

libraries/AP_HAL_SITL/Semaphores.cpp

@@ -8,9 +8,28 @@ extern const AP_HAL::HAL& hal;
 
 using namespace HALSITL;
 
+// construct a semaphore
+Semaphore::Semaphore()
+{
+    pthread_mutex_init(&_lock, nullptr);
+}
+
+// construct a recursive semaphore (allows a thread to take it more than once)
+Semaphore_Recursive::Semaphore_Recursive()
+{
+    pthread_mutexattr_t attr;
+    pthread_mutexattr_init(&attr);
+    pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
+    pthread_mutex_init(&_lock, &attr);
+}
+
+
 bool Semaphore::give()
 {
-    return pthread_mutex_unlock(&_lock) == 0;
+    if (pthread_mutex_unlock(&_lock) != 0) {
+        AP_HAL::panic("Bad semaphore usage");
+    }
+    return true;
 }
 
 bool Semaphore::take(uint32_t timeout_ms)

libraries/AP_HAL_SITL/Semaphores.h

@@ -9,14 +9,18 @@
 
 class HALSITL::Semaphore : public AP_HAL::Semaphore {
 public:
-    Semaphore() {
-        pthread_mutex_init(&_lock, nullptr);
-    }
+    Semaphore();
     bool give();
     bool take(uint32_t timeout_ms);
     bool take_nonblocking();
-private:
+protected:
     pthread_mutex_t _lock;
 };
 
 
+class HALSITL::Semaphore_Recursive : public HALSITL::Semaphore {
+public:
+    Semaphore_Recursive();
+};
+
+