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

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

@ -94,7 +94,7 @@ 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 deltaOld[3][7]; static float deltaOld[3][9];
float delta, deltaSum; float delta, deltaSum;
int axis, deltaCount; int axis, deltaCount;
float horizonLevelStrength = 1; float horizonLevelStrength = 1;
@ -194,11 +194,15 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
delta *= (1.0f / dT); delta *= (1.0f / dT);
// Apply median filter for averaging // 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][deltaCount] = deltaOld[axis][deltaCount-1];
} }
deltaOld[axis][0] = delta; deltaOld[axis][0] = delta;
if (targetLooptime < 1000){
deltaSum = quickMedianFilter9f(deltaOld[axis]);
} else {
deltaSum = quickMedianFilter7f(deltaOld[axis]); deltaSum = quickMedianFilter7f(deltaOld[axis]);
}
if (pidProfile->dterm_cut_hz) { if (pidProfile->dterm_cut_hz) {
// Dterm low pass // Dterm low pass
@ -232,7 +236,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int32_t errorAngle; int32_t errorAngle;
int axis, deltaCount; int axis, deltaCount;
int32_t delta, deltaSum; int32_t delta, deltaSum;
static int32_t deltaOld[3][7]; 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 };
@ -335,11 +339,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;
// Apply median filter for averaging // 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][deltaCount] = deltaOld[axis][deltaCount-1];
} }
deltaOld[axis][0] = delta; deltaOld[axis][0] = delta;
if (targetLooptime < 1000){
deltaSum = quickMedianFilter9(deltaOld[axis]);
} else {
deltaSum = quickMedianFilter7(deltaOld[axis]); deltaSum = quickMedianFilter7(deltaOld[axis]);
}
deltaSum *= 3; // Get same scaling deltaSum *= 3; // Get same scaling
if (pidProfile->dterm_cut_hz) { if (pidProfile->dterm_cut_hz) {