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

New experimental parameters for airmode

Browse source
This commit is contained in:
borisbstyle 2016-01-10 19:28:10 +01:00
parent 2ee55ece3c
commit bd39f07c1f
1 changed files with 29 additions and 27 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -85,39 +85,41 @@ void pidResetErrorGyro(void)
void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile, float referenceTerm) { void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile, float referenceTerm) {
float rcCommandReflection = (float)rcCommand[axis] / 500.0f; float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
axisState->wowFactor = 1;
axisState->factor = 0;
axisState->iTermScaler = 1;
//Ki scaler if (rcCommandReflection > 0.7f) {
axisState->iTermScaler = constrainf(1.0f - (1.5f * ABS(rcCommandReflection)), 0.0f, 1.0f); //Ki scaler
axisState->iTermScaler = constrainf(1.0f - (1.5f * ABS(rcCommandReflection)), 0.0f, 1.0f);
//dynamic Ki handler //dynamic Ki handler
if (axisState->isCurrentlyAtZero) { if (axisState->isCurrentlyAtZero) {
if (axisState->previousReferenceIsPositive ^ IS_POSITIVE(referenceTerm)) { if (axisState->previousReferenceIsPositive ^ IS_POSITIVE(referenceTerm)) {
axisState->isCurrentlyAtZero = false; 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 { } else {
axisState->previousReferenceIsPositive = false; axisState->iTermScaler = 0;
errorGyroIf[axis] = 0;
errorGyroI[axis] = 0;
} }
} else {
axisState->isCurrentlyAtZero = true;
} }
}
if (axis != YAW && pidProfile->airModeInsaneAcrobilityFactor) { if (!axisState->iTermScaler) {
axisState->wowFactor = 1.0f - (ABS(rcCommandReflection) * ((float)pidProfile->airModeInsaneAcrobilityFactor / 100.0f)); //0-1f if (!axisState->isCurrentlyAtZero) {
axisState->factor = (axisState->wowFactor * rcCommandReflection) * 1000; if (IS_POSITIVE(referenceTerm)) {
} else { axisState->previousReferenceIsPositive = true;
axisState->wowFactor = 1; } else {
axisState->factor = 0; axisState->previousReferenceIsPositive = false;
}
} else {
axisState->isCurrentlyAtZero = true;
}
}
if (axis != YAW && pidProfile->airModeInsaneAcrobilityFactor) {
axisState->wowFactor = 1.0f - (ABS(rcCommandReflection) * ((float)pidProfile->airModeInsaneAcrobilityFactor / 100.0f)); //0-1f
axisState->factor = (axisState->wowFactor * rcCommandReflection) * 1000;
}
} }
} }