diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 5516fbc9d2..9ad377bba2 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -139,6 +139,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .throttle_boost = 0, .throttle_boost_cutoff = 15, .iterm_rotation = false, + .smart_feedforward = false ); } @@ -308,6 +309,7 @@ static FAST_RAM_ZERO_INIT float itermLimit; FAST_RAM_ZERO_INIT float throttleBoost; pt1Filter_t throttleLpf; static FAST_RAM_ZERO_INIT bool itermRotation; +static FAST_RAM_ZERO_INIT bool smartFeedforward; void pidInitConfig(const pidProfile_t *pidProfile) { @@ -344,6 +346,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) itermLimit = pidProfile->itermLimit; throttleBoost = pidProfile->throttle_boost * 0.1f; itermRotation = pidProfile->iterm_rotation == 1; + smartFeedforward = pidProfile->smart_feedforward == 1; } void pidInit(const pidProfile_t *pidProfile) @@ -641,12 +644,15 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an const float pidFeedForward = pidCoefficient[axis].Kd * dynCd * transition * (currentPidSetpoint - previousPidSetpoint[axis]) * tpaFactor / dT; - if (pidData[axis].P * pidFeedForward > 0) { - if (ABS(pidFeedForward) > ABS(pidData[axis].P)) { - pidData[axis].P = 0; - pidData[axis].D += pidFeedForward; + bool addFeedforward = true; + if (smartFeedforward) + if (pidData[axis].P * pidFeedForward > 0) { + if (ABS(pidFeedForward) > ABS(pidData[axis].P)) { + pidData[axis].P = 0; + } + else addFeedforward = false; } - } + if (addFeedforward) pidData[axis].D += pidFeedForward; #ifdef USE_YAW_SPIN_RECOVERY if (yawSpinActive) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 3f4cd89bfc..21d84edc78 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -115,6 +115,7 @@ typedef struct pidProfile_s { uint8_t throttle_boost; // how much should throttle be boosted during transient changes 0-100, 100 adds 10x hpf filtered throttle uint8_t throttle_boost_cutoff; // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz. uint8_t iterm_rotation; // rotates iterm to translate world errors to local coordinate system + uint8_t smart_feedforward; } pidProfile_t; #ifndef USE_OSD_SLAVE diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 973ed31597..5ec31f1552 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -752,6 +752,7 @@ const clivalue_t valueTable[] = { { "crash_recovery", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRASH_RECOVERY }, PG_PID_PROFILE, offsetof(pidProfile_t, crash_recovery) }, { "iterm_rotation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_rotation) }, + { "smart_feedforward", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, smart_feedforward) }, { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) }, { "iterm_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermLimit) }, { "pidsum_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { PIDSUM_LIMIT_MIN, PIDSUM_LIMIT_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, pidSumLimit) },