Launch control testable (#1967)

* testable launch start

* extract base class

* add a test

* fix ts debug

firmware/controllers/algo/launch_control.cpp

@@ -52,74 +52,98 @@ static bool getActivateSwitchCondition(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	}
 }
 
-class LaunchControl: public PeriodicTimerController {
-	efitick_t launchTimer;
-
-	DECLARE_ENGINE_PTR;
-
+class LaunchControlImpl : public LaunchControlBase, public PeriodicTimerController {
 	int getPeriodMs() override {
 		return 50;
 	}
 
-	void PeriodicTask() override {
-		if (!CONFIG(launchControlEnabled)) {
-			return;
-		}
-
-		int rpm = GET_RPM();
-		int speed = getVehicleSpeed();
-		auto tps = Sensor::get(SensorType::DriverThrottleIntent);
-		int tpstreshold = engineConfiguration->launchTpsTreshold;
-		float timeDelay = engineConfiguration->launchActivateDelay;
-		int cutRpmRange = engineConfiguration->hardCutRpmRange;
-		int launchAdvanceRpmRange = engineConfiguration->launchTimingRpmRange;
-		int launchRpm = engineConfiguration->launchRpm;
-
-		bool activateSwitchCondition = getActivateSwitchCondition(PASS_ENGINE_PARAMETER_SIGNATURE);
-
-		bool rpmCondition = (launchRpm < rpm);
-		bool tpsCondition = tps.Valid && (tpstreshold < tps.Value);
-
-		bool speedCondition = (CONFIG(launchSpeedTreshold) > speed) || !engineConfiguration->launchDisableBySpeed;
-
-		bool combinedConditions = speedCondition && activateSwitchCondition && rpmCondition && tpsCondition;
-
-		if (!combinedConditions) {
-			// conditions not met, reset timer
-			launchTimer = getTimeNowNt();
-			engine->isLaunchCondition = false;
-			engine->setLaunchBoostDuty = false;
-			engine->applyLaunchControlRetard = false;
-			engine->applyLaunchExtraFuel = false;
-		} else {
-			// If conditions are met...
-			if ((getTimeNowNt() - launchTimer > MS2NT(timeDelay * 1000)) && combinedConditions) {
-				engine->isLaunchCondition = true;           // ...enable launch!
-				engine->applyLaunchExtraFuel = true;
-			}
-			if (engineConfiguration->enableLaunchBoost) {
-				engine->setLaunchBoostDuty = true;           // ...enable boost!
-			}
-			if (engineConfiguration->enableLaunchRetard) {
-				engine->applyLaunchControlRetard = true;    // ...enable retard!
-			}
-		}
-		if (engineConfiguration->debugMode == DBG_LAUNCH) {
-#if EFI_TUNER_STUDIO
-			tsOutputChannels.debugIntField1 = rpmCondition;
-			tsOutputChannels.debugIntField2 = tpsCondition;
-			tsOutputChannels.debugIntField3 = speedCondition;
-			tsOutputChannels.debugIntField4 = activateSwitchCondition;
-			tsOutputChannels.debugIntField5 = engine->clutchDownState;
-			tsOutputChannels.debugFloatField1 = engine->launchActivatePinState;
-			tsOutputChannels.debugFloatField2 = engine->isLaunchCondition;
-			tsOutputChannels.debugFloatField3 = combinedConditions;
-#endif /* EFI_TUNER_STUDIO */
-		}
+	void PeriodicTask() {
+		update();
 	}
 };
 
-static LaunchControl Launch;
+bool LaunchControlBase::isInsideSpeedCondition() const {
+	int speed = getVehicleSpeed();
+	return (CONFIG(launchSpeedTreshold) > speed) || !engineConfiguration->launchDisableBySpeed;
+}
+
+bool LaunchControlBase::isInsideTpsCondition() const {
+	auto tps = Sensor::get(SensorType::DriverThrottleIntent);
+
+	// Disallow launch without valid TPS
+	if (!tps.Valid) {
+		return false;
+	}
+
+	return CONFIG(launchTpsTreshold) < tps.Value;
+}
+
+bool LaunchControlBase::isLaunchConditionMet(int rpm) const {
+	bool activateSwitchCondition = getActivateSwitchCondition(PASS_ENGINE_PARAMETER_SIGNATURE);
+
+	// TODO shadowm60: move this condition to its own function too
+	int launchRpm = engineConfiguration->launchRpm;
+	bool rpmCondition = (launchRpm < rpm);
+
+	bool speedCondition = isInsideSpeedCondition();
+	bool tpsCondition = isInsideTpsCondition();
+
+#if EFI_TUNER_STUDIO
+	if (engineConfiguration->debugMode == DBG_LAUNCH) {
+		tsOutputChannels.debugIntField1 = rpmCondition;
+		tsOutputChannels.debugIntField2 = tpsCondition;
+		tsOutputChannels.debugIntField3 = speedCondition;
+		tsOutputChannels.debugIntField4 = activateSwitchCondition;
+	}
+#endif /* EFI_TUNER_STUDIO */
+
+	return speedCondition && activateSwitchCondition && rpmCondition && tpsCondition;
+}
+
+void LaunchControlBase::update() {
+	if (!CONFIG(launchControlEnabled)) {
+		return;
+	}
+
+	int rpm = GET_RPM();
+	bool combinedConditions = isLaunchConditionMet(rpm);
+
+	float timeDelay = engineConfiguration->launchActivateDelay;
+	int cutRpmRange = engineConfiguration->hardCutRpmRange;
+	int launchAdvanceRpmRange = engineConfiguration->launchTimingRpmRange;
+
+	if (!combinedConditions) {
+		// conditions not met, reset timer
+		launchTimer = getTimeNowNt();
+		engine->isLaunchCondition = false;
+		engine->setLaunchBoostDuty = false;
+		engine->applyLaunchControlRetard = false;
+		engine->applyLaunchExtraFuel = false;
+	} else {
+		// If conditions are met...
+		if ((getTimeNowNt() - launchTimer > MS2NT(timeDelay * 1000)) && combinedConditions) {
+			engine->isLaunchCondition = true;           // ...enable launch!
+			engine->applyLaunchExtraFuel = true;
+		}
+		if (engineConfiguration->enableLaunchBoost) {
+			engine->setLaunchBoostDuty = true;           // ...enable boost!
+		}
+		if (engineConfiguration->enableLaunchRetard) {
+			engine->applyLaunchControlRetard = true;    // ...enable retard!
+		}
+	}
+
+#if EFI_TUNER_STUDIO
+	if (engineConfiguration->debugMode == DBG_LAUNCH) {
+		tsOutputChannels.debugIntField5 = engine->clutchDownState;
+		tsOutputChannels.debugFloatField1 = engine->launchActivatePinState;
+		tsOutputChannels.debugFloatField2 = engine->isLaunchCondition;
+		tsOutputChannels.debugFloatField3 = combinedConditions;
+	}
+#endif /* EFI_TUNER_STUDIO */
+}
+
+static LaunchControlImpl Launch;
 
 void setDefaultLaunchParameters(DECLARE_CONFIG_PARAMETER_SIGNATURE) {
 	engineConfiguration->launchRpm = 4000;    // Rpm to trigger Launch condition

firmware/controllers/algo/launch_control.h

@@ -7,8 +7,25 @@
 
 #pragma once
 
-#include "engine.h"
+#include "engine_ptr.h"
 
+class Logging;
 void initLaunchControl(Logging *sharedLogger DECLARE_ENGINE_PARAMETER_SUFFIX);
 void setDefaultLaunchParameters(DECLARE_CONFIG_PARAMETER_SIGNATURE);
 void applyLaunchControlLimiting(bool *limitedSpark, bool *limitedFuel DECLARE_ENGINE_PARAMETER_SUFFIX);
+
+class LaunchControlBase {
+public:
+	DECLARE_ENGINE_PTR;
+
+	// Update the state of the launch control system
+	void update();
+
+	bool isInsideSpeedCondition() const;
+	bool isInsideTpsCondition() const;
+
+	bool isLaunchConditionMet(int rpm) const;
+
+private:
+	efitick_t launchTimer = 0;
+};

unit_tests/tests/test_launch.cpp

@@ -0,0 +1,25 @@
+#include "engine_test_helper.h"
+#include "launch_control.h"
+
+#include <gtest/gtest.h>
+
+TEST(LaunchControl, TpsCondition) {
+	WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
+
+	LaunchControlBase dut;
+	INJECT_ENGINE_REFERENCE(&dut);
+
+	engineConfiguration->launchTpsTreshold = 10;
+
+	// Should return false with failed sensor
+	Sensor::resetMockValue(SensorType::DriverThrottleIntent);
+	EXPECT_FALSE(dut.isInsideTpsCondition());
+
+	// Should return false when throttle is closed
+	Sensor::setMockValue(SensorType::DriverThrottleIntent, 5.0f);
+	EXPECT_FALSE(dut.isInsideTpsCondition());
+	
+	// Should return true when throttle is opened past the threshold
+	Sensor::setMockValue(SensorType::DriverThrottleIntent, 20.0f);
+	EXPECT_TRUE(dut.isInsideTpsCondition());
+}

unit_tests/tests/tests.mk

@@ -29,6 +29,7 @@ TESTS_SRC_CPP = \
 	tests/test_idle_controller.cpp \
 	tests/test_issue_898.cpp \
 	tests/test_etb.cpp \
+	tests/test_launch.cpp \
 	tests/test_fuel_map.cpp \
 	tests/test_fuel_wall_wetting.cpp \
 	tests/test_one_cylinder_logic.cpp \