diff --git a/libraries/AP_HAL/Semaphores.cpp b/libraries/AP_HAL/Semaphores.cpp
new file mode 100644
index 0000000000..65890007e2
--- /dev/null
+++ b/libraries/AP_HAL/Semaphores.cpp
@@ -0,0 +1,28 @@
+#include "AP_HAL.h"
+
+extern const AP_HAL::HAL &hal;
+
+/*
+  implement WithSemaphore class for WITH_SEMAPHORE() support
+ */
+WithSemaphore::WithSemaphore(AP_HAL::Semaphore *mtx, uint32_t line) :
+    WithSemaphore(*mtx, line)
+{}
+
+WithSemaphore::WithSemaphore(AP_HAL::Semaphore &mtx, uint32_t line) :
+    _mtx(mtx)
+{
+    bool in_main = hal.scheduler->in_main_thread();
+    if (in_main) {
+        hal.util->persistent_data.semaphore_line = line;
+    }
+    _mtx.take_blocking();
+    if (in_main) {
+        hal.util->persistent_data.semaphore_line = 0;
+    }
+}
+
+WithSemaphore::~WithSemaphore()
+{
+    _mtx.give();
+}
diff --git a/libraries/AP_HAL/Semaphores.h b/libraries/AP_HAL/Semaphores.h
index 7cce91a248..ee26b73509 100644
--- a/libraries/AP_HAL/Semaphores.h
+++ b/libraries/AP_HAL/Semaphores.h
@@ -20,3 +20,31 @@ public:
     virtual bool give() = 0;
     virtual ~Semaphore(void) {}
 };
+
+/*
+  a method to make semaphores less error prone. The WITH_SEMAPHORE()
+  macro will block forever for a semaphore, and will automatically
+  release the semaphore when it goes out of scope
+
+  Note that we have two types of semaphores. A normal semaphore can
+  only be taken once. A recursive semaphore allows for the thread
+  holding the semaphore to take it again. It must be released the same
+  number of times it is taken.
+
+  The WITH_SEMAPHORE() macro can be used with either type of semaphore
+ */
+
+class WithSemaphore {
+public:
+    WithSemaphore(AP_HAL::Semaphore *mtx, uint32_t line);
+    WithSemaphore(AP_HAL::Semaphore &mtx, uint32_t line);
+
+    ~WithSemaphore();
+private:
+    AP_HAL::Semaphore &_mtx;
+};
+
+// From: https://stackoverflow.com/questions/19666142/why-is-a-level-of-indirection-needed-for-this-concatenation-macro
+#define WITH_SEMAPHORE( sem ) JOIN( sem, __LINE__, __COUNTER__ )
+#define JOIN( sem, line, counter ) _DO_JOIN( sem, line, counter )
+#define _DO_JOIN( sem, line, counter ) WithSemaphore _getsem ## counter(sem, line)