diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index cd8b412ce..3b159c04a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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) {