Idle refactor open loop (#2146)

* impl

* typo

* tests

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);

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();

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<MockOpenLoopIdler> 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<MockOpenLoopIdler> 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<MockOpenLoopIdler> 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));
+}