rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
kamaloo 2016-01-30 16:16:51 +01:00 committed by borisbstyle
parent 35125ecf10
commit c767fe8e1e
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/main/flight/pid.c
View File

@ -221,6 +221,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
lastGyroRate[axis] = gyroRate; 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) { if (deltaStateIsSet) {
delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]); delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]);
} else { } else {
@ -232,10 +236,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
delta = (deltaSum / DELTA_TOTAL_SAMPLES); 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); DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
// -----calculate total PID output // -----calculate total PID output
@ -369,6 +369,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
lastGyroRate[axis] = gyroRate; 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) { if (deltaStateIsSet) {
delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta
} else { } else {
@ -380,10 +384,6 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
delta = deltaSum; 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; DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
// -----calculate total PID output // -----calculate total PID output