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

Fixing the #5705 (D-Term spikes) (#5728) 

* * Fixing the #5705 (D-Term spikes)
  - For D-Term calculation I reverted back to use dT instead of jittery deltaT
  - Moved calculation of deltaT to handleItermRotation because it is the only place now where it is used

* * Changed the constant expression per DieHertz request

* * Changed the type of argument to match the type from caller

* * Redone also the handleItermRotation to dT per @ledvinap request
This commit is contained in:
Miroslav Drbal [ApoC] 2018-04-21 02:09:13 +02:00 committed by Michael Keller
parent 0306accf6e
commit 4968352446
1 changed files with 6 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -489,7 +489,7 @@ static void handleCrashRecovery(
}
static void detectAndSetCrashRecovery(
const pidCrashRecovery_e crash_recovery, const uint8_t axis,
const pidCrashRecovery_e crash_recovery, const int axis,
const timeUs_t currentTimeUs, const float delta, const float errorRate)
{
// if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
@ -515,10 +515,10 @@ static void detectAndSetCrashRecovery(
}
}
static void handleItermRotation(const float deltaT)
static void handleItermRotation()
{
// rotate old I to the new coordinate system
const float gyroToAngle = deltaT * RAD;
const float gyroToAngle = dT * RAD;
for (int i = FD_ROLL; i <= FD_YAW; i++) {
int i_1 = (i + 1) % 3;
int i_2 = (i + 2) % 3;
@ -535,7 +535,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
{
static float previousGyroRateDterm[2];
static float previousPidSetpoint[2];
static timeUs_t previousTimeUs;
// Disable PID control if at zero throttle or if gyro overflow detected
if (!pidStabilisationEnabled || gyroOverflowDetected()) {
@ -553,13 +552,8 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
const float tpaFactor = getThrottlePIDAttenuation();
const float motorMixRange = getMotorMixRange();
// calculate actual deltaT in seconds
const float deltaT = (currentTimeUs - previousTimeUs) * 0.000001f;
previousTimeUs = currentTimeUs;
// 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
// gradually scale back integration when above windup point
const float dynCi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f) * dT * itermAccelerator;
// Dynamic d component, enable 2-DOF PID controller only for rate mode
@ -574,7 +568,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
}
if (itermRotation) {
handleItermRotation(deltaT);
handleItermRotation();
}
// ----------PID controller----------
@ -623,7 +617,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// Divide rate change by deltaT to get differential (ie dr/dt)
const float delta = (
dynCd * transition * (currentPidSetpoint - previousPidSetpoint[axis]) -
(gyroRateDterm[axis] - previousGyroRateDterm[axis])) / deltaT;
(gyroRateDterm[axis] - previousGyroRateDterm[axis])) / dT;
previousPidSetpoint[axis] = currentPidSetpoint;
previousGyroRateDterm[axis] = gyroRateDterm[axis];