From b7b50e5cb3f316d10bd61bc8121a2b1047706dfb Mon Sep 17 00:00:00 2001 From: Andrei Date: Wed, 22 Jul 2020 18:39:09 +0300 Subject: [PATCH] Idle PID improvements --- .../controllers/actuators/idle_thread.cpp | 90 ++++++++++++++----- 1 file changed, 66 insertions(+), 24 deletions(-) diff --git a/firmware/controllers/actuators/idle_thread.cpp b/firmware/controllers/actuators/idle_thread.cpp index 43cc10902a..20d5cd196a 100644 --- a/firmware/controllers/actuators/idle_thread.cpp +++ b/firmware/controllers/actuators/idle_thread.cpp @@ -61,6 +61,8 @@ EXTERN_ENGINE; Engine *unitTestEngine; #endif +// todo: move all static vars to engine->engineState.idle? + static bool prettyClose = false; static bool shouldResetPid = false; @@ -70,6 +72,13 @@ static bool shouldResetPid = false; // See automaticIdleController(). static bool mightResetPid = false; +// This is needed to slowly turn on the PID back after it was reset. +static bool wasResetPid = false; +// This is used when the PID configuration is changed, to guarantee the reset +static bool mustResetPid = false; +static efitimeus_t restoreAfterPidResetTimeUs = 0; + + class PidWithOverrides : public PidIndustrial { public: float getOffset() const override { @@ -283,19 +292,34 @@ static void undoIdleBlipIfNeeded() { } } -static bool isOutOfAutomaticIdleCondition(DECLARE_ENGINE_PARAMETER_SIGNATURE) { +static bool isOutOfAutomaticIdleCondition(float rpm, int targetRpm DECLARE_ENGINE_PARAMETER_SUFFIX) { + // first, check the pedal threshold if (CONFIG(throttlePedalUpPin) != GPIO_UNASSIGNED) { - return !engine->engineState.idle.throttlePedalUpState; + if (!engine->engineState.idle.throttlePedalUpState) { + return true; + } + } else { + const auto [valid, pos] = Sensor::get(SensorType::DriverThrottleIntent); + + // Disable auto idle in case of TPS/Pedal failure + if (!valid) { + return true; + } + + if (pos > CONFIG(idlePidDeactivationTpsThreshold)) + return true; } - const auto [valid, pos] = Sensor::get(SensorType::DriverThrottleIntent); - - // Disable auto idle in case of TPS/Pedal failure - if (!valid) { - return true; + // then, check the RPM threshold (if in coasting mode) + if (CONFIG(idlePidRpmUpperLimit) > 0) { + int idlePidLowerRpm = targetRpm + CONFIG(idlePidRpmDeadZone); + int upperRpmLimit = idlePidLowerRpm + CONFIG(idlePidRpmUpperLimit); + if (rpm > upperRpmLimit) { + return true; + } } - return pos > CONFIG(idlePidDeactivationTpsThreshold); + return false; } /** @@ -307,7 +331,21 @@ static percent_t automaticIdleController(float tpsPos DECLARE_ENGINE_PARAMETER_S industrialWithOverrideIdlePid.antiwindupFreq = engineConfiguration->idle_antiwindupFreq; industrialWithOverrideIdlePid.derivativeFilterLoss = engineConfiguration->idle_derivativeFilterLoss; - if (isOutOfAutomaticIdleCondition(PASS_ENGINE_PARAMETER_SIGNATURE)) { + // get Target RPM for Auto-PID from a separate table + int targetRpm = getTargetRpmForIdleCorrection(PASS_ENGINE_PARAMETER_SIGNATURE); + + efitick_t nowNt = getTimeNowNt(); + efitimeus_t nowUs = getTimeNowUs(); + + float rpm; + if (CONFIG(useInstantRpmForIdle)) { + rpm = engine->triggerCentral.triggerState.calculateInstantRpm(NULL, nowNt PASS_ENGINE_PARAMETER_SUFFIX); + } else { + rpm = GET_RPM(); + } + + + if (isOutOfAutomaticIdleCondition(rpm, targetRpm PASS_ENGINE_PARAMETER_SUFFIX)) { // Don't store old I and D terms if PID doesn't work anymore. // Otherwise they will affect the idle position much later, when the throttle is closed. if (mightResetPid) { @@ -320,21 +358,8 @@ static percent_t automaticIdleController(float tpsPos DECLARE_ENGINE_PARAMETER_S return engine->engineState.idle.baseIdlePosition; } - // get Target RPM for Auto-PID from a separate table - int targetRpm = getTargetRpmForIdleCorrection(PASS_ENGINE_PARAMETER_SIGNATURE); - - efitick_t nowNt = getTimeNowNt(); - - float rpm; - if (CONFIG(useInstantRpmForIdle)) { - rpm = engine->triggerCentral.triggerState.calculateInstantRpm(NULL, nowNt PASS_ENGINE_PARAMETER_SUFFIX); - } else { - rpm = GET_RPM(); - } - - // #1553 we need to give FSIO variable offset or minValue a chance - bool acToggleJustTouched = (getTimeNowUs() - engine->acSwitchLastChangeTime) < MS2US(500); + bool acToggleJustTouched = (nowUs - engine->acSwitchLastChangeTime) < MS2US(500); // check if within the dead zone if (!acToggleJustTouched && absI(rpm - targetRpm) <= CONFIG(idlePidRpmDeadZone)) { engine->engineState.idle.idleState = RPM_DEAD_ZONE; @@ -347,6 +372,17 @@ static percent_t automaticIdleController(float tpsPos DECLARE_ENGINE_PARAMETER_S percent_t errorAmpCoef = 1.0f; if (rpm < targetRpm) errorAmpCoef += (float)CONFIG(pidExtraForLowRpm) / PERCENT_MULT; + + // if PID was previously reset, we store the time when it turned on back (see errorAmpCoef correction below) + if (wasResetPid) { + restoreAfterPidResetTimeUs = nowUs; + wasResetPid = false; + } + // increase the errorAmpCoef slowly to restore the process correctly after the PID reset + // todo: move restoreAfterPidResetTimeUs to engineState.idle? + efitimeus_t timeSincePidResetUs = nowUs - /*engine->engineState.idle.*/restoreAfterPidResetTimeUs; + // todo: add 'pidAfterResetDampingPeriodMs' setting + errorAmpCoef = interpolateClamped(0.0f, 0.0f, MS2US(/*CONFIG(pidAfterResetDampingPeriodMs)*/1000), errorAmpCoef, timeSincePidResetUs); // If errorAmpCoef > 1.0, then PID thinks that RPM is lower than it is, and controls IAC more aggressively getIdlePid(PASS_ENGINE_PARAMETER_SIGNATURE)->setErrorAmplification(errorAmpCoef); @@ -415,9 +451,14 @@ static percent_t automaticIdleController(float tpsPos DECLARE_ENGINE_PARAMETER_S } if (shouldResetPid) { - getIdlePid(PASS_ENGINE_PARAMETER_SIGNATURE)->reset(); + // we reset only if I-term is negative, because the positive I-term is good - it keeps RPM from dropping too low + if (getIdlePid(PASS_ENGINE_PARAMETER_SIGNATURE)->getIntegration() <= 0 || mustResetPid) { + getIdlePid(PASS_ENGINE_PARAMETER_SIGNATURE)->reset(); + mustResetPid = false; + } // alternatorPidResetCounter++; shouldResetPid = false; + wasResetPid = true; } @@ -557,6 +598,7 @@ void setDefaultIdleParameters(DECLARE_CONFIG_PARAMETER_SIGNATURE) { void onConfigurationChangeIdleCallback(engine_configuration_s *previousConfiguration) { shouldResetPid = !getIdlePid(PASS_ENGINE_PARAMETER_SIGNATURE)->isSame(&previousConfiguration->idleRpmPid); + mustResetPid = shouldResetPid; idleSolenoidOpen.setFrequency(CONFIG(idle).solenoidFrequency); idleSolenoidClose.setFrequency(CONFIG(idle).solenoidFrequency); }