Idle timing PID uses phase logic (#2156)

* use phase computation

* move & simplify implementation

* test

* bad merge

* initialize

Co-authored-by: Matthew Kennedy <makenne@microsoft.com>

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;

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

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 = getIdleTimingAdjustment(rpm);
-	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
@@ -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));
 }
 
 /**

unit_tests/tests/test_idle_controller.cpp

@@ -62,76 +62,32 @@ TEST(idle, fsioPidParameters) {
 //	ASSERT_EQ(1, engine->acSwitchState);
 }
 
-// see also util.pid test
+using ICP = IIdleController::Phase;
-TEST(idle, timingPid) {
+
+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<float>(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
+	pid_s pidCfg{};
-	corr = getAdvanceCorrections((idleRpmTarget + engineConfiguration->idleTimingPidDeadZone) PASS_ENGINE_PARAMETER_SUFFIX);
+	pidCfg.pFactor = 0.1;
-	ASSERT_EQ(0, corr) << "getAdvanceCorrections#3";
+	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
+	// Check that out of idle mode it doesn't do anything
-	int tooHighRpm = idleRpmTarget + engineConfiguration->idleTimingPidWorkZone + engineConfiguration->idlePidFalloffDeltaRpm;
+	EXPECT_EQ(0, dut.getIdleTimingAdjustment(1050, 1000, ICP::Cranking));
-	corr = getAdvanceCorrections(tooHighRpm PASS_ENGINE_PARAMETER_SUFFIX);
+	EXPECT_EQ(0, dut.getIdleTimingAdjustment(1050, 1000, ICP::Coasting));
-	ASSERT_EQ(0, corr) << "getAdvanceCorrections#4";
+	EXPECT_EQ(0, dut.getIdleTimingAdjustment(1050, 1000, ICP::Running));
 
-	// check if rpm is within the falloff zone
+	// Check that it works in idle mode
-	int falloffRpm = idleRpmTarget + engineConfiguration->idleTimingPidWorkZone + (engineConfiguration->idlePidFalloffDeltaRpm / 2);
+	EXPECT_FLOAT_EQ(-5, dut.getIdleTimingAdjustment(1050, 1000, ICP::Idling));
-	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";
 
+	// ...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;