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

Code improbements, thanks for the feedback!

Browse source
This commit is contained in:
ctzsnooze 2020-09-14 11:08:22 +10:00
parent d1bf5d0d11
commit 6a143beb6f
1 changed files with 6 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -271,8 +271,7 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
{
if (pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) {
// 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);
const float antiGravityThrottleLpf = pt1FilterApply(&pidRuntime.antiGravityThrottleLpf, throttle);
// focus P boost on low throttle range only
if (throttle < 0.5f) {
pidRuntime.antiGravityPBoost = 0.5f - throttle;
@ -280,11 +279,11 @@ void pidUpdateAntiGravityThrottleFilter(float throttle)
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) {
if (antiGravityThrottleLpf < throttle) {
pidRuntime.antiGravityPBoost *= 0.5f;
}
// high-passed throttle focuses boost on faster throttle changes
pidRuntime.antiGravityThrottleHpf = fabsf(throttle - pidRuntime.antiGravityThrottleHpf);
pidRuntime.antiGravityThrottleHpf = fabsf(throttle - antiGravityThrottleLpf);
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);
@ -842,7 +841,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
if ((pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) && pidRuntime.antiGravityEnabled) {
// traditional itermAccelerator factor for iTerm
pidRuntime.itermAccelerator = pidRuntime.antiGravityThrottleHpf * 0.01f * pidRuntime.itermAcceleratorGain;
DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(pidRuntime.itermAccelerator * 1000));
DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, 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
@ -852,7 +851,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
} else {
pidRuntime.antiGravityPBoost = 0.0f;
}
DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(pidRuntime.itermAccelerator * 1000));
DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(pidRuntime.itermAccelerator * 1000));
float agGain = pidRuntime.dT * pidRuntime.itermAccelerator * AG_KI;
@ -1147,9 +1146,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// 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;
}
agBoostAttenuator = MAX(agBoostAttenuator, 1.0f);
const float agBoost = 1.0f + (pidRuntime.antiGravityPBoost / agBoostAttenuator);
pidData[axis].P *= agBoost;
if (axis == FD_ROLL){