Launch control unit tests update (#1972)

* testable launch start

* extract base class

* add a test

* fix ts debug

* Launch control unit_tests

updated launch control for a better split
VSS mock for unit tests

Co-authored-by: Matthew Kennedy <matthewkennedy@outlook.com>

firmware/controllers/algo/launch_control.cpp

@@ -33,24 +33,11 @@ extern TunerStudioOutputChannels tsOutputChannels;
 
 EXTERN_ENGINE;
 
-static bool getActivateSwitchCondition(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
-	switch (engineConfiguration->launchActivationMode) {
-	case SWITCH_INPUT_LAUNCH:
-		if (CONFIG(launchActivatePin) != GPIO_UNASSIGNED) {
-			engine->launchActivatePinState = efiReadPin(CONFIG(launchActivatePin));
-		}
-		return engine->launchActivatePinState;
+#define RETART_THD_CALC	CONFIG(launchRpm) +\
+						(CONFIG(enableLaunchRetard) ? CONFIG(launchAdvanceRpmRange) : 0) +\
+						CONFIG(hardCutRpmRange)
+static int retardThresholdRpm;
 
-	case CLUTCH_INPUT_LAUNCH:
-		if (CONFIG(clutchDownPin) != GPIO_UNASSIGNED) {
-			engine->clutchDownState = efiReadPin(CONFIG(clutchDownPin));
-		}
-		return engine->clutchDownState;
-	default:
-		// ALWAYS_ACTIVE_LAUNCH
-		return true;
-	}
-}
 
 class LaunchControlImpl : public LaunchControlBase, public PeriodicTimerController {
 	int getPeriodMs() override {
@@ -62,11 +49,53 @@ class LaunchControlImpl : public LaunchControlBase, public PeriodicTimerControll
 	}
 };
 
+/**
+ * We can have active condition from switch or from clutch.
+ * In case we are dependent on VSS we just return true.
+ */
+bool LaunchControlBase::isInsideSwitchCondition() const {
+	switch (CONFIG(launchActivationMode)) {
+	case SWITCH_INPUT_LAUNCH:
+		if (CONFIG(launchActivatePin) != GPIO_UNASSIGNED) {
+			//todo: we should take into consideration if this sw is pulled high or low!
+			engine->launchActivatePinState = efiReadPin(CONFIG(launchActivatePin));
+		}
+		return engine->launchActivatePinState;
+
+	case CLUTCH_INPUT_LAUNCH:
+		if (CONFIG(clutchDownPin) != GPIO_UNASSIGNED) {
+			engine->clutchDownState = efiReadPin(CONFIG(clutchDownPin));
+			
+			if (CONFIG(clutchDownPinMode) == PI_PULLDOWN)
+			{
+				return !engine->clutchDownState;
+			} else {
+				return engine->clutchDownState;
+			}
+		} else {
+			return false;
+		}
+		
+	default:
+		// ALWAYS_ACTIVE_LAUNCH
+		return true;
+	}
+}
+
+/**
+ * Returns True in case Vehicle speed is less then trashold. 
+ * This condiiion would only return true based on speed if DisablebySpeed is true
+ * The condition logic is written in that way, that if we do not use disable by speed
+ * then we have to return true, and trust that we would disable by other condition!
+ */ 
 bool LaunchControlBase::isInsideSpeedCondition() const {
 	int speed = getVehicleSpeed();
 	return (CONFIG(launchSpeedTreshold) > speed) || !engineConfiguration->launchDisableBySpeed;
 }
 
+/**
+ * Returns false if TPS is invalid or TPS > preset trashold
+ */
 bool LaunchControlBase::isInsideTpsCondition() const {
 	auto tps = Sensor::get(SensorType::DriverThrottleIntent);
 
@@ -78,13 +107,18 @@ bool LaunchControlBase::isInsideTpsCondition() const {
 	return CONFIG(launchTpsTreshold) < tps.Value;
 }
 
+/**
+ * Condition is true as soon as we are above LaunchRpm
+ */
+bool LaunchControlBase::isInsideRPMCondition(int rpm) const {
+	int launchRpm = CONFIG(launchRpm);
+	return (launchRpm < rpm);
+}
+
 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 activateSwitchCondition = isInsideSwitchCondition();
+	bool rpmCondition = isInsideRPMCondition(rpm);
 	bool speedCondition = isInsideSpeedCondition();
 	bool tpsCondition = isInsideTpsCondition();
 
@@ -108,9 +142,13 @@ void LaunchControlBase::update() {
 	int rpm = GET_RPM();
 	bool combinedConditions = isLaunchConditionMet(rpm);
 
-	float timeDelay = engineConfiguration->launchActivateDelay;
-	int cutRpmRange = engineConfiguration->hardCutRpmRange;
-	int launchAdvanceRpmRange = engineConfiguration->launchTimingRpmRange;
+	float timeDelay = CONFIG(launchActivateDelay);
+	int cutRpmRange = CONFIG(hardCutRpmRange);
+	int launchAdvanceRpmRange = CONFIG(launchTimingRpmRange);
+
+	//recalculate in periodic task, this way we save time in applyLaunchControlLimiting
+	//and still recalculat in case user changed the values
+	retardThresholdRpm = RETART_THD_CALC;
 
 	if (!combinedConditions) {
 		// conditions not met, reset timer
@@ -125,16 +163,16 @@ void LaunchControlBase::update() {
 			engine->isLaunchCondition = true;           // ...enable launch!
 			engine->applyLaunchExtraFuel = true;
 		}
-		if (engineConfiguration->enableLaunchBoost) {
+		if (CONFIG(enableLaunchBoost)) {
 			engine->setLaunchBoostDuty = true;           // ...enable boost!
 		}
-		if (engineConfiguration->enableLaunchRetard) {
+		if (CONFIG(enableLaunchRetard)) {
 			engine->applyLaunchControlRetard = true;    // ...enable retard!
 		}
 	}
 
 #if EFI_TUNER_STUDIO
-	if (engineConfiguration->debugMode == DBG_LAUNCH) {
+	if (CONFIG(debugMode) == DBG_LAUNCH) {
 		tsOutputChannels.debugIntField5 = engine->clutchDownState;
 		tsOutputChannels.debugFloatField1 = engine->launchActivatePinState;
 		tsOutputChannels.debugFloatField2 = engine->isLaunchCondition;
@@ -163,12 +201,6 @@ void setDefaultLaunchParameters(DECLARE_CONFIG_PARAMETER_SIGNATURE) {
 }
 
 void applyLaunchControlLimiting(bool *limitedSpark, bool *limitedFuel DECLARE_ENGINE_PARAMETER_SUFFIX) {
-	int rpm = GET_RPM();
-
-	// todo: pre-calculate 'retardThresholdRpm' less often that on each 'applyLaunchControlLimiting' invocation
-	int retardThresholdRpm = CONFIG(launchRpm) +
-		(CONFIG(enableLaunchRetard) ? CONFIG(launchAdvanceRpmRange) : 0) +
-		CONFIG(hardCutRpmRange);
 
 	if (retardThresholdRpm < GET_RPM()) {
 		*limitedSpark = engineConfiguration->launchSparkCutEnable;
@@ -178,6 +210,7 @@ void applyLaunchControlLimiting(bool *limitedSpark, bool *limitedFuel DECLARE_EN
 
 void initLaunchControl(Logging *sharedLogger DECLARE_ENGINE_PARAMETER_SUFFIX) {
 	logger = sharedLogger;
+	retardThresholdRpm = RETART_THD_CALC;
 	Launch.Start();
 }
 

firmware/controllers/algo/launch_control.h

@@ -23,6 +23,8 @@ public:
 
 	bool isInsideSpeedCondition() const;
 	bool isInsideTpsCondition() const;
+	bool isInsideSwitchCondition() const;
+	bool isInsideRPMCondition(int rpm) const;
 
 	bool isLaunchConditionMet(int rpm) const;
 

firmware/hw_layer/vehicle_speed.cpp

@@ -108,9 +108,22 @@ void initVehicleSpeed(Logging *l) {
 }
 #else  /* EFI_VEHICLE_SPEED */
 
+#if EFI_UNIT_TEST
+static float mockVehicleSpeed = 0.0; // in kilometers per hour
+
+void setMockVehicleSpeed(float speedKPH) {
+	mockVehicleSpeed = speedKPH;
+}
+float getVehicleSpeed(void) {
+	
+	// Mock return to be used in unit tests
+	return mockVehicleSpeed;	
+}
+#else
 float getVehicleSpeed(void) {
 	
 	// no VSS support
 	return 0;	
 }
+#endif /* EFI_UNIT_TEST */
 #endif /* EFI_VEHICLE_SPEED */

unit_tests/tests/test_launch.cpp

@@ -1,5 +1,7 @@
 #include "engine_test_helper.h"
+#include "engine_controller.h"
 #include "launch_control.h"
+#include "vehicle_speed.h"
 
 #include <gtest/gtest.h>
 
@@ -23,3 +25,109 @@ TEST(LaunchControl, TpsCondition) {
 	Sensor::setMockValue(SensorType::DriverThrottleIntent, 20.0f);
 	EXPECT_TRUE(dut.isInsideTpsCondition());
 }
+
+
+TEST(LaunchControl, VSSCondition) {
+	WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
+
+	LaunchControlBase dut;
+	INJECT_ENGINE_REFERENCE(&dut);
+
+	// Test Speed trashold
+    engineConfiguration->launchSpeedTreshold = 30; 
+	engineConfiguration->launchDisableBySpeed = 1;
+	setMockVehicleSpeed(10);
+    EXPECT_TRUE(dut.isInsideSpeedCondition());
+
+	setMockVehicleSpeed(40);
+	EXPECT_FALSE(dut.isInsideSpeedCondition());
+
+	//we do not want to disable launch by speed
+    engineConfiguration->launchSpeedTreshold = 30; 
+	engineConfiguration->launchDisableBySpeed = 0;
+
+	setMockVehicleSpeed(10.0);
+    EXPECT_TRUE(dut.isInsideSpeedCondition());
+
+	setMockVehicleSpeed(40.0);
+	EXPECT_TRUE(dut.isInsideSpeedCondition());
+}
+
+TEST(LaunchControl, RPMCondition) {
+	WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
+
+	LaunchControlBase dut;
+	INJECT_ENGINE_REFERENCE(&dut);
+
+	engineConfiguration->launchRpm = 3000;
+
+	EXPECT_FALSE(dut.isInsideRPMCondition(2900));
+
+	EXPECT_TRUE(dut.isInsideRPMCondition(3100));
+}
+
+TEST(LaunchControl, SwitchInputCondition) {
+	WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
+
+	LaunchControlBase dut;
+	INJECT_ENGINE_REFERENCE(&dut);
+
+	//activation based on VSS
+	engineConfiguration->launchActivationMode=ALWAYS_ACTIVE_LAUNCH;
+	EXPECT_TRUE(dut.isInsideSwitchCondition());
+
+	//active by switch
+	engineConfiguration->launchActivationMode=SWITCH_INPUT_LAUNCH;
+	engineConfiguration->launchActivatePin = GPIOG_1;
+	setMockState(engineConfiguration->launchActivatePin, true);
+	EXPECT_TRUE(dut.isInsideSwitchCondition());
+
+	setMockState(engineConfiguration->launchActivatePin, false);
+	EXPECT_FALSE(dut.isInsideSwitchCondition());
+
+	//by clutch
+	engineConfiguration->launchActivationMode=CLUTCH_INPUT_LAUNCH;
+	engineConfiguration->clutchDownPin = GPIOG_2;
+	engineConfiguration->clutchDownPinMode = PI_PULLUP;
+	setMockState(engineConfiguration->clutchDownPin, true);
+	EXPECT_TRUE(dut.isInsideSwitchCondition());
+
+	setMockState(engineConfiguration->clutchDownPin, false);
+	EXPECT_FALSE(dut.isInsideSwitchCondition());
+
+	engineConfiguration->clutchDownPinMode = PI_PULLDOWN;
+	setMockState(engineConfiguration->clutchDownPin, false);
+	EXPECT_TRUE(dut.isInsideSwitchCondition());
+
+	setMockState(engineConfiguration->clutchDownPin, true);
+	EXPECT_FALSE(dut.isInsideSwitchCondition());
+
+}
+
+TEST(LaunchControl, CombinedCondition) {
+	WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
+
+	LaunchControlBase dut;
+	INJECT_ENGINE_REFERENCE(&dut);
+
+	//check VSS normal usage
+	engineConfiguration->launchActivationMode=ALWAYS_ACTIVE_LAUNCH;
+    engineConfiguration->launchSpeedTreshold = 30; 
+	engineConfiguration->launchDisableBySpeed = 1;
+	engineConfiguration->launchRpm = 3000;
+	engineConfiguration->launchTpsTreshold = 10;
+	//valid TPS
+	Sensor::setMockValue(SensorType::DriverThrottleIntent, 20.0f);
+	
+	setMockVehicleSpeed(10);
+	engine->rpmCalculator.mockRpm = 1200;
+
+    EXPECT_FALSE(dut.isLaunchConditionMet(1200));
+
+	engine->rpmCalculator.mockRpm = 3200;
+	EXPECT_TRUE(dut.isLaunchConditionMet(3200));
+
+	setMockVehicleSpeed(40);
+	EXPECT_FALSE(dut.isLaunchConditionMet(3200));
+
+}