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

Possibility to use median and dterm_cut_hz together

Browse source
This commit is contained in:
borisbstyle 2015-12-25 23:06:10 +01:00
parent 7504ba6dd3
commit 39c0427d27
1 changed files with 17 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -193,15 +193,14 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// would be scaled by different dt each time. Division by dT fixes that.
delta *= (1.0f / dT);
if (!pidProfile->dterm_cut_hz) {
// Apply median filter for averaging
for (deltaCount = 8; deltaCount > 0; deltaCount--) {
deltaOld[axis][deltaCount] = deltaOld[axis][deltaCount-1];
}
deltaOld[axis][0] = delta;
deltaSum = quickMedianFilter9f(deltaOld[axis]);
} else {
deltaSum = delta;
// Apply median filter for averaging
for (deltaCount = 8; deltaCount > 0; deltaCount--) {
deltaOld[axis][deltaCount] = deltaOld[axis][deltaCount-1];
}
deltaOld[axis][0] = delta;
deltaSum = quickMedianFilter9f(deltaOld[axis]);
if (pidProfile->dterm_cut_hz) {
// Dterm low pass
deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
@ -335,16 +334,15 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// would be scaled by different dt each time. Division by dT fixes that.
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6;
if (!pidProfile->dterm_cut_hz) {
// Apply median filter for averaging
for (deltaCount = 8; deltaCount > 0; deltaCount--) {
deltaOld[axis][deltaCount] = deltaOld[axis][deltaCount-1];
}
deltaOld[axis][0] = delta;
deltaSum = quickMedianFilter9(deltaOld[axis]);
deltaSum *= 3; // Get same scaling
} else {
deltaSum = delta * 2;
// Apply median filter for averaging
for (deltaCount = 8; deltaCount > 0; deltaCount--) {
deltaOld[axis][deltaCount] = deltaOld[axis][deltaCount-1];
}
deltaOld[axis][0] = delta;
deltaSum = quickMedianFilter9(deltaOld[axis]);
deltaSum *= 3; // Get same scaling
if (pidProfile->dterm_cut_hz) {
// Dterm delta low pass
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}