Dynamic D Implementation

src/main/config/config.c

@@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 131;
+static const uint8_t EEPROM_CONF_VERSION = 132;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 {
@@ -178,7 +178,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
 
     pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
     pidProfile->yaw_lpf_hz = 70.0f;
-    pidProfile->dterm_average_count = 4;
+    pidProfile->dterm_average_count = 0;
+    pidProfile->dynamic_dterm_threshold = 20;
     pidProfile->rollPitchItermResetRate = 200;
     pidProfile->yawItermResetRate = 50;
     pidProfile->dterm_lpf_hz = 70.0f;    // filtering ON by default

src/main/flight/pid.c

@@ -133,6 +133,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     static float lastErrorForDelta[3];
     static float deltaState[3][DELTA_MAX_SAMPLES];
     float delta;
+    float dynamicDTermGain;
     int axis;
     float horizonLevelStrength = 1;
 
@@ -141,7 +142,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     // Scaling factors for Pids to match rewrite and use same defaults
     static const float luxPTermScale = 1.0f / 128;
     static const float luxITermScale = 1000000.0f / 0x1000000;
-    static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 256;
+    static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 508;
 
     if (FLIGHT_MODE(HORIZON_MODE)) {
         // Figure out the raw stick positions
@@ -245,6 +246,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
             // would be scaled by different dt each time. Division by dT fixes that.
             delta *= (1.0f / getdT());
 
+            // Several different Dterm filtering methods to prevent noise. All of them can be combined
+
             // Filter delta
             if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
 
@@ -252,6 +255,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
             if (pidProfile->dterm_average_count) delta = filterApplyAveragef(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2;
 
             DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f);
+
+            // Dynamic D Implementation
+            if (pidProfile->dynamic_dterm_threshold) {
+                dynamicDTermGain = constrainf(ABS(DTerm) / pidProfile->dynamic_dterm_threshold, 0.0f, 1.0f);
+                DTerm *= dynamicDTermGain;
+            }
         }
 
         // -----calculate total PID output
@@ -283,6 +292,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
     static int32_t lastErrorForDelta[3] = { 0, 0, 0 };
     static int32_t deltaState[3][DELTA_MAX_SAMPLES];
     int32_t AngleRateTmp, RateError, gyroRate;
+    int32_t dynamicDTermGain;
 
     int8_t horizonLevelStrength = 100;
 
@@ -389,15 +399,23 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
 
             // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
             // would be scaled by different dt each time. Division by dT fixes that.
-            delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 6;
+            delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5;
+
+            // Several different Dterm filtering methods to prevent noise. All of them can be combined
 
             // Filter delta
             if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT());
 
             // Apply moving average
-            if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]) * 2;
+            if (pidProfile->dterm_average_count) delta = filterApplyAverage(delta, pidProfile->dterm_average_count, deltaState[axis]);
 
             DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
+
+            // Dynamic D Implementation
+            if (pidProfile->dynamic_dterm_threshold) {
+                dynamicDTermGain = constrain((ABS(DTerm) << 7) / pidProfile->dynamic_dterm_threshold, 0, 1 << 7);
+                DTerm = (DTerm * dynamicDTermGain) >> 7;
+            }
         }
 
         // -----calculate total PID output

src/main/flight/pid.h

@@ -71,6 +71,7 @@ typedef struct pidProfile_s {
     uint8_t deltaMethod;                    // Alternative delta Calculation
     uint16_t yaw_p_limit;
     uint8_t dterm_average_count;            // Configurable delta count for dterm
+    uint8_t dynamic_dterm_threshold;
 
 #ifdef GTUNE
     uint8_t  gtune_lolimP[3];               // [0..200] Lower limit of P during G tune

src/main/io/serial_cli.c

@@ -722,6 +722,7 @@ const clivalue_t valueTable[] = {
     { "pid_delta_method",           VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } },
     { "dterm_lowpass_hz",           VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
     { "dterm_average_count",        VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {0, 12 } },
+    { "dynamic_dterm_threshold",    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dynamic_dterm_threshold, .config.minmax = {0, 100 } },
     { "iterm_reset_degrees",        VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } },
     { "yaw_iterm_reset_degrees",    VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } },
     { "yaw_lowpass_hz",             VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },

src/main/version.h

@@ -17,7 +17,7 @@
 
 #define FC_VERSION_MAJOR            2  // increment when a major release is made (big new feature, etc)
 #define FC_VERSION_MINOR            6  // increment when a minor release is made (small new feature, change etc)
-#define FC_VERSION_PATCH_LEVEL      1  // increment when a bug is fixed
+#define FC_VERSION_PATCH_LEVEL      2  // increment when a bug is fixed
 
 #define STR_HELPER(x) #x
 #define STR(x) STR_HELPER(x)