From 0d1cc8f4484e9c5149b4471fdcf16a9e2c34e1bb Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 8 Sep 2016 16:55:34 +0200 Subject: [PATCH] Reimplementation of P setpoint weight. Applied to S rates now for nicer feel --- src/main/blackbox/blackbox.c | 2 +- src/main/config/config.c | 10 +++++----- src/main/flight/pid.c | 11 +++-------- src/main/flight/pid.h | 2 +- src/main/io/serial_cli.c | 2 +- src/main/io/serial_msp.c | 4 ++-- src/main/mw.c | 24 ++++++++++++++++++------ 7 files changed, 31 insertions(+), 24 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index b9d60e1c10..5c0ca9f89a 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1283,7 +1283,7 @@ static bool blackboxWriteSysinfo() 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("ptermSetpointWeight:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.ptermSetpointWeight); + 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); 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 3e058a6fec..ffb4367c95 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -172,7 +172,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 145; +static const uint8_t EEPROM_CONF_VERSION = 146; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) { @@ -235,7 +235,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D8[PIDVEL] = 75; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; - pidProfile->yaw_lpf_hz = 80; + pidProfile->yaw_lpf_hz = 0; pidProfile->rollPitchItermIgnoreRate = 130; pidProfile->yawItermIgnoreRate = 32; pidProfile->dterm_filter_type = FILTER_BIQUAD; @@ -247,7 +247,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidAtMinThrottle = PID_STABILISATION_ON; // Betaflight PID controller parameters - pidProfile->ptermSetpointWeight = 85; + pidProfile->ptermSRateWeight = 50; pidProfile->dtermSetpointWeight = 150; pidProfile->yawRateAccelLimit = 220; pidProfile->rateAccelLimit = 0; @@ -491,8 +491,8 @@ void createDefaultConfig(master_t *config) #endif config->gyro_soft_type = FILTER_PT1; config->gyro_soft_lpf_hz = 90; - config->gyro_soft_notch_hz = 0; - config->gyro_soft_notch_cutoff = 150; + config->gyro_soft_notch_hz = 210; + 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 8950db6ab1..234b98462b 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]; +extern float setpointRate[3], ptermSetpointRate[3]; extern float rcInput[3]; static bool pidStabilisationEnabled; @@ -135,7 +135,7 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc float errorRate = 0, rP = 0, rD = 0, PVRate = 0; float ITerm,PTerm,DTerm; static float lastRateError[2]; - static float Kp[3], Ki[3], Kd[3], b[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; + static float Kp[3], Ki[3], Kd[3], c[3], rollPitchMaxVelocity, yawMaxVelocity, previousSetpoint[3]; float delta; int axis; float horizonLevelStrength = 1; @@ -187,7 +187,6 @@ static void pidBetaflight(const pidProfile_t *pidProfile, uint16_t max_angle_inc Kp[axis] = PTERM_SCALE * pidProfile->P8[axis]; Ki[axis] = ITERM_SCALE * pidProfile->I8[axis]; Kd[axis] = DTERM_SCALE * pidProfile->D8[axis]; - b[axis] = pidProfile->ptermSetpointWeight / 100.0f; c[axis] = pidProfile->dtermSetpointWeight / 100.0f; yawMaxVelocity = pidProfile->yawRateAccelLimit * 1000 * getdT(); rollPitchMaxVelocity = pidProfile->rateAccelLimit * 1000 * getdT(); @@ -234,11 +233,7 @@ 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 = b[axis] * setpointRate[axis] - PVRate; // br - y - - // Slowly restore original setpoint with more stick input - float diffRate = errorRate - rP; - rP += diffRate * rcInput[axis]; + 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; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index fd1fb08f5a..56082360eb 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -98,7 +98,7 @@ typedef struct pidProfile_s { 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 ptermSetpointWeight; // Setpoint weight for Pterm (lower means more PV tracking) + 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) 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/serial_cli.c b/src/main/io/serial_cli.c index 0b64d0526f..6099732163 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -854,7 +854,7 @@ const clivalue_t valueTable[] = { { "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_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {40, 100 } }, + { "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 } }, { "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 33bd4a74c9..5d1367f999 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1268,7 +1268,7 @@ static bool processOutCommand(uint8_t cmdMSP) serialize16(currentProfile->pidProfile.yaw_p_limit); serialize8(currentProfile->pidProfile.deltaMethod); serialize8(currentProfile->pidProfile.vbatPidCompensation); - serialize8(currentProfile->pidProfile.ptermSetpointWeight); + serialize8(currentProfile->pidProfile.ptermSRateWeight); serialize8(currentProfile->pidProfile.dtermSetpointWeight); serialize8(currentProfile->pidProfile.toleranceBand); serialize8(currentProfile->pidProfile.toleranceBandReduction); @@ -1866,7 +1866,7 @@ static bool processInCommand(void) currentProfile->pidProfile.yaw_p_limit = read16(); currentProfile->pidProfile.deltaMethod = read8(); currentProfile->pidProfile.vbatPidCompensation = read8(); - currentProfile->pidProfile.ptermSetpointWeight = read8(); + currentProfile->pidProfile.ptermSRateWeight = read8(); currentProfile->pidProfile.dtermSetpointWeight = read8(); currentProfile->pidProfile.toleranceBand = read8(); currentProfile->pidProfile.toleranceBandReduction = read8(); diff --git a/src/main/mw.c b/src/main/mw.c index faed2088b0..de92425d56 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -123,7 +123,7 @@ extern uint8_t PIDweight[3]; uint16_t filteredCycleTime; static bool isRXDataNew; static bool armingCalibrationWasInitialised; -float setpointRate[3]; +float setpointRate[3], ptermSetpointRate[3]; float rcInput[3]; extern pidControllerFuncPtr pid_controller; @@ -176,7 +176,7 @@ bool isCalibrating() #define RC_RATE_INCREMENTAL 14.54f #define RC_EXPO_POWER 3 -float calculateSetpointRate(int axis, int16_t rc) { +void calculateSetpointRate(int axis, int16_t rc) { float angleRate, rcRate, rcSuperfactor, rcCommandf; uint8_t rcExpo; @@ -201,7 +201,19 @@ float 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)); - angleRate *= rcSuperfactor; + if (currentProfile->pidProfile.pidController == PID_CONTROLLER_BETAFLIGHT) { + ptermSetpointRate[axis] = angleRate * rcSuperfactor; + if (currentProfile->pidProfile.ptermSRateWeight < 100) { + 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; } if (debugMode == DEBUG_ANGLERATE) { @@ -209,9 +221,9 @@ float calculateSetpointRate(int axis, int16_t rc) { } if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY) - return constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection + setpointRate[axis] = constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection else - return constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec) + setpointRate[axis] = constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec) } void scaleRcCommandToFpvCamAngle(void) { @@ -290,7 +302,7 @@ void processRcCommand(void) if (masterConfig.rxConfig.fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) scaleRcCommandToFpvCamAngle(); - for (int axis = 0; axis < 3; axis++) setpointRate[axis] = calculateSetpointRate(axis, rcCommand[axis]); + for (int axis = 0; axis < 3; axis++) calculateSetpointRate(axis, rcCommand[axis]); isRXDataNew = false; }