From 935c9167ab1de6c1f6f820bd10369bade6d1f268 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 20 Aug 2018 11:20:13 +1000
Subject: [PATCH] AP_Avoidance: added semaphore

---
 libraries/AP_Avoidance/AP_Avoidance.cpp | 2 ++
 libraries/AP_Avoidance/AP_Avoidance.h   | 4 ++++
 2 files changed, 6 insertions(+)

diff --git a/libraries/AP_Avoidance/AP_Avoidance.cpp b/libraries/AP_Avoidance/AP_Avoidance.cpp
index 32871b43f0..74ade89f02 100644
--- a/libraries/AP_Avoidance/AP_Avoidance.cpp
+++ b/libraries/AP_Avoidance/AP_Avoidance.cpp
@@ -212,6 +212,8 @@ void AP_Avoidance::add_obstacle(const uint32_t obstacle_timestamp_ms,
             oldest_index = i;
         }
     }
+    WITH_SEMAPHORE(_rsem);
+    
     if (index == -1) {
         // existing obstacle not found.  See if we can store it anyway:
         if (i <_obstacles_allocated) {
diff --git a/libraries/AP_Avoidance/AP_Avoidance.h b/libraries/AP_Avoidance/AP_Avoidance.h
index bb7d3581c5..694fd74fd3 100644
--- a/libraries/AP_Avoidance/AP_Avoidance.h
+++ b/libraries/AP_Avoidance/AP_Avoidance.h
@@ -27,6 +27,7 @@
 
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_ADSB/AP_ADSB.h>
+#include <AP_Common/Semaphore.h>
 
 // F_RCVRY possible parameter values
 #define AP_AVOIDANCE_RECOVERY_REMAIN_IN_AVOID_ADSB                  0
@@ -195,6 +196,9 @@ private:
     AP_Int8     _warn_time_horizon;
     AP_Float    _warn_distance_xy;
     AP_Float    _warn_distance_z;
+
+    // multi-thread support for avoidance
+    HAL_Semaphore_Recursive _rsem;
 };
 
 float closest_distance_between_radial_and_point(const Vector2f &w,