diff --git a/firmware/controllers/lua/examples/launch_control.lua b/firmware/controllers/lua/examples/launch_control.lua new file mode 100644 index 0000000000..0f6719abc6 --- /dev/null +++ b/firmware/controllers/lua/examples/launch_control.lua @@ -0,0 +1,156 @@ +-- scriptname launch_control.lua + +function setIgnDisabled(value) end +function setFuelDisabled(value) end + +SPEED_THRESHOLD = 30 -- emulates `Speed Threshold(Kph)` setting on `Launch Control` dialog in TunerStudio +local LAUNCH_RPM = 3000 -- emulates `Launch RPM(rpm)` setting on `Launch Control` dialog in TunerStudio +local LAUNCH_CONTROL_WINDOW = 500 -- emulates `Launch Control Window(RPM)` setting on `Launch Control` dialog in TunerStudio +local TPS_THRESHOLD = 0 -- emulates `Speed Threshold(Kph)` setting on `Launch Control` dialog in TunerStudio +local LAUNCH_CORRECTIONS_END_RPM = 0 -- emulates `Launch Corrections End RPM` setting on `Launch Control` dialog in TunerStudio +local IGNITION_CUT = true -- emulates `Ignition Cut` setting on `Launch Control` dialog in TunerStudio +local INITIAL_IGNITION_CUT = 0 -- emulates `Initial Ignition Cut %` setting on `Launch Control` dialog in TunerStudio +local FINAL_IGNITION_CUT_BEFORE_LAUNCH = 0 -- emulates `Final Ignition Cut % Before launch (%)` setting on `Launch Control` dialog in TunerStudio +local FUEL_CUT = false -- emulates `Fuel Cut` setting on `Launch Control` dialog in TunerStudio + +local isSwitchConditionSatisfied +function isSwitchConditionSatisfied() + --[[ Here we assume that Launch Control is already activated, i.e. condition specified by the following settings on `Launch Control` dialog in TunerStudio: + * `Enable Launch Control` + * `Activation Mode` + * `Launch Button` + * `Launch Button inverted` + * `Launch Button mode` + * `Clutch Down` + * `Clutch Down inverted` + * `Clutch Down mode` + - is already satisfied + ]]-- + return true +end + +print('SPEED_THRESHOLD = ' .. SPEED_THRESHOLD) + +local isSpeedConditionSatisfied +function isSpeedConditionSatisfied() + if SPEED_THRESHOLD == 0 then + print('SPEED_THRESHOLD = ' .. tostring(SPEED_THRESHOLD)) + return true + else + local currentVehicleSpeed = getSensor("VehicleSpeed") + if currentVehicleSpeed == nil then + print('currentVehicleSpeed is undefined, so we are using default zero value') + currentVehicleSpeed = 0 + else + print('currentVehicleSpeed = ' .. currentVehicleSpeed) + end + return currentVehicleSpeed < SPEED_THRESHOLD + end +end + +local isTpsConditionSatisfied +function isTpsConditionSatisfied() + local currentDriverThrotrrentDriverThrottleIntetleIntent = getSensor("DriverThrottleIntent") + if currentDriverThrottleIntent == nil then + print('currentVehicleSpeed is undefined, so TPS condition is not satisfied') + return false + else + print('currentDriverThrottleIntent = ' .. currentDriverThrottleIntent) + end + return TPS_THRESHOLD < currentDriverThrottleIntent +end + +-- Launch condition enumeration +local LaunchCondition = { + NOT_MET = {}, + PRE_LAUNCH = {}, + LAUNCH = {} +} + +local calculateRpmLaunchCondition +function calculateRpmLaunchCondition(rpm) + if rpm < LAUNCH_RPM - LAUNCH_CONTROL_WINDOW then + return LaunchCondition.NOT_MET + elseif LAUNCH_RPM <= rpm then + return LaunchCondition.LAUNCH + else + return PRE_LAUNCH + end +end + +local calculateLaunchCondition +function calculateLaunchCondition(rpm) + local switchCondition = isSwitchConditionSatisfied() + + if not switchCondition then + print('Switch condition is not satisfied') + end + + local speedCondition = isSpeedConditionSatisfied() + if not speedCondition then + print('Speed condition is not satisfied') + end + + local tpsCondition = isTpsConditionSatisfied() + if not tpsCondition then + print('TPS condition is not satisfied') + end + + if switchCondition and speedCondition and tpsCondition then + return calculateRpmLaunchCondition(rpm) + else + return LaunchCondition.NOT_MET + end +end + +function interpolateClamped(x1, y1, x2, y2, x) + if x <= x1 then + return y1 + elseif x2 <= x then + return y2 + else + return interpolate(x1, y1, x2, y2, x) + end +end + +local updateSparkSkipRatio +function updateSparkSkipRatio(launchCondition, rpm) + local sparkSkipRatio = 0.0 + if launchCondition == LaunchCondition.LAUNCH then + sparkSkipRatio = 1.0 + elseif launchCondition == LaunchCondition.PRE_LAUNCH then + local sparkSkipStartRpm = LAUNCH_RPM - LAUNCH_CONTROL_WINDOW + local sparkSkipEndRpm = LAUNCH_RPM - LAUNCH_CORRECTIONS_END_RPM + sparkSkipRatio = interpolateClamped(sparkSkipStartRpm, INITIAL_IGNITION_CUT, sparkSkipEndRpm, FINAL_IGNITION_CUT_BEFORE_LAUNCH) + end + setSparkHardSkipRatio(sparkSkipRatio) +end + +local updateIgnitionCut +function updateIgnitionCut(launchCondition) + local disableIgnition = (IGNITION_CUT and launchCondition == LaunchCondition.LAUNCH) + setIgnDisabled(disableIgnition) +end + +local updateFuelCut +function updateFuelCut(launchCondition) + setFuelDisabled(FUEL_CUT and launchCondition == LaunchCondition.LAUNCH) +end + +function onTick() + local currentRpm = getSensor("Rpm") + print('currentRpm = ' .. currentRpm) + + local launchCondition = calculateLaunchCondition(currentRpm) + if launchCondition == LaunchCondition.NOT_MET then + print('NOT_MET') + elseif launchCondition == LaunchCondition.PRE_LAUNCH then + print('PRE_LAUNCH') + else -- launchCondition == LaunchCondition.LAUNCH + print('LAUNCH') + end + + updateSparkSkipRatio(launchCondition, currentRpm) + updateIgnitionCut(launchCondition) + updateFuelCut(launchCondition) +end