From 1d2a64951c24a765b68ab6c342d6d205a8e1db85 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 9 Mar 2022 16:56:47 +1100
Subject: [PATCH] Plane: use pos control for most of LAND_FINAL

this gives more accurate landing with some velocity drift
---
 ArduPlane/quadplane.cpp | 21 +++++++++++++++++++--
 ArduPlane/quadplane.h   |  1 +
 2 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 83091235d5..e2b41272c1 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -2178,6 +2178,10 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s)
         } else if (s == QPOS_LAND_DESCEND) {
             // reset throttle descent control
             qp.thr_ctrl_land = false;
+        } else if (s == QPOS_LAND_FINAL) {
+            // remember last pos reset to handle GPS glitch in LAND_FINAL
+            Vector2f rpos;
+            last_pos_reset_ms = plane.ahrs.getLastPosNorthEastReset(rpos);
         }
         qp.log_QPOS();
     }
@@ -2510,10 +2514,23 @@ void QuadPlane::vtol_position_controller(void)
         if (should_relax()) {
             pos_control->relax_velocity_controller_xy();
         } else {
-            // we use velocity control in QPOS_LAND_FINAL to allow for GPS glitch handling
             Vector2f zero;
             Vector2f vel_cms = poscontrol.target_vel_cms.xy() + landing_velocity*100;
-            pos_control->input_vel_accel_xy(vel_cms, zero);
+            Vector2f rpos;
+            const uint32_t last_reset_ms = plane.ahrs.getLastPosNorthEastReset(rpos);
+            /* we use velocity control when we may be touching the
+              ground or if we've had a position reset from AHRS. This
+              helps us handle a GPS glitch in the final land phase,
+              and also prevents trying to reposition after touchdown
+            */
+            if (motors->limit.throttle_lower ||
+                motors->get_throttle() < 0.5*motors->get_throttle_hover() ||
+                last_reset_ms != poscontrol.last_pos_reset_ms) {
+                pos_control->input_vel_accel_xy(vel_cms, zero);
+            } else {
+                // otherwise use full pos control
+                pos_control->input_pos_vel_accel_xy(poscontrol.target_cm.xy(), vel_cms, zero);
+            }
         }
 
         run_xy_controller();
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index bad2f79544..68b3ad5035 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -452,6 +452,7 @@ private:
         uint32_t last_velocity_match_ms;
         float target_speed;
         float target_accel;
+        uint32_t last_pos_reset_ms;
     private:
         uint32_t last_state_change_ms;
         enum position_control_state state;