Scale antiWindup increment to looptime. It is now always 500ms
This commit is contained in:
parent
cdb671b0d6
commit
0e735ae313
|
@ -85,18 +85,22 @@ void pidResetErrorGyro(void)
|
||||||
float scaleItermToRcInput(int axis) {
|
float scaleItermToRcInput(int axis) {
|
||||||
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
|
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
|
||||||
static float iTermScaler[3] = {1.0f, 1.0f, 1.0f};
|
static float iTermScaler[3] = {1.0f, 1.0f, 1.0f};
|
||||||
|
static float antiWindUpIncrement = 0;
|
||||||
|
|
||||||
|
if (antiWindUpIncrement) antiWindUpIncrement = 0.001 * (targetLooptime / 500); // Calculate increment for 500ms period
|
||||||
|
|
||||||
if (ABS(rcCommandReflection) > 0.7f && (!flightModeFlags)) { /* 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 */
|
||||||
iTermScaler[axis] = 0.0f;
|
iTermScaler[axis] = 0.0f;
|
||||||
} else {
|
} else {
|
||||||
/* Prevent rapid windup during acro recoveries. Slowly enable Iterm activity. Perhaps more scaling to looptime needed for consistency */
|
/* Prevent rapid windup during acro recoveries. Slowly enable Iterm for period of 500ms */
|
||||||
if (iTermScaler[axis] < 1) {
|
if (iTermScaler[axis] < 1) {
|
||||||
iTermScaler[axis] = constrainf(iTermScaler[axis] + 0.001f, 0.0f, 1.0f);
|
iTermScaler[axis] = constrainf(iTermScaler[axis] + antiWindUpIncrement, 0.0f, 1.0f);
|
||||||
} else {
|
} else {
|
||||||
iTermScaler[axis] = 1;
|
iTermScaler[axis] = 1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return iTermScaler[axis];
|
return iTermScaler[axis];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue