Merge pull request #4475 from iNavFlight/dzikuvx-dterm-fir-configurable

Make Dterm FIR filter configurable

src/main/fc/settings.yaml

@@ -956,6 +956,9 @@ groups:
       - name: dterm_lpf_hz
         min: 0
         max: 200
+      - name: use_dterm_fir_filter
+        field: use_dterm_fir_filter
+        type: bool
       - name: yaw_lpf_hz
         min: 0
         max: 200

src/main/flight/pid.c

@@ -183,6 +183,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .dterm_lpf_hz = 40,
         .yaw_lpf_hz = 30,
         .dterm_setpoint_weight = 1.0f,
+        .use_dterm_fir_filter = 1,
 
         .itermWindupPointPercent = 50,       // Percent
 
@@ -211,8 +212,10 @@ void pidInit(void)
     // by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
     // h[0] = 5/8, h[-1] = 1/4, h[-2] = -1, h[-3] = -1/4, h[-4] = 3/8
     static const float dtermCoeffs[PID_GYRO_RATE_BUF_LENGTH] = {5.0f/8, 2.0f/8, -8.0f/8, -2.0f/8, 3.0f/8};
-    for (int axis = 0; axis < 3; ++ axis) {
+    if (pidProfile()->use_dterm_fir_filter) {
-        firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs);
+        for (int axis = 0; axis < 3; ++ axis) {
+            firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs);
+        }
     }
 
     pidResetTPAFilter();
@@ -566,9 +569,19 @@ static void pidApplyMulticopterRateController(pidState_t *pidState, flight_dynam
             deltaFiltered = biquadFilterApply(&pidState->deltaLpfState, deltaFiltered);
         }
 
+        /*
+         * Apply "Classical" INAV noise robust differentiator if configured to do so
+         * This filter is not configurable in any way, it only can be enabled or disabled
+         */
+        if (pidProfile()->use_dterm_fir_filter) {
+            firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered);
+            newDTerm = firFilterApply(&pidState->gyroRateFilter);
+        } else {
+            newDTerm = deltaFiltered;
+        }
+
         // Calculate derivative
-        firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered);
+        newDTerm =  newDTerm * (pidState->kD / dT);
-        newDTerm = firFilterApply(&pidState->gyroRateFilter) * (pidState->kD / dT);
 
         // Additionally constrain D
         newDTerm = constrainf(newDTerm, -300.0f, 300.0f);

src/main/flight/pid.h

@@ -83,6 +83,7 @@ typedef struct pidProfile_s {
     uint16_t dterm_soft_notch_hz;           // Dterm Notch frequency
     uint16_t dterm_soft_notch_cutoff;       // Dterm Notch Cutoff frequency
     uint8_t dterm_lpf_hz;                   // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
+    uint8_t use_dterm_fir_filter;           // Use classical INAV FIR differentiator. Very noise robust, can be quite slowish
 
     uint8_t yaw_pterm_lpf_hz;               // Used for filering Pterm noise on noisy frames
     uint8_t yaw_lpf_hz;