diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.cpp b/libraries/AP_Proximity/AP_Proximity_SITL.cpp index 576d84d208..7fc9c0476c 100644 --- a/libraries/AP_Proximity/AP_Proximity_SITL.cpp +++ b/libraries/AP_Proximity/AP_Proximity_SITL.cpp @@ -107,7 +107,7 @@ bool AP_Proximity_SITL::get_distance_to_fence(float angle_deg, float &distance) while (max_dist - min_dist > PROXIMITY_ACCURACY) { float test_dist = (max_dist+min_dist)*0.5f; Location loc = current_loc; - location_update(loc, angle_deg, test_dist); + loc.offset_bearing(angle_deg, test_dist); Vector2l vecloc(loc.lat, loc.lng); if (fence_loader.boundary_breached(vecloc, fence_count->get(), fence)) { max_dist = test_dist; diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.h b/libraries/AP_Proximity/AP_Proximity_SITL.h index 1922d65bf6..baa2a4b8c4 100644 --- a/libraries/AP_Proximity/AP_Proximity_SITL.h +++ b/libraries/AP_Proximity/AP_Proximity_SITL.h @@ -5,6 +5,7 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include #include +#include class AP_Proximity_SITL : public AP_Proximity_Backend {