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

update antigravity for 4.4, no boost on yaw

Browse source 
Update to anti-gravity, including removal of the old Step mode, ability to adjust the P contribution (thanks @Limon), PT2 smoothed derivative model, inherent limiting of P boost during extremely fast stick travels to minimise P oscillations, less I during the middle of a throttle up, no I boost on yaw, add hz to cutoff labels

No antigravity on yaw, fix longstanding typo

h
This commit is contained in:
ctzsnooze 2022-06-20 08:55:31 +10:00
parent 4fa4d4851e
commit 6aaaf727ff
13 changed files with 92 additions and 160 deletions
Download patch file Download diff file Expand all files Collapse all files

23
src/main/flight/pid_init.c
View file

@ -52,8 +52,6 @@
#define D_MIN_SETPOINT_GAIN_FACTOR 0.00008f
#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
#define ATTITUDE_CUTOFF_HZ 250
static void pidSetTargetLooptime(uint32_t pidLooptime)
@ -237,15 +235,14 @@ 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));
#ifdef USE_ACC
const float k = pt3FilterGain(ATTITUDE_CUTOFF_HZ, pidRuntime.dT);
for (int axis = 0; axis < 2; axis++) { // ROLL and PITCH only
pt3FilterInit(&pidRuntime.attitudeFilter[axis], k);
}
#endif
pt2FilterInit(&pidRuntime.antiGravityLpf, pt2FilterGain(pidProfile->anti_gravity_cutoff_hz, pidRuntime.dT));
}
void pidInit(const pidProfile_t *pidProfile)
@ -307,7 +304,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f;
pidRuntime.itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint);
}
pidRuntime.itermAcceleratorGain = pidProfile->itermAcceleratorGain;
pidRuntime.antiGravityGain = pidProfile->anti_gravity_gain;
pidRuntime.crashTimeLimitUs = pidProfile->crash_time * 1000;
pidRuntime.crashTimeDelayUs = pidProfile->crash_delay * 1000;
pidRuntime.crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10;
@ -321,17 +318,11 @@ void pidInitConfig(const pidProfile_t *pidProfile)
throttleBoost = pidProfile->throttle_boost * 0.1f;
#endif
pidRuntime.itermRotation = pidProfile->iterm_rotation;
pidRuntime.antiGravityMode = pidProfile->antiGravityMode;
// Calculate the anti-gravity value that will trigger the OSD display.
// For classic AG it's either 1.0 for off and > 1.0 for on.
// For the new AG it's a continuous floating value so we want to trigger the OSD
// display when it exceeds 25% of its possible range. This gives a useful indication
// of AG activity without excessive display.
pidRuntime.antiGravityOsdCutoff = 0.0f;
if (pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) {
pidRuntime.antiGravityOsdCutoff += (pidRuntime.itermAcceleratorGain / 1000.0f) * 0.25f;
}
// Calculate the anti-gravity value that will trigger the OSD display when its strength exceeds 25% of max.
// This gives a useful indication of AG activity without excessive display.
pidRuntime.antiGravityOsdCutoff = (pidRuntime.antiGravityGain / 10.0f) * 0.25f;
pidRuntime.antiGravityPGain = ((float)(pidProfile->anti_gravity_p_gain) / 100.0f) * ANTIGRAVITY_KP;
#if defined(USE_ITERM_RELAX)
pidRuntime.itermRelax = pidProfile->iterm_relax;