From e1e411404cfd34262a0b87dfece184962111b8c9 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 1 Nov 2022 14:43:10 -0700 Subject: [PATCH] AP_Mission: DO_LAND_START to consider altitude (3D distance) --- libraries/AP_Mission/AP_Mission.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 1d0c88ede0600..fc1964bd7f067 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -2136,6 +2136,7 @@ uint16_t AP_Mission::get_landing_sequence_start() return 0; } + const Location::AltFrame current_loc_alt_frame = current_loc.get_alt_frame(); uint16_t landing_start_index = 0; float min_distance = -1; @@ -2150,7 +2151,16 @@ uint16_t AP_Mission::get_landing_sequence_start() // command does not have a valid location and cannot get next valid continue; } - float tmp_distance = tmp.content.location.get_distance(current_loc); + + float tmp_distance; + if (current_loc_alt_frame == tmp.content.location.get_alt_frame() || tmp.content.location.change_alt_frame(current_loc_alt_frame)) { + // 3D distance - altitudes are able to be compared in the same frame + tmp_distance = tmp.content.location.get_distance_NED(current_loc).length(); + } else { + // 2D distance - no altitude + tmp_distance = tmp.content.location.get_distance(current_loc); + } + if (min_distance < 0 || tmp_distance < min_distance) { min_distance = tmp_distance; landing_start_index = i;