diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3b87ab358a..3a5beae970 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -34,6 +34,7 @@ #include "config/parameter_group_ids.h" #include "drivers/sound_beeper.h" +#include "drivers/time.h" #include "fc/fc_core.h" #include "fc/fc_rc.h" @@ -409,6 +410,11 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an const float tpaFactor = getThrottlePIDAttenuation(); const float motorMixRange = getMotorMixRange(); static timeUs_t crashDetectedAtUs; + static timeUs_t previousTime; + + // calculate actual deltaT + const float deltaT = currentTimeUs - previousTime; + previousTime = currentTimeUs; // Dynamic ki component to gradually scale back integration when above windup point const float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f); @@ -491,7 +497,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an // -----calculate I component const float ITerm = axisPID_I[axis]; - const float ITermNew = constrainf(ITerm + Ki[axis] * errorRate * dT * dynKi * itermAccelerator, -itermLimit, itermLimit); + const float ITermNew = constrainf(ITerm + Ki[axis] * errorRate * deltaT * dynKi * itermAccelerator, -itermLimit, itermLimit); const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate); if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) { // Only increase ITerm if output is not saturated @@ -509,8 +515,8 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an dynC = dtermSetpointWeight * MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f); } const float rD = dynC * currentPidSetpoint - gyroRateFiltered; // cr - y - // Divide rate change by dT to get differential (ie dr/dt) - float delta = (rD - previousRateError[axis]) / dT; + // Divide rate change by deltaT to get differential (ie dr/dt) + float delta = (rD - previousRateError[axis]) / deltaT; previousRateError[axis] = rD;