diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 8ada09a22..3f1a48140 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1296,8 +1296,8 @@ static bool blackboxWriteSysinfo() (int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f)); break; case 40: - blackboxPrintfHeaderLine("H_sensitivity:%d", - masterConfig.profile[masterConfig.current_profile_index].pidProfile.H_sensitivity); + blackboxPrintfHeaderLine("iterm_reset_offset:%d", + masterConfig.profile[masterConfig.current_profile_index].pidProfile.itermResetOffset); break; case 41: blackboxPrintfHeaderLine("deadband:%d", masterConfig.rcControlsConfig.deadband); diff --git a/src/main/config/config.c b/src/main/config/config.c index d3759bdd3..1e0f4961e 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -187,6 +187,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->rollPitchItermResetRate = 200; pidProfile->rollPitchItermResetAlways = 0; pidProfile->yawItermResetRate = 50; + pidProfile->itermResetOffset = 15; pidProfile->dterm_lpf_hz = 80; // filtering ON by default pidProfile->dynamic_pterm = 1; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0a5cf87db..85de0917a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -88,7 +88,7 @@ float calculateExpoPlus(int axis, rxConfig_t *rxConfig) { propFactor = 1.0f; } else { superExpoFactor = (axis == YAW) ? rxConfig->superExpoFactorYaw : rxConfig->superExpoFactor; - propFactor = 1.0f - ((superExpoFactor / 100.0f) * (ABS(rcCommand[axis]) / 500.0f)); + propFactor = constrainf(1.0f - ((superExpoFactor / 100.0f) * (ABS(rcCommand[axis]) / 500.0f)), 0.0f, 1.0f); } return propFactor; @@ -223,11 +223,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * pidProfile->I8[axis], -250.0f, 250.0f); if ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) { - if (ABS(gyroRate / 4.1f) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD); + if (ABS(gyroRate / 4.1f) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -pidProfile->itermResetOffset, pidProfile->itermResetOffset); } if (axis == YAW) { - if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ITERM_RESET_THRESHOLD, ITERM_RESET_THRESHOLD); + if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -YAW_ITERM_RESET_OFFSET, YAW_ITERM_RESET_OFFSET); } if (antiWindupProtection || motorLimitReached) { @@ -364,11 +364,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], (int32_t) -ITERM_RESET_THRESHOLD << 13, (int32_t) + ITERM_RESET_THRESHOLD << 13); + if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) -pidProfile->itermResetOffset << 13, (int32_t) + pidProfile->itermResetOffset << 13); } if (axis == 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 (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) -YAW_ITERM_RESET_OFFSET << 13, (int32_t) + YAW_ITERM_RESET_OFFSET << 13); } if (antiWindupProtection || motorLimitReached) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 361ff9508..cdb571c17 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -22,7 +22,7 @@ #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 ITERM_RESET_THRESHOLD 15 +#define YAW_ITERM_RESET_OFFSET 15 // May be made configurable in the future, but not really needed for yaw #define DYNAMIC_PTERM_STICK_THRESHOLD 400 typedef enum { @@ -71,10 +71,9 @@ typedef struct pidProfile_s { uint8_t I8[PID_ITEM_COUNT]; uint8_t D8[PID_ITEM_COUNT]; - uint8_t H_sensitivity; - - uint16_t dterm_lpf_hz; // Delta Filter in hz - uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy + uint8_t itermResetOffset; // Reset offset for Iterm + uint16_t dterm_lpf_hz; // Delta Filter in hz + uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy uint16_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates uint8_t rollPitchItermResetAlways; // Reset Iterm also without SUPER EXPO uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 423c59bc4..f30b86fb5 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -734,6 +734,7 @@ const clivalue_t valueTable[] = { { "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 } }, + { "iterm_reset_offset", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermResetOffset, .config.minmax = { 0, 100 } }, { "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 } },