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

Fix windup issues in aidModePlus

Browse source
This commit is contained in:
borisbstyle 2016-01-11 01:42:40 +01:00
parent 91a30da9c8
commit 0216d9a18d
2 changed files with 19 additions and 36 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -83,43 +83,29 @@ void pidResetErrorGyro(void)
errorGyroIf[YAW] = 0.0f; errorGyroIf[YAW] = 0.0f;
} }
void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile, float referenceTerm) { static float minItermScaler = 1;
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile) {
float rcCommandReflection = ABS((float)rcCommand[axis] / 500.0f);
axisState->wowFactor = 1; axisState->wowFactor = 1;
axisState->factor = 0; axisState->factor = 0;
axisState->iTermScaler = 1;
if (rcCommandReflection > 0.7f) { if (rcCommandReflection > 0.7f) {
//Ki scaler //Ki scaler
axisState->iTermScaler = constrainf(1.0f - (1.5f * ABS(rcCommandReflection)), 0.0f, 1.0f); axisState->iTermScaler = constrainf(1.0f - (1.5f * rcCommandReflection), 0.0f, minItermScaler);
if (minItermScaler > axisState->iTermScaler) minItermScaler = axisState->iTermScaler;
//dynamic Ki handler
if (axisState->isCurrentlyAtZero) {
if (axisState->previousReferenceIsPositive ^ IS_POSITIVE(referenceTerm)) {
axisState->isCurrentlyAtZero = false;
} else {
axisState->iTermScaler = 0;
errorGyroIf[axis] = 0;
errorGyroI[axis] = 0;
}
}
if (!axisState->iTermScaler) {
if (!axisState->isCurrentlyAtZero) {
if (IS_POSITIVE(referenceTerm)) {
axisState->previousReferenceIsPositive = true;
} else {
axisState->previousReferenceIsPositive = false;
}
} else {
axisState->isCurrentlyAtZero = true;
}
}
if (axis != YAW && pidProfile->airModeInsaneAcrobilityFactor) { if (axis != YAW && pidProfile->airModeInsaneAcrobilityFactor) {
axisState->wowFactor = 1.0f - (ABS(rcCommandReflection) * ((float)pidProfile->airModeInsaneAcrobilityFactor / 100.0f)); //0-1f axisState->wowFactor = 1.0f - (rcCommandReflection * ((float)pidProfile->airModeInsaneAcrobilityFactor / 100.0f)); //0-1f
axisState->factor = (axisState->wowFactor * rcCommandReflection) * 1000; axisState->factor = (axisState->wowFactor * rcCommandReflection) * 1000;
} }
} else {
// Prevent rapid windup
if (minItermScaler < 1) {
minItermScaler = constrainf(minItermScaler + 0.001f, 0.0f, 1.0f);
} else {
minItermScaler = 1;
}
} }
} }
@ -202,8 +188,8 @@ 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)) { if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
airModePlus(&airModePlusAxisState[axis], axis, pidProfile, PTerm); airModePlus(&airModePlusAxisState[axis], axis, pidProfile);
errorGyroIf[axis] *= airModePlusAxisState[axis].iTermScaler; errorGyroIf[axis] *= minItermScaler;
} }
if (allowITermShrinkOnly || motorLimitReached) { if (allowITermShrinkOnly || motorLimitReached) {
@ -336,14 +322,13 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// I coefficient (I8) moved before integration to make limiting independent from PID settings // 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); errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
ITerm = errorGyroI[axis] >> 13;
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) { if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
airModePlus(&airModePlusAxisState[axis], axis, pidProfile, (float) PTerm); airModePlus(&airModePlusAxisState[axis], axis, pidProfile);
errorGyroI[axis] *= airModePlusAxisState[axis].iTermScaler; errorGyroI[axis] *= minItermScaler;
} }
ITerm = errorGyroI[axis] >> 13;
if (allowITermShrinkOnly || motorLimitReached) { if (allowITermShrinkOnly || motorLimitReached) {
if (ABS(errorGyroI[axis]) < ABS(previousErrorGyroI[axis])) { if (ABS(errorGyroI[axis]) < ABS(previousErrorGyroI[axis])) {
previousErrorGyroI[axis] = errorGyroI[axis]; previousErrorGyroI[axis] = errorGyroI[axis];

2
src/main/flight/pid.h
View file

@ -74,8 +74,6 @@ typedef struct airModePlus {
float factor; float factor;
float wowFactor; float wowFactor;
float iTermScaler; float iTermScaler;
bool isCurrentlyAtZero;
bool previousReferenceIsPositive;
} airModePlus_t; } airModePlus_t;
extern int16_t axisPID[XYZ_AXIS_COUNT]; extern int16_t axisPID[XYZ_AXIS_COUNT];