From 42218f1ea56888ddf08ff39784f76816d606c04c Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 19 May 2015 17:42:03 +0200 Subject: [PATCH 1/3] The yaw_p_limit minimum is now set to 100 to prevent misconfigurations. Maximum value of 500 is now removing the limit (same as for yaw_jump_prevention_limit) Moving defines for PID controllers to headerfiles. Some general code cleanup. Documentation update --- docs/Cli.md | 1 + src/main/config/config.c | 2 +- src/main/flight/mixer.c | 2 +- src/main/flight/mixer.h | 3 +++ src/main/flight/pid.c | 12 +++--------- src/main/flight/pid.h | 6 ++++++ src/main/io/serial_cli.c | 6 +++--- 7 files changed, 18 insertions(+), 14 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index a86d3d37c2..0007c0678e 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -246,5 +246,6 @@ Re-apply any new defaults as desired. | p_vel | | 0 | 200 | 120 | Profile | UINT8 | | i_vel | | 0 | 200 | 45 | Profile | UINT8 | | d_vel | | 0 | 200 | 1 | Profile | UINT8 | +| yaw_p_limit | Limitter for yaw P term. To disable set to 500 (actual default). | 100 | 500 | 500 | Profile | UINT16 | | blackbox_rate_num | | 1 | 32 | 1 | Master | UINT8 | | blackbox_rate_denom | | 1 | 32 | 1 | Master | UINT8 | diff --git a/src/main/config/config.c b/src/main/config/config.c index 6f609bc6f5..64ecdbd419 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -180,7 +180,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->I8[PIDVEL] = 45; pidProfile->D8[PIDVEL] = 1; - pidProfile->yaw_p_limit = 0; + pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->P_f[ROLL] = 2.5f; // new PID with preliminary defaults test carefully pidProfile->I_f[ROLL] = 0.6f; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 55ae12cab1..498f7865ef 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -564,7 +564,7 @@ void mixTable(void) { uint32_t i; - if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < 500) { + if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) { // prevent "yaw jump" during yaw correction (500 is disabled jump protection) axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW])); } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index ed73f8d935..0d3fcf633e 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -19,6 +19,9 @@ #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 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ca2c77925a..50151cbd21 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -298,8 +298,6 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } } -#define GYRO_I_MAX 256 - static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { @@ -394,7 +392,7 @@ 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 >= 4 && pidProfile->yaw_p_limit) { + if(motorCount >= 4 && pidProfile->yaw_p_limit < YAW_P_LIMIT_MAX) { PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); } @@ -504,7 +502,7 @@ 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 >= 4 && pidProfile->yaw_p_limit) { + if(motorCount >= 4 && pidProfile->yaw_p_limit < YAW_P_LIMIT_MAX) { PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); } @@ -520,10 +518,6 @@ static void pidMultiWiiHybrid(pidProfile_t *pidProfile, controlRateConfig_t *con #endif } -#define RCconstPI 0.159154943092f // 0.5f / M_PI; -#define MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz) -#define OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. - static void pidHarakiri(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { @@ -633,7 +627,7 @@ 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 && pidProfile->yaw_p_limit) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply + if(motorCount >= 4 && pidProfile->yaw_p_limit < YAW_P_LIMIT_MAX) { // 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); } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 6ec5dbdfde..261f734d44 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -17,6 +17,12 @@ #pragma once +#define GYRO_I_MAX 256 // Gyro I limiter +#define RCconstPI 0.159154943092f // 0.5f / M_PI; +#define MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz) +#define OLD_YAW 0 // [0/1] 0 = MultiWii 2.3 yaw, 1 = older yaw. +#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter +#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter typedef enum { PIDROLL, diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index e8da1c72c3..51c279f5d1 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -359,7 +359,7 @@ const clivalue_t valueTable[] = { { "vbat_max_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50 }, { "vbat_min_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmincellvoltage, 10, 50 }, { "vbat_warning_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatwarningcellvoltage, 10, 50 }, - { "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, -10000, 10000 }, + { "current_meter_scale", VAR_INT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterScale, -10000, 10000 }, { "current_meter_offset", VAR_UINT16 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterOffset, 0, 3300 }, { "multiwii_current_meter_output", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.multiwiiCurrentMeterOutput, 0, 1 }, { "current_meter_type", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.currentMeterType, 0, CURRENT_SENSOR_MAX }, @@ -391,7 +391,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_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_jump_prevention_limit, 80, 500 }, + { "yaw_jump_prevention_limit", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_jump_prevention_limit, YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH }, #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}, @@ -476,7 +476,7 @@ 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 }, + { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX }, #ifdef BLACKBOX { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 }, From 50d5ec4e21f733495ec4bfbd797e33dc1d4a0300 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 19 May 2015 17:47:06 +0200 Subject: [PATCH 2/3] Fix typo in CLI.md --- docs/Cli.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Cli.md b/docs/Cli.md index 0007c0678e..a0b12cef80 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -246,6 +246,6 @@ Re-apply any new defaults as desired. | p_vel | | 0 | 200 | 120 | Profile | UINT8 | | i_vel | | 0 | 200 | 45 | Profile | UINT8 | | d_vel | | 0 | 200 | 1 | Profile | UINT8 | -| yaw_p_limit | Limitter for yaw P term. To disable set to 500 (actual default). | 100 | 500 | 500 | Profile | UINT16 | +| yaw_p_limit | Limiter for yaw P term. To disable set to 500 (actual default). | 100 | 500 | 500 | Profile | UINT16 | | blackbox_rate_num | | 1 | 32 | 1 | Master | UINT8 | | blackbox_rate_denom | | 1 | 32 | 1 | Master | UINT8 | From 0ef236a50a3c750d3bc72f2ce5605bcd2e765842 Mon Sep 17 00:00:00 2001 From: Michael Jakob Date: Tue, 19 May 2015 17:54:57 +0200 Subject: [PATCH 3/3] Another documentation update --- docs/Cli.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Cli.md b/docs/Cli.md index a0b12cef80..fb0fd9febe 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -246,6 +246,6 @@ Re-apply any new defaults as desired. | p_vel | | 0 | 200 | 120 | Profile | UINT8 | | i_vel | | 0 | 200 | 45 | Profile | UINT8 | | d_vel | | 0 | 200 | 1 | Profile | UINT8 | -| yaw_p_limit | Limiter for yaw P term. To disable set to 500 (actual default). | 100 | 500 | 500 | Profile | UINT16 | +| yaw_p_limit | Limiter for yaw P term. This parameter is only affecting PID controller 3-5. To disable set to 500 (actual default). | 100 | 500 | 500 | Profile | UINT16 | | blackbox_rate_num | | 1 | 32 | 1 | Master | UINT8 | | blackbox_rate_denom | | 1 | 32 | 1 | Master | UINT8 |