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

Replace moving average by median Filter for Dterm

Browse source
This commit is contained in:
borisbstyle 2015-12-25 02:20:25 +01:00
parent a0bcb1b95f
commit 7504ba6dd3
1 changed files with 17 additions and 34 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -94,9 +94,9 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
float ITerm,PTerm,DTerm; float ITerm,PTerm,DTerm;
int32_t stickPosAil, stickPosEle, mostDeflectedPos; int32_t stickPosAil, stickPosEle, mostDeflectedPos;
static float lastError[3]; static float lastError[3];
static float delta1[3], delta2[3], delta3[3], delta4[3], delta5[3]; static float deltaOld[3][9];
float delta, deltaSum; float delta, deltaSum;
int axis; int axis, deltaCount;
float horizonLevelStrength = 1; float horizonLevelStrength = 1;
static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
@ -194,25 +194,15 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
delta *= (1.0f / dT); delta *= (1.0f / dT);
if (!pidProfile->dterm_cut_hz) { if (!pidProfile->dterm_cut_hz) {
// add moving average here to reduce noise. More averaging needed for 2khz mode // Apply median filter for averaging
deltaSum = delta1[axis] + delta2[axis] + delta; for (deltaCount = 8; deltaCount > 0; deltaCount--) {
if (targetLooptime < 1000) { deltaOld[axis][deltaCount] = deltaOld[axis][deltaCount-1];
deltaSum += delta3[axis] + delta4[axis] + delta5[axis];
delta5[axis] = delta4[axis];
delta4[axis] = delta3[axis];
delta3[axis] = delta2[axis];
deltaSum /= 6;
} else {
deltaSum /= 3;
} }
delta2[axis] = delta1[axis]; deltaOld[axis][0] = delta;
delta1[axis] = delta; deltaSum = quickMedianFilter9f(deltaOld[axis]);
} else { } else {
deltaSum = delta; deltaSum = delta;
} // Dterm low pass
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT); deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
} }
@ -241,9 +231,9 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
UNUSED(rxConfig); UNUSED(rxConfig);
int32_t errorAngle; int32_t errorAngle;
int axis; int axis, deltaCount;
int32_t delta, deltaSum; int32_t delta, deltaSum;
static int32_t delta1[3], delta2[3], delta3[3], delta4[3], delta5[3];; static int32_t deltaOld[3][9];
int32_t PTerm, ITerm, DTerm; int32_t PTerm, ITerm, DTerm;
static int32_t lastError[3] = { 0, 0, 0 }; static int32_t lastError[3] = { 0, 0, 0 };
static int32_t previousErrorGyroI[3] = { 0, 0, 0 }; static int32_t previousErrorGyroI[3] = { 0, 0, 0 };
@ -346,23 +336,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6; delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6;
if (!pidProfile->dterm_cut_hz) { if (!pidProfile->dterm_cut_hz) {
// add moving average here to reduce noise (More moving average required for 2khz mode) // Apply median filter for averaging
deltaSum = delta1[axis] + delta2[axis] + delta; for (deltaCount = 8; deltaCount > 0; deltaCount--) {
if (targetLooptime < 1000) { deltaOld[axis][deltaCount] = deltaOld[axis][deltaCount-1];
deltaSum += delta3[axis] + delta4[axis] + delta5[axis];
delta5[axis] = delta4[axis];
delta4[axis] = delta3[axis];
delta3[axis] = delta2[axis];
deltaSum /= 2; // Get same scaling
} }
delta2[axis] = delta1[axis]; deltaOld[axis][0] = delta;
delta1[axis] = delta; deltaSum = quickMedianFilter9(deltaOld[axis]);
deltaSum *= 3; // Get same scaling
} else { } else {
deltaSum = delta * 2; deltaSum = delta * 2;
} // Dterm delta low pass
// Dterm delta low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT); deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
} }