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

Update PID calculations to use actual deltaT

This commit is contained in:
Martin Budden 2017-11-10 13:49:48 +00:00
parent 391588958e
commit 0491eae52e
1 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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;