diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 5c0ca9f89a..78d5ffc821 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1279,9 +1279,6 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.pidAtMinThrottle); // Betaflight PID controller parameters - BLACKBOX_PRINT_HEADER_LINE("toleranceBand:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.toleranceBand); - BLACKBOX_PRINT_HEADER_LINE("toleranceBandReduction:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.toleranceBandReduction); - BLACKBOX_PRINT_HEADER_LINE("zeroCrossAllowanceCount:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.zeroCrossAllowanceCount); BLACKBOX_PRINT_HEADER_LINE("itermThrottleGain:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.itermThrottleGain); BLACKBOX_PRINT_HEADER_LINE("ptermSRateWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.ptermSRateWeight); BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dtermSetpointWeight); diff --git a/src/main/config/config.c b/src/main/config/config.c index ffb4367c95..22cd6b94f1 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -247,13 +247,10 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; // Betaflight PID controller parameters - pidProfile->ptermSRateWeight = 50; + pidProfile->ptermSRateWeight = 100; pidProfile->dtermSetpointWeight = 150; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; - pidProfile->toleranceBand = 0; - pidProfile->toleranceBandReduction = 40; - pidProfile->zeroCrossAllowanceCount = 2; pidProfile->itermThrottleGain = 0; #ifdef GTUNE @@ -491,7 +488,7 @@ void createDefaultConfig(master_t *config) #endif config->gyro_soft_type = FILTER_PT1; config->gyro_soft_lpf_hz = 90; - config->gyro_soft_notch_hz = 210; + config->gyro_soft_notch_hz = 220; config->gyro_soft_notch_cutoff = 110; config->debug_mode = DEBUG_NONE; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 234b98462b..523a50fc0c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -235,36 +235,8 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc errorRate = setpointRate[axis] - PVRate; // r - y rP = ptermSetpointRate[axis] - PVRate; // br - y - // Reduce Hunting effect and jittering near setpoint. Limit multiple zero crossing within deadband and lower PID affect during low error amount - float dynReduction = tpaFactor; - if (pidProfile->toleranceBand) { - const float minReduction = (float)pidProfile->toleranceBandReduction / 100.0f; - static uint8_t zeroCrossCount[3]; - static uint8_t currentErrorPolarity[3]; - if (ABS(errorRate) < pidProfile->toleranceBand) { - if (zeroCrossCount[axis]) { - if (currentErrorPolarity[axis] == POSITIVE_ERROR) { - if (errorRate < 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = NEGATIVE_ERROR; - } - } else { - if (errorRate > 0 ) { - zeroCrossCount[axis]--; - currentErrorPolarity[axis] = POSITIVE_ERROR; - } - } - } else { - dynReduction *= constrainf(ABS(errorRate) / pidProfile->toleranceBand, minReduction, 1.0f); - } - } else { - zeroCrossCount[axis] = pidProfile->zeroCrossAllowanceCount; - currentErrorPolarity[axis] = (errorRate > 0) ? POSITIVE_ERROR : NEGATIVE_ERROR; - } - } - // -----calculate P component - PTerm = Kp[axis] * rP * dynReduction; + PTerm = Kp[axis] * rP * tpaFactor; // -----calculate I component. // Reduce strong Iterm accumulation during higher stick inputs @@ -289,7 +261,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // Divide delta by targetLooptime to get differential (ie dr/dt) delta *= (1.0f / getdT()); - if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * dynReduction; + if (debugMode == DEBUG_DTERM_FILTER) debug[axis] = Kd[axis] * delta * tpaFactor; // Filter delta if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta); @@ -302,7 +274,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc } } - DTerm = Kd[axis] * delta * dynReduction; + DTerm = Kd[axis] * delta * tpaFactor; // -----calculate total PID output axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -900, 900); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 56082360eb..df266e3273 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -63,11 +63,6 @@ typedef enum { SUPEREXPO_YAW_ALWAYS } pidSuperExpoYaw_e; -typedef enum { - NEGATIVE_ERROR = 0, - POSITIVE_ERROR -} pidErrorPolarity_e; - typedef enum { PID_STABILISATION_OFF = 0, PID_STABILISATION_ON @@ -94,9 +89,6 @@ typedef struct pidProfile_s { uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active. // Betaflight PID controller parameters - uint8_t toleranceBand; // Error tolerance area where toleranceBandReduction is applied under certain circumstances - uint8_t toleranceBandReduction; // Lowest possible P and D reduction in percentage - uint8_t zeroCrossAllowanceCount; // Amount of bouncebacks within tolerance band allowed before reduction kicks in uint8_t itermThrottleGain; // Throttle coupling to iterm. Quick throttle changes will bump iterm uint8_t ptermSRateWeight; // Setpoint super expo ratio for Pterm (lower means that pretty much only P has super expo rates) uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 6099732163..6cbb66ee5c 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -850,9 +850,6 @@ const clivalue_t valueTable[] = { { "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_notch_cutoff, .config.minmax = { 1, 500 } }, { "vbat_pid_compensation", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.vbatPidCompensation, .config.lookup = { TABLE_OFF_ON } }, { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidAtMinThrottle, .config.lookup = { TABLE_OFF_ON } }, - { "pid_tolerance_band", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBand, .config.minmax = {0, 200 } }, - { "tolerance_band_reduction", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.toleranceBandReduction, .config.minmax = {0, 100 } }, - { "zero_cross_allowance", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } }, { "iterm_throttle_gain", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } }, { "pterm_srate_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSRateWeight, .config.minmax = {0, 100 } }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 5d1367f999..9b40111dbd 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1270,8 +1270,8 @@ static bool processOutCommand(uint8_t cmdMSP) serialize8(currentProfile->pidProfile.vbatPidCompensation); serialize8(currentProfile->pidProfile.ptermSRateWeight); serialize8(currentProfile->pidProfile.dtermSetpointWeight); - serialize8(currentProfile->pidProfile.toleranceBand); - serialize8(currentProfile->pidProfile.toleranceBandReduction); + serialize8(0); // reserved + serialize8(0); // reserved serialize8(currentProfile->pidProfile.itermThrottleGain); serialize16(currentProfile->pidProfile.rateAccelLimit); serialize16(currentProfile->pidProfile.yawRateAccelLimit); @@ -1868,8 +1868,8 @@ static bool processInCommand(void) currentProfile->pidProfile.vbatPidCompensation = read8(); currentProfile->pidProfile.ptermSRateWeight = read8(); currentProfile->pidProfile.dtermSetpointWeight = read8(); - currentProfile->pidProfile.toleranceBand = read8(); - currentProfile->pidProfile.toleranceBandReduction = read8(); + read8(); // reserved + read8(); // reserved currentProfile->pidProfile.itermThrottleGain = read8(); currentProfile->pidProfile.rateAccelLimit = read16(); currentProfile->pidProfile.yawRateAccelLimit = read16();