Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 06:15:16 +03:00
Code Issues Wiki Activity

Merge pull request #5962 from joelucid/pid_ff

Browse source 
If P and FF have same sign take larger one
This commit is contained in:
Michael Keller 2018-05-27 21:36:23 +12:00 • committed by GitHub
commit 5c3599a028
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 28 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

31
src/main/flight/pid.c
View file

@ -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)
@ -627,17 +630,33 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
// 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;
previousPidSetpoint[axis] = currentPidSetpoint;
previousGyroRateDterm[axis] = gyroRateDterm[axis];
const float delta =
- (gyroRateDterm[axis] - previousGyroRateDterm[axis]) / dT;
detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate);
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 (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;
}
previousGyroRateDterm[axis] = gyroRateDterm[axis];
previousPidSetpoint[axis] = currentPidSetpoint;
#ifdef USE_YAW_SPIN_RECOVERY
if (yawSpinActive) {
// zero PIDs on pitch and roll leaving yaw P to correct spin

3
src/main/flight/pid.h
View file

@ -114,7 +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 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

1
src/main/interface/settings.c
View file

@ -754,6 +754,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) },