Merge pull request #6943 from kmitchel/dynamic_lowpass

Implement throttle based dynamic gyro and dterm filters.

src/main/flight/mixer.c

@@ -815,6 +815,10 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
     }
 
     pidUpdateAntiGravityThrottleFilter(throttle);
+#ifdef USE_DYN_LPF
+    dynLpfGyroUpdate(throttle);
+    dynLpfDTermUpdate(throttle);
+#endif
 
 #if defined(USE_THROTTLE_BOOST)
     if (throttleBoost > 0.0f) {

src/main/flight/pid.c

@@ -121,16 +121,11 @@ void resetPidProfile(pidProfile_t *pidProfile)
             [PID_LEVEL] = { 50, 50, 75, 0 },
             [PID_MAG] =   { 40, 0, 0, 0 },
         },
-
         .pidSumLimit = PIDSUM_LIMIT,
         .pidSumLimitYaw = PIDSUM_LIMIT_YAW,
         .yaw_lowpass_hz = 0,
-        .dterm_lowpass_hz = 100,    // dual PT1 filtering ON by default
-        .dterm_lowpass2_hz = 200,   // second Dterm LPF ON by default
         .dterm_notch_hz = 0,
         .dterm_notch_cutoff = 0,
-        .dterm_filter_type = FILTER_PT1,
-        .dterm_filter2_type = FILTER_PT1,
         .itermWindupPointPercent = 40,
         .vbatPidCompensation = 0,
         .pidAtMinThrottle = PID_STABILISATION_ON,
@@ -167,7 +162,19 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .abs_control_limit = 90,
         .abs_control_error_limit = 20,
         .antiGravityMode = ANTI_GRAVITY_SMOOTH,
+        .dyn_lpf_dterm_max_hz = 200,
+        .dyn_lpf_dterm_idle = 20,
+        .dterm_lowpass_hz = 100,    // dual PT1 filtering ON by default
+        .dterm_lowpass2_hz = 200,   // second Dterm LPF ON by default
+        .dterm_filter_type = FILTER_PT1,
+        .dterm_filter2_type = FILTER_PT1,
     );
+#ifdef USE_DYN_LPF
+    pidProfile->dterm_lowpass_hz = 120;
+    pidProfile->dterm_lowpass2_hz = 180;
+    pidProfile->dterm_filter_type = FILTER_BIQUAD;
+    pidProfile->dterm_filter2_type = FILTER_BIQUAD;
+#endif
 }
 
 void pgResetFn_pidProfiles(pidProfile_t *pidProfiles)
@@ -280,7 +287,11 @@ void pidInitFilters(const pidProfile_t *pidProfile)
             }
             break;
         case FILTER_BIQUAD:
+#ifdef USE_DYN_LPF
+            dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
+#else
             dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
+#endif
             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
                 biquadFilterInitLPF(&dtermLowpass[axis].biquadFilter, pidProfile->dterm_lowpass_hz, targetPidLooptime);
             }
@@ -436,6 +447,14 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
     }
 }
 
+#ifdef USE_DYN_LPF
+static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE;
+static FAST_RAM_ZERO_INIT float dynLpfIdle;
+static FAST_RAM_ZERO_INIT float dynLpfIdlePoint;
+static FAST_RAM_ZERO_INIT float dynLpfInvIdlePointScaled;
+static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
+#endif
+
 void pidInitConfig(const pidProfile_t *pidProfile)
 {
     if (pidProfile->feedForwardTransition == 0) {
@@ -510,6 +529,30 @@ void pidInitConfig(const pidProfile_t *pidProfile)
     acLimit = (float)pidProfile->abs_control_limit;
     acErrorLimit = (float)pidProfile->abs_control_error_limit;
 #endif
+
+#ifdef USE_DYN_LPF
+    if (pidProfile->dyn_lpf_dterm_idle > 0 && pidProfile->dyn_lpf_dterm_max_hz > 0) {
+        if (pidProfile->dterm_lowpass_hz > 0 ) {
+            dynLpfMin = pidProfile->dterm_lowpass_hz;
+            switch (pidProfile->dterm_filter_type) {
+            case FILTER_PT1:
+                dynLpfFilter = DYN_LPF_PT1;
+                break;
+            case FILTER_BIQUAD:
+                dynLpfFilter = DYN_LPF_BIQUAD;
+                break;
+            default:
+                dynLpfFilter = DYN_LPF_NONE;
+                break;
+            }
+        }
+    } else {
+        dynLpfFilter = DYN_LPF_NONE;
+    }
+    dynLpfIdle = pidProfile->dyn_lpf_dterm_idle / 100.0f;
+    dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f;
+    dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (pidProfile->dyn_lpf_dterm_max_hz - dynLpfMin);
+#endif
 }
 
 void pidInit(const pidProfile_t *pidProfile)
@@ -1130,3 +1173,26 @@ bool pidAntiGravityEnabled(void)
 {
     return antiGravityEnabled;
 }
+
+#ifdef USE_DYN_LPF
+void dynLpfDTermUpdate(float throttle)
+{
+    if (dynLpfFilter != DYN_LPF_NONE) {
+        uint16_t cutoffFreq = dynLpfMin;
+        if (throttle > dynLpfIdle) {
+            const float dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f;
+            cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled;
+         }
+
+         if (dynLpfFilter == DYN_LPF_PT1) {
+            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                pt1FilterUpdateCutoff(&dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, dT));
+            }
+        } else {
+            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                biquadFilterUpdateLPF(&dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime);
+            }
+        }
+    }
+}
+#endif

src/main/flight/pid.h

@@ -149,6 +149,8 @@ typedef struct pidProfile_s {
     uint8_t abs_control_limit;              // Limit to the correction
     uint8_t abs_control_error_limit;        // Limit to the accumulated error
     uint8_t dterm_filter2_type;             // Filter selection for 2nd dterm
+    uint16_t dyn_lpf_dterm_max_hz;
+    uint8_t  dyn_lpf_dterm_idle;
 } pidProfile_t;
 
 PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);
@@ -214,3 +216,5 @@ float pidLevel(int axis, const pidProfile_t *pidProfile,
     const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint);
 float calcHorizonLevelStrength(void);
 #endif
+void dynLpfDTermUpdate(float throttle);
+

src/main/interface/settings.c

@@ -550,6 +550,12 @@ const clivalue_t valueTable[] = {
     { "dyn_filter_width_percent",   VAR_UINT8  | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_width_percent) },
     { "dyn_filter_range",           VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_range) },
 #endif
+#ifdef USE_DYN_LPF
+    { "dyn_lpf_gyro_max_hz",        VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_max_hz) },
+    { "dyn_lpf_gyro_idle",          VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_idle) },
+    { "dyn_lpf_dterm_max_hz",       VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_max_hz) },
+    { "dyn_lpf_dterm_idle",         VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_idle) },
+#endif
 
 // PG_ACCELEROMETER_CONFIG
     { "align_acc",                  VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) },

src/main/sensors/gyro.c

@@ -183,38 +183,44 @@ static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int typ
 #define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980  // 97.5% full scale (1950dps for 2000dps gyro)
 #define GYRO_OVERFLOW_RESET_THRESHOLD 30340    // 92.5% full scale (1850dps for 2000dps gyro)
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5);
+PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5);
 
 #ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
 #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
 #endif
 
-PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
+void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
-    .gyro_align = ALIGN_DEFAULT,
+{ 
-    .gyroCalibrationDuration = 125,        // 1.25 seconds
+    gyroConfig->gyro_align = ALIGN_DEFAULT;
-    .gyroMovementCalibrationThreshold = 48,
+    gyroConfig->gyroCalibrationDuration = 125;        // 1.25 seconds
-    .gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT,
+    gyroConfig->gyroMovementCalibrationThreshold = 48;
-    .gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL,
+    gyroConfig->gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT;
-    .gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL,
+    gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
-    .gyro_lowpass_type = FILTER_PT1,
+    gyroConfig->gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL;
-    .gyro_lowpass_hz = 100,
+    gyroConfig->gyro_lowpass_type = FILTER_PT1;
-    .gyro_lowpass2_type = FILTER_PT1,
+    gyroConfig->gyro_lowpass_hz = 100;
-    .gyro_lowpass2_hz = 300,
+    gyroConfig->gyro_lowpass2_type = FILTER_PT1;
-    .gyro_high_fsr = false,
+    gyroConfig->gyro_lowpass2_hz = 300;
-    .gyro_use_32khz = false,
+    gyroConfig->gyro_high_fsr = false;
-    .gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT,
+    gyroConfig->gyro_use_32khz = false;
-    .gyro_soft_notch_hz_1 = 0,
+    gyroConfig->gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT;
-    .gyro_soft_notch_cutoff_1 = 0,
+    gyroConfig->gyro_soft_notch_hz_1 = 0;
-    .gyro_soft_notch_hz_2 = 0,
+    gyroConfig->gyro_soft_notch_cutoff_1 = 0;
-    .gyro_soft_notch_cutoff_2 = 0,
+    gyroConfig->gyro_soft_notch_hz_2 = 0;
-    .checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES,
+    gyroConfig->gyro_soft_notch_cutoff_2 = 0;
-    .gyro_offset_yaw = 0,
+    gyroConfig->checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES;
-    .yaw_spin_recovery = true,
+    gyroConfig->gyro_offset_yaw = 0;
-    .yaw_spin_threshold = 1950,
+    gyroConfig->yaw_spin_recovery = true;
-    .dyn_filter_width_percent = 40,
+    gyroConfig->yaw_spin_threshold = 1950;
-    .dyn_fft_location = DYN_FFT_AFTER_STATIC_FILTERS,
+    gyroConfig->dyn_filter_width_percent = 40;
-    .dyn_filter_range = DYN_FILTER_RANGE_MEDIUM,
+    gyroConfig->dyn_fft_location = DYN_FFT_AFTER_STATIC_FILTERS;
-);
+    gyroConfig->dyn_filter_range = DYN_FILTER_RANGE_MEDIUM;
+    gyroConfig->dyn_lpf_gyro_max_hz = 400;
+    gyroConfig->dyn_lpf_gyro_idle = 20;
+#ifdef USE_DYN_LPF
+    gyroConfig->gyro_lowpass_hz = 120;
+#endif
+}
 
 #ifdef USE_MULTI_GYRO
 #define ACTIVE_GYRO ((gyroToUse == GYRO_CONFIG_USE_GYRO_2) ? &gyroSensor2 : &gyroSensor1)
@@ -533,6 +539,39 @@ bool gyroInit(void)
     return ret;
 }
 
+#ifdef USE_DYN_LPF
+static FAST_RAM int8_t dynLpfFilter = DYN_LPF_NONE;
+static FAST_RAM_ZERO_INIT float dynLpfIdle;
+static FAST_RAM_ZERO_INIT float dynLpfIdlePoint;
+static FAST_RAM_ZERO_INIT float dynLpfInvIdlePointScaled;
+static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
+
+static void dynLpfFilterInit()
+{
+    if (gyroConfig()->dyn_lpf_gyro_idle > 0 && gyroConfig()->dyn_lpf_gyro_max_hz > 0) {
+        if (gyroConfig()->gyro_lowpass_hz > 0 ) {
+            dynLpfMin = gyroConfig()->gyro_lowpass_hz;
+            switch (gyroConfig()->gyro_lowpass_type) {
+            case FILTER_PT1:
+                dynLpfFilter = DYN_LPF_PT1;
+                break;
+            case FILTER_BIQUAD:
+                dynLpfFilter = DYN_LPF_BIQUAD;
+                break;
+            default:
+                dynLpfFilter = DYN_LPF_NONE;
+                break;
+            }
+        }
+    } else {
+        dynLpfFilter = DYN_LPF_NONE;
+    }
+    dynLpfIdle = gyroConfig()->dyn_lpf_gyro_idle / 100.0f;
+    dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f;
+    dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (gyroConfig()->dyn_lpf_gyro_max_hz - dynLpfMin);
+}
+#endif
+
 void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz)
 {
     filterApplyFnPtr *lowpassFilterApplyFn;
@@ -574,7 +613,11 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint
             }
             break;
         case FILTER_BIQUAD:
+#ifdef USE_DYN_LPF
+            *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1;
+#else
             *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
+#endif
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
                 biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, gyro.targetLooptime);
             }
@@ -682,6 +725,9 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
 #ifdef USE_GYRO_DATA_ANALYSE
     gyroInitFilterDynamicNotch(gyroSensor);
 #endif
+#ifdef USE_DYN_LPF
+    dynLpfFilterInit();
+#endif
 }
 
 void gyroInitFilters(void)
@@ -1150,3 +1196,44 @@ uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg)
     return mpuGyroReadRegister(gyroSensorBusByDevice(whichSensor), reg);
 }
 #endif // USE_GYRO_REGISTER_DUMP
+
+#ifdef USE_DYN_LPF
+void dynLpfGyroUpdate(float throttle)
+{
+    if (dynLpfFilter != DYN_LPF_NONE) {
+        uint16_t cutoffFreq = dynLpfMin;
+        if (throttle > dynLpfIdle) {
+            const float dynThrottle = (throttle - (throttle * throttle * throttle) / 3.0f) * 1.5f;
+            cutoffFreq += (dynThrottle - dynLpfIdlePoint) * dynLpfInvIdlePointScaled;
+        }
+        if (dynLpfFilter == DYN_LPF_PT1) {
+            const float gyroDt = gyro.targetLooptime * 1e-6f;
+            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+#ifdef USE_MULTI_GYRO
+                if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
+                    pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
+                }
+                if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
+                    pt1FilterUpdateCutoff(&gyroSensor2.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
+                }
+#else
+                pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
+#endif
+            }
+        } else {
+            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+#ifdef USE_MULTI_GYRO
+                if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
+                    biquadFilterUpdateLPF(&gyroSensor1.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
+                }
+                if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {     
+                    biquadFilterUpdateLPF(&gyroSensor2.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
+                }
+#else
+                biquadFilterUpdateLPF(&gyroSensor1.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
+#endif
+            }
+        }
+    }
+}
+#endif

src/main/sensors/gyro.h

@@ -53,6 +53,12 @@ enum {
     DYN_FILTER_RANGE_LOW
 } ;
 
+enum {
+    DYN_LPF_NONE = 0,
+    DYN_LPF_PT1,
+    DYN_LPF_BIQUAD
+};
+
 #define GYRO_CONFIG_USE_GYRO_1      0
 #define GYRO_CONFIG_USE_GYRO_2      1
 #define GYRO_CONFIG_USE_GYRO_BOTH   2
@@ -95,6 +101,8 @@ typedef struct gyroConfig_s {
     uint8_t dyn_filter_width_percent;
     uint8_t dyn_fft_location; // before or after static filters
     uint8_t dyn_filter_range; // ignore any FFT bin below this threshold
+    uint16_t dyn_lpf_gyro_max_hz;
+    uint8_t  dyn_lpf_gyro_idle;
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);
@@ -117,3 +125,6 @@ bool gyroOverflowDetected(void);
 bool gyroYawSpinDetected(void);
 uint16_t gyroAbsRateDps(int axis);
 uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg);
+#ifdef USE_DYN_LPF
+void dynLpfGyroUpdate(float throttle);
+#endif

src/main/target/common_pre.h

@@ -189,6 +189,7 @@
 #define USE_THROTTLE_BOOST
 #define USE_RC_SMOOTHING_FILTER
 #define USE_ITERM_RELAX
+#define USE_DYN_LPF
 
 #ifdef USE_SERIALRX_SPEKTRUM
 #define USE_SPEKTRUM_BIND