Dynamic gyro LPF

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)
 {
@@ -48,6 +45,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 +61,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 +72,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 +95,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

src/main/common/filter.h

@@ -59,12 +59,16 @@ 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);
 
 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);

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,
     },
 };

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`"

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"
@@ -101,7 +103,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 +118,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,
@@ -510,3 +516,39 @@ 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) {
+        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);
+        }
+    }
+}

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;
@@ -87,3 +91,4 @@ bool gyroReadTemperature(void);
 int16_t gyroGetTemperature(void);
 int16_t gyroRateDps(int axis);
 bool gyroSyncCheckUpdate(void);
+void gyroUpdateDynamicLpf(void);