diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 6acb6813f5..e79c9e5c7f 100644 --- 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 index 3159e7a187..92470fd0a6 100644 --- 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 index 02b62671fd..f09a346d74 100644 --- 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 index 0b1185cfad..a58894855d 100644 --- 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 index e653bab540..4c7286216a 100644 --- 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 index 83ca2c0ee9..2ae70a8709 100644 --- 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 index 7b0384537a..cb1dcdc6d7 100644 --- 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 index 9507212faa..bc4255fb18 100644 --- 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 index 80ae31983b..f8e31f1da3 100644 --- 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 index 01005dc3a1..5cd7978d12 100644 --- 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