diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 1d682623ec..104776acf3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1327,11 +1327,16 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs) */ pidControllerFlags_e flags = PID_LIMIT_INTEGRATOR; - //Iterm should freeze when pitch stick is deflected + //Iterm should freeze when sticks are deflected + bool areSticksDeflected = false; + for (int stick = ROLL; stick <= YAW; stick++) { + areSticksDeflected = areSticksDeflected || + rxGetChannelValue(stick) > (PWM_RANGE_MIDDLE + pidProfile()->fixedWingLevelTrimDeadband) || + rxGetChannelValue(stick) < (PWM_RANGE_MIDDLE - pidProfile()->fixedWingLevelTrimDeadband); + } if ( !IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) || - rxGetChannelValue(PITCH) > (PWM_RANGE_MIDDLE + pidProfile()->fixedWingLevelTrimDeadband) || - rxGetChannelValue(PITCH) < (PWM_RANGE_MIDDLE - pidProfile()->fixedWingLevelTrimDeadband) || + areSticksDeflected || (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) || navigationIsControllingAltitude() ) {