From 3ae5830eb0c2743383c6aa95bd7ac93fbc8a60de Mon Sep 17 00:00:00 2001
From: Nicola De Pasquale <ndp7c7c0@gmail.com>
Date: Sat, 11 Jan 2020 15:01:06 +0100
Subject: [PATCH]  added dterm lpf configurable curve

---
 src/main/cli/settings.c |  1 +
 src/main/flight/pid.c   | 17 ++++++++++++++++-
 src/main/flight/pid.h   |  2 ++
 3 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 28a8e3db4..6fe76a484 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -994,6 +994,7 @@ const clivalue_t valueTable[] = {
 #ifdef USE_DYN_LPF
     { "dyn_lpf_dterm_min_hz",       VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_min_hz) },
     { "dyn_lpf_dterm_max_hz",       VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_dterm_max_hz) },
+    { "dyn_lpf_dterm_curve_expo",   VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 10 }, PG_PID_PROFILE, offsetof(pidProfile_t, dyn_lpf_curve_expo) },
 #endif
     { "dterm_lowpass_type",         VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DTERM_LOWPASS_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_filter_type) },
     { "dterm_lowpass_hz",           VAR_INT16  | PROFILE_VALUE, .config.minmax = { 0, FILTER_FREQUENCY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_lowpass_hz) },
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 30c094212..124cf4c7f 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -217,6 +217,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .ff_max_rate_limit = 100,
         .ff_smooth_factor = 37,
         .ff_boost = 15,
+        .dyn_lpf_curve_expo = 0,
     );
 #ifndef USE_D_MIN
     pidProfile->pid[PID_ROLL].D = 30;
@@ -588,6 +589,7 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
 static FAST_RAM uint8_t dynLpfFilter = DYN_LPF_NONE;
 static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
 static FAST_RAM_ZERO_INIT uint16_t dynLpfMax;
+static FAST_RAM_ZERO_INIT uint8_t dynLpfCurveExpo;
 #endif
 
 #ifdef USE_D_MIN
@@ -703,6 +705,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
     }
     dynLpfMin = pidProfile->dyn_lpf_dterm_min_hz;
     dynLpfMax = pidProfile->dyn_lpf_dterm_max_hz;
+    dynLpfCurveExpo = pidProfile->dyn_lpf_curve_expo;
 #endif
 
 #ifdef USE_LAUNCH_CONTROL
@@ -1597,8 +1600,13 @@ bool pidAntiGravityEnabled(void)
 #ifdef USE_DYN_LPF
 void dynLpfDTermUpdate(float throttle)
 {
+    static unsigned int cutoffFreq;
     if (dynLpfFilter != DYN_LPF_NONE) {
-        const unsigned int cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin);
+        if (dynLpfCurveExpo > 0) {
+            cutoffFreq = dynDtermLpfCutoffFreq(throttle, dynLpfMin, dynLpfMax, dynLpfCurveExpo);
+        } else {
+            cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin);
+        }
 
          if (dynLpfFilter == DYN_LPF_PT1) {
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@@ -1613,6 +1621,13 @@ void dynLpfDTermUpdate(float throttle)
 }
 #endif
 
+float dynDtermLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo) {
+    const float expof = expo / 10.0f;
+    static float curve;
+    curve = 2 * throttle * (1 - throttle) * expof + powerf(throttle, 2);
+    return (dynLpfMax - dynLpfMin) * curve + dynLpfMin;
+}
+
 void pidSetItermReset(bool enabled)
 {
     zeroThrottleItermReset = enabled;
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 53f36470c..4c53f75e7 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -185,6 +185,7 @@ typedef struct pidProfile_s {
     uint8_t ff_max_rate_limit;              // Maximum setpoint rate percentage for FF
     uint8_t ff_spike_limit;                 // FF stick extrapolation lookahead period in ms
     uint8_t ff_smooth_factor;               // Amount of smoothing for interpolated FF steps
+    uint8_t dyn_lpf_curve_expo;             // set the curve for dynamic dterm lowpass filter
 } pidProfile_t;
 
 PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@@ -264,3 +265,4 @@ float pidGetPidFrequency();
 float pidGetFfBoostFactor();
 float pidGetFfSmoothFactor();
 float pidGetSpikeLimitInverse();
+float dynDtermLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo);