rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fix Iterm reset in super expo mode // new defaults

This commit is contained in:
borisbstyle 2016-03-30 21:23:14 +02:00
parent 712beb2ae8
commit 3e5e30c42c
4 changed files with 30 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/config/config.c
View File

@ -179,8 +179,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
pidProfile->yaw_lpf_hz = 0.0f;
pidProfile->dterm_average_count = 4;
pidProfile->rollPitchItermResetRate = 0;
pidProfile->yawItermResetRate = 0;
pidProfile->rollPitchItermResetRate = 200;
pidProfile->yawItermResetRate = 50;
pidProfile->dterm_lpf_hz = 70.0f; // filtering ON by default
pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;

54
src/main/flight/pid.c
View File

@ -204,22 +204,20 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate I component.
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * getdT() * pidProfile->I8[axis] / 10.0f, -250.0f, 250.0f);
if (pidProfile->rollPitchItermResetRate && axis != YAW) {
if (ABS(gyroRate) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0;
}
if (pidProfile->yawItermResetRate && axis == YAW) {
if (ABS(gyroRate) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0;
}
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
if (antiWindupProtection || motorLimitReached) {
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) {
if (axis == YAW) {
if (ABS(gyroRate) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0;
} else {
errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]);
if (ABS(gyroRate) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0;
}
}
if (antiWindupProtection || motorLimitReached) {
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
} else {
errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]);
}
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
ITerm = errorGyroIf[axis];
@ -299,12 +297,10 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
}
// Anti windup protection
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
if (antiWindupProtection || motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {
errorGyroILimit[axis] = ABS(errorGyroI[axis]);
}
if (antiWindupProtection || motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {
errorGyroILimit[axis] = ABS(errorGyroI[axis]);
}
ITerm = (errorGyroI[axis] >> 7) * pidProfile->I8[axis] >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
@ -495,22 +491,20 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
// I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
if (pidProfile->rollPitchItermResetRate && axis != YAW) {
if (ABS(gyroRate / 4) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = 0;
}
if (pidProfile->yawItermResetRate && axis == YAW) {
if (ABS(gyroRate / 4) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = 0;
}
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
if (antiWindupProtection || motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
if (IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) {
if (axis == YAW) {
if (ABS(gyroRate / 4) >= pidProfile->yawItermResetRate) errorGyroI[axis] = 0;
} else {
errorGyroILimit[axis] = ABS(errorGyroI[axis]);
if (ABS(gyroRate / 4) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = 0;
}
}
if (antiWindupProtection || motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {
errorGyroILimit[axis] = ABS(errorGyroI[axis]);
}
ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term

4
src/main/flight/pid.h
View File

@ -67,9 +67,9 @@ typedef struct pidProfile_s {
float dterm_lpf_hz; // Delta Filter in hz
float yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy
uint8_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates
uint16_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates
uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates
uint16_t deltaMethod; // Alternative delta Calculation
uint8_t deltaMethod; // Alternative delta Calculation
uint16_t yaw_p_limit;
uint8_t dterm_average_count; // Configurable delta count for dterm

4
src/main/io/serial_cli.c
View File

@ -729,8 +729,8 @@ const clivalue_t valueTable[] = {
{ "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } },
{ "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {0, 12 } },
{ "roll_pitch_iterm_reset_deg", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {0, 1000 } },
{ "yaw_iterm_reset_deg", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {0, 1000 } },
{ "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_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
{ "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } },