From 6464d4dc7bed0c80649689986d0ee819a2d23bdc Mon Sep 17 00:00:00 2001
From: kifir <kifir239@gmail.com>
Date: Mon, 27 May 2024 15:18:24 +0300
Subject: [PATCH] fix smooth retard functionality #5611

---
 firmware/controllers/algo/advance_map.cpp    | 15 ++++++++-------
 firmware/controllers/algo/launch_control.cpp | 11 ++++++++++-
 firmware/controllers/algo/launch_control.h   |  1 +
 3 files changed, 19 insertions(+), 8 deletions(-)

diff --git a/firmware/controllers/algo/advance_map.cpp b/firmware/controllers/algo/advance_map.cpp
index 7921f12f71..fc421521be 100644
--- a/firmware/controllers/algo/advance_map.cpp
+++ b/firmware/controllers/algo/advance_map.cpp
@@ -104,15 +104,16 @@ angle_t getRunningAdvance(int rpm, float engineLoad) {
 #endif
 
 #if EFI_LAUNCH_CONTROL
-	if (engine->launchController.isLaunchCondition && engineConfiguration->enableLaunchRetard) {
+	if (engine->launchController.isSmoothRetardCondition && engineConfiguration->enableLaunchRetard) {
+		const float launchAngle = engineConfiguration->launchTimingRetard;
 		if (engineConfiguration->launchSmoothRetard) {
-			float launchAngle = engineConfiguration->launchTimingRetard;
-			int launchRpm = engineConfiguration->launchRpm;
-			int launchRpmWithTimingRange = launchRpm + engineConfiguration->launchRpmWindow;
-			 // interpolate timing from rpm at launch triggered to full retard at launch launchRpm + launchTimingRpmRange
-			return interpolateClamped(launchRpm, advanceAngle, launchRpmWithTimingRange, launchAngle, rpm);
+			const int launchRpm = engineConfiguration->launchRpm;
+			const int smoothRetardStartRpm = (launchRpm - engineConfiguration->launchRpmWindow);
+			const int smoothRetardEndRpm = (launchRpm - engineConfiguration->smoothRetardEndRpm);
+			// https://github.com/rusefi/rusefi/issues/5611#issuecomment-2130431696
+			return interpolateClamped(smoothRetardStartRpm, advanceAngle, smoothRetardEndRpm, launchAngle, rpm);
 		} else {
-			return engineConfiguration->launchTimingRetard;
+			return launchAngle;
 		}
 	}
 #endif /* EFI_LAUNCH_CONTROL */
diff --git a/firmware/controllers/algo/launch_control.cpp b/firmware/controllers/algo/launch_control.cpp
index 537df0036b..b7286043ee 100644
--- a/firmware/controllers/algo/launch_control.cpp
+++ b/firmware/controllers/algo/launch_control.cpp
@@ -119,6 +119,7 @@ LaunchControlBase::LaunchControlBase() {
 	launchActivatePinState = false;
 	isLaunchPreCondition = false;
 	isLaunchCondition = false;
+	isSmoothRetardCondition = false;
 }
 
 float LaunchControlBase::getFuelCoefficient() const {
@@ -130,7 +131,7 @@ void LaunchControlBase::update() {
 		return;
 	}
 
-	int rpm = Sensor::getOrZero(SensorType::Rpm);
+	const int rpm = Sensor::getOrZero(SensorType::Rpm);
 	combinedConditions = isLaunchConditionMet(rpm);
 
 	//and still recalculate in case user changed the values
@@ -150,6 +151,8 @@ void LaunchControlBase::update() {
 		// If conditions are met...
 		isLaunchCondition = m_launchTimer.hasElapsedSec(engineConfiguration->launchActivateDelay);
 	}
+
+	isSmoothRetardCondition = isSmoothRetardRpmCondition(rpm);
 }
 
 bool LaunchControlBase::isLaunchRpmRetardCondition() const {
@@ -160,6 +163,12 @@ bool LaunchControlBase::isLaunchSparkRpmRetardCondition() const {
 	return isLaunchRpmRetardCondition() && engineConfiguration->launchSparkCutEnable;
 }
 
+bool LaunchControlBase::isSmoothRetardRpmCondition(const int rpm) const {
+	const int smoothRetardStartRpm = engineConfiguration->launchRpm - engineConfiguration->launchRpmWindow;
+	const int smoothRetardEndRpm = engineConfiguration->launchRpm - engineConfiguration->smoothRetardEndRpm;
+	return (smoothRetardStartRpm <= rpm) && (rpm <= smoothRetardEndRpm);
+}
+
 bool LaunchControlBase::isLaunchFuelRpmRetardCondition() const {
 	return isLaunchRpmRetardCondition() && engineConfiguration->launchFuelCutEnable;
 }
diff --git a/firmware/controllers/algo/launch_control.h b/firmware/controllers/algo/launch_control.h
index 9a6f877501..42818d634c 100644
--- a/firmware/controllers/algo/launch_control.h
+++ b/firmware/controllers/algo/launch_control.h
@@ -37,6 +37,7 @@ public:
 
 private:
 	bool isLaunchRpmRetardCondition() const;
+	bool isSmoothRetardRpmCondition(int rpm) const;
 
 	Timer m_launchTimer;
 };