Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_Mission: DO_LAND_START to consider altitude (3D distance) #22147

Merged
merged 1 commit into from Nov 14, 2022
Merged

AP_Mission: DO_LAND_START to consider altitude (3D distance) #22147

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
12 changes: 11 additions & 1 deletion libraries/AP_Mission/AP_Mission.cpp
View file
Open in desktop
Expand Up @@ -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;

Expand All @@ -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;
Expand Down