diff --git a/firmware/controllers/actuators/idle_thread.cpp b/firmware/controllers/actuators/idle_thread.cpp index 02d297913d..ce04e28b2b 100644 --- a/firmware/controllers/actuators/idle_thread.cpp +++ b/firmware/controllers/actuators/idle_thread.cpp @@ -226,6 +226,49 @@ IIdleController::Phase IdleController::determinePhase(int rpm, int targetRpm, Se return Phase::Idling; } +float IdleController::getCrankingOpenLoop(float clt) const { + return + CONFIG(crankingIACposition) // Base cranking position (cranking page) + * interpolate2d("cltCrankingT", clt, config->cltCrankingCorrBins, config->cltCrankingCorr); +} + +float IdleController::getRunningOpenLoop(float clt, SensorResult tps) const { + float running = + CONFIG(manIdlePosition) // Base idle position (slider) + * interpolate2d("cltT", clt, config->cltIdleCorrBins, config->cltIdleCorr); + + // Now we bump it by the AC/fan amount if necessary + running += engine->acSwitchState ? CONFIG(acIdleExtraOffset) : 0; + // TODO: fan idle bump needs its own config field + running += enginePins.fanRelay.getLogicValue() ? CONFIG(acIdleExtraOffset) : 0; + + // Now bump it by the specified amount when the throttle is opened (if configured) + // nb: invalid tps will make no change, no explicit check required + running += interpolateClamped( + 0, 0, + CONFIG(idlePidDeactivationTpsThreshold), CONFIG(iacByTpsTaper), + tps.value_or(0)); + + return clampF(0, running, 100); +} + +float IdleController::getOpenLoop(Phase phase, float clt, SensorResult tps) const { + float running = getRunningOpenLoop(clt, tps); + + // Cranking value is either its own table, or the running value if not overriden + float cranking = CONFIG(overrideCrankingIacSetting) ? getCrankingOpenLoop(clt) : running; + + // if we're cranking, nothing more to do. + if (phase == Phase::Cranking) { + return cranking; + } + + // Interpolate between cranking and running over a short time + // This clamps once you fall off the end, so no explicit check for running required + auto revsSinceStart = engine->rpmCalculator.getRevolutionCounterSinceStart(); + return interpolateClamped(0, cranking, CONFIG(afterCrankingIACtaperDuration), running, revsSinceStart); +} + static percent_t manualIdleController(float cltCorrection DECLARE_ENGINE_PARAMETER_SUFFIX) { percent_t correctedPosition = cltCorrection * CONFIG(manIdlePosition); diff --git a/firmware/controllers/actuators/idle_thread.h b/firmware/controllers/actuators/idle_thread.h index 6711a15cd4..ac8413e3fb 100644 --- a/firmware/controllers/actuators/idle_thread.h +++ b/firmware/controllers/actuators/idle_thread.h @@ -20,8 +20,11 @@ struct IIdleController { Running, // On throttle }; - virtual Phase determinePhase(int rpm, int targetRpm, SensorResult tps) const; + virtual Phase determinePhase(int rpm, int targetRpm, SensorResult tps) const = 0; virtual int getTargetRpm(float clt) const = 0; + virtual float getCrankingOpenLoop(float clt) const = 0; + virtual float getRunningOpenLoop(float clt, SensorResult tps) const = 0; + virtual float getOpenLoop(Phase phase, float clt, SensorResult tps) const = 0; }; class Logging; @@ -40,6 +43,11 @@ public: // PHASE DETERMINATION: what is the driver trying to do right now? Phase determinePhase(int rpm, int targetRpm, SensorResult tps) const override; + + // OPEN LOOP CORRECTIONS + float getCrankingOpenLoop(float clt) const override; + float getRunningOpenLoop(float clt, SensorResult tps) const override; + float getOpenLoop(Phase phase, float clt, SensorResult tps) const override; }; void updateIdleControl(); diff --git a/unit_tests/tests/test_idle_controller.cpp b/unit_tests/tests/test_idle_controller.cpp index 92ef1de5b2..afddfb796c 100644 --- a/unit_tests/tests/test_idle_controller.cpp +++ b/unit_tests/tests/test_idle_controller.cpp @@ -16,6 +16,9 @@ #include "electronic_throttle.h" #include "sensor.h" +using ::testing::StrictMock; +using ::testing::_; + extern IdleController idleControllerInstance; extern int timeNowUs; @@ -184,3 +187,122 @@ TEST(idle_v2, testDeterminePhase) { EXPECT_EQ(ICP::Coasting, dut.determinePhase(1101, 1000, 0)); EXPECT_EQ(ICP::Coasting, dut.determinePhase(5000, 1000, 0)); } + +TEST(idle_v2, crankingOpenLoop) { + WITH_ENGINE_TEST_HELPER(TEST_ENGINE); + IdleController dut; + INJECT_ENGINE_REFERENCE(&dut); + + engineConfiguration->crankingIACposition = 50; + + for (size_t i = 0; i < efi::size(config->cltCrankingCorrBins); i++) { + config->cltCrankingCorrBins[i] = i * 10; + config->cltCrankingCorr[i] = i * 0.1f; + } + + EXPECT_FLOAT_EQ(5, dut.getCrankingOpenLoop(10)); + EXPECT_FLOAT_EQ(25, dut.getCrankingOpenLoop(50)); +} + +TEST(idle_v2, runningOpenLoopBasic) { + WITH_ENGINE_TEST_HELPER(TEST_ENGINE); + IdleController dut; + INJECT_ENGINE_REFERENCE(&dut); + + engineConfiguration->manIdlePosition = 50; + + for (size_t i = 0; i < efi::size(config->cltIdleCorrBins); i++) { + config->cltIdleCorrBins[i] = i * 10; + config->cltIdleCorr[i] = i * 0.1f; + } + + EXPECT_FLOAT_EQ(5, dut.getRunningOpenLoop(10, 0)); + EXPECT_FLOAT_EQ(25, dut.getRunningOpenLoop(50, 0)); +} + +// TODO: test AC/fan open loop compensation + +TEST(idle_v2, runningOpenLoopTpsTaper) { + WITH_ENGINE_TEST_HELPER(TEST_ENGINE); + IdleController dut; + INJECT_ENGINE_REFERENCE(&dut); + + // Zero out base tempco table + setArrayValues(config->cltIdleCorr, 0.0f); + + // Add 50% idle position + CONFIG(iacByTpsTaper) = 50; + // At 10% TPS + CONFIG(idlePidDeactivationTpsThreshold) = 10; + + // Check in-bounds points + EXPECT_FLOAT_EQ(0, dut.getRunningOpenLoop(0, 0)); + EXPECT_FLOAT_EQ(25, dut.getRunningOpenLoop(0, 5)); + EXPECT_FLOAT_EQ(50, dut.getRunningOpenLoop(0, 10)); + + // Check out of bounds - shouldn't leave the interval [0, 10] + EXPECT_FLOAT_EQ(0, dut.getRunningOpenLoop(0, -5)); + EXPECT_FLOAT_EQ(50, dut.getRunningOpenLoop(0, 20)); +} + +struct MockOpenLoopIdler : public IdleController { + MOCK_METHOD(float, getCrankingOpenLoop, (float clt), (const, override)); + MOCK_METHOD(float, getRunningOpenLoop, (float clt, SensorResult tps), (const, override)); +}; + +TEST(idle_v2, testOpenLoopCrankingNoOverride) { + WITH_ENGINE_TEST_HELPER(TEST_ENGINE); + StrictMock dut; + INJECT_ENGINE_REFERENCE(&dut); + + EXPECT_CALL(dut, getRunningOpenLoop(30, SensorResult(0))).WillOnce(Return(33)); + + EXPECT_FLOAT_EQ(33, dut.getOpenLoop(ICP::Cranking, 30, 0)); +} + +TEST(idle_v2, testOpenLoopCrankingOverride) { + WITH_ENGINE_TEST_HELPER(TEST_ENGINE); + StrictMock dut; + INJECT_ENGINE_REFERENCE(&dut); + + CONFIG(overrideCrankingIacSetting) = true; + + EXPECT_CALL(dut, getRunningOpenLoop(30, SensorResult(0))).WillOnce(Return(33)); + EXPECT_CALL(dut, getCrankingOpenLoop(30)).WillOnce(Return(44)); + + // Should return the value from getCrankingOpenLoop, and ignore running numbers + EXPECT_FLOAT_EQ(44, dut.getOpenLoop(ICP::Cranking, 30, 0)); +} + +TEST(idle_v2, openLoopRunningTaper) { + WITH_ENGINE_TEST_HELPER(TEST_ENGINE); + StrictMock dut; + INJECT_ENGINE_REFERENCE(&dut); + + CONFIG(overrideCrankingIacSetting) = true; + CONFIG(afterCrankingIACtaperDuration) = 500; + + EXPECT_CALL(dut, getRunningOpenLoop(30, SensorResult(0))).WillRepeatedly(Return(25)); + EXPECT_CALL(dut, getCrankingOpenLoop(30)).WillRepeatedly(Return(75)); + + // 0 cycles - no taper yet, pure cranking value + EXPECT_FLOAT_EQ(75, dut.getOpenLoop(ICP::Idling, 30, 0)); + + // 250 cycles - half way, 50% each value -> outputs 50 + for (size_t i = 0; i < 250; i++) { + engine->rpmCalculator.onNewEngineCycle(); + } + EXPECT_FLOAT_EQ(50, dut.getOpenLoop(ICP::Idling, 30, 0)); + + // 500 cycles - fully tapered, should be running value + for (size_t i = 0; i < 250; i++) { + engine->rpmCalculator.onNewEngineCycle(); + } + EXPECT_FLOAT_EQ(25, dut.getOpenLoop(ICP::Idling, 30, 0)); + + // 1000 cycles - still fully tapered, should be running value + for (size_t i = 0; i < 500; i++) { + engine->rpmCalculator.onNewEngineCycle(); + } + EXPECT_FLOAT_EQ(25, dut.getOpenLoop(ICP::Idling, 30, 0)); +}