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

Simplify anti gravity

Browse source
This commit is contained in:
borisbstyle 2017-02-07 10:26:21 +01:00
parent 2a77107376
commit 89527df273
4 changed files with 2 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -152,7 +152,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
static float Kp[3], Ki[3], Kd[3], maxVelocity[3];
static float relaxFactor;
static float dtermSetpointWeight;
static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv, itermAcceleratorRateLimit;
static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermWindupPointInv;
void pidInitConfig(const pidProfile_t *pidProfile) {
for(int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -169,7 +169,6 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT;
ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
itermAcceleratorRateLimit = (float)pidProfile->itermAcceleratorRateLimit;
}
static float calcHorizonLevelStrength(void) {
@ -255,12 +254,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
float ITerm = previousGyroIf[axis];
if (motorMixRange < 1.0f) {
// Only increase ITerm if motor output is not saturated
float ITermDelta = Ki[axis] * errorRate * dT * dynKi;
if (ABS(currentPidSetpoint) < itermAcceleratorRateLimit) {
// ITerm will only be accelerated below steady rate threshold
ITermDelta *= itermAccelerator;
}
ITerm += ITermDelta;
ITerm += Ki[axis] * errorRate * dT * dynKi * itermAccelerator;
previousGyroIf[axis] = ITerm;
}