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

add smart_feedforward config setting

Browse source
This commit is contained in:
Thorsten Laux 2018-05-24 21:55:49 +02:00
parent 82edaaaf8a
commit 5aad57c3a7
3 changed files with 13 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

16
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)
@ -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) {