AP_Avoidance: added semaphore

This commit is contained in:
Andrew Tridgell 2018-08-20 11:20:13 +10:00
parent 9897bf6127
commit 935c9167ab
2 changed files with 6 additions and 0 deletions

View File

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

View File

@ -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,