diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 78d5ffc821..50e4097c51 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1280,7 +1280,7 @@ static bool blackboxWriteSysinfo() // Betaflight PID controller parameters 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("setpointRelaxRatio:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.setpointRelaxRatio); BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.dtermSetpointWeight); BLACKBOX_PRINT_HEADER_LINE("yawRateAccelLimit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawRateAccelLimit); BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.rateAccelLimit); diff --git a/src/main/config/config.c b/src/main/config/config.c index 90effae403..f5f9f57a72 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -247,8 +247,8 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; // Betaflight PID controller parameters - pidProfile->ptermSRateWeight = 85; - pidProfile->dtermSetpointWeight = 150; + pidProfile->setpointRelaxRatio = 70; + pidProfile->dtermSetpointWeight = 200; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; pidProfile->itermThrottleGain = 0; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 39fa4b8064..d00dd3b0f3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -49,7 +49,7 @@ extern uint8_t motorCount; uint32_t targetPidLooptime; -extern float setpointRate[3], ptermSetpointRate[3]; +extern float setpointRate[3]; extern float rcInput[3]; static bool pidStabilisationEnabled; @@ -132,10 +132,10 @@ void initFilters(const pidProfile_t *pidProfile) { static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig) { - float errorRate = 0, rP = 0, rD = 0, PVRate = 0; + float errorRate = 0, rD = 0, PVRate = 0, dynC; float ITerm,PTerm,DTerm; static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; + static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3], relaxFactor[3]; float delta; int axis; float horizonLevelStrength = 1; @@ -188,6 +188,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; c[axis] = pidProfile->dtermSetpointWeight / 100.0f; + relaxFactor[axis] = 1.0f - (pidProfile->setpointRelaxRatio / 100.0f); yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT(); rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT(); @@ -218,11 +219,11 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc #endif if (FLIGHT_MODE(ANGLE_MODE)) { // ANGLE mode - control is angle based, so control loop is needed - ptermSetpointRate[axis] = setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; + setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; } else { // HORIZON mode - direct sticks control is applied to rate PID // mix up angle error to desired AngleRate to add a little auto-level feel - ptermSetpointRate[axis] = setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); + setpointRate[axis] = setpointRate[axis] + (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 10.0f); } } @@ -233,10 +234,9 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes // ----- calculate error / angle rates ---------- errorRate = setpointRate[axis] - PVRate; // r - y - rP = ptermSetpointRate[axis] - PVRate; // br - y - // -----calculate P component - PTerm = Kp[axis] * rP * tpaFactor; + // -----calculate P component and add Dynamic Part based on stick input + PTerm = Kp[axis] * errorRate * tpaFactor; // -----calculate I component. // Reduce strong Iterm accumulation during higher stick inputs @@ -254,7 +254,11 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc //-----calculate D-term (Yaw D not yet supported) if (axis != YAW) { - rD = c[axis] * setpointRate[axis] - PVRate; // cr - y + if (pidProfile->setpointRelaxRatio < 100) + dynC = c[axis] * powerf(rcInput[axis], 2) * relaxFactor[axis] + c[axis] * (1-relaxFactor[axis]); + else + dynC = c[axis]; + rD = dynC * setpointRate[axis] - PVRate; // cr - y delta = rD - lastRateError[axis]; lastRateError[axis] = rD; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index df266e3273..c1684b3c76 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -90,7 +90,7 @@ typedef struct pidProfile_s { // Betaflight PID controller parameters 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 setpointRelaxRatio; // Setpoint weight relaxation effect uint8_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > agressive derivative) uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1fdbb2507d..997389493c 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -477,7 +477,7 @@ static OSD_FLOAT_t entryRcExpo = {&rateProfile.rcExpo8, 0, 100, 1, 10}; static OSD_FLOAT_t entryRcExpoYaw = {&rateProfile.rcYawExpo8, 0, 100, 1, 10}; static OSD_FLOAT_t extryTpaEntry = {&rateProfile.dynThrPID, 0, 70, 1, 10}; static OSD_UINT16_t entryTpaBreak = {&rateProfile.tpa_breakpoint, 1100, 1800, 10}; -static OSD_FLOAT_t entryPSetpoint = {&masterConfig.profile[0].pidProfile.ptermSRateWeight, 0, 100, 1, 10}; +static OSD_FLOAT_t entryPSetpoint = {&masterConfig.profile[0].pidProfile.setpointRelaxRatio, 0, 100, 1, 10}; static OSD_FLOAT_t entryDSetpoint = {&masterConfig.profile[0].pidProfile.dtermSetpointWeight, 0, 255, 1, 10}; OSD_Entry menuRateExpo[] = diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 7d1b475715..526e3a9a2a 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -851,7 +851,7 @@ const clivalue_t valueTable[] = { { "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 } }, { "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 } }, + { "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.setpointRelaxRatio, .config.minmax = {0, 100 } }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } }, { "yaw_rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } }, { "rate_accel_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 19f12dd8a6..9a63f909ee 100755 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1274,7 +1274,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(currentProfile->pidProfile.yaw_p_limit); serialize8(currentProfile->pidProfile.deltaMethod); serialize8(currentProfile->pidProfile.vbatPidCompensation); - serialize8(currentProfile->pidProfile.ptermSRateWeight); + serialize8(currentProfile->pidProfile.setpointRelaxRatio); serialize8(currentProfile->pidProfile.dtermSetpointWeight); serialize8(0); // reserved serialize8(0); // reserved @@ -1877,7 +1877,7 @@ static bool processInCommand(void) currentProfile->pidProfile.yaw_p_limit = read16(); currentProfile->pidProfile.deltaMethod = read8(); currentProfile->pidProfile.vbatPidCompensation = read8(); - currentProfile->pidProfile.ptermSRateWeight = read8(); + currentProfile->pidProfile.setpointRelaxRatio = read8(); currentProfile->pidProfile.dtermSetpointWeight = read8(); read8(); // reserved read8(); // reserved diff --git a/src/main/mw.c b/src/main/mw.c index cc5f416ec7..58ed04bb65 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -122,7 +122,7 @@ extern uint8_t PIDweight[3]; static bool isRXDataNew; static bool armingCalibrationWasInitialised; -float setpointRate[3], ptermSetpointRate[3]; +float setpointRate[3]; float rcInput[3]; extern pidControllerFuncPtr pid_controller; @@ -200,19 +200,7 @@ void calculateSetpointRate(int axis, int16_t rc) { if (currentControlRateProfile->rates[axis]) { rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f)); - if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) { - ptermSetpointRate[axis] = constrainf(angleRate * rcSuperfactor, -1998.0f, 1998.0f); - if (currentProfile->pidProfile.ptermSRateWeight < 100 && axis != YAW && !flightModeFlags) { - const float pWeight = currentProfile->pidProfile.ptermSRateWeight / 100.0f; - angleRate = angleRate + (pWeight * ptermSetpointRate[axis] - angleRate); - } else { - angleRate = ptermSetpointRate[axis]; - } - } else { - angleRate *= rcSuperfactor; - } - } else { - if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) ptermSetpointRate[axis] = angleRate; + angleRate *= rcSuperfactor; } if (debugMode == DEBUG_ANGLERATE) {