From 183369b92ac85f0c7dbc70d86b6444a963ca8127 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Mon, 18 Apr 2016 09:37:30 -0700 Subject: [PATCH] Plane: take advantage of L1 stale flag and correctly inhibit flight stage if not lined up on approach --- ArduPlane/ArduPlane.cpp | 6 ++++-- ArduPlane/commands_logic.cpp | 2 ++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 8bf201ea6a..4dfb06abe4 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -927,9 +927,11 @@ void Plane::update_flight_stage(void) } else if (auto_state.land_pre_flare == true) { set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_PREFLARE); } else if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) { - bool lined_up = abs(nav_controller->bearing_error_cd()) < 1000; + 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.15 && lined_up && below_prev_WP) || + if ((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); } else { diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index fb648692b6..70523fba31 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -32,6 +32,8 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) // once landed, post some landing statistics to the GCS auto_state.post_landing_stats = false; + nav_controller->set_data_is_stale(); + // reset loiter start time. New command is a new loiter loiter.start_time_ms = 0;