diff --git a/firmware/controllers/actuators/idle_thread.cpp b/firmware/controllers/actuators/idle_thread.cpp index cbe4943c0b..13d230c62f 100644 --- a/firmware/controllers/actuators/idle_thread.cpp +++ b/firmware/controllers/actuators/idle_thread.cpp @@ -193,10 +193,6 @@ 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)? @@ -273,26 +269,6 @@ 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); @@ -456,11 +432,9 @@ 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; @@ -555,10 +529,6 @@ 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)); @@ -635,7 +605,6 @@ 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 c3a0ec5f9c..ac8413e3fb 100644 --- a/firmware/controllers/actuators/idle_thread.h +++ b/firmware/controllers/actuators/idle_thread.h @@ -11,7 +11,6 @@ #include "engine_ptr.h" #include "rusefi_types.h" #include "periodic_task.h" -#include "pid.h" struct IIdleController { enum class Phase : uint8_t { @@ -29,13 +28,13 @@ struct IIdleController { }; class Logging; +class Pid; + class IdleController : public IIdleController { public: DECLARE_ENGINE_PTR; - void init(pid_s* idlePidConfig); - float getIdlePosition(); void update(); @@ -49,23 +48,11 @@ 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 9d3c474e3d..4c703377f9 100644 --- a/firmware/controllers/algo/advance_map.cpp +++ b/firmware/controllers/algo/advance_map.cpp @@ -37,6 +37,10 @@ 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 @@ -118,10 +122,41 @@ angle_t getAdvanceCorrections(int rpm DECLARE_ENGINE_PARAMETER_SUFFIX) { if (!iatValid) { iatCorrection = 0; } else { - iatCorrection = iatAdvanceCorrectionMap.getValue(rpm, iat); + iatCorrection = iatAdvanceCorrectionMap.getValue((float) rpm, iat); } - float pidTimingCorrection = getIdleTimingAdjustment(rpm); + // 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; + } if (engineConfiguration->debugMode == DBG_IGNITION_TIMING) { #if EFI_TUNER_STUDIO @@ -256,6 +291,8 @@ 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 fd0fc09bd0..afddfb796c 100644 --- a/unit_tests/tests/test_idle_controller.cpp +++ b/unit_tests/tests/test_idle_controller.cpp @@ -62,32 +62,76 @@ TEST(idle, fsioPidParameters) { // ASSERT_EQ(1, engine->acSwitchState); } -using ICP = IIdleController::Phase; - -TEST(idle_v2, timingPid) { +// see also util.pid test +TEST(idle, timingPid) { WITH_ENGINE_TEST_HELPER(TEST_ENGINE); - IdleController dut; - INJECT_ENGINE_REFERENCE(&dut); - engineConfiguration->useIdleTimingPidControl = true; + // 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; - pid_s pidCfg{}; - pidCfg.pFactor = 0.1; - pidCfg.minValue = -10; - pidCfg.maxValue = 10; - dut.init(&pidCfg); + // setup TimingPid settings + engineConfiguration->idleTimingPidDeadZone = 10; + engineConfiguration->idleTimingPidWorkZone = 100; + engineConfiguration->idlePidFalloffDeltaRpm = 30; - // 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)); + // 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; - // Check that it works in idle mode - EXPECT_FLOAT_EQ(-5, dut.getIdleTimingAdjustment(1050, 1000, ICP::Idling)); + // configure TPS + engineConfiguration->idlePidDeactivationTpsThreshold = 10; + Sensor::setMockValue(SensorType::Tps1, 0); - // ...but not when disabled + // all corrections disabled, should be 0 engineConfiguration->useIdleTimingPidControl = false; - EXPECT_EQ(0, dut.getIdleTimingAdjustment(1050, 1000, ICP::Idling)); + 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"; + + // 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 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"; + } TEST(idle_v2, testTargetRpm) { @@ -104,6 +148,8 @@ 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;