diff --git a/src/main/config/config.c b/src/main/config/config.c index 893b451ae..9b56e39b0 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -179,8 +179,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 0.0f; pidProfile->dterm_average_count = 4; - pidProfile->rollPitchItermResetRate = 0; - pidProfile->yawItermResetRate = 0; + pidProfile->rollPitchItermResetRate = 200; + pidProfile->yawItermResetRate = 50; pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 08470d5b1..65907ec00 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -204,22 +204,20 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // -----calculate I component. errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * getdT() * pidProfile->I8[axis] / 10.0f, -250.0f, 250.0f); - if (pidProfile->rollPitchItermResetRate && axis != YAW) { - if (ABS(gyroRate) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0; - } - - if (pidProfile->yawItermResetRate && axis == YAW) { - if (ABS(gyroRate) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0; - } - - if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { - if (antiWindupProtection || motorLimitReached) { - errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]); + if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) { + if (axis == YAW) { + if (ABS(gyroRate) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0; } else { - errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]); + if (ABS(gyroRate) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0; } } + if (antiWindupProtection || motorLimitReached) { + errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]); + } else { + errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]); + } + // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings ITerm = errorGyroIf[axis]; @@ -299,12 +297,10 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control } // Anti windup protection - if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { - if (antiWindupProtection || motorLimitReached) { - errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]); - } else { - errorGyroILimit[axis] = ABS(errorGyroI[axis]); - } + if (antiWindupProtection || motorLimitReached) { + errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]); + } else { + errorGyroILimit[axis] = ABS(errorGyroI[axis]); } ITerm = (errorGyroI[axis] >> 7) * pidProfile->I8[axis] >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 @@ -495,22 +491,20 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co // 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 (pidProfile->rollPitchItermResetRate && axis != YAW) { - if (ABS(gyroRate / 4) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0; - } - - if (pidProfile->yawItermResetRate && axis == YAW) { - if (ABS(gyroRate / 4) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0; - } - - if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { - if (antiWindupProtection || motorLimitReached) { - errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]); + if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) { + if (axis == YAW) { + if (ABS(gyroRate / 4) >= pidProfile->yawItermResetRate) errorGyroI[axis] = 0; } else { - errorGyroILimit[axis] = ABS(errorGyroI[axis]); + if (ABS(gyroRate / 4) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = 0; } } + if (antiWindupProtection || motorLimitReached) { + errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]); + } else { + errorGyroILimit[axis] = ABS(errorGyroI[axis]); + } + ITerm = errorGyroI[axis] >> 13; //-----calculate D-term diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 857138381..a30fdfdf1 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -67,9 +67,9 @@ typedef struct pidProfile_s { float dterm_lpf_hz; // Delta Filter in hz float yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy - uint8_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates + uint16_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates - uint16_t deltaMethod; // Alternative delta Calculation + uint8_t deltaMethod; // Alternative delta Calculation uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 1e178c10d..f5fac205a 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -729,8 +729,8 @@ const clivalue_t valueTable[] = { { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {0, 12 } }, - { "roll_pitch_iterm_reset_deg", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {0, 1000 } }, - { "yaw_iterm_reset_deg", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {0, 1000 } }, + { "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } }, + { "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } }, { "yaw_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } },