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

Fix non Working Anti Windup

Browse source
This commit is contained in:
borisbstyle 2016-02-04 01:55:44 +01:00
parent 5174e96549
commit c55d0baf31
4 changed files with 21 additions and 21 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -50,6 +50,7 @@
extern float dT;
extern bool motorLimitReached;
bool lowThrottleWindupProtection;
#define PREVENT_WINDUP(x,y) { if (ABS(x) > ABS(y)) { if (x < 0) { x = -ABS(y); } else { x = ABS(y); } } }
@ -75,21 +76,12 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig
pidControllerFuncPtr pid_controller = pidRewrite; // which pid controller are we using, defaultMultiWii
void pidResetErrorGyro(rxConfig_t *rxConfig)
void pidResetErrorGyro(void)
{
int axis;
for (axis = 0; axis < 3; axis++) {
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
int rollPitchStatus = calculateRollPitchCenterStatus(rxConfig);
if (rollPitchStatus == CENTERED) {
PREVENT_WINDUP(errorGyroI[axis], previousErrorGyroI[axis]);
PREVENT_WINDUP(errorGyroIf[axis], previousErrorGyroIf[axis]);
}
} else {
errorGyroI[axis] = 0;
errorGyroIf[axis] = 0.0f;
}
errorGyroI[axis] = 0;
errorGyroIf[axis] = 0.0f;
}
}
@ -208,9 +200,9 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate I component.
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
errorGyroIf[axis] *= scaleItermToRcInput(axis);
if (motorLimitReached) {
if (motorLimitReached || lowThrottleWindupProtection) {
PREVENT_WINDUP(errorGyroIf[axis], previousErrorGyroIf[axis]);
} else {
previousErrorGyroIf[axis] = errorGyroIf[axis];
@ -355,10 +347,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
errorGyroI[axis] = (int32_t) (errorGyroI[axis] * scaleItermToRcInput(axis));
if (motorLimitReached) {
PREVENT_WINDUP(errorGyroIf[axis], previousErrorGyroIf[axis]);
if (motorLimitReached || lowThrottleWindupProtection) {
PREVENT_WINDUP(errorGyroI[axis], previousErrorGyroI[axis]);
} else {
previousErrorGyroI[axis] = errorGyroI[axis];
}