From d0b6676547fa69f206ceadc593aad1e698991c05 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 26 Aug 2014 21:15:43 +1000 Subject: [PATCH] Plane: do landing flare if past landing point this helps prevent us keeping the throttle on after we've landed if the baro has drifted enough that we think we are not yet at the flare altitude --- ArduPlane/landing.pde | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ArduPlane/landing.pde b/ArduPlane/landing.pde index fd124ea412..59c364fe01 100644 --- a/ArduPlane/landing.pde +++ b/ArduPlane/landing.pde @@ -30,13 +30,16 @@ static bool verify_land() // low pass the sink rate to take some of the noise out auto_state.land_sink_rate = 0.8f * auto_state.land_sink_rate + 0.2f*sink_rate; - /* Set land_complete (which starts the flare) under 2 conditions: + /* Set land_complete (which starts the flare) under 3 conditions: 1) we are within LAND_FLARE_ALT meters of the landing altitude 2) we are within LAND_FLARE_SEC of the landing point vertically by the calculated sink rate + 3) we have gone past the landing point (to prevent us keeping + throttle on after landing if we've had positive baro drift) */ if (height <= g.land_flare_alt || - height <= -auto_state.land_sink_rate * g.land_flare_sec) { + height <= -auto_state.land_sink_rate * g.land_flare_sec || + location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) { if (!auto_state.land_complete) { gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f"),