diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 5b846475c3..49ceee0848 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1250,8 +1250,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", currentProfile->pidProfile.yaw_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz:%d", currentProfile->pidProfile.dterm_notch_hz); BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff:%d", currentProfile->pidProfile.dterm_notch_cutoff); - BLACKBOX_PRINT_HEADER_LINE("rollPitchItermIgnoreRate:%d", currentProfile->pidProfile.rollPitchItermIgnoreRate); - BLACKBOX_PRINT_HEADER_LINE("yawItermIgnoreRate:%d", currentProfile->pidProfile.yawItermIgnoreRate); + BLACKBOX_PRINT_HEADER_LINE("itermWindupPointPercent:%d", currentProfile->pidProfile.itermWindupPointPercent); BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", currentProfile->pidProfile.yaw_p_limit); BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", currentProfile->pidProfile.dterm_average_count); BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", currentProfile->pidProfile.vbatPidCompensation); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 6b6cc87736..17cdfd794a 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -707,8 +707,8 @@ const clivalue_t valueTable[] = { { "yaw_accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0.1f, 50.0f } }, { "accel_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0.1f, 50.0f } }, - { "accum_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, - { "yaw_accum_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, + { "iterm_windup", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermWindupPointPercent, .config.minmax = {50, 100 } }, + { "iterm_noise_threshold", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermNoiseThreshold, .config.minmax = {0, 20 } }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &pidConfig()->pid_process_denom, .config.minmax = { 1, 16 } }, diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 6ee2698c1f..4ce27a28b7 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -169,8 +169,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->pidSumLimit = PIDSUM_LIMIT; pidProfile->yaw_lpf_hz = 0; - pidProfile->rollPitchItermIgnoreRate = 200; - pidProfile->yawItermIgnoreRate = 55; + pidProfile->itermWindupPointPercent = 75; + pidProfile->itermNoiseThreshold = 0; pidProfile->dterm_filter_type = FILTER_BIQUAD; pidProfile->dterm_lpf_hz = 100; // filtering ON by default pidProfile->dterm_notch_hz = 260; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index c4a4599e8a..332f24eb88 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1159,8 +1159,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn break; case MSP_PID_ADVANCED: - sbufWriteU16(dst, currentProfile->pidProfile.rollPitchItermIgnoreRate); - sbufWriteU16(dst, currentProfile->pidProfile.yawItermIgnoreRate); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); sbufWriteU16(dst, currentProfile->pidProfile.yaw_p_limit); sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, currentProfile->pidProfile.vbatPidCompensation); @@ -1515,8 +1515,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) break; case MSP_SET_PID_ADVANCED: - currentProfile->pidProfile.rollPitchItermIgnoreRate = sbufReadU16(src); - currentProfile->pidProfile.yawItermIgnoreRate = sbufReadU16(src); + sbufReadU16(src); + sbufReadU16(src); currentProfile->pidProfile.yaw_p_limit = sbufReadU16(src); sbufReadU8(src); // reserved currentProfile->pidProfile.vbatPidCompensation = sbufReadU8(src); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 8ee4bb5742..accc55505e 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -56,6 +56,7 @@ #define EXTERNAL_CONVERSION_MAX_VALUE 2000 static uint8_t motorCount; +static float motorMixRange; int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; @@ -245,6 +246,11 @@ uint8_t getMotorCount() return motorCount; } +float getMotorMixRange() +{ + return motorMixRange; +} + bool isMotorProtocolDshot(void) { #ifdef USE_DSHOT switch(motorConfig->motorPwmProtocol) { @@ -500,7 +506,7 @@ void mixTable(pidProfile_t *pidProfile) } } - const float motorMixRange = motorMixMax - motorMixMin; + motorMixRange = motorMixMax - motorMixMin; if (motorMixRange > 1.0f) { for (int i = 0; i < motorCount; i++) { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index da4c04c159..8a84ea8acb 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -113,10 +113,12 @@ typedef struct airplaneConfig_s { extern const mixer_t mixers[]; extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; + struct motorConfig_s; struct rxConfig_s; uint8_t getMotorCount(); +float getMotorMixRange(); void mixerUseConfigs( flight3DConfig_t *flight3DConfigToUse, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e3415cb01a..3ae40d0a0e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -34,6 +34,7 @@ #include "flight/pid.h" #include "flight/imu.h" +#include "flight/mixer.h" #include "flight/navigation.h" #include "sensors/gyro.h" @@ -146,7 +147,8 @@ void pidInitFilters(const pidProfile_t *pidProfile) } } -static float Kp[3], Ki[3], Kd[3], c[3], levelGain, horizonGain, horizonTransition, maxVelocity[3], relaxFactor[3]; +static float Kp[3], Ki[3], Kd[3], c[3], maxVelocity[3], relaxFactor[3]; +static float levelGain, horizonGain, horizonTransition, ITermWindupPoint, ITermNoiseThresholdDps; void pidInitConfig(const pidProfile_t *pidProfile) { for(int axis = FD_ROLL; axis <= FD_YAW; axis++) { @@ -161,6 +163,8 @@ void pidInitConfig(const pidProfile_t *pidProfile) { horizonTransition = 100.0f / pidProfile->D8[PIDLEVEL]; maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 1000 * dT; maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 1000 * dT; + ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f; + ITermNoiseThresholdDps = (float)pidProfile->itermNoiseThreshold / 10.0f; } static float calcHorizonLevelStrength(void) { @@ -238,15 +242,21 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an } // -----calculate I component - // Reduce strong Iterm accumulation during higher stick inputs - const float accumulationThreshold = (axis == FD_YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate; - const float setpointRateScaler = constrainf(1.0f - (ABS(currentPidSetpoint) / accumulationThreshold), 0.0f, 1.0f); - float ITerm = previousGyroIf[axis]; - ITerm += Ki[axis] * errorRate * dT * setpointRateScaler * itermAccelerator; - // limit maximum integrator value to prevent WindUp - ITerm = constrainf(ITerm, -250.0f, 250.0f); - previousGyroIf[axis] = ITerm; + const float motorMixRangeAbs = ABS(getMotorMixRange()); + if (motorMixRangeAbs < 1.0f && (errorRate > ITermNoiseThresholdDps || errorRate < -ITermNoiseThresholdDps)) { + // Only increase ITerm if motor output is not saturated and errorRate exceeds noise threshold + // Reduce strong Iterm accumulation during higher stick inputs + float ITermDelta = Ki[axis] * errorRate * dT * itermAccelerator; + // gradually scale back integration when above windup point (default is 75%) + if (motorMixRangeAbs > ITermWindupPoint) { + ITermDelta *= (1.0f - motorMixRangeAbs) / (1.0f - ITermWindupPoint); + } + ITerm += ITermDelta; + // also limit maximum integrator value to prevent windup + ITerm = constrainf(ITerm, -250.0f, 250.0f); + previousGyroIf[axis] = ITerm; + } // -----calculate D component (Yaw D not yet supported) float DTerm = 0.0; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 435cc7f9a0..87ac84f2ea 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -66,8 +66,8 @@ typedef struct pidProfile_s { uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy uint16_t dterm_notch_hz; // Biquad dterm notch hz uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff - uint16_t rollPitchItermIgnoreRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates - uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates + uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation + uint8_t itermNoiseThreshold; // Experimental ITerm noise threshold uint16_t yaw_p_limit; float pidSumLimit; uint8_t dterm_average_count; // Configurable delta count for dterm