Plane: for better helical landings allow for instant approach stage if previous nav cmd was LOITER_TO_ALT

This commit is contained in:
Tom Pittenger 2016-05-12 15:24:42 -07:00
parent f1186b8b5c
commit 6c0579a895
1 changed files with 2 additions and 1 deletions

View File

@ -944,7 +944,8 @@ void Plane::update_flight_stage(void)
bool heading_lined_up = abs(nav_controller->bearing_error_cd()) < 1000 && !nav_controller->data_is_stale();
bool on_flight_line = abs(nav_controller->crosstrack_error() < 5) && !nav_controller->data_is_stale();
bool below_prev_WP = current_loc.alt < prev_WP_loc.alt;
if ((auto_state.wp_proportion >= 0 && heading_lined_up && on_flight_line) ||
if ((mission.get_prev_nav_cmd_id() == MAV_CMD_NAV_LOITER_TO_ALT) ||
(auto_state.wp_proportion >= 0 && heading_lined_up && on_flight_line) ||
(auto_state.wp_proportion > 0.15f && heading_lined_up && below_prev_WP) ||
(auto_state.wp_proportion > 0.5f)) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH);