From a3c1f6e168bc4a442023c2383e06c051dc86da7a Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 1 Jun 2016 20:58:00 +0200 Subject: [PATCH] Add Back Iterm limit for saturation scenarios --- src/main/flight/mixer.c | 14 ++++++++++---- src/main/flight/mixer.h | 2 +- src/main/flight/pid.c | 17 +++++++++++++++-- src/main/io/serial_cli.c | 2 +- 4 files changed, 27 insertions(+), 8 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index e6d6b0011..abf8e2783 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -817,6 +817,7 @@ void mixTable(void) throttleRange = throttleMax - throttleMin; if (rollPitchYawMixRange > throttleRange) { + motorLimitReached = true; mixReduction = qConstruct(throttleRange, rollPitchYawMixRange); for (i = 0; i < motorCount; i++) { @@ -827,6 +828,7 @@ void mixTable(void) if (debugMode == DEBUG_AIRMODE && i < 3) debug[1] = rollPitchYawMixRange; } else { + motorLimitReached = false; throttleMin = throttleMin + (rollPitchYawMixRange / 2); throttleMax = throttleMax - (rollPitchYawMixRange / 2); } @@ -857,11 +859,15 @@ void mixTable(void) // Experimental Code. Anti Desync feature for ESC's if (escAndServoConfig->escDesyncProtection) { - const int16_t maxThrottleStep = escAndServoConfig->escDesyncProtection / (1000 / targetPidLooptime); - static int16_t motorPrevious[MAX_SUPPORTED_MOTORS]; + const int16_t maxThrottleStep = constrain(escAndServoConfig->escDesyncProtection / (1000 / targetPidLooptime), 5, 10000); - motor[i] = constrain(motor[i], motorPrevious[i] - maxThrottleStep, motorPrevious[i] + maxThrottleStep); - motorPrevious[i] = motor[i]; + // Only makes sense when it's within the range + if (maxThrottleStep < throttleRange) { + static int16_t motorPrevious[MAX_SUPPORTED_MOTORS]; + + motor[i] = constrain(motor[i], motorPrevious[i] - maxThrottleStep, motorPrevious[i] + maxThrottleStep); + motorPrevious[i] = motor[i]; + } } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 109102f20..8ebd0fd22 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -189,7 +189,7 @@ void filterServos(void); extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; - +bool motorLimitReached; struct escAndServoConfig_s; struct rxConfig_s; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 45ddc7268..cc363bdd3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -49,6 +49,7 @@ #include "config/runtime_config.h" extern uint8_t motorCount; +extern bool motorLimitReached; uint32_t targetPidLooptime; int16_t axisPID[3]; @@ -60,9 +61,9 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction uint8_t PIDweight[3]; -static int32_t errorGyroI[3]; +static int32_t errorGyroI[3], errorGyroILimit[3]; #ifndef SKIP_PID_LUXFLOAT -static float errorGyroIf[3]; +static float errorGyroIf[3], errorGyroIfLimit[3];; #endif static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, @@ -215,6 +216,12 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_ errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * kI, -250.0f, 250.0f); + if (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]; @@ -341,6 +348,12 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate // 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 (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/io/serial_cli.c b/src/main/io/serial_cli.c index 05aefaa7a..b71ae0b10 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -574,7 +574,7 @@ const clivalue_t valueTable[] = { { "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, - { "anti_desync_power_step", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.escDesyncProtection, .config.minmax = { 0, 2000 } }, + { "anti_desync_power_step", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.escDesyncProtection, .config.minmax = { 0, 10000 } }, { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently