From 5fc3cae33aab4bacd3878c6ca394e766dcb282c9 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 24 Nov 2021 19:37:11 +1100
Subject: [PATCH] APM_Control: make 2nd reduction of P smaller

this prevents severe P reductions when we get a small oscillation
glitch after we've already got the primary P gain
---
 libraries/APM_Control/AP_AutoTune.cpp | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/libraries/APM_Control/AP_AutoTune.cpp b/libraries/APM_Control/AP_AutoTune.cpp
index 0d2fa6a580..979faa30fd 100644
--- a/libraries/APM_Control/AP_AutoTune.cpp
+++ b/libraries/APM_Control/AP_AutoTune.cpp
@@ -351,8 +351,16 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e
                 action = Action::LOWER_D;
                 GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sD: %.4f", type==AUTOTUNE_ROLL?"Roll":"Pitch", D_limit);
                 done_count = 0;
-            } else if (now - P_set_ms > 2000) {
-                P *= 0.35;
+            } else if (now - P_set_ms > 2500) {
+                if (is_positive(P_limit)) {
+                    // if we've already got a P estimate then don't
+                    // reduce as quickly, stopping small spikes at the
+                    // later part of the tune from giving us a very
+                    // low P gain
+                    P *= 0.7;
+                } else {
+                    P *= 0.35;
+                }
                 P_limit = P;
                 P_set_ms = now;
                 action = Action::LOWER_P;