From f390f35c304eaf88392dd7701178a89f7b8a6d5c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 4 Mar 2017 07:53:03 +0000 Subject: [PATCH] Added separate yaw pidsum limit --- src/main/blackbox/blackbox.c | 3 ++- src/main/cms/cms_menu_imu.c | 2 -- src/main/fc/cli.c | 4 +++- src/main/fc/config.c | 2 +- src/main/fc/fc_msp.c | 4 ++-- src/main/flight/mixer.c | 6 +++--- src/main/flight/pid.h | 5 ++--- 7 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 49ceee0848..f666f89523 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1251,7 +1251,6 @@ static bool blackboxWriteSysinfo() 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("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); BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle:%d", currentProfile->pidProfile.pidAtMinThrottle); @@ -1263,6 +1262,8 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d", currentProfile->pidProfile.dtermSetpointWeight); BLACKBOX_PRINT_HEADER_LINE("yawRateAccelLimit:%d", castFloatBytesToInt(currentProfile->pidProfile.yawRateAccelLimit)); BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit:%d", castFloatBytesToInt(currentProfile->pidProfile.rateAccelLimit)); + BLACKBOX_PRINT_HEADER_LINE("pidSumLimit:%d", castFloatBytesToInt(currentProfile->pidProfile.pidSumLimit)); + BLACKBOX_PRINT_HEADER_LINE("pidSumLimitYaw:%d", castFloatBytesToInt(currentProfile->pidProfile.pidSumLimitYaw)); // End of Betaflight controller parameters BLACKBOX_PRINT_HEADER_LINE("deadband:%d", rcControlsConfig()->deadband); diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 26cce45584..e292b50ec2 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -307,7 +307,6 @@ static uint16_t cmsx_dterm_lpf_hz; static uint16_t cmsx_dterm_notch_hz; static uint16_t cmsx_dterm_notch_cutoff; static uint16_t cmsx_yaw_lpf_hz; -static uint16_t cmsx_yaw_p_limit; static long cmsx_FilterPerProfileRead(void) { @@ -341,7 +340,6 @@ static OSD_Entry cmsx_menuFilterPerProfileEntries[] = { "DTERM NF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_hz, 0, 500, 1 }, 0 }, { "DTERM NFCO", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_notch_cutoff, 0, 500, 1 }, 0 }, { "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_lpf_hz, 0, 500, 1 }, 0 }, - { "YAW P LIM", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_yaw_p_limit, 100, 500, 1 }, 0 }, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index e666b8e77d..538cb1d429 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -643,8 +643,10 @@ const clivalue_t valueTable[] = { { "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &rcControlsConfig()->yaw_control_direction, .config.minmax = { -1, 1 } }, { "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &mixerConfig()->yaw_motor_direction, .config.minmax = { -1, 1 } }, - { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } }, + { "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 0.1, 1.0 } }, + { "pidsum_limit_yaw", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimitYaw, .config.minmax = { 0.1, 1.0 } }, + #ifdef USE_SERVOS { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 425fd33fcc..581adf720d 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -162,8 +162,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->I8[PIDVEL] = 55; pidProfile->D8[PIDVEL] = 75; - pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; pidProfile->pidSumLimit = PIDSUM_LIMIT; + pidProfile->pidSumLimitYaw = PIDSUM_LIMIT_YAW; pidProfile->yaw_lpf_hz = 0; pidProfile->itermWindupPointPercent = 50; pidProfile->dterm_filter_type = FILTER_BIQUAD; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 6e7adf2027..80da41769f 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1163,7 +1163,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_PID_ADVANCED: sbufWriteU16(dst, 0); sbufWriteU16(dst, 0); - sbufWriteU16(dst, currentProfile->pidProfile.yaw_p_limit); + sbufWriteU16(dst, 0); // was pidProfile.yaw_p_limit sbufWriteU8(dst, 0); // reserved sbufWriteU8(dst, currentProfile->pidProfile.vbatPidCompensation); sbufWriteU8(dst, currentProfile->pidProfile.setpointRelaxRatio); @@ -1549,7 +1549,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_PID_ADVANCED: sbufReadU16(src); sbufReadU16(src); - currentProfile->pidProfile.yaw_p_limit = sbufReadU16(src); + sbufReadU16(src); // was pidProfile.yaw_p_limit sbufReadU8(src); // reserved currentProfile->pidProfile.vbatPidCompensation = sbufReadU8(src); currentProfile->pidProfile.setpointRelaxRatio = sbufReadU8(src); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 838c98cf16..85aff2815a 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -480,9 +480,9 @@ void mixTable(pidProfile_t *pidProfile) float scaledAxisPIDf[3]; // Limit the PIDsum - for (int axis = 0; axis < 3; axis++) { - scaledAxisPIDf[axis] = constrainf(axisPIDf[axis] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit); - } + scaledAxisPIDf[FD_ROLL] = constrainf(axisPIDf[FD_ROLL] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit); + scaledAxisPIDf[FD_PITCH] = constrainf(axisPIDf[FD_PITCH] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit); + scaledAxisPIDf[FD_YAW] = constrainf(axisPIDf[FD_YAW] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimitYaw); // Calculate voltage compensation const float vbatCompensationFactor = (batteryConfig && pidProfile->vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 9c045245c6..6838ad9398 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -23,9 +23,8 @@ #define PID_CONTROLLER_BETAFLIGHT 1 #define PID_MIXER_SCALING 1000.0f #define PID_SERVO_MIXER_SCALING 0.7f -#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 PIDSUM_LIMIT 0.5f +#define PIDSUM_LIMIT_YAW 0.4f // Scaling factors for Pids for better tunable range in configurator for betaflight pid controller. The scaling is based on legacy pid controller or previous float #define PTERM_SCALE 0.032029f @@ -68,8 +67,8 @@ typedef struct pidProfile_s { uint16_t dterm_notch_hz; // Biquad dterm notch hz uint16_t dterm_notch_cutoff; // Biquad dterm notch low cutoff uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation - uint16_t yaw_p_limit; float pidSumLimit; + float pidSumLimitYaw; uint8_t dterm_average_count; // Configurable delta count for dterm uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active.