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

Optimize pid loop

Browse source
This commit is contained in:
jirif 2018-01-30 11:23:03 +01:00
parent 701e29f2e8
commit b612df0c53
1 changed files with 9 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -417,8 +417,13 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
const float deltaT = (currentTimeUs - previousTimeUs) * 0.000001f;
previousTimeUs = currentTimeUs;
// Dynamic ki component to gradually scale back integration when above windup point
const float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f);
// Dynamic i component,
// gradually scale back integration when above windup point,
// use dT (not deltaT) for ITerm calculation to avoid wind-up caused by jitter
const float dynCi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f) * dT * itermAccelerator;
// Dynamic d component, enable 2-DOF PID controller only for rate mode
const float dynCd = flightModeFlags ? 0.0f : dtermSetpointWeight;
// ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -484,8 +489,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// -----calculate I component
const float ITerm = axisPID_I[axis];
// use dT (not deltaT) for ITerm calculation to avoid wind-up caused by jitter
const float ITermNew = constrainf(ITerm + Ki[axis] * errorRate * dT * dynKi * itermAccelerator, -itermLimit, itermLimit);
const float ITermNew = constrainf(ITerm + Ki[axis] * errorRate * dynCi, -itermLimit, itermLimit);
const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate);
if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) {
// Only increase ITerm if output is not saturated
@ -498,11 +502,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
float gyroRateFiltered = dtermNotchFilterApplyFn(dtermFilterNotch[axis], gyroRate);
gyroRateFiltered = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateFiltered);
float dynC = 0;
if ( (pidProfile->dtermSetpointWeight > 0) && (!flightModeFlags) ) {
dynC = dtermSetpointWeight * MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f);
}
const float rD = dynC * currentPidSetpoint - gyroRateFiltered; // cr - y
const float rD = dynCd * MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f) * currentPidSetpoint - gyroRateFiltered; // cr - y
// Divide rate change by deltaT to get differential (ie dr/dt)
float delta = (rD - previousRateError[axis]) / deltaT;