From c6a6427d6ac354fb56c9658d9c17d6b8d20c62a7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 11 Jan 2020 15:17:23 +1100 Subject: [PATCH] Plane: prevent lidar glitches from triggering land final this prevents short term lidar glitches from triggering the change from descend to final in quadplane --- ArduPlane/quadplane.cpp | 10 +++++++++- ArduPlane/quadplane.h | 2 ++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 239344a23c..1a3f7c5b33 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1125,6 +1125,7 @@ void QuadPlane::init_qland(void) init_loiter(); throttle_wait = false; poscontrol.state = QPOS_LAND_DESCEND; + last_land_final_agl = plane.relative_ground_altitude(plane.g.rangefinder_landing); landing_detect.lower_limit_start_ms = 0; landing_detect.land_start_ms = 0; } @@ -2744,9 +2745,15 @@ void QuadPlane::check_land_complete(void) bool QuadPlane::check_land_final(void) { float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing); - if (height_above_ground < land_final_alt) { + // we require 2 readings at 10Hz to be within 5m of each other to + // trigger the switch to land final. This prevents a short term + // glitch at high altitude from triggering land final + const float max_change = 5; + if (height_above_ground < land_final_alt && + fabsf(height_above_ground - last_land_final_agl) < max_change) { return true; } + last_land_final_agl = height_above_ground; /* also apply landing detector, in case we have landed in descent @@ -2766,6 +2773,7 @@ bool QuadPlane::verify_vtol_land(void) if (poscontrol.state == QPOS_POSITION2 && plane.auto_state.wp_distance < 2) { poscontrol.state = QPOS_LAND_DESCEND; + last_land_final_agl = plane.relative_ground_altitude(plane.g.rangefinder_landing); gcs().send_text(MAV_SEVERITY_INFO,"Land descend started"); if (plane.control_mode == &plane.mode_auto) { // set height to mission height, so we can use the mission diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index c0d943bd26..c283da4b5a 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -538,6 +538,8 @@ private: uint32_t takeoff_start_time_ms; uint32_t takeoff_time_limit_ms; + float last_land_final_agl; + /* return true if current mission item is a vtol takeoff */