From ee883b6ad034e1a66d305644c2bdb3099ff11b74 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Wed, 19 Jul 2023 13:40:09 +0100
Subject: [PATCH] APM_Control: allow autotune FLTD and FLTT updates to be
 disabled

---
 libraries/APM_Control/AP_AutoTune.cpp | 12 ++++++++++--
 libraries/APM_Control/AP_AutoTune.h   |  9 +++++++++
 2 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/libraries/APM_Control/AP_AutoTune.cpp b/libraries/APM_Control/AP_AutoTune.cpp
index c8008b61d4..0cc30e59cb 100644
--- a/libraries/APM_Control/AP_AutoTune.cpp
+++ b/libraries/APM_Control/AP_AutoTune.cpp
@@ -424,9 +424,17 @@ void AP_AutoTune::update(AP_PIDInfo &pinfo, float scaler, float angle_err_deg)
     }
 
     // setup filters to be suitable for time constant and gyro filter
-    rpid.filt_T_hz().set(10.0/(current.tau * 2 * M_PI));
+    // filtering T can  prevent P/D oscillation being seen, so allow the
+    // user to switch it off
+    if (!has_option(DISABLE_FLTT_UPDATE)) {
+        rpid.filt_T_hz().set(10.0/(current.tau * 2 * M_PI));
+    }
     rpid.filt_E_hz().set(0);
-    rpid.filt_D_hz().set(AP::ins().get_gyro_filter_hz()*0.5);
+    // filtering D at the same level as VTOL can allow unwanted oscillations to be seen,
+    // so allow the user to switch it off and select their own (usually lower) value
+    if (!has_option(DISABLE_FLTD_UPDATE)) {
+        rpid.filt_D_hz().set(AP::ins().get_gyro_filter_hz()*0.5);
+    }
 
     current.FF = FF;
     current.P = P;
diff --git a/libraries/APM_Control/AP_AutoTune.h b/libraries/APM_Control/AP_AutoTune.h
index 1859a2489f..67b6cf07f9 100644
--- a/libraries/APM_Control/AP_AutoTune.h
+++ b/libraries/APM_Control/AP_AutoTune.h
@@ -25,6 +25,11 @@ public:
         AUTOTUNE_YAW = 2,
     };
 
+    enum Options {
+        DISABLE_FLTD_UPDATE = 0,
+        DISABLE_FLTT_UPDATE = 1
+    };
+
     struct PACKED log_ATRP {
         LOG_PACKET_HEADER;
         uint64_t time_us;
@@ -116,6 +121,10 @@ private:
     // update rmax and tau towards target
     void update_rmax();
 
+    bool has_option(Options option) {
+        return (aparm.autotune_options.get() & uint32_t(1<<uint32_t(option))) != 0;
+    }
+
     // 5 point mode filter for FF estimate
     ModeFilterFloat_Size5 ff_filter;