From a4dbf82134ce1b2953afdbfeb7df108a11b7ca79 Mon Sep 17 00:00:00 2001
From: Hydra <dominic.clifton@cleanflight.com>
Date: Mon, 19 Sep 2016 23:52:13 +0200
Subject: [PATCH] use the dterm filter in luxfloat

---
 src/main/flight/pid.c          |  2 +-
 src/main/flight/pid_luxfloat.c | 16 ++++++++++++++--
 2 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 29e0b756b..941a4ca9d 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -66,7 +66,7 @@ pt1Filter_t deltaFilter[3];
 pt1Filter_t yawFilter;
 biquadFilter_t dtermFilterLpf[3];
 biquadFilter_t dtermFilterNotch[3];
-static bool dtermNotchInitialised, dtermBiquadLpfInitialised;
+bool dtermNotchInitialised, dtermBiquadLpfInitialised;
 
 
 void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
diff --git a/src/main/flight/pid_luxfloat.c b/src/main/flight/pid_luxfloat.c
index b81214155..246b5f4de 100644
--- a/src/main/flight/pid_luxfloat.c
+++ b/src/main/flight/pid_luxfloat.c
@@ -62,6 +62,10 @@ extern float lastITermf[3], ITermLimitf[3];
 extern pt1Filter_t deltaFilter[3];
 extern pt1Filter_t yawFilter;
 
+extern bool dtermNotchInitialised, dtermBiquadLpfInitialised;
+extern biquadFilter_t dtermFilterNotch[3];
+extern biquadFilter_t dtermFilterLpf[3];
+
 extern uint8_t motorCount;
 
 #ifdef BLACKBOX
@@ -135,9 +139,17 @@ STATIC_UNIT_TESTED int16_t pidLuxFloatCore(int axis, const pidProfile_t *pidProf
         }
         // Divide delta by targetLooptime to get differential (ie dr/dt)
         delta *= (1.0f / getdT());
+
+        // Filter delta
+        if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta);
+
         if (pidProfile->dterm_lpf_hz) {
-            // DTerm delta low pass filter
-            delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, dT);
+            if (dtermBiquadLpfInitialised) {
+                delta = biquadFilterApply(&dtermFilterLpf[axis], delta);
+            } else {
+                // DTerm delta low pass filter
+                delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
+            }
         }
         DTerm = luxDTermScale * delta * pidProfile->D8[axis] * PIDweight[axis] / 100;
         DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D);
-- 
2.11.4.GIT