From cb276879c5d3f6d4a98607b1bd5b1d9b03c2d02d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Spychalski?= Date: Tue, 17 Mar 2026 09:41:56 +0100 Subject: [PATCH] Minor PID controller improvement: use cache millis instead of calling millis() multiple times in the loop. --- src/main/flight/pid.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0a2faa648ca..58fa056e14f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -170,6 +170,7 @@ static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM bool restartAngleHoldMode = true; static EXTENDED_FASTRAM bool angleHoldIsLevel = false; +static EXTENDED_FASTRAM timeMs_t pidLoopNowMs; #define FIXED_WING_LEVEL_TRIM_MAX_ANGLE 10.0f // Max angle auto trimming can demand #define FIXED_WING_LEVEL_TRIM_DIVIDER 50.0f @@ -820,7 +821,7 @@ static void iTermLockApply(pidState_t *pidState, const float rateTarget, const f //When abs of rate target is above 20% of max rate, we start tracking time if (fabsf(rateTarget) > maxRate * 0.2f) { - pidState->attenuation.targetOverThresholdTimeMs = millis(); + pidState->attenuation.targetOverThresholdTimeMs = pidLoopNowMs; } //If error is below threshold, we no longer track time for lock mechanism @@ -833,7 +834,7 @@ static void iTermLockApply(pidState_t *pidState, const float rateTarget, const f * - dampingFactor * - for 500ms (fw_iterm_lock_time_max_ms) force 0 if error is above threshold */ - pidState->attenuation.aI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->attenuation.targetOverThresholdTimeMs) < pidProfile()->fwItermLockTimeMaxMs) ? 0.0f : 1.0f); + pidState->attenuation.aI = MIN(dampingFactor, (errorThresholdReached && (pidLoopNowMs - pidState->attenuation.targetOverThresholdTimeMs) < pidProfile()->fwItermLockTimeMaxMs) ? 0.0f : 1.0f); //P & D damping factors are always the same and based on current damping factor pidState->attenuation.aP = dampingFactor; @@ -1222,6 +1223,7 @@ void updateAngleHold(float *angleTarget, uint8_t axis) void FAST_CODE pidController(float dT) { const float dT_inv = 1.0f / dT; + pidLoopNowMs = millis(); if (!pidFiltersConfigured) { return;