From aa61bd4fb4e5219dfacae91c3b9ad20d6c4051d4 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 24 May 2016 21:47:57 +0200 Subject: [PATCH] Replace yaw_jump_prevention_limit by D gain for yaw // new defaults based on flight tests --- src/main/config/config.c | 17 ++++++++--------- src/main/flight/mixer.c | 5 ----- src/main/flight/mixer.h | 4 ---- src/main/flight/pid.c | 31 ++++++++++++++++++++++++------- src/main/flight/pid.h | 2 ++ src/main/io/serial_cli.c | 1 - 6 files changed, 34 insertions(+), 26 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 055c27a77..9936b78a1 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -140,7 +140,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 136; +static const uint8_t EEPROM_CONF_VERSION = 137; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -154,14 +154,14 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidController = 1; pidProfile->P8[ROLL] = 45; - pidProfile->I8[ROLL] = 55; + pidProfile->I8[ROLL] = 40; pidProfile->D8[ROLL] = 15; pidProfile->P8[PITCH] = 45; - pidProfile->I8[PITCH] = 55; + pidProfile->I8[PITCH] = 40; pidProfile->D8[PITCH] = 15; pidProfile->P8[YAW] = 90; - pidProfile->I8[YAW] = 50; - pidProfile->D8[YAW] = 0; + pidProfile->I8[YAW] = 45; + pidProfile->D8[YAW] = 10; pidProfile->P8[PIDALT] = 50; pidProfile->I8[PIDALT] = 0; pidProfile->D8[PIDALT] = 0; @@ -184,8 +184,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->yaw_lpf_hz = 80; - pidProfile->rollPitchItermIgnoreRate = 200; - pidProfile->yawItermIgnoreRate = 50;; + pidProfile->rollPitchItermIgnoreRate = 180; + pidProfile->yawItermIgnoreRate = 35; pidProfile->dterm_lpf_hz = 110; // filtering ON by default pidProfile->dynamic_pid = 1; @@ -308,7 +308,7 @@ void resetSerialConfig(serialConfig_t *serialConfig) static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { controlRateConfig->rcRate8 = 100; - controlRateConfig->rcExpo8 = 60; + controlRateConfig->rcExpo8 = 70; controlRateConfig->thrMid8 = 50; controlRateConfig->thrExpo8 = 0; controlRateConfig->dynThrPID = 20; @@ -330,7 +330,6 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) { void resetMixerConfig(mixerConfig_t *mixerConfig) { mixerConfig->yaw_motor_direction = 1; - mixerConfig->yaw_jump_prevention_limit = 200; #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 c777ca39f..87ee5f187 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -761,11 +761,6 @@ void mixTable(void) bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code - if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) { - // prevent "yaw jump" during yaw correction - axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW])); - } - // Initial mixer concept by bdoiron74 reused and optimized for Air Mode int16_t rollPitchYawMix[MAX_SUPPORTED_MOTORS]; int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero. diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index a1f79d5a8..109102f20 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -19,9 +19,6 @@ #define MAX_SUPPORTED_MOTORS 12 #define MAX_SUPPORTED_SERVOS 8 -#define YAW_JUMP_PREVENTION_LIMIT_LOW 80 -#define YAW_JUMP_PREVENTION_LIMIT_HIGH 500 - // Note: this is called MultiType/MULTITYPE_* in baseflight. typedef enum mixerMode @@ -71,7 +68,6 @@ typedef struct mixer_s { typedef struct mixerConfig_s { int8_t yaw_motor_direction; - uint16_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 uint16_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 dc17f886e..96cf037ca 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -231,7 +231,15 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_ //-----calculate D-term if (axis == YAW) { if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); - DTerm = 0; + + axisPID[axis] = lrintf(PTerm + ITerm); + + if (motorCount >= 4) { + int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); + + // prevent "yaw jump" during yaw correction + axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); + } } else { delta = -(gyroRate - lastRate[axis]); lastRate[axis] = gyroRate; @@ -243,10 +251,10 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_ if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f); - } - // -----calculate total PID output - axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); + // -----calculate total PID output + axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); + } #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { @@ -354,7 +362,15 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate //-----calculate D-term if (axis == YAW) { if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); - DTerm = 0; + + axisPID[axis] = PTerm + ITerm; + + if (motorCount >= 4) { + int16_t yaw_jump_prevention_limit = constrain(YAW_JUMP_PREVENTION_LIMIT_HIGH - (pidProfile->D8[axis] << 3), YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH); + + // prevent "yaw jump" during yaw correction + axisPID[YAW] = constrain(axisPID[YAW], -yaw_jump_prevention_limit - ABS(rcCommand[YAW]), yaw_jump_prevention_limit + ABS(rcCommand[YAW])); + } } else { delta = -(gyroRate - lastRate[axis]); lastRate[axis] = gyroRate; @@ -366,10 +382,11 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; + + // -----calculate total PID output + axisPID[axis] = PTerm + ITerm + DTerm; } - // -----calculate total PID output - axisPID[axis] = PTerm + ITerm + DTerm; #ifdef GTUNE if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 30bffb98f..1103f7bd9 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -21,6 +21,8 @@ #define GYRO_I_MAX 256 // Gyro I limiter #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter +#define YAW_JUMP_PREVENTION_LIMIT_LOW 80 +#define YAW_JUMP_PREVENTION_LIMIT_HIGH 400 #define DYNAMIC_PTERM_STICK_THRESHOLD 400 diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 7f224e16a..12fa08adf 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -677,7 +677,6 @@ const clivalue_t valueTable[] = { { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, .config.minmax = { -1, 1 } }, { "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_motor_direction, .config.minmax = { -1, 1 } }, - { "yaw_jump_prevention_limit", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_jump_prevention_limit, .config.minmax = { YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH } }, { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } }, #ifdef USE_SERVOS { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } },