AP_Avoidance: added semaphore

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) {

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,