betaflight · mikeller · Jan 13, 2019 · Jan 13, 2019
diff --git a/src/main/build/debug.c b/src/main/build/debug.c
@@ -80,4 +80,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "RX_SPEKTRUM_SPI",
     "DSHOT_TELEMETRY",
     "RPM_FILTER",
+    "D_CUT",
 };
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
@@ -98,6 +98,7 @@ typedef enum {
     DEBUG_RX_SPEKTRUM_SPI,
     DEBUG_RPM_TELEMETRY,
     DEBUG_RPM_FILTER,
+    DEBUG_D_CUT,
     DEBUG_COUNT
 } debugType_e;
 

diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
@@ -548,6 +548,9 @@ static uint16_t cmsx_dterm_lowpass_hz;
 static uint16_t cmsx_dterm_lowpass2_hz;
 static uint16_t cmsx_dterm_notch_hz;
 static uint16_t cmsx_dterm_notch_cutoff;
+#ifdef USE_D_CUT
+static uint8_t  cmsx_dterm_cut_percent;
+#endif
 static uint16_t cmsx_yaw_lowpass_hz;
 
 static long cmsx_FilterPerProfileRead(void)
@@ -558,6 +561,9 @@ static long cmsx_FilterPerProfileRead(void)
     cmsx_dterm_lowpass2_hz  = pidProfile->dterm_lowpass2_hz;
     cmsx_dterm_notch_hz     = pidProfile->dterm_notch_hz;
     cmsx_dterm_notch_cutoff = pidProfile->dterm_notch_cutoff;
+#ifdef USE_D_CUT
+    cmsx_dterm_cut_percent  = pidProfile->dterm_cut_percent;
+#endif
     cmsx_yaw_lowpass_hz     = pidProfile->yaw_lowpass_hz;
 
     return 0;
@@ -573,6 +579,9 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self)
     pidProfile->dterm_lowpass2_hz  = cmsx_dterm_lowpass2_hz;
     pidProfile->dterm_notch_hz     = cmsx_dterm_notch_hz;
     pidProfile->dterm_notch_cutoff = cmsx_dterm_notch_cutoff;
+#ifdef USE_D_CUT
+    pidProfile->dterm_cut_percent  = cmsx_dterm_cut_percent;
+#endif
     pidProfile->yaw_lowpass_hz     = cmsx_yaw_lowpass_hz;
 
     return 0;
@@ -586,8 +595,10 @@ static OSD_Entry cmsx_menuFilterPerProfileEntries[] =
     { "DTERM LPF2", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz,    0, 500, 1 }, 0 },
     { "DTERM NF",   OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz,       0, 500, 1 }, 0 },
     { "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff,   0, 500, 1 }, 0 },
+#ifdef USE_D_CUT
+    { "DTERM CUT",  OME_UINT8,  NULL, &(OSD_UINT8_t) { &cmsx_dterm_cut_percent,    0, 100, 1 }, 0 },
+#endif
     { "YAW LPF",    OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lowpass_hz,       0, 500, 1 }, 0 },
-
     { "BACK", OME_Back, NULL, NULL, 0 },
     { NULL, OME_END, NULL, NULL, 0 }
 };

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
@@ -90,6 +90,9 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
 #else
 #define PID_PROCESS_DENOM_DEFAULT       2
 #endif
+#if defined(USE_D_CUT)
+#define D_CUT_GAIN_FACTOR 0.00005f
+#endif
 
 #ifdef USE_RUNAWAY_TAKEOFF
 PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
@@ -115,14 +118,14 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
 
 #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
 
-PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 7);
+PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 8);
 
 void resetPidProfile(pidProfile_t *pidProfile)
 {
     RESET_CONFIG(pidProfile_t, pidProfile,
         .pid = {
-            [PID_ROLL] =  { 46, 65, 30, 60 },
-            [PID_PITCH] = { 50, 75, 32, 60 },
+            [PID_ROLL] =  { 46, 65, 40, 60 },
+            [PID_PITCH] = { 50, 75, 44, 60 },
             [PID_YAW] =   { 45, 100, 0, 100 },
             [PID_LEVEL] = { 50, 50, 75, 0 },
             [PID_MAG] =   { 40, 0, 0, 0 },
@@ -183,6 +186,10 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .use_integrated_yaw = false,
         .integrated_yaw_relax = 200,
         .thrustLinearization = 0,
+        .dterm_cut_percent = 65,
+        .dterm_cut_gain = 15,
+        .dterm_cut_range_hz = 40,
+        .dterm_cut_lowpass_hz = 7,
     );
 #ifdef USE_DYN_LPF
     pidProfile->dterm_lowpass_hz = 150;
@@ -238,12 +245,14 @@ static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn;
 static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass2[XYZ_AXIS_COUNT];
 static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn;
 static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass;
+
 #if defined(USE_ITERM_RELAX)
 static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
 static FAST_RAM_ZERO_INIT uint8_t itermRelax;
 static FAST_RAM_ZERO_INIT uint8_t itermRelaxType;
 static FAST_RAM_ZERO_INIT uint8_t itermRelaxCutoff;
 #endif
+
 #if defined(USE_ABSOLUTE_CONTROL)
 STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT float axisError[XYZ_AXIS_COUNT];
 static FAST_RAM_ZERO_INIT float acGain;
@@ -253,6 +262,12 @@ static FAST_RAM_ZERO_INIT float acCutoff;
 static FAST_RAM_ZERO_INIT pt1Filter_t acLpf[XYZ_AXIS_COUNT];
 #endif
 
+#if defined(USE_D_CUT)
+static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermCutRangeApplyFn; 
+static FAST_RAM_ZERO_INIT biquadFilter_t dtermCutRange[XYZ_AXIS_COUNT];
+static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermCutLowpassApplyFn; 
+static FAST_RAM_ZERO_INIT pt1Filter_t dtermCutLowpass[XYZ_AXIS_COUNT];
+#endif
 
 #ifdef USE_RC_SMOOTHING_FILTER
 static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
@@ -373,6 +388,19 @@ void pidInitFilters(const pidProfile_t *pidProfile)
         }
     }
 #endif
+#if defined(USE_D_CUT)
+    if (pidProfile->dterm_cut_percent == 0) {
+        dtermCutRangeApplyFn = nullFilterApply;
+        dtermCutLowpassApplyFn = nullFilterApply;
+    } else {
+        dtermCutRangeApplyFn = (filterApplyFnPtr)biquadFilterApply;
+        dtermCutLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
+        for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+            biquadFilterInitLPF(&dtermCutRange[axis], pidProfile->dterm_cut_range_hz, targetPidLooptime);
+            pt1FilterInit(&dtermCutLowpass[axis], pt1FilterGain(pidProfile->dterm_cut_lowpass_hz, dT));
+        }
+    }
+#endif
 
     pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
 }
@@ -495,6 +523,15 @@ static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
 static FAST_RAM_ZERO_INIT uint16_t dynLpfMax;
 #endif
 
+#ifdef USE_D_CUT
+static FAST_RAM_ZERO_INIT float dtermCutPercent;
+static FAST_RAM_ZERO_INIT float dtermCutPercentInv;
+static FAST_RAM_ZERO_INIT float dtermCutGain;
+static FAST_RAM_ZERO_INIT float dtermCutRangeHz;
+static FAST_RAM_ZERO_INIT float dtermCutLowpassHz;
+#endif
+static FAST_RAM_ZERO_INIT float dtermCutFactor;
+
 void pidInitConfig(const pidProfile_t *pidProfile)
 {
     if (pidProfile->feedForwardTransition == 0) {
@@ -613,6 +650,15 @@ void pidInitConfig(const pidProfile_t *pidProfile)
         thrustLinearizationB = (1.0f - thrustLinearization) / (2.0f * thrustLinearization);
     }
 #endif    
+#if defined(USE_D_CUT)
+    dtermCutPercent = pidProfile->dterm_cut_percent / 100.0f;
+    dtermCutPercentInv = 1.0f - dtermCutPercent;
+    dtermCutRangeHz = pidProfile->dterm_cut_range_hz;
+    dtermCutLowpassHz = pidProfile->dterm_cut_lowpass_hz;
+    dtermCutGain = pidProfile->dterm_cut_gain * dtermCutPercent * D_CUT_GAIN_FACTOR / dtermCutLowpassHz;
+    // lowpass included inversely in gain since stronger lowpass decreases peak effect
+#endif
+    dtermCutFactor = 1.0f;
 }
 
 void pidInit(const pidProfile_t *pidProfile)
@@ -1240,8 +1286,23 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
             if (cmpTimeUs(currentTimeUs, levelModeStartTimeUs) > CRASH_RECOVERY_DETECTION_DELAY_US) {
                 detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate);
             }
+#if defined(USE_D_CUT)
+            if (dtermCutPercent){
+                dtermCutFactor = dtermCutRangeApplyFn((filter_t *) &dtermCutRange[axis], delta);
+                dtermCutFactor = fabsf(dtermCutFactor) * dtermCutGain;
+                dtermCutFactor = dtermCutLowpassApplyFn((filter_t *) &dtermCutLowpass[axis], dtermCutFactor);
+                dtermCutFactor = MIN(dtermCutFactor, 1.0f);
+                dtermCutFactor = dtermCutPercentInv + (dtermCutFactor * dtermCutPercent);
+            }
+            if (axis == FD_ROLL) {
+                DEBUG_SET(DEBUG_D_CUT, 2, lrintf(pidCoefficient[axis].Kd * dtermCutFactor * 10.0f / DTERM_SCALE));
+            } else if (axis == FD_PITCH) {
+                DEBUG_SET(DEBUG_D_CUT, 3, lrintf(pidCoefficient[axis].Kd * dtermCutFactor * 10.0f / DTERM_SCALE));
+            }
+#endif
+
+            pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor * dtermCutFactor;
 
-            pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor;
         } else {
             pidData[axis].D = 0;
         }

diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
@@ -160,6 +160,10 @@ typedef struct pidProfile_s {
     uint8_t use_integrated_yaw;             // Selects whether the yaw pidsum should integrated
     uint8_t integrated_yaw_relax;           // Specifies how much integrated yaw should be reduced to offset the drag based yaw component
     uint8_t thrustLinearization;            // Compensation factor for pid linearization
+    uint8_t dterm_cut_percent;              // Amount to cut D by with no gyro activity, zero disables, 20 means cut 20%, 50 means cut 50%
+    uint8_t dterm_cut_gain;                 // Gain factor for amount of gyro activity required to remove the dterm cut
+    uint8_t dterm_cut_range_hz;             // Biquad to prevent high frequency gyro noise from removing the dterm cut
+    uint8_t dterm_cut_lowpass_hz;           // First order lowpass to delay and smooth dterm cut factor
 } pidProfile_t;
 
 PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);

diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
@@ -962,6 +962,13 @@ const clivalue_t valueTable[] = {
     { "integrated_yaw_relax",    VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, integrated_yaw_relax) },
 #endif
 
+#ifdef USE_D_CUT
+    { "dterm_cut_percent",          VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_cut_percent) },
+    { "dterm_cut_gain",             VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_cut_gain) },
+    { "dterm_cut_range_hz",         VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 10, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_cut_range_hz) },
+    { "dterm_cut_lowpass_hz",       VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 1, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_cut_lowpass_hz) },
+#endif
+
 #ifdef USE_LAUNCH_CONTROL
     { "launch_control_mode",        VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LAUNCH_CONTROL_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlMode) },
     { "launch_trigger_allow_reset", VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlAllowTriggerReset) },

diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h
@@ -51,6 +51,7 @@
 
 //#undef USE_BOARD_INFO
 #undef USE_EXTENDED_CMS_MENUS
+#undef USE_D_CUT
 //#undef USE_RTC_TIME
 #undef USE_RX_MSP
 //#undef USE_ESC_SENSOR_INFO

diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h
@@ -31,6 +31,7 @@
 
 #undef USE_RC_SMOOTHING_FILTER
 #undef USE_DYN_LPF
+#undef USE_D_CUT
 
 #undef USE_ITERM_RELAX
 #undef USE_RC_SMOOTHING_FILTER

diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h
@@ -37,6 +37,7 @@
 #undef USE_ITERM_RELAX
 #undef USE_RC_SMOOTHING_FILTER
 #undef USE_THRUST_LINEARIZATION
+#undef USE_D_CUT
 
 #undef USE_HUFFMAN
 #undef USE_PINIO

diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h
@@ -202,6 +202,7 @@
 #define USE_DYN_LPF
 #define USE_INTEGRATED_YAW_CONTROL
 #define USE_THRUST_LINEARIZATION
+#define USE_D_CUT
 
 #ifdef USE_SERIALRX_SPEKTRUM
 #define USE_SPEKTRUM_BIND