From fefcf39335ed4acc9cbbdccf68325ba04a08c64c Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Thu, 21 Apr 2016 16:56:50 -0700 Subject: [PATCH] Plane: must be on land approach before flare alt/sec are used --- ArduPlane/landing.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ArduPlane/landing.cpp b/ArduPlane/landing.cpp index 6daa5b5c61..78033463d1 100644 --- a/ArduPlane/landing.cpp +++ b/ArduPlane/landing.cpp @@ -61,12 +61,15 @@ bool Plane::verify_land() // Below we check for wp_proportion being greater then 50%. In otherwords ensure that the vehicle // has covered 50% of the distance to the landing point before it can flare + + bool on_approach_stage = (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH || + flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE); bool below_flare_alt = (height <= g.land_flare_alt); bool below_flare_sec = (aparm.land_flare_sec > 0 && height <= auto_state.sink_rate * aparm.land_flare_sec); bool probably_crashed = (fabsf(auto_state.sink_rate) < 0.2f && !is_flying()); - if (below_flare_alt || - (below_flare_sec && (auto_state.wp_proportion > 0.5)) || + if ((on_approach_stage && below_flare_alt) || + (on_approach_stage && below_flare_sec && (auto_state.wp_proportion > 0.5)) || (!rangefinder_in_range && auto_state.wp_proportion >= 1) || probably_crashed) {