rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Dynamic median filter

This commit is contained in:
borisbstyle 2015-12-27 01:59:14 +01:00
parent b14a3ac29c
commit 34d65ad0b9
1 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/main/flight/pid.c
View File

@ -94,7 +94,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
float ITerm,PTerm,DTerm;
int32_t stickPosAil, stickPosEle, mostDeflectedPos;
static float lastError[3];
static float deltaOld[3][7];
static float deltaOld[3][9];
float delta, deltaSum;
int axis, deltaCount;
float horizonLevelStrength = 1;
@ -194,11 +194,15 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
delta *= (1.0f / dT);
// Apply median filter for averaging
for (deltaCount = 6; deltaCount > 0; deltaCount--) {
for (deltaCount = 8; deltaCount > 0; deltaCount--) {
deltaOld[axis][deltaCount] = deltaOld[axis][deltaCount-1];
}
deltaOld[axis][0] = delta;
deltaSum = quickMedianFilter7f(deltaOld[axis]);
if (targetLooptime < 1000){
deltaSum = quickMedianFilter9f(deltaOld[axis]);
} else {
deltaSum = quickMedianFilter7f(deltaOld[axis]);
}
if (pidProfile->dterm_cut_hz) {
// Dterm low pass
@ -232,7 +236,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int32_t errorAngle;
int axis, deltaCount;
int32_t delta, deltaSum;
static int32_t deltaOld[3][7];
static int32_t deltaOld[3][9];
int32_t PTerm, ITerm, DTerm;
static int32_t lastError[3] = { 0, 0, 0 };
static int32_t previousErrorGyroI[3] = { 0, 0, 0 };
@ -335,11 +339,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6;
// Apply median filter for averaging
for (deltaCount = 6; deltaCount > 0; deltaCount--) {
for (deltaCount = 8; deltaCount > 0; deltaCount--) {
deltaOld[axis][deltaCount] = deltaOld[axis][deltaCount-1];
}
deltaOld[axis][0] = delta;
deltaSum = quickMedianFilter7(deltaOld[axis]);
if (targetLooptime < 1000){
deltaSum = quickMedianFilter9(deltaOld[axis]);
} else {
deltaSum = quickMedianFilter7(deltaOld[axis]);
}
deltaSum *= 3; // Get same scaling
if (pidProfile->dterm_cut_hz) {