diff --git a/firmware/controllers/algo/launch_control.cpp b/firmware/controllers/algo/launch_control.cpp index 6d7c8da10b..15b9b24af4 100644 --- a/firmware/controllers/algo/launch_control.cpp +++ b/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(); } diff --git a/firmware/controllers/algo/launch_control.h b/firmware/controllers/algo/launch_control.h index 6ac1d37d2b..cfd5c32785 100644 --- a/firmware/controllers/algo/launch_control.h +++ b/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; diff --git a/firmware/hw_layer/vehicle_speed.cpp b/firmware/hw_layer/vehicle_speed.cpp index 243ac16c02..1341ebb0f9 100644 --- a/firmware/hw_layer/vehicle_speed.cpp +++ b/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 */ diff --git a/unit_tests/tests/test_launch.cpp b/unit_tests/tests/test_launch.cpp index bca779bb72..614c257bd1 100644 --- a/unit_tests/tests/test_launch.cpp +++ b/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 @@ -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)); + +}