rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add smart_feedforward config setting

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
Show Stats 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) {

1
src/main/flight/pid.h
View File

@ -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

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

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