diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b8d202acd1..d74c5b3488 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -112,6 +112,8 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig, #define CRASH_RECOVERY_DETECTION_DELAY_US 1000000 // 1 second delay before crash recovery detection is active after entering a self-level mode +#define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes) + PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 5); void resetPidProfile(pidProfile_t *pidProfile) @@ -1230,10 +1232,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT // Disable P/I appropriately based on the launch control mode if (launchControlActive) { // if not using FULL mode then disable I accumulation on yaw as - // yaw has a tendency to windup - if (launchControlMode != LAUNCH_CONTROL_MODE_FULL) { - pidData[FD_YAW].I = 0; - } + // yaw has a tendency to windup. Otherwise limit yaw iterm accumulation. + const int launchControlYawItermLimit = (launchControlMode == LAUNCH_CONTROL_MODE_FULL) ? LAUNCH_CONTROL_YAW_ITERM_LIMIT : 0; + pidData[FD_YAW].I = constrainf(pidData[FD_YAW].I, -launchControlYawItermLimit, launchControlYawItermLimit); // for pitch-only mode we disable everything except pitch P/I if (launchControlMode == LAUNCH_CONTROL_MODE_PITCHONLY) {