Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +03:00
Code Issues Wiki Activity

Merge pull request #4556 from martinbudden/bfa_pid_deltat

Browse source 
Update PID calculations to use actual deltaT
This commit is contained in:
Michael Keller 2017-11-14 21:15:16 +13:00 • committed by GitHub
commit 971abbd9f6
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 9 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -34,6 +34,7 @@
#include "config/parameter_group_ids.h" #include "config/parameter_group_ids.h"
#include "drivers/sound_beeper.h" #include "drivers/sound_beeper.h"
#include "drivers/time.h"
#include "fc/fc_core.h" #include "fc/fc_core.h"
#include "fc/fc_rc.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 tpaFactor = getThrottlePIDAttenuation();
const float motorMixRange = getMotorMixRange(); const float motorMixRange = getMotorMixRange();
static timeUs_t crashDetectedAtUs; 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 // Dynamic ki component to gradually scale back integration when above windup point
const float dynKi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f); 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 // -----calculate I component
const float ITerm = axisPID_I[axis]; 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); const bool outputSaturated = mixerIsOutputSaturated(axis, errorRate);
if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) { if (outputSaturated == false || ABS(ITermNew) < ABS(ITerm)) {
// Only increase ITerm if output is not saturated // 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); dynC = dtermSetpointWeight * MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f);
} }
const float rD = dynC * currentPidSetpoint - gyroRateFiltered; // cr - y const float rD = dynC * currentPidSetpoint - gyroRateFiltered; // cr - y
// Divide rate change by dT to get differential (ie dr/dt) // Divide rate change by deltaT to get differential (ie dr/dt)
float delta = (rD - previousRateError[axis]) / dT; float delta = (rD - previousRateError[axis]) / deltaT;
previousRateError[axis] = rD; previousRateError[axis] = rD;