From 1397c85f8ead6c96d0b9dd5f4b2b217b3fb94735 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 20 Aug 2018 11:09:20 +1000
Subject: [PATCH] HAL_PX4: added Semaphore_Recursive

---
 libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h |  1 +
 libraries/AP_HAL_PX4/Semaphores.cpp         | 15 +++++++++++++++
 libraries/AP_HAL_PX4/Semaphores.h           | 12 ++++++++----
 3 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h
index 5a8896ad15..f55a68c74d 100644
--- a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h
+++ b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h
@@ -15,6 +15,7 @@ namespace PX4 {
     class PX4I2CDriver;
     class PX4_I2C;
     class Semaphore;
+    class Semaphore_Recursive;
     class PX4CAN;
     class PX4CANManager;
 }
diff --git a/libraries/AP_HAL_PX4/Semaphores.cpp b/libraries/AP_HAL_PX4/Semaphores.cpp
index f4e3b1e450..2a2daacb18 100644
--- a/libraries/AP_HAL_PX4/Semaphores.cpp
+++ b/libraries/AP_HAL_PX4/Semaphores.cpp
@@ -9,6 +9,21 @@ extern const AP_HAL::HAL& hal;
 
 using namespace PX4;
 
+// 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;
diff --git a/libraries/AP_HAL_PX4/Semaphores.h b/libraries/AP_HAL_PX4/Semaphores.h
index 59300889bb..70ae0a52a0 100644
--- a/libraries/AP_HAL_PX4/Semaphores.h
+++ b/libraries/AP_HAL_PX4/Semaphores.h
@@ -9,13 +9,17 @@
 
 class PX4::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 PX4::Semaphore_Recursive : public PX4::Semaphore {
+public:
+    Semaphore_Recursive();
+};
+
+