From c767fe8e1ed3e88445bf75dba59123fd34ca4828 Mon Sep 17 00:00:00 2001 From: kamaloo Date: Sat, 30 Jan 2016 16:16:51 +0100 Subject: [PATCH] Move dT scaling before filtering From DSP point of view scaling by dT before filtering is more correct. This should yield less delta noise with noisy dT values. BiQuad/Moving average adds some delay to data so current dT is not valid for current filter output. --- src/main/flight/pid.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 61fd3b929..f69fee961 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -221,6 +221,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa lastGyroRate[axis] = gyroRate; } + // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference + // would be scaled by different dt each time. Division by dT fixes that. + delta *= (1.0f / dT); + if (deltaStateIsSet) { delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]); } else { @@ -232,10 +236,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa delta = (deltaSum / DELTA_TOTAL_SAMPLES); } - // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference - // would be scaled by different dt each time. Division by dT fixes that. - delta *= (1.0f / dT); - DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); // -----calculate total PID output @@ -368,6 +368,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat delta = -(gyroRate - lastGyroRate[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 lastGyroRate[axis] = gyroRate; } + + // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference + // would be scaled by different dt each time. Division by dT fixes that. + delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6; if (deltaStateIsSet) { delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta @@ -380,10 +384,6 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat delta = deltaSum; } - // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference - // would be scaled by different dt each time. Division by dT fixes that. - delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6; - DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; // -----calculate total PID output