From 06cd077c36ad7e8e61fe8d6ad022456d1591c504 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 8 Jun 2021 16:00:54 +1000
Subject: [PATCH] Plane: fixed stick mixing with land reposition in quadplanes

prevent stick mixing overriding landing control
---
 ArduPlane/Attitude.cpp  |  9 +++++++++
 ArduPlane/quadplane.cpp | 12 ++++++++++++
 ArduPlane/quadplane.h   |  5 +++++
 3 files changed, 26 insertions(+)

diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp
index 0763d3ae3c..49bfa9b777 100644
--- a/ArduPlane/Attitude.cpp
+++ b/ArduPlane/Attitude.cpp
@@ -64,6 +64,15 @@ bool Plane::stick_mixing_enabled(void)
 #else
     const bool stickmixing = true;
 #endif
+    if (control_mode == &mode_qrtl &&
+        quadplane.poscontrol.get_state() >= QuadPlane::QPOS_POSITION1) {
+        // user may be repositioning
+        return false;
+    }
+    if (quadplane.in_vtol_land_poscontrol()) {
+        // user may be repositioning
+        return false;
+    }
     if (control_mode->does_auto_throttle() && plane.control_mode->does_auto_navigation()) {
         // we're in an auto mode. Check the stick mixing flag
         if (g.stick_mixing != STICK_MIXING_DISABLED &&
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index deb075cc1a..920b15708f 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -3938,6 +3938,18 @@ bool QuadPlane::in_vtol_land_sequence(void) const
     return in_vtol_land_approach() || in_vtol_land_descent() || in_vtol_land_final();
 }
 
+/*
+  see if we are in the VTOL position control phase of a landing
+ */
+bool QuadPlane::in_vtol_land_poscontrol(void) const
+{
+    if (in_vtol_auto() && is_vtol_land(plane.mission.get_current_nav_cmd().id) &&
+        poscontrol.get_state() >= QPOS_POSITION1) {
+        return true;
+    }
+    return false;
+}
+
 // return true if we should show VTOL view
 bool QuadPlane::show_vtol_view() const
 {
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index dcb5a0babd..16435d9b59 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -687,6 +687,11 @@ private:
      */
     bool in_vtol_land_sequence(void) const;
 
+    /*
+      see if we are in the VTOL position control phase of a landing
+    */
+    bool in_vtol_land_poscontrol(void) const;
+    
     // Q assist state, can be enabled, disabled or force. Default to enabled
     Q_ASSIST_STATE_ENUM q_assist_state = Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED;