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

Revert new PID stuff and move it to a separate branch for now.

Browse source
This commit is contained in:
fiendie 2013-11-07 13:15:31 +01:00
parent 14f087a140
commit 6ab48fc438
11 changed files with 30 additions and 187 deletions
Download patch file Download diff file Expand all files Collapse all files

88
src/mw.c
View file

@ -25,7 +25,6 @@ rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) o
static void pidMultiWii(void);
static void pidRewrite(void);
static void pidBaseflight(void);
pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
uint8_t dynP8[3], dynI8[3], dynD8[3];
@ -279,78 +278,8 @@ static void mwVario(void)
}
static int32_t errorGyroI[3] = { 0, 0, 0 };
static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
static int32_t errorAngleI[2] = { 0, 0 };
static void pidBaseflight()
{
float RateError, errorAngle, AngleRate, gyroRate;
float ITerm,PTerm,DTerm;
static float lastGyroRate[3];
static float delta1[3], delta2[3];
float delta, deltaSum;
float dT;
int axis;
dT = (float)cycleTime * 0.000001f;
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
// -----Get the desired angle rate depending on flight mode
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2 ) { // MODE relying on ACC
// calculate error and limit the angle to 50 degrees max inclination
errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis]) / 10.0f; // 16 bits is ok here
}
if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
AngleRate = (float)((cfg.yawRate + 10) * rcCommand[YAW]) / 50.0f;
} else {
if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRate = (float)((cfg.rollPitchRate + 20) * rcCommand[axis]) / 50.0f; // 200dps to 1200dps max yaw rate
if (f.HORIZON_MODE) {
// mix up angle error to desired AngleRateTmp to add a little auto-level feel
AngleRate += errorAngle * cfg.H_level;
}
} else { // it's the ANGLE mode - control is angle based, so control loop is needed
AngleRate = errorAngle * cfg.A_level;
}
}
gyroRate = gyroData[axis] * gyro.scale; // gyro output scaled to dps
// --------low-level gyro-based PID. ----------
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
// -----calculate scaled error.AngleRates
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRate - gyroRate;
// -----calculate P component
PTerm = RateError * cfg.P_f[axis];
// -----calculate I component
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * cfg.I_f[axis], -250.0f, 250.0f);
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
ITerm = errorGyroIf[axis];
//-----calculate D-term
delta = gyroRate - lastGyroRate[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyroRate[axis] = gyroRate;
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
// would be scaled by different dt each time. Division by dT fixes that.
delta *= (1.0f / dT);
// add moving average here to reduce noise
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = constrainf((deltaSum / 3.0f) * cfg.D_f[axis], -300.0f, 300.0f);
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm - DTerm), -1000, 1000);
}
}
static void pidMultiWii(void)
{
int axis, prop;
@ -375,14 +304,14 @@ static void pidMultiWii(void)
}
if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { // MODE relying on GYRO or YAW axis
error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis];
error -= gyroData[axis] / 4;
error -= gyroData[axis];
PTermGYRO = rcCommand[axis];
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
if (abs(gyroData[axis]) > 2560)
if (abs(gyroData[axis]) > 640)
errorGyroI[axis] = 0;
ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) / 64;
ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6;
}
if (f.HORIZON_MODE && axis < 2) {
PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
@ -397,8 +326,8 @@ static void pidMultiWii(void)
}
}
PTerm -= ((int32_t)gyroData[axis] / 4) * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = (gyroData[axis] - lastGyro[axis]) / 4;
PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
delta = gyroData[axis] - lastGyro[axis];
lastGyro[axis] = gyroData[axis];
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
@ -445,7 +374,7 @@ static void pidRewrite(void)
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
// -----calculate scaled error.AngleRates
// multiplication of rcCommand corresponds to changing the sticks scaling here
RateError = AngleRateTmp - (gyroData[axis] / 4);
RateError = AngleRateTmp - gyroData[axis];
// -----calculate P component
PTerm = (RateError * cfg.P8[axis]) >> 7;
@ -489,8 +418,6 @@ void setPIDController(int type)
case 1:
pid_controller = pidRewrite;
break;
case 2:
pid_controller = pidBaseflight;
}
}
@ -587,9 +514,6 @@ void loop(void)
errorGyroI[ROLL] = 0;
errorGyroI[PITCH] = 0;
errorGyroI[YAW] = 0;
errorGyroIf[ROLL] = 0;
errorGyroIf[PITCH] = 0;
errorGyroIf[YAW] = 0;
errorAngleI[ROLL] = 0;
errorAngleI[PITCH] = 0;
if (cfg.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX