From 695a59b278a6ba97a71da2d3c7c1897385583b3b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 3 Jan 2022 12:53:05 +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 4d6905d958..db3a483cb7 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 2a45b9684d..b4c0819339 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -394,6 +394,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),
@@ -844,7 +847,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);
     void stabilize_pitch(float speed_scaler);
diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp
index eb35322133..e68a86fbb0 100644
--- a/ArduPlane/sensors.cpp
+++ b/ArduPlane/sensors.cpp
@@ -65,6 +65,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);
 }
 
 /*