Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity

Fix Iterm Bug

Browse source
This commit is contained in:
borisbstyle 2016-02-19 23:28:50 +01:00
parent 293820f567
commit 850700df14
1 changed files with 20 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

41
src/main/flight/pid.c
View file

@ -94,32 +94,29 @@ void pidResetErrorGyro(void)
} }
} }
q_number_t scaleItermToRcInput(int axis) { void scaleItermToRcInput(int axis, pidProfile_t *pidProfile) {
int16_t rcCommandReflection = ABS(rcCommand[axis]); float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
static int16_t antiWindUpIncrement; static float iTermScaler[3] = {1.0f, 1.0f, 1.0f};
static q_number_t qItermScaler[3] = { static float antiWindUpIncrement = 0;
{ .num= Q13(1), .den = Q13(1) },
{ .num= Q13(1), .den = Q13(1) },
{ .num= Q13(1), .den = Q13(1) },
};
if (!antiWindUpIncrement) { if (!antiWindUpIncrement) antiWindUpIncrement = (0.001 / 500) * targetLooptime; // Calculate increment for 500ms period
antiWindUpIncrement = constrain(targetLooptime >> 6, 1, Q13(1)); // Calculate increment for 512ms period. Should be consistent up to 8khz
}
if (rcCommandReflection > 350) { /* scaling should not happen in level modes */ if (ABS(rcCommandReflection) > 0.7f && (!flightModeFlags)) { /* scaling should not happen in level modes */
/* Reset Iterm on high stick inputs. No scaling necessary here */ /* Reset Iterm on high stick inputs. No scaling necessary here */
qItermScaler[axis].num = 0; iTermScaler[axis] = 0.0f;
errorGyroI[axis] = 0;
errorGyroIf[axis] = 0.0f;
} else { } else {
/* Prevent rapid windup during acro recoveries. Slowly enable Iterm for period of 512ms */ /* Prevent rapid windup during acro recoveries. Slowly enable Iterm for period of 500ms */
if (qItermScaler[axis].num <= (Q13(1) - antiWindUpIncrement)) { if (iTermScaler[axis] < 1) {
qItermScaler[axis].num = qItermScaler[axis].num + antiWindUpIncrement; iTermScaler[axis] = constrainf(iTermScaler[axis] + antiWindUpIncrement, 0.0f, 1.0f);
if (pidProfile->pidController != PID_CONTROLLER_LUX_FLOAT) {
errorGyroI[axis] *= iTermScaler[axis];
} else { } else {
qItermScaler[axis].num = Q13(1); errorGyroIf[axis] *= iTermScaler[axis];
}
} }
} }
return qItermScaler[axis];
} }
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
@ -202,7 +199,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f); errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) { if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
errorGyroIf[axis] = qMultiply(scaleItermToRcInput(axis),(int16_t) errorGyroIf[axis] * 10) / 10.0f; // Scale up and down to avoid truncating scaleItermToRcInput(axis, pidProfile);
if (antiWindupProtection || motorLimitReached) { if (antiWindupProtection || motorLimitReached) {
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]); errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
} else { } else {
@ -293,7 +290,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
// Anti windup protection // Anti windup protection
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) { if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
errorGyroI[axis] = qMultiply(scaleItermToRcInput(axis),errorGyroI[axis]); scaleItermToRcInput(axis, pidProfile);
if (antiWindupProtection || motorLimitReached) { if (antiWindupProtection || motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]); errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else { } else {
@ -484,7 +481,7 @@ 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); errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) { if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
errorGyroI[axis] = qMultiply(scaleItermToRcInput(axis),errorGyroI[axis]); scaleItermToRcInput(axis, pidProfile);
if (antiWindupProtection || motorLimitReached) { if (antiWindupProtection || motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]); errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else { } else {