From 2fd79a7aa23b56f0e57956c3ace53de90a864833 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Thu, 23 Apr 2015 00:48:37 +0200 Subject: [PATCH] This fix is for avoiding yaw overshoot and bounce back for some configurations The hardcoded limit in the mixer and PID controllers 3-5 would be removed by default and will be configurable by CLI variables: yaw_jump_prevention_limit, global setting (original fixed value was 100) yaw_p_limit, per profile setting (fixed value was 300) --- src/main/config/config.c | 3 +++ src/main/flight/mixer.c | 4 ++-- src/main/flight/mixer.h | 1 + src/main/flight/pid.c | 17 ++++++----------- src/main/flight/pid.h | 1 + src/main/io/serial_cli.c | 3 +++ 6 files changed, 16 insertions(+), 13 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 1ce1276ab..cbc672b0d 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -180,6 +180,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->I8[PIDVEL] = 45; pidProfile->D8[PIDVEL] = 1; + pidProfile->yaw_p_limit = 0; + pidProfile->P_f[ROLL] = 2.5f; // new PID with preliminary defaults test carefully pidProfile->I_f[ROLL] = 0.6f; pidProfile->D_f[ROLL] = 0.06f; @@ -317,6 +319,7 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) { void resetMixerConfig(mixerConfig_t *mixerConfig) { mixerConfig->pid_at_min_throttle = 1; mixerConfig->yaw_direction = 1; + mixerConfig->yaw_jump_prevention_limit = 0; #ifdef USE_SERVOS mixerConfig->tri_unarmed_servo = 1; mixerConfig->servo_lowpass_freq = 400; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d9da96c1d..fe70eff86 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -564,9 +564,9 @@ void mixTable(void) { uint32_t i; - if (motorCount > 3) { + if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit) { // prevent "yaw jump" during yaw correction - axisPID[YAW] = constrain(axisPID[YAW], -100 - ABS(rcCommand[YAW]), +100 + ABS(rcCommand[YAW])); + axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW])); } // motors for non-servo mixes diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 121ab8fa8..40db869ac 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -66,6 +66,7 @@ typedef struct mixer_t { typedef struct mixerConfig_s { uint8_t pid_at_min_throttle; // when enabled pids are used at minimum throttle int8_t yaw_direction; + int8_t yaw_jump_prevention_limit; // make limit configurable (original fixed value was 100) #ifdef USE_SERVOS uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed int16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 22b72b2f2..74ad06dc2 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -297,7 +297,6 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } } -#define GYRO_P_MAX 300 #define GYRO_I_MAX 256 static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, @@ -394,9 +393,8 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; // TODO: Bitwise shift on a signed integer is not recommended // Constrain YAW by D value if not servo driven in that case servolimits apply - if(motorCount > 3) { - int16_t limit = GYRO_P_MAX - pidProfile->D8[FD_YAW]; - PTerm = constrain(PTerm, -limit, +limit); + if(motorCount >= 4 && pidProfile->yaw_p_limit) { + PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); } ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX); @@ -505,9 +503,8 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; // Constrain YAW by D value if not servo driven in that case servolimits apply - if(motorCount > 3) { - int16_t limit = GYRO_P_MAX - pidProfile->D8[FD_YAW]; - PTerm = constrain(PTerm, -limit, +limit); + if(motorCount >= 4 && pidProfile->yaw_p_limit) { + PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); } ITerm = constrain((int16_t)(errorGyroI[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX); @@ -635,10 +632,8 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) ITerm = constrain(errorGyroI[FD_YAW] >> 13, -GYRO_I_MAX, +GYRO_I_MAX); PTerm = ((int32_t)error * (int32_t)pidProfile->P8[FD_YAW]) >> 6; - if (motorCount >= 4) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply - int32_t limit = 300; - if (pidProfile->D8[FD_YAW]) limit -= (int32_t)pidProfile->D8[FD_YAW]; - PTerm = constrain(PTerm, -limit, limit); + if(motorCount >= 4 && pidProfile->yaw_p_limit) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply + PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); } } axisPID[FD_YAW] = PTerm + ITerm; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index e1ef9d34e..a10192b82 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -47,6 +47,7 @@ typedef struct pidProfile_s { float A_level; float H_level; uint8_t H_sensitivity; + uint16_t yaw_p_limit; // set P term limit (fixed value was 300) } pidProfile_t; #define DEGREES_TO_DECIDEGREES(angle) (angle * 10) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 0e1d4687b..ac2a8a85e 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -387,6 +387,7 @@ const clivalue_t valueTable[] = { { "pid_at_min_throttle", VAR_UINT8 | MASTER_VALUE, &masterConfig.mixerConfig.pid_at_min_throttle, 0, 1 }, { "yaw_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_direction, -1, 1 }, + { "yaw_jump_prevention_limit", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_jump_prevention_limit, 0, 250 }, #ifdef USE_SERVOS { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.tri_unarmed_servo, 0, 1 }, { "servo_lowpass_freq", VAR_INT16 | MASTER_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, 10, 400}, @@ -470,6 +471,8 @@ const clivalue_t valueTable[] = { { "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], 0, 200 }, { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 }, + { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, 0, 500 }, + #ifdef BLACKBOX { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 }, { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, 1, 32 },