diff --git a/firmware/controllers/actuators/idle_thread.cpp b/firmware/controllers/actuators/idle_thread.cpp index 154a56557b..a643d6ba4f 100644 --- a/firmware/controllers/actuators/idle_thread.cpp +++ b/firmware/controllers/actuators/idle_thread.cpp @@ -193,6 +193,10 @@ void setManualIdleValvePosition(int positionPercent) { #endif /* EFI_UNIT_TEST */ +void IdleController::init(pid_s* idlePidConfig) { + m_timingPid.initPidClass(idlePidConfig); +} + int IdleController::getTargetRpm(float clt) const { // TODO: bump target rpm based on AC and/or fan(s)? @@ -269,6 +273,26 @@ float IdleController::getOpenLoop(Phase phase, float clt, SensorResult tps) cons return interpolateClamped(0, cranking, CONFIG(afterCrankingIACtaperDuration), running, revsSinceStart); } +float IdleController::getIdleTimingAdjustment(int rpm) { + return getIdleTimingAdjustment(rpm, m_lastTargetRpm, m_lastPhase); +} + +float IdleController::getIdleTimingAdjustment(int rpm, int targetRpm, Phase phase) { + // if not enabled, do nothing + if (!CONFIG(useIdleTimingPidControl)) { + return 0; + } + + // If not idling, do nothing + if (phase != Phase::Idling) { + m_timingPid.reset(); + return 0; + } + + // We're now in the idle mode, and RPM is inside the Timing-PID regulator work zone! + return m_timingPid.getOutput(targetRpm, rpm, FAST_CALLBACK_PERIOD_MS / 1000.0f); +} + static percent_t manualIdleController(float cltCorrection DECLARE_ENGINE_PARAMETER_SUFFIX) { percent_t correctedPosition = cltCorrection * CONFIG(manIdlePosition); @@ -432,9 +456,11 @@ static percent_t automaticIdleController(float tpsPos, float rpm, int targetRpm, // Compute the target we're shooting for auto targetRpm = getTargetRpm(clt); + m_lastTargetRpm = targetRpm; // Determine what operation phase we're in - idling or not auto phase = determinePhase(rpm, targetRpm, tps); + m_lastPhase = phase; engine->engineState.isAutomaticIdle = tps.Valid && engineConfiguration->idleMode == IM_AUTO; @@ -530,6 +556,10 @@ void updateIdleControl() idleControllerInstance.update(); } +float getIdleTimingAdjustment(int rpm) { + return idleControllerInstance.getIdleTimingAdjustment(rpm); +} + static void applyPidSettings(DECLARE_ENGINE_PARAMETER_SIGNATURE) { getIdlePid(PASS_ENGINE_PARAMETER_SIGNATURE)->updateFactors(engineConfiguration->idleRpmPid.pFactor, engineConfiguration->idleRpmPid.iFactor, engineConfiguration->idleRpmPid.dFactor); iacPidMultMap.init(CONFIG(iacPidMultTable), CONFIG(iacPidMultLoadBins), CONFIG(iacPidMultRpmBins)); @@ -606,6 +636,7 @@ void startIdleBench(void) { void startIdleThread(Logging*sharedLogger DECLARE_ENGINE_PARAMETER_SUFFIX) { logger = sharedLogger; INJECT_ENGINE_REFERENCE(&idleControllerInstance); + idleControllerInstance.init(&CONFIG(idleTimingPid)); INJECT_ENGINE_REFERENCE(&industrialWithOverrideIdlePid); ENGINE(idleController) = &idleControllerInstance; diff --git a/firmware/controllers/actuators/idle_thread.h b/firmware/controllers/actuators/idle_thread.h index ac8413e3fb..c3a0ec5f9c 100644 --- a/firmware/controllers/actuators/idle_thread.h +++ b/firmware/controllers/actuators/idle_thread.h @@ -11,6 +11,7 @@ #include "engine_ptr.h" #include "rusefi_types.h" #include "periodic_task.h" +#include "pid.h" struct IIdleController { enum class Phase : uint8_t { @@ -28,13 +29,13 @@ struct IIdleController { }; class Logging; -class Pid; - class IdleController : public IIdleController { public: DECLARE_ENGINE_PTR; + void init(pid_s* idlePidConfig); + float getIdlePosition(); void update(); @@ -48,11 +49,23 @@ public: float getCrankingOpenLoop(float clt) const override; float getRunningOpenLoop(float clt, SensorResult tps) const override; float getOpenLoop(Phase phase, float clt, SensorResult tps) const override; + + float getIdleTimingAdjustment(int rpm); + float getIdleTimingAdjustment(int rpm, int targetRpm, Phase phase); + +private: + // These are stored by getIdlePosition() and used by getIdleTimingAdjustment() + Phase m_lastPhase = Phase::Cranking; + int m_lastTargetRpm = 0; + + Pid m_timingPid; }; void updateIdleControl(); percent_t getIdlePosition(); +float getIdleTimingAdjustment(int rpm); + void applyIACposition(percent_t position DECLARE_ENGINE_PARAMETER_SUFFIX); void setManualIdleValvePosition(int positionPercent); diff --git a/firmware/controllers/algo/advance_map.cpp b/firmware/controllers/algo/advance_map.cpp index 4c703377f9..9d3c474e3d 100644 --- a/firmware/controllers/algo/advance_map.cpp +++ b/firmware/controllers/algo/advance_map.cpp @@ -37,10 +37,6 @@ static ign_Map3D_t advanceMap("advance"); // This coeff in ctor parameter is sufficient for int16<->float conversion! static ign_Map3D_t iatAdvanceCorrectionMap("iat corr"); -// Init PID later (make it compatible with unit-tests) -static Pid idleTimingPid; -static bool shouldResetTimingPid = false; - static int minCrankingRpm = 0; #if IGN_LOAD_COUNT == DEFAULT_IGN_LOAD_COUNT @@ -122,41 +118,10 @@ angle_t getAdvanceCorrections(int rpm DECLARE_ENGINE_PARAMETER_SUFFIX) { if (!iatValid) { iatCorrection = 0; } else { - iatCorrection = iatAdvanceCorrectionMap.getValue((float) rpm, iat); + iatCorrection = iatAdvanceCorrectionMap.getValue(rpm, iat); } - // PID Ignition Advance angle correction - float pidTimingCorrection = 0.0f; - if (CONFIG(useIdleTimingPidControl)) { - int targetRpm = ENGINE(idleController)->getTargetRpm(Sensor::get(SensorType::Clt).value_or(0)); - int rpmDelta = absI(rpm - targetRpm); - - auto [valid, tps] = Sensor::get(SensorType::Tps1); - - // If TPS is invalid, or we aren't in the region, so reset state and don't apply PID - if (!valid || tps >= CONFIG(idlePidDeactivationTpsThreshold)) { - // we are not in the idle mode anymore, so the 'reset' flag will help us when we return to the idle. - shouldResetTimingPid = true; - } - else if (rpmDelta > CONFIG(idleTimingPidDeadZone) && rpmDelta < CONFIG(idleTimingPidWorkZone) + CONFIG(idlePidFalloffDeltaRpm)) { - // We're now in the idle mode, and RPM is inside the Timing-PID regulator work zone! - // So if we need to reset the PID, let's do it now - if (shouldResetTimingPid) { - idleTimingPid.reset(); - shouldResetTimingPid = false; - } - // get PID value (this is not an actual Advance Angle, but just a additive correction!) - percent_t timingRawCorr = idleTimingPid.getOutput(targetRpm, rpm, FAST_CALLBACK_PERIOD_MS / 1000.0f); - // tps idle-running falloff - pidTimingCorrection = interpolateClamped(0.0f, timingRawCorr, CONFIG(idlePidDeactivationTpsThreshold), 0.0f, tps); - // rpm falloff - pidTimingCorrection = interpolateClamped(0.0f, pidTimingCorrection, CONFIG(idlePidFalloffDeltaRpm), 0.0f, rpmDelta - CONFIG(idleTimingPidWorkZone)); - } else { - shouldResetTimingPid = true; - } - } else { - shouldResetTimingPid = true; - } + float pidTimingCorrection = getIdleTimingAdjustment(rpm); if (engineConfiguration->debugMode == DBG_IGNITION_TIMING) { #if EFI_TUNER_STUDIO @@ -291,8 +256,6 @@ void initTimingMap(DECLARE_ENGINE_PARAMETER_SIGNATURE) { config->ignitionRpmBins); iatAdvanceCorrectionMap.init(config->ignitionIatCorrTable, config->ignitionIatCorrLoadBins, config->ignitionIatCorrRpmBins); - // init timing PID - idleTimingPid = Pid(&CONFIG(idleTimingPid)); } /** diff --git a/unit_tests/tests/test_idle_controller.cpp b/unit_tests/tests/test_idle_controller.cpp index afddfb796c..fd0fc09bd0 100644 --- a/unit_tests/tests/test_idle_controller.cpp +++ b/unit_tests/tests/test_idle_controller.cpp @@ -62,76 +62,32 @@ TEST(idle, fsioPidParameters) { // ASSERT_EQ(1, engine->acSwitchState); } -// see also util.pid test -TEST(idle, timingPid) { +using ICP = IIdleController::Phase; + +TEST(idle_v2, timingPid) { WITH_ENGINE_TEST_HELPER(TEST_ENGINE); + IdleController dut; + INJECT_ENGINE_REFERENCE(&dut); - // set PID settings - pid_s pidS; - pidS.pFactor = 0.1; - pidS.iFactor = 0; - pidS.dFactor = 0; - pidS.offset = 0; - pidS.minValue = -20; - pidS.maxValue = +20; - pidS.periodMs = 1; - - // setup TimingPid settings - engineConfiguration->idleTimingPidDeadZone = 10; - engineConfiguration->idleTimingPidWorkZone = 100; - engineConfiguration->idlePidFalloffDeltaRpm = 30; - - // setup target rpm curve - const int idleRpmTarget = 700; - setArrayValues(engineConfiguration->cltIdleRpm, idleRpmTarget); - - // setup other settings - engineConfiguration->idleTimingPid = pidS; - eth.engine.fsioState.fsioTimingAdjustment = 0; - eth.engine.fsioState.fsioIdleTargetRPMAdjustment = 0; - eth.engine.engineState.cltTimingCorrection = 0; - - // configure TPS - engineConfiguration->idlePidDeactivationTpsThreshold = 10; - Sensor::setMockValue(SensorType::Tps1, 0); - - // all corrections disabled, should be 0 - engineConfiguration->useIdleTimingPidControl = false; - angle_t corr = getAdvanceCorrections(idleRpmTarget PASS_ENGINE_PARAMETER_SUFFIX); - ASSERT_EQ(0, corr) << "getAdvanceCorrections#1"; - - // basic IDLE PID correction test engineConfiguration->useIdleTimingPidControl = true; - int baseTestRpm = idleRpmTarget + engineConfiguration->idleTimingPidWorkZone; - corr = getAdvanceCorrections(baseTestRpm PASS_ENGINE_PARAMETER_SUFFIX); - // (delta_rpm=-100) * (p-factor=0.1) = -10 degrees - ASSERT_EQ(-10, corr) << "getAdvanceCorrections#2"; - // check if rpm is too close to the target - corr = getAdvanceCorrections((idleRpmTarget + engineConfiguration->idleTimingPidDeadZone) PASS_ENGINE_PARAMETER_SUFFIX); - ASSERT_EQ(0, corr) << "getAdvanceCorrections#3"; + pid_s pidCfg{}; + pidCfg.pFactor = 0.1; + pidCfg.minValue = -10; + pidCfg.maxValue = 10; + dut.init(&pidCfg); - // check if rpm is too high (just outside the workzone and even falloff) so we disable the PID correction - int tooHighRpm = idleRpmTarget + engineConfiguration->idleTimingPidWorkZone + engineConfiguration->idlePidFalloffDeltaRpm; - corr = getAdvanceCorrections(tooHighRpm PASS_ENGINE_PARAMETER_SUFFIX); - ASSERT_EQ(0, corr) << "getAdvanceCorrections#4"; + // Check that out of idle mode it doesn't do anything + EXPECT_EQ(0, dut.getIdleTimingAdjustment(1050, 1000, ICP::Cranking)); + EXPECT_EQ(0, dut.getIdleTimingAdjustment(1050, 1000, ICP::Coasting)); + EXPECT_EQ(0, dut.getIdleTimingAdjustment(1050, 1000, ICP::Running)); - // check if rpm is within the falloff zone - int falloffRpm = idleRpmTarget + engineConfiguration->idleTimingPidWorkZone + (engineConfiguration->idlePidFalloffDeltaRpm / 2); - corr = getAdvanceCorrections(falloffRpm PASS_ENGINE_PARAMETER_SUFFIX); - // -(100+30/2) * 0.1 / 2 = -5.75 - ASSERT_FLOAT_EQ(-5.75f, corr) << "getAdvanceCorrections#5"; - - // check if PID correction is disabled in running mode (tps > threshold): - Sensor::setMockValue(SensorType::Tps1, engineConfiguration->idlePidDeactivationTpsThreshold + 1); - corr = getAdvanceCorrections(idleRpmTarget PASS_ENGINE_PARAMETER_SUFFIX); - ASSERT_EQ(0, corr) << "getAdvanceCorrections#6"; - - // check if PID correction is interpolated for transient idle-running TPS positions - Sensor::setMockValue(SensorType::Tps1, engineConfiguration->idlePidDeactivationTpsThreshold / 2); - corr = getAdvanceCorrections(baseTestRpm PASS_ENGINE_PARAMETER_SUFFIX); - ASSERT_FLOAT_EQ(-5.0f, corr) << "getAdvanceCorrections#7"; + // Check that it works in idle mode + EXPECT_FLOAT_EQ(-5, dut.getIdleTimingAdjustment(1050, 1000, ICP::Idling)); + // ...but not when disabled + engineConfiguration->useIdleTimingPidControl = false; + EXPECT_EQ(0, dut.getIdleTimingAdjustment(1050, 1000, ICP::Idling)); } TEST(idle_v2, testTargetRpm) { @@ -148,8 +104,6 @@ TEST(idle_v2, testTargetRpm) { EXPECT_FLOAT_EQ(500, dut.getTargetRpm(50)); } -using ICP = IIdleController::Phase; - TEST(idle_v2, testDeterminePhase) { WITH_ENGINE_TEST_HELPER(TEST_ENGINE); IdleController dut;