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

Remove old mixer // Separate Acro Plus from Airmode // Fix MOTOLAB merge issues

Browse source
This commit is contained in:
borisbstyle 2016-02-03 00:58:38 +01:00
parent 61186c0227
commit a1ebe6fd4f
8 changed files with 143 additions and 247 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -50,7 +50,8 @@
extern float dT;
extern bool motorLimitReached;
extern bool preventItermWindup;
#define PREVENT_WINDUP(x,y) { if (ABS(x) > ABS(y)) { if (x < 0) { x = -ABS(y); } else { x = ABS(y); } } }
int16_t axisPID[3];
@ -63,8 +64,8 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
uint8_t PIDweight[3];
static int32_t errorGyroI[3] = { 0, 0, 0 };
static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
static int32_t errorGyroI[3], previousErrorGyroI[3];
static float errorGyroIf[3], previousErrorGyroIf[3];
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);
@ -76,32 +77,42 @@ pidControllerFuncPtr pid_controller = pidRewrite; // which pid controller are we
void pidResetErrorGyro(void)
{
errorGyroI[ROLL] = 0;
errorGyroI[PITCH] = 0;
errorGyroI[YAW] = 0;
int axis;
errorGyroIf[ROLL] = 0.0f;
errorGyroIf[PITCH] = 0.0f;
errorGyroIf[YAW] = 0.0f;
for (axis = 0; axis < 3; axis++) {
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
PREVENT_WINDUP(errorGyroI[axis], previousErrorGyroI[axis]);
PREVENT_WINDUP(errorGyroIf[axis], previousErrorGyroIf[axis]);
} else {
errorGyroI[axis] = 0;
errorGyroIf[axis] = 0.0f;
}
}
}
void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile) {
float scaleItermToRcInput(int axis) {
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
static float iTermScaler[3] = {1.0f, 1.0f, 1.0f};
if (ABS(rcCommandReflection) > 0.7f && (!flightModeFlags)) { /* scaling should not happen in level modes */
/* Reset Iterm on high stick inputs. No scaling necessary here */
iTermScaler[axis] = 0.0f;
} else {
/* Prevent rapid windup during acro recoveries. Slowly enable Iterm activity. Perhaps more scaling to looptime needed for consistency */
if (iTermScaler[axis] < 1) {
iTermScaler[axis] = constrainf(iTermScaler[axis] + 0.001f, 0.0f, 1.0f);
} else {
iTermScaler[axis] = 1;
}
}
return iTermScaler[axis];
}
void acroPlusApply(acroPlus_t *axisState, int axis, pidProfile_t *pidProfile) {
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
axisState->wowFactor = 1;
axisState->factor = 0;
if (ABS(rcCommandReflection) > 0.7f && (!flightModeFlags)) { /* scaling should not happen in level modes */
/* Ki scaler axis*/
axisState->iTermScaler = 0.0f;
} else {
/* Prevent rapid windup during acro recoveries */
if (axisState->iTermScaler < 1) {
axisState->iTermScaler = constrainf(axisState->iTermScaler + 0.001f, 0.0f, 1.0f);
} else {
axisState->iTermScaler = 1;
}
}
/* acro plus factor handling */
if (axis != YAW && pidProfile->airModeInsaneAcrobilityFactor && (!flightModeFlags)) {
axisState->wowFactor = ABS(rcCommandReflection) * ((float)pidProfile->airModeInsaneAcrobilityFactor / 100.0f); //0-1f
@ -112,7 +123,7 @@ void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile) {
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static airModePlus_t airModePlusAxisState[3];
static acroPlus_t acroPlusState[3];
static biquad_t deltaBiQuadState[3];
static bool deltaStateIsSet;
@ -126,7 +137,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
float delta, deltaSum;
int axis, deltaCount;
float horizonLevelStrength = 1;
static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
@ -196,16 +206,12 @@ 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);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
airModePlus(&airModePlusAxisState[axis], axis, pidProfile);
errorGyroIf[axis] *= airModePlusAxisState[axis].iTermScaler;
}
if (preventItermWindup || motorLimitReached) {
if (ABS(errorGyroIf[axis]) > ABS(previousErrorGyroIf[axis])) {
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ABS(previousErrorGyroIf[axis]), ABS(previousErrorGyroIf[axis]));
errorGyroIf[axis] *= scaleItermToRcInput(axis);
if (motorLimitReached) {
PREVENT_WINDUP(errorGyroIf[axis], previousErrorGyroIf[axis]);
} else {
previousErrorGyroIf[axis] = errorGyroIf[axis];
}
} else {
previousErrorGyroIf[axis] = errorGyroIf[axis];
}
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
@ -241,8 +247,9 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
axisPID[axis] = lrintf(airModePlusAxisState[axis].factor + airModePlusAxisState[axis].wowFactor * axisPID[axis]);
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
acroPlusApply(&acroPlusState[axis], axis, pidProfile);
axisPID[axis] = acroPlusState[axis].factor + acroPlusState[axis].wowFactor * axisPID[axis];
}
#ifdef GTUNE
@ -268,7 +275,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int32_t PTerm, ITerm, DTerm, delta, deltaSum;
static int32_t lastError[3] = { 0, 0, 0 };
static int32_t previousDelta[3][DELTA_TOTAL_SAMPLES];
static int32_t previousErrorGyroI[3], lastGyroRate[3];
static int32_t lastGyroRate[3];
int32_t AngleRateTmp, RateError, gyroRate;
int8_t horizonLevelStrength = 100;
@ -345,20 +352,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// 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);
ITerm = errorGyroI[axis] >> 13;
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
airModePlus(&airModePlusAxisState[axis], axis, pidProfile);
errorGyroI[axis] *= airModePlusAxisState[axis].iTermScaler;
errorGyroI[axis] = (int32_t) (errorGyroI[axis] * scaleItermToRcInput(axis));
if (motorLimitReached) {
PREVENT_WINDUP(errorGyroIf[axis], previousErrorGyroIf[axis]);
} else {
previousErrorGyroI[axis] = errorGyroI[axis];
}
}
if (preventItermWindup || motorLimitReached) {
if (ABS(errorGyroI[axis]) > ABS(previousErrorGyroI[axis])) {
errorGyroI[axis] = constrain(errorGyroI[axis], -ABS(previousErrorGyroI[axis]), ABS(previousErrorGyroI[axis]));
}
} else {
previousErrorGyroI[axis] = errorGyroI[axis];
}
ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term
if (!pidProfile->deltaFromGyro) { // quick and dirty solution for testing
@ -389,8 +392,9 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
axisPID[axis] = lrintf(airModePlusAxisState[axis].factor + airModePlusAxisState[axis].wowFactor * axisPID[axis]);
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
acroPlusApply(&acroPlusState[axis], axis, pidProfile);
axisPID[axis] = lrintf(acroPlusState[axis].factor + acroPlusState[axis].wowFactor * axisPID[axis]);
}
#ifdef GTUNE