From 59809307a2e81a49b89575660f4beb1e82bafe18 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 8 Mar 2019 20:24:47 +0100
Subject: [PATCH] Make Dterm FIR filer configurable

---
 src/main/fc/settings.yaml |  3 +++
 src/main/flight/pid.c     | 21 +++++++++++++++++----
 src/main/flight/pid.h     |  1 +
 3 files changed, 21 insertions(+), 4 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index ad6b9f54a15..73406b0da18 100644
--- a/src/main/fc/settings.yaml
+++ b/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
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 06d0fa8747f..6de457cf14e 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -175,6 +175,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
 
@@ -203,8 +204,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) {
-        firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs);
+    if (pidProfile()->use_dterm_fir_filter) {
+        for (int axis = 0; axis < 3; ++ axis) {
+            firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs);
+        }
     }
 
     pidResetTPAFilter();
@@ -558,9 +561,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 = firFilterApply(&pidState->gyroRateFilter) * (pidState->kD / dT);
+        newDTerm =  newDTerm * (pidState->kD / dT);
 
         // Additionally constrain D
         newDTerm = constrainf(newDTerm, -300.0f, 300.0f);
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index a0943320890..b19a2f42862 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -82,6 +82,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;