From 0b76a8018f63a6024d88bbb74f815945d87a2784 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 2 Apr 2021 13:31:45 +1100
Subject: [PATCH] APM_Control: move rmax and tau more slowly

---
 libraries/APM_Control/AP_AutoTune.cpp | 30 ++++++++++++++++++++++++---
 libraries/APM_Control/AP_AutoTune.h   |  5 +++++
 2 files changed, 32 insertions(+), 3 deletions(-)

diff --git a/libraries/APM_Control/AP_AutoTune.cpp b/libraries/APM_Control/AP_AutoTune.cpp
index 28196fa3a3..194ffe972c 100644
--- a/libraries/APM_Control/AP_AutoTune.cpp
+++ b/libraries/APM_Control/AP_AutoTune.cpp
@@ -126,9 +126,12 @@ void AP_AutoTune::start(void)
         level = 1;
     }
 
-    current.rmax_pos.set(tuning_table[level-1].rmax);
-    current.rmax_neg.set(tuning_table[level-1].rmax);
-    current.tau.set(tuning_table[level-1].tau);
+    target_rmax = tuning_table[level-1].rmax;
+    target_tau = tuning_table[level-1].tau;
+
+    // do first update of rmax and tau now
+    update_rmax();
+
     rpid.kIMAX().set(constrain_float(rpid.kIMAX(), AUTOTUNE_MIN_IMAX, AUTOTUNE_MAX_IMAX));
 
     next_save = current;
@@ -354,6 +357,9 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler)
           rpid.kI().get(),
           rpid.kD().get());
 
+    // move rmax and tau towards target
+    update_rmax();
+
     min_Dmod = 1;
     max_P = max_D = 0;
     state = new_state;
@@ -460,3 +466,21 @@ void AP_AutoTune::set_gains(const ATGains &v)
     rpid.kD().set(v.D);
     rpid.kIMAX().set(v.IMAX);
 }
+
+void AP_AutoTune::update_rmax(void)
+{
+    if (current.rmax_pos == 0) {
+        // conservative initial value
+        current.rmax_pos.set(75);
+    }
+    // move by 20 deg/s per step
+    current.rmax_pos.set(constrain_int32(target_rmax,
+                                         current.rmax_pos.get()-20,
+                                         current.rmax_pos.get()+20));
+    current.rmax_neg.set(current.rmax_pos.get());
+
+    // move tau by max 15% per loop
+    current.tau.set(constrain_float(target_tau,
+                                    current.tau*0.85,
+                                    current.tau*1.15));
+}
diff --git a/libraries/APM_Control/AP_AutoTune.h b/libraries/APM_Control/AP_AutoTune.h
index b25eef59ab..60ffca524b 100644
--- a/libraries/APM_Control/AP_AutoTune.h
+++ b/libraries/APM_Control/AP_AutoTune.h
@@ -99,6 +99,9 @@ private:
     ATGains get_gains(const ATGains &current);
     void set_gains(const ATGains &v);
 
+    // update rmax and tau towards target
+    void update_rmax();
+
     // 5 point mode filter for FF estimate
     ModeFilterFloat_Size5 ff_filter;
 
@@ -115,4 +118,6 @@ private:
     float max_D;
     float min_Dmod;
     float FF_single;
+    int16_t target_rmax;
+    float target_tau;
 };