Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 22:35:23 +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) {
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
axisState->wowFactor = 1;
axisState->factor = 0;
axisState->iTermScaler = 1;
//Ki scaler
axisState->iTermScaler = constrainf(1.0f - (1.5f * ABS(rcCommandReflection)), 0.0f, 1.0f);
if (rcCommandReflection > 0.7f) {
//Ki scaler
axisState->iTermScaler = constrainf(1.0f - (1.5f * ABS(rcCommandReflection)), 0.0f, 1.0f);
//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;
//dynamic Ki handler
if (axisState->isCurrentlyAtZero) {
if (axisState->previousReferenceIsPositive ^ IS_POSITIVE(referenceTerm)) {
axisState->isCurrentlyAtZero = false;
} else {
axisState->previousReferenceIsPositive = false;
axisState->iTermScaler = 0;
errorGyroIf[axis] = 0;
errorGyroI[axis] = 0;
}
} 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;
} else {
axisState->wowFactor = 1;
axisState->factor = 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) {
axisState->wowFactor = 1.0f - (ABS(rcCommandReflection) * ((float)pidProfile->airModeInsaneAcrobilityFactor / 100.0f)); //0-1f
axisState->factor = (axisState->wowFactor * rcCommandReflection) * 1000;
}
}
}