From 533c3a5ae481f5d843f490e2df67d8add7e01e02 Mon Sep 17 00:00:00 2001 From: Hydra Date: Tue, 20 Sep 2016 01:23:25 +0200 Subject: [PATCH] ensure dT is calculated the same way for all pid controllers. --- src/main/fc/cleanflight_fc.c | 6 +----- src/main/flight/pid.c | 8 ++++++++ src/main/flight/pid.h | 1 + src/main/flight/pid_luxfloat.c | 12 ++---------- src/main/flight/pid_mw23.c | 3 +-- src/main/flight/pid_mwrewrite.c | 5 ++--- 6 files changed, 15 insertions(+), 20 deletions(-) diff --git a/src/main/fc/cleanflight_fc.c b/src/main/fc/cleanflight_fc.c index 2a9ecff3e..eb7e68efc 100644 --- a/src/main/fc/cleanflight_fc.c +++ b/src/main/fc/cleanflight_fc.c @@ -111,8 +111,6 @@ enum { uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop -float dT; - int16_t magHold; int16_t headFreeModeHold; @@ -698,7 +696,7 @@ void subTaskMainSubprocesses(void) #endif #if defined(BARO) || defined(SONAR) - // FIXME outdates comments? + // FIXME outdated comments? // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState // this must be called here since applyAltHold directly manipulates rcCommands[] updateRcCommands(); @@ -795,8 +793,6 @@ void taskMainPidLoopCheck(void) cycleTime = getTaskDeltaTime(TASK_SELF); - dT = (float)cycleTime * 0.000001f; - if (debugMode == DEBUG_CYCLETIME) { debug[0] = cycleTime; debug[1] = averageSystemLoadPercent; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 941a4ca9d..ab9a87adc 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -126,6 +126,14 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .horizon_tilt_mode = HORIZON_TILT_MODE_SAFE, ); +float getdT(void) +{ + static float dT; + if (!dT) dT = (float)targetPidLooptime * 0.000001f; + + return dT; +} + void pidSetTargetLooptime(uint32_t pidLooptime) { targetPidLooptime = pidLooptime; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index bc5b63ea2..6202afb23 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -89,6 +89,7 @@ extern uint32_t targetPidLooptime; float pidScaleITermToRcInput(int axis); void pidFilterIsSetCheck(const pidProfile_t *pidProfile); +float getdT(void); void pidInitFilters(const pidProfile_t *pidProfile); void pidSetController(pidControllerType_e type); void pidSetTargetLooptime(uint32_t pidLooptime); diff --git a/src/main/flight/pid_luxfloat.c b/src/main/flight/pid_luxfloat.c index 246b5f4de..844fb062a 100644 --- a/src/main/flight/pid_luxfloat.c +++ b/src/main/flight/pid_luxfloat.c @@ -78,14 +78,6 @@ static const float luxITermScale = 1000000.0f / 0x1000000; static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 512; static const float luxGyroScale = 16.4f / 4; // the 16.4 is needed because mwrewrite does not scale according to the gyro model gyro.scale -float getdT (void) -{ - static float dT; - if (!dT) dT = (float)targetPidLooptime * 0.000001f; - - return dT; -} - STATIC_UNIT_TESTED int16_t pidLuxFloatCore(int axis, const pidProfile_t *pidProfile, float gyroRate, float angleRate) { static float lastRateForDelta[3]; @@ -101,7 +93,7 @@ STATIC_UNIT_TESTED int16_t pidLuxFloatCore(int axis, const pidProfile_t *pidProf // Constrain YAW by yaw_p_limit value if not servo driven, in that case servolimits apply if (axis == YAW) { if (pidProfile->yaw_lpf_hz) { - PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, dT); + PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); } if (pidProfile->yaw_p_limit && motorCount >= 4) { PTerm = constrainf(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); @@ -109,7 +101,7 @@ STATIC_UNIT_TESTED int16_t pidLuxFloatCore(int axis, const pidProfile_t *pidProf } // -----calculate I component - float ITerm = lastITermf[axis] + luxITermScale * rateError * dT * pidProfile->I8[axis]; + float ITerm = lastITermf[axis] + luxITermScale * rateError * getdT() * pidProfile->I8[axis]; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings ITerm = constrainf(ITerm, -PID_MAX_I, PID_MAX_I); diff --git a/src/main/flight/pid_mw23.c b/src/main/flight/pid_mw23.c index 15330f01d..ac234cfc5 100644 --- a/src/main/flight/pid_mw23.c +++ b/src/main/flight/pid_mw23.c @@ -57,7 +57,6 @@ static int32_t ITermAngle[2]; uint8_t dynP8[3], dynI8[3], dynD8[3]; -extern float dT; extern uint8_t motorCount; #ifdef BLACKBOX @@ -150,7 +149,7 @@ void pidMultiWii23(const pidProfile_t *pidProfile, const controlRateConfig_t *co if (pidProfile->dterm_lpf_hz) { // Dterm delta low pass DTerm = delta; - DTerm = lrintf(pt1FilterApply4(&deltaFilter[axis], (float)DTerm, pidProfile->dterm_lpf_hz, dT)) * 3; // Keep same scaling as unfiltered DTerm + DTerm = lrintf(pt1FilterApply4(&deltaFilter[axis], (float)DTerm, pidProfile->dterm_lpf_hz, getdT())) * 3; // Keep same scaling as unfiltered DTerm } else { // When dterm filter disabled apply moving average to reduce noise DTerm = delta1[axis] + delta2[axis] + delta; diff --git a/src/main/flight/pid_mwrewrite.c b/src/main/flight/pid_mwrewrite.c index 15f36a5aa..f033f2439 100644 --- a/src/main/flight/pid_mwrewrite.c +++ b/src/main/flight/pid_mwrewrite.c @@ -55,7 +55,6 @@ #include "flight/gtune.h" #include "flight/mixer.h" -extern float dT; extern uint8_t PIDweight[3]; extern int32_t lastITerm[3], ITermLimit[3]; @@ -81,7 +80,7 @@ STATIC_UNIT_TESTED int16_t pidMultiWiiRewriteCore(int axis, const pidProfile_t * // Constrain YAW by yaw_p_limit value if not servo driven, in that case servolimits apply if (axis == YAW) { if (pidProfile->yaw_lpf_hz) { - PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, dT); + PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); } if (pidProfile->yaw_p_limit && motorCount >= 4) { PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); @@ -126,7 +125,7 @@ STATIC_UNIT_TESTED int16_t pidMultiWiiRewriteCore(int axis, const pidProfile_t * delta = (delta * ((uint16_t)0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; if (pidProfile->dterm_lpf_hz) { // DTerm delta low pass filter - delta = lrintf(pt1FilterApply4(&deltaFilter[axis], (float)delta, pidProfile->dterm_lpf_hz, dT)); + delta = lrintf(pt1FilterApply4(&deltaFilter[axis], (float)delta, pidProfile->dterm_lpf_hz, getdT())); } DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; DTerm = constrain(DTerm, -PID_MAX_D, PID_MAX_D); -- 2.11.4.GIT