From fde2aae4f5b6311dd7a3b918524fb7da5a9630fd Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Thu, 24 May 2018 14:08:52 +0200 Subject: [PATCH 1/9] If P and FF have same sign take larger one --- src/main/flight/pid.c | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f3ed603b28..40e06a8498 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -627,9 +627,8 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an // This is done to avoid DTerm spikes that occur with dynamically // calculated deltaT whenever another task causes the PID // loop execution to be delayed. - const float delta = ( - dynCd * transition * (currentPidSetpoint - previousPidSetpoint[axis]) - - (gyroRateDterm[axis] - previousGyroRateDterm[axis])) / dT; + const float delta = + - (gyroRateDterm[axis] - previousGyroRateDterm[axis]) / dT; previousPidSetpoint[axis] = currentPidSetpoint; previousGyroRateDterm[axis] = gyroRateDterm[axis]; @@ -638,6 +637,17 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor; + + const float pid_ff = + pidCoefficient[axis].Kd * dynCd * transition * + (currentPidSetpoint - previousPidSetpoint[axis]) * tpaFactor / dT; + if ((pidData[axis].P > 0) == (pid_ff > 0)) { + if (ABS(pid_ff) > ABS(pidData[axis].P)) { + pidData[axis].P = 0; + pidData[axis].D += pid_ff; + } + } + #ifdef USE_YAW_SPIN_RECOVERY if (yawSpinActive) { // zero PIDs on pitch and roll leaving yaw P to correct spin @@ -649,6 +659,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an } } + // calculating the PID sum pidData[FD_ROLL].Sum = pidData[FD_ROLL].P + pidData[FD_ROLL].I + pidData[FD_ROLL].D; pidData[FD_PITCH].Sum = pidData[FD_PITCH].P + pidData[FD_PITCH].I + pidData[FD_PITCH].D; From dc452ced0041c1ebda216af676fa257b9346e503 Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Thu, 24 May 2018 15:26:08 +0200 Subject: [PATCH 2/9] rename pid_ff --- src/main/flight/pid.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 40e06a8498..5d2f6367bb 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -638,13 +638,13 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor; - const float pid_ff = + const float pidFeedForward = pidCoefficient[axis].Kd * dynCd * transition * (currentPidSetpoint - previousPidSetpoint[axis]) * tpaFactor / dT; - if ((pidData[axis].P > 0) == (pid_ff > 0)) { - if (ABS(pid_ff) > ABS(pidData[axis].P)) { + if ((pidData[axis].P > 0) == (pidFeedForward > 0)) { + if (ABS(pidFeedForward) > ABS(pidData[axis].P)) { pidData[axis].P = 0; - pidData[axis].D += pid_ff; + pidData[axis].D += pidFeedForward; } } From 25cc445d7e35d58b4f38ff1bb9c1bf73e3e9cc7d Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Thu, 24 May 2018 15:30:55 +0200 Subject: [PATCH 3/9] switch micro-optimizations --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 5d2f6367bb..640597b3c1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -641,7 +641,7 @@ 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 > 0) == (pidFeedForward > 0)) { + if (pidData[axis].P * pidFeedForward > 0) { if (ABS(pidFeedForward) > ABS(pidData[axis].P)) { pidData[axis].P = 0; pidData[axis].D += pidFeedForward; From 82edaaaf8a2db1b71731c6a3723ef7e4ad5e4d33 Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Thu, 24 May 2018 15:31:45 +0200 Subject: [PATCH 4/9] remove newline --- src/main/flight/pid.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 640597b3c1..5516fbc9d2 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -659,7 +659,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an } } - // calculating the PID sum pidData[FD_ROLL].Sum = pidData[FD_ROLL].P + pidData[FD_ROLL].I + pidData[FD_ROLL].D; pidData[FD_PITCH].Sum = pidData[FD_PITCH].P + pidData[FD_PITCH].I + pidData[FD_PITCH].D; From 5aad57c3a7f937e5b9f66e507eee9a4d5939a50c Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Thu, 24 May 2018 21:55:49 +0200 Subject: [PATCH 5/9] add smart_feedforward config setting --- src/main/flight/pid.c | 16 +++++++++++----- src/main/flight/pid.h | 1 + src/main/interface/settings.c | 1 + 3 files changed, 13 insertions(+), 5 deletions(-) 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) }, From af5fde98c1edfabe8ae60466286097b8f447221c Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Fri, 25 May 2018 07:12:12 +0200 Subject: [PATCH 6/9] address style requests --- src/main/flight/pid.c | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9ad377bba2..eea2cf7b61 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -640,19 +640,23 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor; - const float pidFeedForward = pidCoefficient[axis].Kd * dynCd * transition * (currentPidSetpoint - previousPidSetpoint[axis]) * tpaFactor / dT; bool addFeedforward = true; - if (smartFeedforward) + if (smartFeedforward) { if (pidData[axis].P * pidFeedForward > 0) { if (ABS(pidFeedForward) > ABS(pidData[axis].P)) { pidData[axis].P = 0; } - else addFeedforward = false; + else { + addFeedforward = false; + } } - if (addFeedforward) pidData[axis].D += pidFeedForward; + } + if (addFeedforward) { + pidData[axis].D += pidFeedForward; + } #ifdef USE_YAW_SPIN_RECOVERY if (yawSpinActive) { From d337023e87d0c0ed083bf1190b6c025288120bb4 Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Sun, 27 May 2018 07:44:11 +0200 Subject: [PATCH 7/9] assign previousGyroRateDterm after calculating ff --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index eea2cf7b61..7ee9872d63 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -634,7 +634,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an - (gyroRateDterm[axis] - previousGyroRateDterm[axis]) / dT; previousPidSetpoint[axis] = currentPidSetpoint; - previousGyroRateDterm[axis] = gyroRateDterm[axis]; detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate); @@ -657,6 +656,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an if (addFeedforward) { pidData[axis].D += pidFeedForward; } + previousGyroRateDterm[axis] = gyroRateDterm[axis]; #ifdef USE_YAW_SPIN_RECOVERY if (yawSpinActive) { From 62dcd1b2fac253e160d0f2351555386cc1f83819 Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Sun, 27 May 2018 07:47:51 +0200 Subject: [PATCH 8/9] Also move previousPidSetpoint --- src/main/flight/pid.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 7ee9872d63..06dc4d5bc4 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -633,8 +633,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an const float delta = - (gyroRateDterm[axis] - previousGyroRateDterm[axis]) / dT; - previousPidSetpoint[axis] = currentPidSetpoint; - detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate); pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor; @@ -657,6 +655,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an pidData[axis].D += pidFeedForward; } previousGyroRateDterm[axis] = gyroRateDterm[axis]; + previousPidSetpoint[axis] = currentPidSetpoint; #ifdef USE_YAW_SPIN_RECOVERY if (yawSpinActive) { From 03b5c3c872252868560384d53f8bd8f0ac8f0abb Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Sun, 27 May 2018 08:04:27 +0200 Subject: [PATCH 9/9] add description --- src/main/flight/pid.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 21d84edc78..81705db067 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -114,8 +114,8 @@ typedef struct pidProfile_s { uint16_t dterm_lowpass2_hz; // Extra PT1 Filter on D in hz 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; + uint8_t iterm_rotation; // rotates iterm to translate world errors to local coordinate system + uint8_t smart_feedforward; // takes only the larger of P and the D weight feed forward term if they have the same sign. } pidProfile_t; #ifndef USE_OSD_SLAVE