From ae013dd5e6316718cf1a5f6c861fd6ee511ae1cc Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 5 Jul 2021 10:50:49 +1000
Subject: [PATCH] APM_Control: adjust fixed wing filter defaults

adjust defaults based on discussions with Paul, and initialise the
FLTT value based on the controller time constant
---
 libraries/APM_Control/AP_AutoTune.cpp      | 6 ++++++
 libraries/APM_Control/AP_AutoTune.h        | 2 +-
 libraries/APM_Control/AP_PitchController.h | 2 +-
 libraries/APM_Control/AP_RollController.h  | 2 +-
 4 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/libraries/APM_Control/AP_AutoTune.cpp b/libraries/APM_Control/AP_AutoTune.cpp
index 43ed0a9af7..03540abbf5 100644
--- a/libraries/APM_Control/AP_AutoTune.cpp
+++ b/libraries/APM_Control/AP_AutoTune.cpp
@@ -373,6 +373,9 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e
     rpid.kD().set(D);
     rpid.kI().set(MAX(P*AUTOTUNE_I_RATIO, (FF / TRIM_TCONST)));
 
+    // setup target filter to be suitable for time constant
+    rpid.filt_T_hz().set(10.0/(current.tau * 2 * M_PI));
+
     current.FF = FF;
     current.P = P;
     current.I = rpid.kI().get();
@@ -473,6 +476,7 @@ void AP_AutoTune::save_gains(const ATGains &v)
     save_float_if_changed(rpid.kI(), v.I);
     save_float_if_changed(rpid.kD(), v.D);
     save_float_if_changed(rpid.kIMAX(), v.IMAX);
+    save_float_if_changed(rpid.filt_T_hz(), v.flt_T);
     last_save = get_gains(current);
     current = tmp;
 }
@@ -488,6 +492,7 @@ AP_AutoTune::ATGains AP_AutoTune::get_gains(const ATGains &v)
     ret.I = rpid.kI();
     ret.D = rpid.kD();
     ret.IMAX = rpid.kIMAX();
+    ret.flt_T = rpid.filt_T_hz();
     return ret;
 }
 
@@ -502,6 +507,7 @@ void AP_AutoTune::set_gains(const ATGains &v)
     rpid.kI().set(v.I);
     rpid.kD().set(v.D);
     rpid.kIMAX().set(v.IMAX);
+    rpid.filt_T_hz().set(v.flt_T);
 }
 
 /*
diff --git a/libraries/APM_Control/AP_AutoTune.h b/libraries/APM_Control/AP_AutoTune.h
index d523df961d..9cb46253c2 100644
--- a/libraries/APM_Control/AP_AutoTune.h
+++ b/libraries/APM_Control/AP_AutoTune.h
@@ -12,7 +12,7 @@ public:
         AP_Float tau;
         AP_Int16 rmax_pos;
         AP_Int16 rmax_neg;
-        float FF, P, I, D, IMAX;
+        float FF, P, I, D, IMAX, flt_T;
     };
 
     enum ATType {
diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h
index 127530df66..aa81552136 100644
--- a/libraries/APM_Control/AP_PitchController.h
+++ b/libraries/APM_Control/AP_PitchController.h
@@ -53,7 +53,7 @@ private:
 	AP_Float _roll_ff;
 	uint32_t _last_t;
     float _last_out;
-    AC_PID rate_pid{0.04, 0.15, 0, 0.345, 0.666, 1.5, 7, 4, 0.02, 150, 1};
+    AC_PID rate_pid{0.04, 0.15, 0, 0.345, 0.666, 3, 0, 12, 0.02, 150, 1};
     float angle_err_deg;
 
     AP_Logger::PID_Info _pid_info;
diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h
index 71df331c6a..9e3f8b8652 100644
--- a/libraries/APM_Control/AP_RollController.h
+++ b/libraries/APM_Control/AP_RollController.h
@@ -58,7 +58,7 @@ private:
     bool failed_autotune_alloc;
 	uint32_t _last_t;
 	float _last_out;
-    AC_PID rate_pid{0.08, 0.15, 0, 0.345, 0.666, 3, 7, 4, 0.02, 150, 1};
+    AC_PID rate_pid{0.08, 0.15, 0, 0.345, 0.666, 3, 0, 12, 0.02, 150, 1};
     float angle_err_deg;
 
     AP_Logger::PID_Info _pid_info;