Allow PT2 and PT3 filters on Dterm

src/main/common/filter.c

@@ -381,4 +381,37 @@ FAST_CODE float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float
 	return filter->xk;
 }
 
-#endif
+#endif
+
+FUNCTION_COMPILE_FOR_SIZE
+void initFilter(const uint8_t filterType, filter_t *filter, const float cutoffFrequency, const uint32_t refreshRate) {
+    const float dT = refreshRate * 1e-6f;
+
+    if (cutoffFrequency) {
+        if (filterType == FILTER_PT1) {
+            pt1FilterInit(&filter->pt1, cutoffFrequency, dT);
+        } if (filterType == FILTER_PT2) {
+            pt2FilterInit(&filter->pt2, pt2FilterGain(cutoffFrequency, dT));
+        } if (filterType == FILTER_PT3) {
+            pt3FilterInit(&filter->pt3, pt3FilterGain(cutoffFrequency, dT));
+        } else {
+            biquadFilterInitLPF(&filter->biquad, cutoffFrequency, refreshRate);
+        }
+    }
+}
+
+FUNCTION_COMPILE_FOR_SIZE
+void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyFnPtr *applyFn) {
+    *applyFn = nullFilterApply;
+    if (cutoffFrequency) {
+        if (filterType == FILTER_PT1) {
+            *applyFn = (filterApplyFnPtr) pt1FilterApply;
+        } if (filterType == FILTER_PT2) {
+            *applyFn = (filterApplyFnPtr) pt2FilterApply;
+        } if (filterType == FILTER_PT3) {
+            *applyFn = (filterApplyFnPtr) pt3FilterApply;
+        } else {
+            *applyFn = (filterApplyFnPtr) biquadFilterApply;
+        }
+    }
+}

src/main/common/filter.h

@@ -47,12 +47,16 @@ typedef struct biquadFilter_s {
 
 typedef union { 
     biquadFilter_t biquad; 
-    pt1Filter_t pt1; 
+    pt1Filter_t pt1;
+    pt2Filter_t pt2;
+    pt3Filter_t pt3;
 } filter_t;
 
 typedef enum {
     FILTER_PT1 = 0,
-    FILTER_BIQUAD
+    FILTER_BIQUAD,
+    FILTER_PT2,
+    FILTER_PT3
 } filterType_e;
 
 typedef enum {
@@ -127,4 +131,7 @@ float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz);
 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
 
 void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT);
-float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input);
+float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input);
+
+void initFilter(uint8_t filterType, filter_t *filter, float cutoffFrequency, uint32_t refreshRate);
+void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyFnPtr *applyFn);

src/main/drivers/compass/compass_vcm5883.c

@@ -107,7 +107,6 @@ bool vcm5883Detect(magDev_t * mag)
     }
 
     if (!deviceDetect(mag)) {
-        DEBUG_SET(DEBUG_ALWAYS, 0, 7);
         busDeviceDeInit(mag->busDev);
         return false;
     }

src/main/fc/settings.yaml

@@ -129,6 +129,8 @@ tables:
     enum: vtxLowerPowerDisarm_e
   - name: filter_type
     values: ["PT1", "BIQUAD"]
+  - name: filter_type_full
+    values: ["PT1", "BIQUAD", "PT2", "PT3"]
   - name: log_level
     values: ["ERROR", "WARNING", "INFO", "VERBOSE", "DEBUG"]
   - name: iterm_relax
@@ -1884,24 +1886,24 @@ groups:
         max: 900
       - name: dterm_lpf_hz
         description: "Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value"
-        default_value: 40
+        default_value: 110
         min: 0
         max: 500
       - name: dterm_lpf_type
-        description: "Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation."
+        description: "Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`, `PT2`, `PT3`."
-        default_value: "BIQUAD"
+        default_value: "PT2"
         field: dterm_lpf_type
-        table: filter_type
+        table: filter_type_full
       - name: dterm_lpf2_hz
         description: "Cutoff frequency for stage 2 D-term low pass filter"
         default_value: 0
         min: 0
         max: 500
       - name: dterm_lpf2_type
-        description: "Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation."
+        description: "Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`, `PT2`, `PT3`."
-        default_value: "BIQUAD"
+        default_value: "PT1"
         field: dterm_lpf2_type
-        table: filter_type
+        table: filter_type_full
       - name: yaw_lpf_hz
         description: "Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below)"
         default_value: 0

src/main/flight/pid.c

@@ -309,6 +309,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
 #endif
 );
 
+FUNCTION_COMPILE_FOR_SIZE
 bool pidInitFilters(void)
 {
     const uint32_t refreshRate = getLooptime();
@@ -317,26 +318,9 @@ bool pidInitFilters(void)
         return false;
     }
 
-    // Init other filters
+    for (int axis = 0; axis < 3; ++ axis) {
-    if (pidProfile()->dterm_lpf_hz) {
+        initFilter(pidProfile()->dterm_lpf_type, &pidState[axis].dtermLpfState, pidProfile()->dterm_lpf_hz, refreshRate);
-        for (int axis = 0; axis < 3; ++ axis) {
+        initFilter(pidProfile()->dterm_lpf2_type, &pidState[axis].dtermLpf2State, pidProfile()->dterm_lpf2_hz, refreshRate);
-            if (pidProfile()->dterm_lpf_type == FILTER_PT1) {
-                pt1FilterInit(&pidState[axis].dtermLpfState.pt1, pidProfile()->dterm_lpf_hz, refreshRate * 1e-6f);
-            } else {
-                biquadFilterInitLPF(&pidState[axis].dtermLpfState.biquad, pidProfile()->dterm_lpf_hz, refreshRate);
-            }
-        }
-    }
-
-    // Init other filters
-    if (pidProfile()->dterm_lpf2_hz) {
-        for (int axis = 0; axis < 3; ++ axis) {
-            if (pidProfile()->dterm_lpf2_type == FILTER_PT1) {
-                pt1FilterInit(&pidState[axis].dtermLpf2State.pt1, pidProfile()->dterm_lpf2_hz, refreshRate * 1e-6f);
-            } else {
-                biquadFilterInitLPF(&pidState[axis].dtermLpf2State.biquad, pidProfile()->dterm_lpf2_hz, refreshRate);
-            }
-        }
     }
 
     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
@@ -713,8 +697,6 @@ static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) {
     dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT);
     dBoost = constrainf(dBoost, dBoostMin, dBoostMax);
 
-    DEBUG_SET(DEBUG_ALWAYS, pidState->axis, dBoost * 100);
-
     return dBoost;
 }
 #else
@@ -733,10 +715,23 @@ static float dTermProcess(pidState_t *pidState, float dT) {
         newDTerm = 0;
     } else {
         float delta = pidState->previousRateGyro - pidState->gyroRate;
+        
+        if (pidState->axis == FD_ROLL) {
+            DEBUG_SET(DEBUG_ALWAYS, 0, delta);
+        }
 
         delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
+        
+        if (pidState->axis == FD_ROLL) {
+            DEBUG_SET(DEBUG_ALWAYS, 1, delta);
+        }
+        
         delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta);
 
+        if (pidState->axis == FD_ROLL) {
+            DEBUG_SET(DEBUG_ALWAYS, 2, delta);
+        }
+
         // Calculate derivative
         newDTerm =  delta * (pidState->kD / dT) * applyDBoost(pidState, dT);
     }
@@ -1230,23 +1225,8 @@ void pidInit(void)
         usedPidControllerType = pidProfile()->pidControllerType;
     }
 
-    dTermLpfFilterApplyFn = nullFilterApply;
+    assignFilterApplyFn(pidProfile()->dterm_lpf_type, pidProfile()->dterm_lpf_hz, &dTermLpfFilterApplyFn);
-    if (pidProfile()->dterm_lpf_hz) {
+    assignFilterApplyFn(pidProfile()->dterm_lpf2_type, pidProfile()->dterm_lpf2_hz, &dTermLpf2FilterApplyFn);
-        if (pidProfile()->dterm_lpf_type == FILTER_PT1) {
-            dTermLpfFilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
-        } else {
-            dTermLpfFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
-        }
-    }
-
-    dTermLpf2FilterApplyFn = nullFilterApply;
-    if (pidProfile()->dterm_lpf2_hz) {
-        if (pidProfile()->dterm_lpf2_type == FILTER_PT1) {
-            dTermLpf2FilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
-        } else {
-            dTermLpf2FilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
-        }
-    }
 
     if (usedPidControllerType == PID_TYPE_PIFF) {
         pidControllerApplyFn = pidApplyFixedWingRateController;