From 3e2bfec8c6597addeedce83378d4d9f6a819b54d Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 19 Nov 2020 21:20:41 +0100
Subject: [PATCH 1/9] Settings for Dynamic LPF filter

---
 src/main/common/filter.c  | 15 +++++++++++++--
 src/main/common/filter.h  |  1 +
 src/main/fc/settings.yaml | 23 +++++++++++++++++++++++
 src/main/sensors/gyro.c   |  6 +++++-
 src/main/sensors/gyro.h   |  4 ++++
 5 files changed, 46 insertions(+), 3 deletions(-)

diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 1cdba06c190..4bc2f884258 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -48,6 +48,11 @@ float nullFilterApply4(void *filter, float input, float f_cut, float dt)
 
 // PT1 Low Pass filter
 
+static float pt1ComputeRC(const float f_cut)
+{
+    return 1.0f / (2.0f * M_PIf * f_cut);
+}
+
 // f_cut = cutoff frequency
 void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT)
 {
@@ -59,7 +64,7 @@ void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT)
 
 void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT)
 {
-    pt1FilterInitRC(filter, 1.0f / (2.0f * M_PIf * f_cut), dT);
+    pt1FilterInitRC(filter, pt1ComputeRC(f_cut), dT);
 }
 
 void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau) {
@@ -70,6 +75,12 @@ float pt1FilterGetLastOutput(pt1Filter_t *filter) {
     return filter->state;
 }
 
+void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut)
+{
+    filter->RC = pt1ComputeRC(f_cut);
+    filter->alpha = filter->dT / (filter->RC + filter->dT);
+}
+
 float FAST_CODE NOINLINE pt1FilterApply(pt1Filter_t *filter, float input)
 {
     filter->state = filter->state + filter->alpha * (input - filter->state);
@@ -87,7 +98,7 @@ float FAST_CODE NOINLINE pt1FilterApply4(pt1Filter_t *filter, float input, float
 {
     // Pre calculate and store RC
     if (!filter->RC) {
-        filter->RC = 1.0f / ( 2.0f * M_PIf * f_cut );
+        filter->RC = pt1ComputeRC(f_cut);
     }
 
     filter->dT = dT;    // cache latest dT for possible use in pt1FilterApply
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index b0e782c5c9e..ed8500ecdd8 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -65,6 +65,7 @@ float nullFilterApply4(void *filter, float input, float f_cut, float dt);
 void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT);
 void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT);
 void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau);
+void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut);
 float pt1FilterGetLastOutput(pt1Filter_t *filter);
 float pt1FilterApply(pt1Filter_t *filter, float input);
 float pt1FilterApply3(pt1Filter_t *filter, float input, float dT);
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 62c5496df7d..79b9d007a00 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -214,6 +214,29 @@ groups:
         default_value: "`BIQUAD`"
         field: gyro_stage2_lowpass_type
         table: filter_type
+      - name: gyro_use_dyn_lpf
+        description: "Use Dynamic LPF instead of static gyro stage1 LPF."
+        default_value: "OFF"
+        field: useDynamicLpf
+        type: bool
+      - name: gyro_dyn_lpf_min_hz
+        description: "Minimum frequency of the gyro Dynamic LPF"
+        default_value: "200"
+        field: gyroDynamicLpfMinHz
+        min: 40
+        max: 400
+      - name: gyro_dyn_lpf_max_hz
+        description: "Maximum frequency of the gyro Dynamic LPF"
+        default_value: "500"
+        field: gyroDynamicLpfMaxHz
+        min: 40
+        max: 1000
+      - name: gyro_dyn_lpf_curve_expo
+        description: "Expo value for the throttle-to-frequency mapping for Dynamic LPF"
+        default_value: "5"
+        field: gyroDynamicLpFCurveExpo
+        min: 0
+        max: 10
       - name: dynamic_gyro_notch_enabled
         description: "Enable/disable dynamic gyro notch also known as Matrix Filter"
         default_value: "`OFF`"
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index d9cd15891d9..0518d8f2296 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -101,7 +101,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
 
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 10);
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 11);
 
 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_lpf = GYRO_LPF_42HZ,      // 42HZ value is defined for Invensense/TDK gyros
@@ -116,6 +116,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_notch_cutoff = 1,
     .gyro_stage2_lowpass_hz = 0,
     .gyro_stage2_lowpass_type = FILTER_BIQUAD,
+    .useDynamicLpf = 0,
+    .gyroDynamicLpfMinHz = 200,
+    .gyroDynamicLpfMaxHz = 500,
+    .gyroDynamicLpFCurveExpo = 5,
     .dynamicGyroNotchRange = DYN_NOTCH_RANGE_MEDIUM,
     .dynamicGyroNotchQ = 120,
     .dynamicGyroNotchMinHz = 150,
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index b709294b553..57e6c484ffc 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -70,6 +70,10 @@ typedef struct gyroConfig_s {
     uint16_t gyro_notch_cutoff;
     uint16_t gyro_stage2_lowpass_hz;
     uint8_t gyro_stage2_lowpass_type;
+    uint8_t useDynamicLpf;
+    uint16_t gyroDynamicLpfMinHz;
+    uint16_t gyroDynamicLpfMaxHz;
+    uint8_t gyroDynamicLpFCurveExpo;
     uint8_t dynamicGyroNotchRange;
     uint16_t dynamicGyroNotchQ;
     uint16_t dynamicGyroNotchMinHz;

From cf1b17181f76a2461d46bd49f7ab81954cdf6ff9 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 20 Nov 2020 15:15:59 +0100
Subject: [PATCH 2/9] Dynamic update of the gyro stage1 LPF

---
 src/main/common/filter.c |  3 ---
 src/main/common/filter.h |  3 +++
 src/main/fc/fc_tasks.c   |  3 ++-
 src/main/sensors/gyro.c  | 39 +++++++++++++++++++++++++++++++++++++++
 src/main/sensors/gyro.h  |  1 +
 5 files changed, 45 insertions(+), 4 deletions(-)

diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 4bc2f884258..3a4c9797efa 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -28,9 +28,6 @@ FILE_COMPILE_FOR_SPEED
 #include "common/maths.h"
 #include "common/utils.h"
 
-#define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
-#define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - butterworth*/
-
 // NULL filter
 float nullFilterApply(void *filter, float input)
 {
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index ed8500ecdd8..81c76cd0dc2 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -59,6 +59,9 @@ typedef struct firFilter_s {
 typedef float (*filterApplyFnPtr)(void *filter, float input);
 typedef float (*filterApply4FnPtr)(void *filter, float input, float f_cut, float dt);
 
+#define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
+#define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - butterworth*/
+
 float nullFilterApply(void *filter, float input);
 float nullFilterApply4(void *filter, float input, float f_cut, float dt);
 
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 190a299d9d6..3a4517cf568 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -294,6 +294,7 @@ void taskUpdateAux(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
     updatePIDCoefficients();
+    gyroUpdateDynamicLpf();
 }
 
 void fcTasksInit(void)
@@ -620,7 +621,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_AUX] = {
         .taskName = "AUX",
         .taskFunc = taskUpdateAux,
-        .desiredPeriod = TASK_PERIOD_HZ(TASK_AUX_RATE_HZ),          // 300Hz @3,33ms
+        .desiredPeriod = TASK_PERIOD_HZ(TASK_AUX_RATE_HZ),          // 100Hz @10ms
         .staticPriority = TASK_PRIORITY_HIGH,
     },
 };
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 0518d8f2296..5006c3509de 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -59,6 +59,7 @@ FILE_COMPILE_FOR_SPEED
 
 #include "fc/config.h"
 #include "fc/runtime_config.h"
+#include "fc/rc_controls.h"
 
 #include "io/beeper.h"
 #include "io/statusindicator.h"
@@ -72,6 +73,7 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/gyroanalyse.h"
 #include "flight/rpm_filter.h"
 #include "flight/dynamic_gyro_notch.h"
+#include "flight/mixer.h"
 
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
@@ -514,3 +516,40 @@ bool gyroSyncCheckUpdate(void)
 
     return gyroDev[0].intStatusFn(&gyroDev[0]);
 }
+
+static float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo) {
+    const float expof = expo / 10.0f;
+    static float curve;
+    curve = throttle * (1 - throttle) * expof + throttle;
+    return (dynLpfMax - dynLpfMin) * curve + dynLpfMin;
+}
+
+static float dynThrottle(float throttle) {
+    return throttle * (1 - (throttle * throttle) / 3.0f) * 1.5f;
+}
+
+void gyroUpdateDynamicLpf(void) {
+    if (!gyroConfig()->useDynamicLpf) {
+        return;
+    }
+
+    const float throttle = scaleRangef((float) rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f);
+
+    uint16_t cutoffFreq;
+    if (gyroConfig()->gyroDynamicLpFCurveExpo > 0) {
+        cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpFCurveExpo);
+    } else {
+        cutoffFreq = fmax(dynThrottle(throttle) * gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfMinHz);
+    }
+
+    if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) {
+        const float gyroDt = getLooptime() * 1e-6f;
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            pt1FilterUpdateCutoff(&gyroLpfState[axis].pt1, cutoffFreq);
+        }
+    } else if (gyroConfig()->gyro_soft_lpf_type == FILTER_BIQUAD) {
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            biquadFilterUpdate(&gyroLpfState[axis].biquad, cutoffFreq, getLooptime(), BIQUAD_Q, FILTER_LPF);
+        }
+    }
+}
\ No newline at end of file
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 57e6c484ffc..b5bf66a4b29 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -91,3 +91,4 @@ bool gyroReadTemperature(void);
 int16_t gyroGetTemperature(void);
 int16_t gyroRateDps(int axis);
 bool gyroSyncCheckUpdate(void);
+void gyroUpdateDynamicLpf(void);

From 390987597634831791cd20a1f4b18add53a770a1 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 20 Nov 2020 16:00:58 +0100
Subject: [PATCH 3/9] Remove unused variable

---
 src/main/sensors/gyro.c | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 5006c3509de..7adadf2c10b 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -543,7 +543,6 @@ void gyroUpdateDynamicLpf(void) {
     }
 
     if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) {
-        const float gyroDt = getLooptime() * 1e-6f;
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             pt1FilterUpdateCutoff(&gyroLpfState[axis].pt1, cutoffFreq);
         }

From 6dd4cf9b7946121582f0a5b59d5f19fd279a98dd Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 7 Dec 2020 09:56:42 +0100
Subject: [PATCH 4/9] Drop legacy curve

---
 docs/Settings.md          | 10 +++++++---
 src/main/fc/settings.yaml |  4 ++--
 src/main/sensors/gyro.c   | 14 ++------------
 src/main/sensors/gyro.h   |  2 +-
 4 files changed, 12 insertions(+), 18 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 00999bf75a0..986235b09ba 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -137,6 +137,9 @@
 | gps_provider | UBLOX | Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N). |
 | gps_sbas_mode | NONE | Which SBAS mode to be used |
 | gps_ublox_use_galileo | OFF | Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. |
+| gyro_dyn_lpf_curve_expo | 5 | Expo value for the throttle-to-frequency mapping for Dynamic LPF |
+| gyro_dyn_lpf_max_hz | 500 | Maximum frequency of the gyro Dynamic LPF |
+| gyro_dyn_lpf_min_hz | 200 | Minimum frequency of the gyro Dynamic LPF |
 | gyro_hardware_lpf | 42HZ | Hardware lowpass filter for gyro. Allowed values depend on the driver - For example MPU6050 allows 10HZ,20HZ,42HZ,98HZ,188HZ,256Hz (8khz mode). If you have to set gyro lpf below 42Hz generally means the frame is vibrating too much, and that should be fixed first. |
 | gyro_lpf_hz | 60 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. |
 | gyro_lpf_type | BIQUAD | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. |
@@ -146,6 +149,7 @@
 | gyro_stage2_lowpass_type | `BIQUAD` | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
 | gyro_sync | OFF | This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf |
 | gyro_to_use |  |  |
+| gyro_use_dyn_lpf | OFF | Use Dynamic LPF instead of static gyro stage1 LPF. |
 | has_flaps | OFF | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot |
 | heading_hold_rate_limit | 90 | This setting limits yaw rotation rate that HEADING_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when HEADING_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. |
 | hott_alarm_sound_interval | 5 | Battery alarm delay in seconds for Hott telemetry |
@@ -277,8 +281,8 @@
 | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes |
 | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes |
 | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) |
-| nav_fw_pitch2thr_smoothing | 0 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. |
-| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] |
+| nav_fw_pitch2thr_smoothing | 6 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. |
+| nav_fw_pitch2thr_threshold | 50 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] |
 | nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) |
 | nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) |
 | nav_fw_pos_hdg_p | 60 | P gain of heading PID controller. (Fixedwing, rovers, boats) |
@@ -324,7 +328,7 @@
 | nav_mc_vel_z_i | 50 | I gain of velocity PID controller |
 | nav_mc_vel_z_p | 100 | P gain of velocity PID controller |
 | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. |
-| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
+| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
 | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) |
 | nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] |
 | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. |
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 4b7d453b263..5ecc66907dd 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -234,8 +234,8 @@ groups:
       - name: gyro_dyn_lpf_curve_expo
         description: "Expo value for the throttle-to-frequency mapping for Dynamic LPF"
         default_value: "5"
-        field: gyroDynamicLpFCurveExpo
-        min: 0
+        field: gyroDynamicLpfCurveExpo
+        min: 1
         max: 10
       - name: dynamic_gyro_notch_enabled
         description: "Enable/disable dynamic gyro notch also known as Matrix Filter"
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 7adadf2c10b..62bc58887c1 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -121,7 +121,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .useDynamicLpf = 0,
     .gyroDynamicLpfMinHz = 200,
     .gyroDynamicLpfMaxHz = 500,
-    .gyroDynamicLpFCurveExpo = 5,
+    .gyroDynamicLpfCurveExpo = 5,
     .dynamicGyroNotchRange = DYN_NOTCH_RANGE_MEDIUM,
     .dynamicGyroNotchQ = 120,
     .dynamicGyroNotchMinHz = 150,
@@ -524,23 +524,13 @@ static float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLp
     return (dynLpfMax - dynLpfMin) * curve + dynLpfMin;
 }
 
-static float dynThrottle(float throttle) {
-    return throttle * (1 - (throttle * throttle) / 3.0f) * 1.5f;
-}
-
 void gyroUpdateDynamicLpf(void) {
     if (!gyroConfig()->useDynamicLpf) {
         return;
     }
 
     const float throttle = scaleRangef((float) rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f);
-
-    uint16_t cutoffFreq;
-    if (gyroConfig()->gyroDynamicLpFCurveExpo > 0) {
-        cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpFCurveExpo);
-    } else {
-        cutoffFreq = fmax(dynThrottle(throttle) * gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfMinHz);
-    }
+    const uint16_t cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfCurveExpo);
 
     if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index b5bf66a4b29..491ff0a1060 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -73,7 +73,7 @@ typedef struct gyroConfig_s {
     uint8_t useDynamicLpf;
     uint16_t gyroDynamicLpfMinHz;
     uint16_t gyroDynamicLpfMaxHz;
-    uint8_t gyroDynamicLpFCurveExpo;
+    uint8_t gyroDynamicLpfCurveExpo;
     uint8_t dynamicGyroNotchRange;
     uint16_t dynamicGyroNotchQ;
     uint16_t dynamicGyroNotchMinHz;

From a160f2a4d3c3cbcedc8670736ac561b2809eaaf6 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 7 Dec 2020 11:16:07 +0100
Subject: [PATCH 5/9] Update tests

---
 src/test/unit/sensor_gyro_unittest.cc | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc
index 7eb0418397f..0433b889399 100644
--- a/src/test/unit/sensor_gyro_unittest.cc
+++ b/src/test/unit/sensor_gyro_unittest.cc
@@ -37,6 +37,8 @@ extern "C" {
     #include "sensors/gyro.h"
     #include "sensors/acceleration.h"
     #include "sensors/sensors.h"
+    #include "fc/rc_controls.h"
+    #include "flight/mixer.h"
 
     extern zeroCalibrationVector_t gyroCalibration;
     extern gyroDev_t gyroDev[];

From 1405fd3fc1587a515666c3f3e94bff1c4be7283e Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 25 Dec 2020 16:15:56 +0100
Subject: [PATCH 6/9] Refactor

---
 src/main/CMakeLists.txt       |  2 ++
 src/main/build/debug.h        |  1 +
 src/main/fc/fc_tasks.c        |  2 +-
 src/main/fc/settings.yaml     |  2 +-
 src/main/flight/dynamic_lpf.c | 50 +++++++++++++++++++++++++++++++++++
 src/main/flight/dynamic_lpf.h | 29 ++++++++++++++++++++
 src/main/sensors/gyro.c       | 17 +-----------
 src/main/sensors/gyro.h       |  2 +-
 8 files changed, 86 insertions(+), 19 deletions(-)
 create mode 100644 src/main/flight/dynamic_lpf.c
 create mode 100644 src/main/flight/dynamic_lpf.h

diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 793c64c3bdb..927070dcc40 100644
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -321,6 +321,8 @@ main_sources(COMMON_SRC
     flight/rpm_filter.h
     flight/dynamic_gyro_notch.c
     flight/dynamic_gyro_notch.h
+    flight/dynamic_lpf.c
+    flight/dynamic_lpf.h
 
     io/beeper.c
     io/beeper.h
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 860289c0762..b484fd4d35c 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -79,5 +79,6 @@ typedef enum {
     DEBUG_SPM_VS600,    // Smartport master VS600 VTX
     DEBUG_SPM_VARIO,    // Smartport master variometer
     DEBUG_PCF8574,
+    DEBUG_DYNAMIC_GYRO_LPF,
     DEBUG_COUNT
 } debugType_e;
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 3a4517cf568..e4ff976cfc3 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -294,7 +294,7 @@ void taskUpdateAux(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
     updatePIDCoefficients();
-    gyroUpdateDynamicLpf();
+    dynamicLpfGyroTask();
 }
 
 void fcTasksInit(void)
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 5ecc66907dd..59331e7c7a7 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -84,7 +84,7 @@ tables:
       "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
-      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574"]
+      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
diff --git a/src/main/flight/dynamic_lpf.c b/src/main/flight/dynamic_lpf.c
new file mode 100644
index 00000000000..c93cd921462
--- /dev/null
+++ b/src/main/flight/dynamic_lpf.c
@@ -0,0 +1,50 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include "flight/dynamic_lpf.h"
+#include "sensors/gyro.h"
+#include "flight/mixer.h"
+#include "fc/rc_controls.h"
+#include "build/debug.h"
+
+static float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo) {
+    const float expof = expo / 10.0f;
+    static float curve;
+    curve = throttle * (1 - throttle) * expof + throttle;
+    return (dynLpfMax - dynLpfMin) * curve + dynLpfMin;
+}
+
+void dynamicLpfGyroTask(void) {
+
+    if (!gyroConfig()->useDynamicLpf) {
+        return;
+    }
+
+    const float throttle = scaleRangef((float) rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f);
+    const float cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfCurveExpo);
+
+    DEBUG_SET(DEBUG_DYNAMIC_GYRO_LPF, 0, cutoffFreq);
+
+    gyroUpdateDynamicLpf(cutoffFreq);
+}
\ No newline at end of file
diff --git a/src/main/flight/dynamic_lpf.h b/src/main/flight/dynamic_lpf.h
new file mode 100644
index 00000000000..f8824fc5b10
--- /dev/null
+++ b/src/main/flight/dynamic_lpf.h
@@ -0,0 +1,29 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+void dynamicLpfGyroTask(void);
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 62bc58887c1..ced897d8ade 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -73,7 +73,6 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/gyroanalyse.h"
 #include "flight/rpm_filter.h"
 #include "flight/dynamic_gyro_notch.h"
-#include "flight/mixer.h"
 
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
@@ -517,21 +516,7 @@ bool gyroSyncCheckUpdate(void)
     return gyroDev[0].intStatusFn(&gyroDev[0]);
 }
 
-static float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo) {
-    const float expof = expo / 10.0f;
-    static float curve;
-    curve = throttle * (1 - throttle) * expof + throttle;
-    return (dynLpfMax - dynLpfMin) * curve + dynLpfMin;
-}
-
-void gyroUpdateDynamicLpf(void) {
-    if (!gyroConfig()->useDynamicLpf) {
-        return;
-    }
-
-    const float throttle = scaleRangef((float) rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f);
-    const uint16_t cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfCurveExpo);
-
+void gyroUpdateDynamicLpf(float cutoffFreq) {
     if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             pt1FilterUpdateCutoff(&gyroLpfState[axis].pt1, cutoffFreq);
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 491ff0a1060..737055af1f0 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -91,4 +91,4 @@ bool gyroReadTemperature(void);
 int16_t gyroGetTemperature(void);
 int16_t gyroRateDps(int axis);
 bool gyroSyncCheckUpdate(void);
-void gyroUpdateDynamicLpf(void);
+void gyroUpdateDynamicLpf(float cutoffFreq);

From 9cc0d79460ebc36a20dfd45ee0bfba11565e8eff Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 25 Dec 2020 16:26:55 +0100
Subject: [PATCH 7/9] Fix includ

---
 src/main/fc/fc_tasks.c | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index e4ff976cfc3..b000de70177 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -49,6 +49,7 @@
 #include "flight/wind_estimator.h"
 #include "flight/rpm_filter.h"
 #include "flight/servos.h"
+#include "flight/dynamic_lpf.h"
 
 #include "navigation/navigation.h"
 

From 4d3fab588130b5929f149cdb8b84bebbc27c43a6 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 25 Dec 2020 16:31:27 +0100
Subject: [PATCH 8/9] Docs update

---
 docs/Settings.md          | 2 +-
 src/main/fc/settings.yaml | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 986235b09ba..b3454073768 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -149,7 +149,7 @@
 | gyro_stage2_lowpass_type | `BIQUAD` | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
 | gyro_sync | OFF | This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf |
 | gyro_to_use |  |  |
-| gyro_use_dyn_lpf | OFF | Use Dynamic LPF instead of static gyro stage1 LPF. |
+| gyro_use_dyn_lpf | OFF | Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. |
 | has_flaps | OFF | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot |
 | heading_hold_rate_limit | 90 | This setting limits yaw rotation rate that HEADING_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when HEADING_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. |
 | hott_alarm_sound_interval | 5 | Battery alarm delay in seconds for Hott telemetry |
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 59331e7c7a7..85e21cc3c7a 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -215,7 +215,7 @@ groups:
         field: gyro_stage2_lowpass_type
         table: filter_type
       - name: gyro_use_dyn_lpf
-        description: "Use Dynamic LPF instead of static gyro stage1 LPF."
+        description: "Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position."
         default_value: "OFF"
         field: useDynamicLpf
         type: bool

From be73b015e59a6b58540f4886d7a0751148936bbb Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 25 Dec 2020 16:52:26 +0100
Subject: [PATCH 9/9] One more time regenerate settings.md

---
 docs/Settings.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index b3454073768..18cf166e678 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -328,7 +328,7 @@
 | nav_mc_vel_z_i | 50 | I gain of velocity PID controller |
 | nav_mc_vel_z_p | 100 | P gain of velocity PID controller |
 | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. |
-| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
+| nav_overrides_motor_stop | ALL_NAV | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
 | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) |
 | nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] |
 | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. |