Revert "Idle timing PID uses phase logic (#2156)"

This reverts commit 9634a803

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;

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

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));
 }
 
 /**

unit_tests/tests/test_idle_controller.cpp

@@ -62,32 +62,76 @@ TEST(idle, fsioPidParameters) {
 //	ASSERT_EQ(1, engine->acSwitchState);
 }
 
-using ICP = IIdleController::Phase;
+// see also util.pid test
-
+TEST(idle, timingPid) {
-TEST(idle_v2, 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{};
+	// setup TimingPid settings
-	pidCfg.pFactor = 0.1;
+	engineConfiguration->idleTimingPidDeadZone = 10;
-	pidCfg.minValue = -10;
+	engineConfiguration->idleTimingPidWorkZone = 100;
-	pidCfg.maxValue = 10;
+	engineConfiguration->idlePidFalloffDeltaRpm = 30;
-	dut.init(&pidCfg);
 
-	// Check that out of idle mode it doesn't do anything
+	// setup target rpm curve
-	EXPECT_EQ(0, dut.getIdleTimingAdjustment(1050, 1000, ICP::Cranking));
+	const int idleRpmTarget = 700;
-	EXPECT_EQ(0, dut.getIdleTimingAdjustment(1050, 1000, ICP::Coasting));
+	setArrayValues<float>(engineConfiguration->cltIdleRpm, idleRpmTarget);
-	EXPECT_EQ(0, dut.getIdleTimingAdjustment(1050, 1000, ICP::Running));
+	
+	// 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
+	// configure TPS
-	EXPECT_FLOAT_EQ(-5, dut.getIdleTimingAdjustment(1050, 1000, ICP::Idling));
+	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;