From 7e3e5649e186599fcdd276b8498e37a4a0a17f13 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Sat, 12 Jan 2019 21:09:03 -0700 Subject: [PATCH] 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 | 1 + src/main/build/debug.h | 1 + src/main/cms/cms_menu_imu.c | 13 ++++- src/main/flight/pid.c | 69 ++++++++++++++++++++++++-- src/main/flight/pid.h | 4 ++ src/main/interface/settings.c | 7 +++ src/main/target/LUX_RACE/target.h | 1 + src/main/target/OMNIBUS/target.h | 1 + src/main/target/SPRACINGF3NEO/target.h | 1 + src/main/target/common_pre.h | 1 + 10 files changed, 94 insertions(+), 5 deletions(-) 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