Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 09:16:07 +03:00
Code Issues Wiki Activity

Delta PID1

Browse source
This commit is contained in:
borisbstyle 2015-09-19 00:39:24 +02:00
parent e7bf366e1c
commit 7a2762dbf6
1 changed files with 3 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

11
src/main/flight/pid.c
View file

@ -721,8 +721,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int32_t errorAngle; int32_t errorAngle;
int axis; int axis;
int32_t delta, deltaSum; int32_t delta;
static int32_t delta1[3], delta2[3];
int32_t PTerm, ITerm, DTerm; int32_t PTerm, ITerm, DTerm;
static int32_t lastError[3] = { 0, 0, 0 }; static int32_t lastError[3] = { 0, 0, 0 };
int32_t AngleRateTmp, RateError; int32_t AngleRateTmp, RateError;
@ -823,17 +822,13 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference // 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. // would be scaled by different dt each time. Division by dT fixes that.
delta = (delta * ((uint16_t) 0xFFFF / (cycleTime >> 4))) >> 6; delta = (delta * ((uint16_t) 0xFFFF / (cycleTime >> 4))) >> 6;
// add moving average here to reduce noise
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
// Dterm delta low pass // Dterm delta low pass
if (pidProfile->dterm_cut_hz) { if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT); delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
} }
DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
// -----calculate total PID output // -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm; axisPID[axis] = PTerm + ITerm + DTerm;