diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index e73f69387..a84b3768f 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1280,8 +1280,8 @@ static bool blackboxWriteSysinfo() masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count); break; case 36: - blackboxPrintfHeaderLine("dterm_differentiator:%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_differentiator); + blackboxPrintfHeaderLine("dynamic_pterm:%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pterm); break; case 37: blackboxPrintfHeaderLine("rollPitchItermResetRate:%d", diff --git a/src/main/config/config.c b/src/main/config/config.c index fc6fe1218..4d6c04612 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -148,10 +148,10 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->pidController = 1; pidProfile->P8[ROLL] = 45; - pidProfile->I8[ROLL] = 30; + pidProfile->I8[ROLL] = 35; pidProfile->D8[ROLL] = 18; pidProfile->P8[PITCH] = 45; - pidProfile->I8[PITCH] = 30; + pidProfile->I8[PITCH] = 35; pidProfile->D8[PITCH] = 18; pidProfile->P8[YAW] = 90; pidProfile->I8[YAW] = 40; @@ -177,11 +177,12 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D8[PIDVEL] = 75; pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; - pidProfile->yaw_lpf_hz = 100; + pidProfile->yaw_lpf_hz = 70; pidProfile->rollPitchItermResetRate = 200; pidProfile->rollPitchItermResetAlways = 0; pidProfile->yawItermResetRate = 50; pidProfile->dterm_lpf_hz = 70; // filtering ON by default + pidProfile->dynamic_pterm = 1; pidProfile->H_sensitivity = 75; // TODO - Cleanup during next EEPROM changes @@ -307,9 +308,9 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { controlRateConfig->rcExpo8 = 60; controlRateConfig->thrMid8 = 50; controlRateConfig->thrExpo8 = 0; - controlRateConfig->dynThrPID = 0; + controlRateConfig->dynThrPID = 20; controlRateConfig->rcYawExpo8 = 20; - controlRateConfig->tpa_breakpoint = 1500; + controlRateConfig->tpa_breakpoint = 1650; for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) { controlRateConfig->rates[axis] = 50; @@ -401,10 +402,10 @@ static void resetConf(void) masterConfig.dcm_kp = 2500; // 1.0 * 10000 masterConfig.dcm_ki = 0; // 0.003 * 10000 masterConfig.gyro_lpf = 0; // 256HZ default - masterConfig.gyro_sync_denom = 8; - masterConfig.gyro_soft_lpf_hz = 80; + masterConfig.gyro_sync_denom = 4; + masterConfig.gyro_soft_lpf_hz = 100; - masterConfig.pid_process_denom = 1; + masterConfig.pid_process_denom = 2; masterConfig.debug_mode = 0; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0246fa71f..cc3312820 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -93,6 +93,16 @@ float calculateExpoPlus(int axis, rxConfig_t *rxConfig) { return propFactor; } +uint16_t getDynamicKp(int axis, pidProfile_t *pidProfile) { + uint16_t dynamicKp; + + uint32_t dynamicFactor = constrain(ABS(rcCommand[axis] << 8) / DYNAMIC_PTERM_STICK_THRESHOLD, 0, 1 << 7); + + dynamicKp = ((pidProfile->P8[axis] << 8) + (pidProfile->P8[axis] * dynamicFactor)) >> 8; + + return dynamicKp; +} + void pidResetErrorAngle(void) { errorAngleI[ROLL] = 0; @@ -136,7 +146,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa { float RateError, AngleRate, gyroRate; float ITerm,PTerm,DTerm; - static float lastRate[3][PID_LAST_RATE_COUNT]; + static float lastRate[3]; float delta; int axis; float horizonLevelStrength = 1; @@ -200,11 +210,13 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // multiplication of rcCommand corresponds to changing the sticks scaling here RateError = AngleRate - gyroRate; + uint16_t kP = (pidProfile->dynamic_pterm) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis]; + // -----calculate P component if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) { - PTerm = (luxPTermScale * pidProfile->P8[axis] * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig)); + PTerm = (luxPTermScale * kP * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig)); } else { - PTerm = luxPTermScale * RateError * pidProfile->P8[axis] * tpaFactor; + PTerm = luxPTermScale * RateError * kP * tpaFactor; } // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply @@ -220,7 +232,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa } if (axis == YAW) { - if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD_YAW, ITERM_RESET_THRESHOLD_YAW); + if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD); } if (antiWindupProtection || motorLimitReached) { @@ -238,20 +250,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); DTerm = 0; } else { - if (pidProfile->dterm_differentiator) { - // Calculate derivative using noise-robust differentiator without time delay (one-sided or forward filters) - // by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/ - // N=5: h[0] = 3/8, h[-1] = 1/2, h[-2] = -1/2, h[-3] = -3/4, h[-4] = 1/8, h[-5] = 1/4 - delta = -(3*gyroRate + 4*lastRate[axis][0] - 4*lastRate[axis][1] - 6*lastRate[axis][2] + 1*lastRate[axis][3] + 2*lastRate[axis][4]) / 8; - for (int i = PID_LAST_RATE_COUNT - 1; i > 0; i--) { - lastRate[axis][i] = lastRate[axis][i-1]; - } - } else { - // When DTerm low pass filter disabled apply moving average to reduce noise - delta = -(gyroRate - lastRate[axis][0]); - } - - lastRate[axis][0] = gyroRate; + delta = -(gyroRate - lastRate[axis]); + lastRate[axis] = gyroRate; // Divide delta by targetLooptime to get differential (ie dr/dt) delta *= (1.0f / getdT()); @@ -288,7 +288,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co int axis; int32_t PTerm, ITerm, DTerm, delta; - static int32_t lastRate[3][PID_LAST_RATE_COUNT]; + static int32_t lastRate[3]; int32_t AngleRateTmp, RateError, gyroRate; int8_t horizonLevelStrength = 100; @@ -342,11 +342,13 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co gyroRate = gyroADC[axis] / 4; RateError = AngleRateTmp - gyroRate; + uint16_t kP = (pidProfile->dynamic_pterm) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis]; + // -----calculate P component if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) { - PTerm = (pidProfile->P8[axis] * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7; + PTerm = (kP * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7; } else { - PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; + PTerm = (RateError * kP * PIDweight[axis] / 100) >> 7; } // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply @@ -366,11 +368,11 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); if ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) { - if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD); + if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) -ITERM_RESET_THRESHOLD << 13, (int32_t) + ITERM_RESET_THRESHOLD << 13); } if (axis == YAW) { - if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], -ITERM_RESET_THRESHOLD_YAW, ITERM_RESET_THRESHOLD_YAW); + if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) -ITERM_RESET_THRESHOLD << 13, (int32_t) + ITERM_RESET_THRESHOLD << 13); } if (antiWindupProtection || motorLimitReached) { @@ -386,20 +388,8 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); DTerm = 0; } else { - if (pidProfile->dterm_differentiator) { - // Calculate derivative using noise-robust differentiator without time delay (one-sided or forward filters) - // by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/ - // N=5: h[0] = 3/8, h[-1] = 1/2, h[-2] = -1/2, h[-3] = -3/4, h[-4] = 1/8, h[-5] = 1/4 - delta = -(3*gyroRate + 4*lastRate[axis][0] - 4*lastRate[axis][1] - 6*lastRate[axis][2] + 1*lastRate[axis][3] + 2*lastRate[axis][4]) / 8; - for (int i = PID_LAST_RATE_COUNT - 1; i > 0; i--) { - lastRate[axis][i] = lastRate[axis][i-1]; - } - } else { - // When DTerm low pass filter disabled apply moving average to reduce noise - delta = -(gyroRate - lastRate[axis][0]); - } - - lastRate[axis][0] = gyroRate; + delta = -(gyroRate - lastRate[axis]); + lastRate[axis] = gyroRate; // Divide delta by targetLooptime to get differential (ie dr/dt) delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 3f9bb7ef7..3738f9763 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -22,9 +22,8 @@ #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter #define YAW_P_LIMIT_MAX 400 // Maximum value for yaw P limiter -#define PID_LAST_RATE_COUNT 7 -#define ITERM_RESET_THRESHOLD 20 -#define ITERM_RESET_THRESHOLD_YAW 10 +#define ITERM_RESET_THRESHOLD 15 +#define DYNAMIC_PTERM_STICK_THRESHOLD 400 typedef enum { PIDROLL, @@ -81,7 +80,7 @@ typedef struct pidProfile_s { uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates uint16_t yaw_p_limit; uint8_t dterm_average_count; // Configurable delta count for dterm - uint8_t dterm_differentiator; + uint8_t dynamic_pterm; #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index dc7549891..71583fbad 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -653,7 +653,7 @@ const clivalue_t valueTable[] = { { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } }, - { "gyro_lowpass_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } }, + { "gyro_lowpass", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 500 } }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } }, @@ -721,12 +721,12 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, - { "dterm_lowpass_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, - { "dterm_differentiator", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_differentiator, .config.lookup = { TABLE_OFF_ON } }, - { "iterm_always_reset", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetAlways, .config.lookup = { TABLE_OFF_ON } }, + { "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, + { "dynamic_pterm", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dynamic_pterm, .config.lookup = { TABLE_OFF_ON } }, + { "iterm_always_reset", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.rollPitchItermResetAlways, .config.lookup = { TABLE_OFF_ON } }, { "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } }, { "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } }, - { "yaw_lowpass_hz", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, + { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index c78212430..d44dc8e9a 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -152,7 +152,7 @@ void setGyroSamplingSpeed(uint16_t looptime) { masterConfig.pid_process_denom = 1; if (currentProfile->pidProfile.pidController == 2) masterConfig.pid_process_denom = 2; if (looptime < 250) { - masterConfig.pid_process_denom = 3; + masterConfig.pid_process_denom = 4; } else if (looptime < 375) { if (currentProfile->pidProfile.pidController == 2) { masterConfig.pid_process_denom = 3; diff --git a/src/main/mw.c b/src/main/mw.c index 80d588baf..7e8dfcd38 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -770,17 +770,6 @@ uint8_t setPidUpdateCountDown(void) { } } -// Check for oneshot125 protection. With fast looptimes oneshot125 pulse duration gets more near the pid looptime -bool shouldUpdateMotorsAfterPIDLoop(void) { - if (targetPidLooptime > 375 ) { - return true; - } else if ((masterConfig.use_multiShot || masterConfig.use_oneshot42) && feature(FEATURE_ONESHOT125)) { - return true; - } else { - return false; - } -} - // Function for loop trigger void taskMainPidLoopCheck(void) { static uint32_t previousTime; @@ -801,7 +790,6 @@ void taskMainPidLoopCheck(void) { static uint8_t pidUpdateCountdown; if (runTaskMainSubprocesses) { - if (!shouldUpdateMotorsAfterPIDLoop()) taskMotorUpdate(); subTasksMainPidLoop(); runTaskMainSubprocesses = false; } @@ -813,7 +801,7 @@ void taskMainPidLoopCheck(void) { } else { pidUpdateCountdown = setPidUpdateCountDown(); taskMainPidLoop(); - if (shouldUpdateMotorsAfterPIDLoop()) taskMotorUpdate(); + taskMotorUpdate(); runTaskMainSubprocesses = true; }