add PT2 and PT3 lowpass filter options

src/main/cli/settings.c

@@ -294,11 +294,15 @@ static const char * const lookupTablePwmProtocol[] = {
 static const char * const lookupTableLowpassType[] = {
     "PT1",
     "BIQUAD",
+    "PT2",
+    "PT3",
 };
 
 static const char * const lookupTableDtermLowpassType[] = {
     "PT1",
     "BIQUAD",
+    "PT2",
+    "PT3",
 };
 
 static const char * const lookupTableAntiGravityMode[] = {

src/main/flight/pid.c

@@ -1231,15 +1231,27 @@ void dynLpfDTermUpdate(float throttle)
         } else {
             cutoffFreq = fmax(dynThrottle(throttle) * pidRuntime.dynLpfMax, pidRuntime.dynLpfMin);
         }
-
+        switch (pidRuntime.dynLpfFilter) {
-         if (pidRuntime.dynLpfFilter == DYN_LPF_PT1) {
+        case DYN_LPF_PT1:
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
                 pt1FilterUpdateCutoff(&pidRuntime.dtermLowpass[axis].pt1Filter, pt1FilterGain(cutoffFreq, pidRuntime.dT));
             }
-        } else if (pidRuntime.dynLpfFilter == DYN_LPF_BIQUAD) {
+            break;
+        case DYN_LPF_BIQUAD:
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
                 biquadFilterUpdateLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, cutoffFreq, targetPidLooptime);
             }
+            break;
+        case DYN_LPF_PT2:
+            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                pt2FilterUpdateCutoff(&pidRuntime.dtermLowpass[axis].pt2Filter, pt2FilterGain(cutoffFreq, pidRuntime.dT));
+            }
+            break;
+        case DYN_LPF_PT3:
+            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                pt3FilterUpdateCutoff(&pidRuntime.dtermLowpass[axis].pt3Filter, pt3FilterGain(cutoffFreq, pidRuntime.dT));
+            }
+            break;
         }
     }
 }

src/main/flight/pid.h

@@ -253,6 +253,8 @@ typedef struct pidAxisData_s {
 typedef union dtermLowpass_u {
     pt1Filter_t pt1Filter;
     biquadFilter_t biquadFilter;
+    pt2Filter_t pt2Filter;
+    pt3Filter_t pt3Filter;
 } dtermLowpass_t;
 
 typedef struct pidCoefficient_s {

src/main/flight/pid_init.c

@@ -110,7 +110,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
     }
 #endif
 
-    if (dterm_lowpass_hz > 0 && dterm_lowpass_hz < pidFrequencyNyquist) {
+    if (dterm_lowpass_hz > 0) {
         switch (pidProfile->dterm_filter_type) {
         case FILTER_PT1:
             pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
@@ -119,13 +119,29 @@ void pidInitFilters(const pidProfile_t *pidProfile)
             }
             break;
         case FILTER_BIQUAD:
+            if (pidProfile->dterm_lowpass_hz < pidFrequencyNyquist) {
 #ifdef USE_DYN_LPF
-            pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
+                pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
 #else
-            pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
+                pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)biquadFilterApply;
 #endif
+                for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                    biquadFilterInitLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, dterm_lowpass_hz, targetPidLooptime);
+                }
+            } else {
+                pidRuntime.dtermLowpassApplyFn = nullFilterApply;
+            }
+            break;
+        case FILTER_PT2:
+            pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt2FilterApply;
             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
-                biquadFilterInitLPF(&pidRuntime.dtermLowpass[axis].biquadFilter, dterm_lowpass_hz, targetPidLooptime);
+                pt2FilterInit(&pidRuntime.dtermLowpass[axis].pt2Filter, pt2FilterGain(dterm_lowpass_hz, pidRuntime.dT));
+            }
+            break;
+        case FILTER_PT3:
+            pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt3FilterApply;
+            for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                pt3FilterInit(&pidRuntime.dtermLowpass[axis].pt3Filter, pt3FilterGain(dterm_lowpass_hz, pidRuntime.dT));
             }
             break;
         default:
@@ -137,9 +153,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
     }
 
     //2nd Dterm Lowpass Filter
-    if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist) {
+    if (pidProfile->dterm_lowpass2_hz > 0) {
-        pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
-    } else {
         switch (pidProfile->dterm_filter2_type) {
         case FILTER_PT1:
             pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
@@ -148,18 +162,36 @@ void pidInitFilters(const pidProfile_t *pidProfile)
             }
             break;
         case FILTER_BIQUAD:
-            pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
+            if (pidProfile->dterm_lowpass2_hz < pidFrequencyNyquist) {
+                pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
+                for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                    biquadFilterInitLPF(&pidRuntime.dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime);
+                }
+            } else {
+                pidRuntime.dtermLowpassApplyFn = nullFilterApply;
+            }
+            break;
+        case FILTER_PT2:
+            pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt2FilterApply;
             for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
-                biquadFilterInitLPF(&pidRuntime.dtermLowpass2[axis].biquadFilter, pidProfile->dterm_lowpass2_hz, targetPidLooptime);
+                pt2FilterInit(&pidRuntime.dtermLowpass2[axis].pt2Filter, pt2FilterGain(pidProfile->dterm_lowpass2_hz, pidRuntime.dT));
+            }
+            break;
+        case FILTER_PT3:
+            pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt3FilterApply;
+            for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                pt3FilterInit(&pidRuntime.dtermLowpass2[axis].pt3Filter, pt3FilterGain(pidProfile->dterm_lowpass2_hz, pidRuntime.dT));
             }
             break;
         default:
             pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
             break;
         }
+    } else {
+        pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
     }
 
-    if (pidProfile->yaw_lowpass_hz == 0 || pidProfile->yaw_lowpass_hz > pidFrequencyNyquist) {
+    if (pidProfile->yaw_lowpass_hz == 0) {
         pidRuntime.ptermYawLowpassApplyFn = nullFilterApply;
     } else {
         pidRuntime.ptermYawLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
@@ -333,6 +365,12 @@ void pidInitConfig(const pidProfile_t *pidProfile)
         case FILTER_BIQUAD:
             pidRuntime.dynLpfFilter = DYN_LPF_BIQUAD;
             break;
+        case FILTER_PT2:
+            pidRuntime.dynLpfFilter = DYN_LPF_PT2;
+            break;
+        case FILTER_PT3:
+            pidRuntime.dynLpfFilter = DYN_LPF_PT3;
+            break;
         default:
             pidRuntime.dynLpfFilter = DYN_LPF_NONE;
             break;

src/main/sensors/gyro.c

@@ -635,17 +635,29 @@ void dynLpfGyroUpdate(float throttle)
         } else {
             cutoffFreq = fmax(dynThrottle(throttle) * gyro.dynLpfMax, gyro.dynLpfMin);
         }
-        if (gyro.dynLpfFilter == DYN_LPF_PT1) {
+        DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
-            DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
+        const float gyroDt = gyro.targetLooptime * 1e-6f;
-            const float gyroDt = gyro.targetLooptime * 1e-6f;
+        switch (gyro.dynLpfFilter) {
+        case DYN_LPF_PT1:
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
                 pt1FilterUpdateCutoff(&gyro.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
             }
-        } else if (gyro.dynLpfFilter == DYN_LPF_BIQUAD) {
+            break;
-            DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
+        case DYN_LPF_BIQUAD:
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
                 biquadFilterUpdateLPF(&gyro.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
             }
+            break;
+        case  DYN_LPF_PT2:
+            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                pt2FilterUpdateCutoff(&gyro.lowpassFilter[axis].pt2FilterState, pt2FilterGain(cutoffFreq, gyroDt));
+            }
+            break;
+        case DYN_LPF_PT3:
+            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                pt3FilterUpdateCutoff(&gyro.lowpassFilter[axis].pt3FilterState, pt3FilterGain(cutoffFreq, gyroDt));
+            }
+            break;
         }
     }
 }

src/main/sensors/gyro.h

@@ -36,7 +36,7 @@
 
 #include "pg/pg.h"
 
-#define FILTER_FREQUENCY_MAX 4000 // maximum frequency for filter cutoffs (nyquist limit of 8K max sampling)
+#define FILTER_FREQUENCY_MAX 1000 // so little filtering above 1000hz that if the user wants less delay, they must disable the filter
 #define DYN_LPF_FILTER_FREQUENCY_MAX 1000
 
 #define DYN_LPF_GYRO_MIN_HZ_DEFAULT 200
@@ -51,6 +51,8 @@
 typedef union gyroLowpassFilter_u {
     pt1Filter_t pt1FilterState;
     biquadFilter_t biquadFilterState;
+    pt2Filter_t pt2FilterState;
+    pt3Filter_t pt3FilterState;
 } gyroLowpassFilter_t;
 
 typedef enum gyroDetectionFlags_e {
@@ -147,7 +149,9 @@ enum {
 enum {
     DYN_LPF_NONE = 0,
     DYN_LPF_PT1,
-    DYN_LPF_BIQUAD
+    DYN_LPF_BIQUAD,
+    DYN_LPF_PT2,
+    DYN_LPF_PT3,
 };
 
 typedef enum {

src/main/sensors/gyro_init.c

@@ -181,8 +181,8 @@ static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_
     // type. It will be overridden for positive cases.
     *lowpassFilterApplyFn = nullFilterApply;
 
-    // If lowpass cutoff has been specified and is less than the Nyquist frequency
+    // If lowpass cutoff has been specified
-    if (lpfHz && lpfHz <= gyroFrequencyNyquist) {
+    if (lpfHz) {
         switch (type) {
         case FILTER_PT1:
             *lowpassFilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
@@ -192,13 +192,29 @@ static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_
             ret = true;
             break;
         case FILTER_BIQUAD:
+            if (lpfHz <= gyroFrequencyNyquist) {
 #ifdef USE_DYN_LPF
-            *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1;
+                *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1;
 #else
-            *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
+                *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
 #endif
+                for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                    biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, looptime);
+                }
+                ret = true;
+            }
+            break;
+        case FILTER_PT2:
+            *lowpassFilterApplyFn = (filterApplyFnPtr) pt2FilterApply;
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-                biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, looptime);
+                pt2FilterInit(&lowpassFilter[axis].pt2FilterState, gain);
+            }
+            ret = true;
+            break;
+        case FILTER_PT3:
+            *lowpassFilterApplyFn = (filterApplyFnPtr) pt3FilterApply;
+            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                pt3FilterInit(&lowpassFilter[axis].pt3FilterState, gain);
             }
             ret = true;
             break;
@@ -218,6 +234,12 @@ static void dynLpfFilterInit()
         case FILTER_BIQUAD:
             gyro.dynLpfFilter = DYN_LPF_BIQUAD;
             break;
+        case FILTER_PT2:
+            gyro.dynLpfFilter = DYN_LPF_PT2;
+            break;
+        case FILTER_PT3:
+            gyro.dynLpfFilter = DYN_LPF_PT3;
+            break;
         default:
             gyro.dynLpfFilter = DYN_LPF_NONE;
             break;

src/test/unit/pid_unittest.cc

@@ -548,11 +548,11 @@ TEST(pidControllerTest, testFeedForward) {
     EXPECT_NEAR(515.26, pidData[FD_PITCH].F, calculateTolerance(515.26));
     EXPECT_NEAR(515.26, pidData[FD_YAW].F, calculateTolerance(515.26));
 
-     setStickPosition(FD_ROLL, 0.0f);
+    setStickPosition(FD_ROLL, 0.0f);
-     setStickPosition(FD_PITCH, 0.0f);
+    setStickPosition(FD_PITCH, 0.0f);
-     setStickPosition(FD_YAW, 0.0f);
+    setStickPosition(FD_YAW, 0.0f);
 
-     for (int loop = 0; loop <= 15; loop++) {
+    for (int loop = 0; loop <= 15; loop++) {
         gyro.gyroADCf[FD_ROLL] += gyro.gyroADCf[FD_ROLL];
         pidController(pidProfile, currentTestTime());
     }