From 39c0427d27fe33da023c6de36c25866d23a2c9f3 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 25 Dec 2015 23:06:10 +0100 Subject: [PATCH] Possibility to use median and dterm_cut_hz together --- src/main/flight/pid.c | 36 +++++++++++++++++------------------- 1 file changed, 17 insertions(+), 19 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d34c1e2f0..7cc6045b6 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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); } -- 2.11.4.GIT