diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e7bd8ed1ad..7c1e1d0bec 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 54a7fe84ec..053dc0406d 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -14,6 +14,7 @@ * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ +#include "rx/rx.h" #pragma once @@ -81,5 +82,5 @@ extern int16_t axisPID[XYZ_AXIS_COUNT]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; void pidSetController(pidControllerType_e type); -void pidResetErrorGyro(void); +void pidResetErrorGyro(rxConfig_t *rxConfig); diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index fbe7a6b183..e09be60c9c 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -120,7 +120,6 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband return THROTTLE_HIGH; } -/* TODO - Cleanup rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig) { if (((rcData[PITCH] < (rxConfig->midrc + AIRMODEDEADBAND)) && (rcData[PITCH] > (rxConfig->midrc -AIRMODEDEADBAND))) @@ -129,7 +128,6 @@ rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig) return NOT_CENTERED; } -*/ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch) { diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 366d9ae777..4fa39c90e0 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -78,7 +78,7 @@ typedef enum { THROTTLE_HIGH } throttleStatus_e; -#define AIRMODEDEADBAND 10 +#define AIRMODEDEADBAND 12 typedef enum { NOT_CENTERED = 0, diff --git a/src/main/mw.c b/src/main/mw.c index a35fb0f7a9..1778eb8d53 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -478,7 +478,7 @@ void processRx(void) throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); if (throttleStatus == THROTTLE_LOW) { - pidResetErrorGyro(); + pidResetErrorGyro(&masterConfig.rxConfig); } // When armed and motors aren't spinning, do beeps and then disarm