From 0491eae52e8b7361f0e4ea553bddd88187a40849 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 10 Nov 2017 13:49:48 +0000 Subject: [PATCH 1/2] Update PID calculations to use actual deltaT --- src/main/flight/pid.c | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3b87ab358a..88ce802c18 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,14 @@ 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 +#ifndef SITL + currentTimeUs = microsISR(); // re-get current time, since there can be variations in timing since scheduler invocation +#endif + 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 +500,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 +518,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; From 56ed140e330b6677d1cc7b30768cc1b86da88ecd Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 12 Nov 2017 12:49:12 +0000 Subject: [PATCH 2/2] Updated to use currentTimeUs, which is time of gyro sampling --- src/main/flight/pid.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 88ce802c18..3a5beae970 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -413,9 +413,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an static timeUs_t previousTime; // calculate actual deltaT -#ifndef SITL - currentTimeUs = microsISR(); // re-get current time, since there can be variations in timing since scheduler invocation -#endif const float deltaT = currentTimeUs - previousTime; previousTime = currentTimeUs;