Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 14:55:21 +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. // would be scaled by different dt each time. Division by dT fixes that.
delta *= (1.0f / dT); delta *= (1.0f / dT);
if (!pidProfile->dterm_cut_hz) { // Apply median filter for averaging
// Apply median filter for averaging for (deltaCount = 8; 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; deltaSum = quickMedianFilter9f(deltaOld[axis]);
deltaSum = quickMedianFilter9f(deltaOld[axis]);
} else { if (pidProfile->dterm_cut_hz) {
deltaSum = delta;
// Dterm low pass // Dterm low pass
deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT); 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. // would be scaled by different dt each time. Division by dT fixes that.
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) { // Apply median filter for averaging
// Apply median filter for averaging for (deltaCount = 8; 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; deltaSum = quickMedianFilter9(deltaOld[axis]);
deltaSum = quickMedianFilter9(deltaOld[axis]); deltaSum *= 3; // Get same scaling
deltaSum *= 3; // Get same scaling
} else { if (pidProfile->dterm_cut_hz) {
deltaSum = delta * 2;
// Dterm delta low pass // Dterm delta low pass
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT); deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
} }