From 9dfb3e45ee08e9cc75ce0aede4126d3ad33ac9c3 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 26 Jan 2017 18:15:29 +0000 Subject: [PATCH] Add ITerm anti-windup based on motor output saturation Added noise threshold for PID ITerm accumulation Removed ITerm setpoint scaler. Added CLI and MSP settings Made default ITerm noise threshold zero. Added CLI setting. Removed itermWindupPointPercent from MSP --- src/main/blackbox/blackbox.c | 3 +-- src/main/fc/cli.c | 4 ++-- src/main/fc/config.c | 4 ++-- src/main/fc/fc_msp.c | 8 ++++---- src/main/flight/mixer.c | 8 +++++++- src/main/flight/mixer.h | 2 ++ src/main/flight/pid.c | 28 +++++++++++++++++++--------- src/main/flight/pid.h | 4 ++-- 8 files changed, 39 insertions(+), 22 deletions(-) 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