From fb019eb0f3ae04b2ff5d69a677d0ac12791e266b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 29 Dec 2021 12:23:09 +1100
Subject: [PATCH] Plane: add a low pass filter to speed scaler

this is needed due to the filtering done on the target rate in
AC_PID. With a low filter rate in AC_PID a step in the speed scaler
results in a step in the FF output due to the mismatch in the
instantaneous SS and the filtered target rate
---
 ArduPlane/Attitude.cpp | 8 ++++----
 ArduPlane/Plane.h      | 6 +++++-
 ArduPlane/sensors.cpp  | 6 ++++++
 3 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp
index f39bf67d20..14fe0a10ce 100644
--- a/ArduPlane/Attitude.cpp
+++ b/ArduPlane/Attitude.cpp
@@ -1,11 +1,11 @@
 #include "Plane.h"
 
 /*
-  get a speed scaling number for control surfaces. This is applied to
-  PIDs to change the scaling of the PID with speed. At high speed we
-  move the surfaces less, and at low speeds we move them more.
+  calculate speed scaling number for control surfaces. This is applied
+  to PIDs to change the scaling of the PID with speed. At high speed
+  we move the surfaces less, and at low speeds we move them more.
  */
-float Plane::get_speed_scaler(void)
+float Plane::calc_speed_scaler(void)
 {
     float aspeed, speed_scaler;
     if (ahrs.airspeed_estimate(aspeed)) {
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 914677d6e3..ae02b3ec19 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -392,6 +392,9 @@ private:
     // Difference between current altitude and desired altitude.  Centimeters
     int32_t altitude_error_cm;
 
+    // speed scaler for control surfaces, updated at 10Hz
+    float surface_speed_scaler = 1.0;
+
     // Battery Sensors
     AP_BattMonitor battery{MASK_LOG_CURRENT,
                            FUNCTOR_BIND_MEMBER(&Plane::handle_battery_failsafe, void, const char*, const int8_t),
@@ -858,7 +861,8 @@ private:
     void calc_throttle();
     void calc_nav_roll();
     void calc_nav_pitch();
-    float get_speed_scaler(void);
+    float calc_speed_scaler(void);
+    float get_speed_scaler(void) const { return surface_speed_scaler; }
     bool stick_mixing_enabled(void);
     void stabilize_roll(float speed_scaler);
     float stabilize_roll_get_roll_out(float speed_scaler);
diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp
index 9a220d4363..5a7913ef0e 100644
--- a/ArduPlane/sensors.cpp
+++ b/ArduPlane/sensors.cpp
@@ -51,6 +51,12 @@ void Plane::read_airspeed(void)
     if (ahrs.airspeed_estimate(aspeed)) {
         smoothed_airspeed = smoothed_airspeed * 0.8f + aspeed * 0.2f;
     }
+
+    // low pass filter speed scaler, with 1Hz cutoff, at 10Hz
+    const float speed_scaler = calc_speed_scaler();
+    const float cutoff_Hz = 2.0;
+    const float dt = 0.1;
+    surface_speed_scaler += calc_lowpass_alpha_dt(dt, cutoff_Hz) * (speed_scaler - surface_speed_scaler);
 }
 
 /*