From d1bf5d0d11f67bb4974a0e4de89a1a2be564436f Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Sat, 5 Sep 2020 21:52:19 +1000 Subject: [PATCH] Add late P boost to antigravity --- src/main/flight/pid.c | 50 ++++++++++++++++++++++++++++++++++++++++++---- src/main/flight/pid.h | 2 ++ src/main/flight/pid_init.c | 4 +++- 3 files changed, 51 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 312ce8bd2..c6f1ba3b5 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -270,7 +270,24 @@ void pidResetIterm(void) void pidUpdateAntiGravityThrottleFilter(float throttle) { if (pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) { - pidRuntime.antiGravityThrottleHpf = throttle - pt1FilterApply(&pidRuntime.antiGravityThrottleLpf, throttle); + // calculate a boost factor for P in the same way as for I when throttle changes quickly + // at this point the variable antiGravityThrottleHpf is the lowpass of throttle + pidRuntime.antiGravityThrottleHpf = pt1FilterApply(&pidRuntime.antiGravityThrottleLpf, throttle); + // focus P boost on low throttle range only + if (throttle < 0.5f) { + pidRuntime.antiGravityPBoost = 0.5f - throttle; + } else { + pidRuntime.antiGravityPBoost = 0.0f; + } + // use lowpass to identify start of a throttle up, use this to reduce boost at start by half + if (pidRuntime.antiGravityThrottleHpf < throttle) { + pidRuntime.antiGravityPBoost *= 0.5f; + } + // high-passed throttle focuses boost on faster throttle changes + pidRuntime.antiGravityThrottleHpf = fabsf(throttle - pidRuntime.antiGravityThrottleHpf); + pidRuntime.antiGravityPBoost = pidRuntime.antiGravityPBoost * pidRuntime.antiGravityThrottleHpf; + // smooth the P boost at 3hz to remove the jagged edges and prolong the effect after throttle stops + pidRuntime.antiGravityPBoost = pt1FilterApply(&pidRuntime.antiGravitySmoothLpf, pidRuntime.antiGravityPBoost); } } @@ -823,10 +840,19 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim // Dynamic i component, if ((pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) && pidRuntime.antiGravityEnabled) { - pidRuntime.itermAccelerator = fabsf(pidRuntime.antiGravityThrottleHpf) * 0.01f * (pidRuntime.itermAcceleratorGain - 1000); - DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(pidRuntime.antiGravityThrottleHpf * 1000)); + // traditional itermAccelerator factor for iTerm + pidRuntime.itermAccelerator = pidRuntime.antiGravityThrottleHpf * 0.01f * pidRuntime.itermAcceleratorGain; + DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(pidRuntime.itermAccelerator * 1000)); + // users AG Gain changes P boost + pidRuntime.antiGravityPBoost *= pidRuntime.itermAcceleratorGain; + // add some percentage of that slower, longer acting P boost factor to prolong AG effect on iTerm + pidRuntime.itermAccelerator += pidRuntime.antiGravityPBoost * 0.05f; + // set the final P boost amount + pidRuntime.antiGravityPBoost *= 0.02f; + } else { + pidRuntime.antiGravityPBoost = 0.0f; } - DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(pidRuntime.itermAccelerator * 1000)); + DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(pidRuntime.itermAccelerator * 1000)); float agGain = pidRuntime.dT * pidRuntime.itermAccelerator * AG_KI; @@ -1117,6 +1143,22 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim } #endif // calculating the PID sum + + // P boost at the end of throttle chop + // attenuate effect if turning more than 50 deg/s, half at 100 deg/s + float agBoostAttenuator = fabsf(currentPidSetpoint) / 50.0f; + if (agBoostAttenuator < 1.0f) { + agBoostAttenuator = 1.0f; + } + const float agBoost = 1.0f + (pidRuntime.antiGravityPBoost / agBoostAttenuator); + pidData[axis].P *= agBoost; + if (axis == FD_ROLL){ + DEBUG_SET(DEBUG_ANTI_GRAVITY, 2, lrintf(agBoost * 1000)); + } + if (axis == FD_PITCH){ + DEBUG_SET(DEBUG_ANTI_GRAVITY, 3, lrintf(agBoost * 1000)); + } + const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F; #ifdef USE_INTEGRATED_YAW_CONTROL if (axis == FD_YAW && pidRuntime.useIntegratedYaw) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index b2a7a3084..550623082 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -253,8 +253,10 @@ typedef struct pidRuntime_s { bool antiGravityEnabled; uint8_t antiGravityMode; pt1Filter_t antiGravityThrottleLpf; + pt1Filter_t antiGravitySmoothLpf; float antiGravityOsdCutoff; float antiGravityThrottleHpf; + float antiGravityPBoost; float ffBoostFactor; float itermAccelerator; uint16_t itermAcceleratorGain; diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index fb624797f..7685b56e9 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -54,6 +54,7 @@ #endif #define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff +#define ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF 3 // The anti gravity P smoothing filter cutoff static void pidSetTargetLooptime(uint32_t pidLooptime) { @@ -202,6 +203,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) #endif pt1FilterInit(&pidRuntime.antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, pidRuntime.dT)); + pt1FilterInit(&pidRuntime.antiGravitySmoothLpf, pt1FilterGain(ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF, pidRuntime.dT)); pidRuntime.ffBoostFactor = (float)pidProfile->ff_boost / 10.0f; } @@ -309,7 +311,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) // of AG activity without excessive display. pidRuntime.antiGravityOsdCutoff = 0.0f; if (pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) { - pidRuntime.antiGravityOsdCutoff += ((pidRuntime.itermAcceleratorGain - 1000) / 1000.0f) * 0.25f; + pidRuntime.antiGravityOsdCutoff += (pidRuntime.itermAcceleratorGain / 1000.0f) * 0.25f; } #if defined(USE_ITERM_RELAX) -- 2.11.4.GIT