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

D filter rewrite to FIR

Browse source
This commit is contained in:
borisbstyle 2016-01-10 02:55:21 +01:00
parent 9f2de6f46c
commit 2ee55ece3c
5 changed files with 59 additions and 93 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -64,8 +64,6 @@ uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
static int32_t errorGyroI[3] = { 0, 0, 0 };
static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
static uint8_t deltaTotalSamples = 0;
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);
@ -85,18 +83,6 @@ void pidResetErrorGyro(void)
errorGyroIf[YAW] = 0.0f;
}
void setPidDeltaSamples(uint8_t filter) {
if (!filter) {
if (targetLooptime < 1000) {
deltaTotalSamples = 8;
} else {
deltaTotalSamples = 4;
}
} else {
deltaTotalSamples = 1;
}
}
void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile, float referenceTerm) {
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
@ -137,23 +123,22 @@ void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile, f
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static filterStatePt1_t DTermState[3];
static airModePlus_t airModePlusAxisState[3];
static int8_t * deltaFIRTable = 0L;
static int16_t deltaFIRState[3][FILTER_TAPS];
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
{
float RateError, AngleRate, gyroRate;
float ITerm,PTerm,DTerm;
static float lastGyroRate[3];
static float lastError[3];
static float previousDelta[3][8];
float delta, deltaSum;
int axis, deltaCount;
float delta;
int axis;
float horizonLevelStrength = 1;
static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
if (!deltaTotalSamples) setPidDeltaSamples(pidProfile->dterm_cut_hz);
if (!deltaFIRTable) deltaFIRTable = filterGetFIRCoefficientsTable(2, targetLooptime);
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
@ -234,33 +219,20 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
ITerm = errorGyroIf[axis];
//-----calculate D-term
if (pidProfile->delta_from_gyro_error || axis == YAW) {
delta = -(gyroRate - lastGyroRate[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyroRate[axis] = gyroRate;
} else {
delta = RateError - lastError[axis];
lastError[axis] = RateError;
}
delta = RateError - lastError[axis];
lastError[axis] = RateError;
// 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);
deltaSum = 0;
if (pidProfile->dterm_cut_hz) {
// Dterm low pass
deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
/* Delta filtering is in integers, which should be fine */
int16_t deltaTemp = (int16_t) delta;
filterApplyFIR(&deltaTemp, deltaFIRState[axis], deltaFIRTable);
delta = (float) deltaTemp;
// Apply moving average
if (deltaTotalSamples > 1) {
for (deltaCount = deltaTotalSamples-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < deltaTotalSamples; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
deltaSum = (deltaSum / deltaTotalSamples);
}
DTerm = constrainf(deltaSum * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
@ -288,18 +260,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
{
UNUSED(rxConfig);
int axis, deltaCount;
int32_t delta, deltaSum;
static int32_t previousDelta[3][8];
int axis;
int16_t delta;
int32_t PTerm, ITerm, DTerm;
static int32_t lastError[3] = { 0, 0, 0 };
static int32_t lastGyroRate[3] = { 0, 0, 0 };
static int32_t previousErrorGyroI[3] = { 0, 0, 0 };
int32_t AngleRateTmp, RateError, gyroRate;
int8_t horizonLevelStrength = 100;
if (!deltaTotalSamples) setPidDeltaSamples(pidProfile->dterm_cut_hz);
if (!deltaFIRTable) deltaFIRTable = filterGetFIRCoefficientsTable(2, targetLooptime);
if (FLIGHT_MODE(HORIZON_MODE)) {
// Figure out the raw stick positions
@ -383,32 +353,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
}
//-----calculate D-term
if (pidProfile->delta_from_gyro_error || axis == YAW) { // quick and dirty solution for testing
delta = -(gyroRate - lastGyroRate[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyroRate[axis] = gyroRate;
} else {
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError;
}
delta = (int16_t) RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError;
// 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 = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6;
// Apply moving average
deltaSum = 0;
if (pidProfile->dterm_cut_hz) {
// Dterm low pass
deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
if (deltaTotalSamples > 1) {
for (deltaCount = deltaTotalSamples-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
previousDelta[axis][0] = delta;
for (deltaCount = 0; deltaCount < deltaTotalSamples; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
}
deltaSum = (deltaSum / deltaTotalSamples) * 3; // get old scaling by multiplying with 3
filterApplyFIR(&delta, deltaFIRState[axis], deltaFIRTable);
DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
DTerm = (delta * 2 * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;