From 542cb6a420780ce3926434fcb222875f94021e56 Mon Sep 17 00:00:00 2001
From: Matthew Kennedy <matthewkennedy@outlook.com>
Date: Sat, 26 Dec 2020 05:32:01 -0800
Subject: [PATCH] more testable idle logic (#2140)

* header

* test

* impl
---
 .../controllers/actuators/idle_thread.cpp     | 25 ++++++++++++
 firmware/controllers/actuators/idle_thread.h  | 11 +++++
 unit_tests/tests/test_idle_controller.cpp     | 40 +++++++++++++++++++
 3 files changed, 76 insertions(+)

diff --git a/firmware/controllers/actuators/idle_thread.cpp b/firmware/controllers/actuators/idle_thread.cpp
index 1411a82e0b..02d297913d 100644
--- a/firmware/controllers/actuators/idle_thread.cpp
+++ b/firmware/controllers/actuators/idle_thread.cpp
@@ -201,6 +201,31 @@ int IdleController::getTargetRpm(float clt) const {
 	return fsioBump + interpolate2d("cltRpm", clt, CONFIG(cltIdleRpmBins), CONFIG(cltIdleRpm));
 }
 
+IIdleController::Phase IdleController::determinePhase(int rpm, int targetRpm, SensorResult tps) const {
+	if (!engine->rpmCalculator.isRunning()) {
+		return Phase::Cranking;
+	}
+
+	if (!tps) {
+		// If the TPS has failed, assume the engine is running
+		return Phase::Running;
+	}
+
+	// if throttle pressed, we're out of the idle corner
+	if (tps.Value > CONFIG(idlePidDeactivationTpsThreshold)) {
+		return Phase::Running;
+	}
+
+	// If rpm too high (but throttle not pressed), we're coasting
+	int maximumIdleRpm = targetRpm + CONFIG(idlePidRpmUpperLimit);
+	if (rpm > maximumIdleRpm) {
+		return Phase::Coasting;
+	}
+
+	// No other conditions met, we are idling!
+	return Phase::Idling;
+}
+
 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 9324acc51a..6711a15cd4 100644
--- a/firmware/controllers/actuators/idle_thread.h
+++ b/firmware/controllers/actuators/idle_thread.h
@@ -13,6 +13,14 @@
 #include "periodic_task.h"
 
 struct IIdleController {
+	enum class Phase : uint8_t {
+		Cranking,	// Below cranking threshold
+		Idling,		// Below idle RPM, off throttle
+		Coasting,	// Off throttle but above idle RPM
+		Running,	// On throttle
+	};
+
+	virtual Phase determinePhase(int rpm, int targetRpm, SensorResult tps) const;
 	virtual int getTargetRpm(float clt) const = 0;
 };
 
@@ -29,6 +37,9 @@ public:
 
 	// TARGET DETERMINATION
 	int getTargetRpm(float clt) const override;
+
+	// PHASE DETERMINATION: what is the driver trying to do right now?
+	Phase determinePhase(int rpm, int targetRpm, 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 7dc679e9d8..92ef1de5b2 100644
--- a/unit_tests/tests/test_idle_controller.cpp
+++ b/unit_tests/tests/test_idle_controller.cpp
@@ -144,3 +144,43 @@ TEST(idle_v2, testTargetRpm) {
 	EXPECT_FLOAT_EQ(100, dut.getTargetRpm(10));
 	EXPECT_FLOAT_EQ(500, dut.getTargetRpm(50));
 }
+
+using ICP = IIdleController::Phase;
+
+TEST(idle_v2, testDeterminePhase) {
+	WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
+	IdleController dut;
+	INJECT_ENGINE_REFERENCE(&dut);
+
+	// TPS threshold 5% for easy test
+	CONFIG(idlePidDeactivationTpsThreshold) = 5;
+	// RPM window is 100 RPM above target
+	CONFIG(idlePidRpmUpperLimit) = 100;
+
+	// First test stopped engine
+	engine->rpmCalculator.setRpmValue(0);
+	EXPECT_EQ(ICP::Cranking, dut.determinePhase(0, 1000, unexpected));
+
+	// Now engine is running!
+	// Controller doesn't need this other than for isCranking()
+	engine->rpmCalculator.setRpmValue(1000);
+
+	// Test invalid TPS, but inside the idle window
+	EXPECT_EQ(ICP::Running, dut.determinePhase(1000, 1000, unexpected));
+
+	// Valid TPS should now be inside the zone
+	EXPECT_EQ(ICP::Idling, dut.determinePhase(1000, 1000, 0));
+
+	// Above TPS threshold should be outside the zone
+	EXPECT_EQ(ICP::Running, dut.determinePhase(1000, 1000, 10));
+
+	// Above target, below (target + upperLimit) should be in idle zone
+	EXPECT_EQ(ICP::Idling, dut.determinePhase(1099, 1000, 0));
+
+	// above upper limit and on throttle should be out of idle zone
+	EXPECT_EQ(ICP::Running, dut.determinePhase(1101, 1000, 10));
+
+	// Below TPS but above RPM should be outside the zone
+	EXPECT_EQ(ICP::Coasting, dut.determinePhase(1101, 1000, 0));
+	EXPECT_EQ(ICP::Coasting, dut.determinePhase(5000, 1000, 0));
+}