diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 81a28cdea..ebdab67f7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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];