diff --git a/libraries/AC_Avoidance/AP_OADatabase.cpp b/libraries/AC_Avoidance/AP_OADatabase.cpp index 1b22e9fcb6..e569336ec5 100644 --- a/libraries/AC_Avoidance/AP_OADatabase.cpp +++ b/libraries/AC_Avoidance/AP_OADatabase.cpp @@ -151,16 +151,14 @@ void AP_OADatabase::queue_push(const Vector3f &pos, uint32_t timestamp_ms, float // check if this obstacle needs to be rejected from DB because of low altitude near home #if APM_BUILD_COPTER_OR_HELI if (!is_zero(_min_alt)) { - Vector2f current_pos; - if (!AP::ahrs().get_relative_position_NE_home(current_pos)) { + Vector3f current_pos; + if (!AP::ahrs().get_relative_position_NED_home(current_pos)) { // we do not know where the vehicle is return; } - if (current_pos.length() < AP_OADATABASE_DISTANCE_FROM_HOME) { + if (current_pos.xy().length() < AP_OADATABASE_DISTANCE_FROM_HOME) { // vehicle is within a small radius of home - float height_above_home; - AP::ahrs().get_relative_position_D_home(height_above_home); - if (-height_above_home < _min_alt) { + if (-current_pos.z < _min_alt) { // vehicle is below the minimum alt return; }