mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-19 22:35:23 +03:00
Less Anti Winduo in Air mode
This commit is contained in:
parent
9b3a2f3f76
commit
9859bbfcae
5 changed files with 10 additions and 8 deletions
|
@ -75,14 +75,17 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig
|
|||
|
||||
pidControllerFuncPtr pid_controller = pidRewrite; // which pid controller are we using, defaultMultiWii
|
||||
|
||||
void pidResetErrorGyro(void)
|
||||
void pidResetErrorGyro(rxConfig_t *rxConfig)
|
||||
{
|
||||
int axis;
|
||||
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
|
||||
PREVENT_WINDUP(errorGyroI[axis], previousErrorGyroI[axis]);
|
||||
PREVENT_WINDUP(errorGyroIf[axis], previousErrorGyroIf[axis]);
|
||||
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;
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue