DTERM_CUT

This code cuts D by a specified percentage durning normal flight.

It lets D smoothly rise up to normal during rapid gyro moves like flips and rolls, and increase during prop wash events.

D should now be tuned to values the 'normal' 30-45 range.

If D is 40, a dterm_cut_percentage of 65 will cut D to 14 in normal flight, but the quad will still get full 40 of D to control bounce-back after flips and about 25 of D during strong prop wash.

The dterm_cut_percentage can be adjusted via the OSD, from the D filtering page.
Adding d_cut results in cooler motors, lower amounts of noise in motor traces and faster reactions to quick stick inputs.

Too high a dterm_cut_percentage may bring out P oscillation from lack of D.  Values of 70% are generally OK.

Input is gyro differential (delta).  Frequencies above 40hz (above propwash) are attenuated with a configurable (dterm_cut_range_hz) biquad filter.  Lower values for range can be used if the quad is very noisy or gets low frequency D resonant oscillation.  Up to 50 or 60hz may suit clean quads where prop wash control is the main priority.  Too high a range value results in D being boosted from noise in normal flight.

The boost signal is 'integrated, smoothed and delayed' with a 7hz PT1 'dterm_cut_lowpass_hz' filter.  The default of 7Hz gives about the right amount of smoothing and delay.  Higher numbers cause the boost to come on faster, with less delay. Lower values delay the boost effect and cause it to last longer.

The dterm_cut_gain amount controls the strength of the boost effect by amplifying the input to the boosting effect.  If the quad is flow gently, a higher gain value may be needed to gain full boost.

Logging with set debug_mode = D_CUT allows recording of realtime D values on roll and pitch into debug 2 and 3.  The reatime D value should reach its set maximum during rapid turns, ideally at about the time D itself peaks.  If it fails to reach the maximum, gain should be increased.

The D_Cut feature is not enabled on LUXV2RACE, OMNIBUS, SPRACINGF3NEO because there isn't enough flash space.

src/main/build/debug.c

@@ -80,4 +80,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "RX_SPEKTRUM_SPI",
     "DSHOT_TELEMETRY",
     "RPM_FILTER",
+    "D_CUT",
 };

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;
 

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 }
 };

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_ROLL] =  { 46, 65, 40, 60 },
-            [PID_PITCH] = { 50, 75, 32, 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;
         }

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);

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) },

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

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

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

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