diff --git a/firmware/controllers/core/interpolation.cpp b/firmware/controllers/core/interpolation.cpp index c606bb6591..90223b0ec7 100644 --- a/firmware/controllers/core/interpolation.cpp +++ b/firmware/controllers/core/interpolation.cpp @@ -129,6 +129,17 @@ float interpolate(float x1, float y1, float x2, float y2, float x) { return interpolateMsg("", x1, y1, x2, y2, x); } +float interpolateClamped(float x1, float y1, float x2, float y2, float x) { + if (x <= x1) + return y1; + if (x >= x2) + return y2; + + float a = INTERPOLATION_A(x1, y1, x2, y2); + float b = y1 - a * x1; + return a * x + b; +} + /** * Another implementation, which one is faster? */ diff --git a/firmware/controllers/core/interpolation.h b/firmware/controllers/core/interpolation.h index ae85c8d5f7..71ff04579a 100644 --- a/firmware/controllers/core/interpolation.h +++ b/firmware/controllers/core/interpolation.h @@ -25,6 +25,7 @@ int findIndexMsg(const char *msg, const float array[], int size, float value); void ensureArrayIsAscending(const char *msg, const float array[], int size); int findIndex2(const float array[], unsigned size, float value); float interpolate(float x1, float y1, float x2, float y2, float x); +float interpolateClamped(float x1, float y1, float x2, float y2, float x); float interpolateMsg(const char *msg, float x1, float y1, float x2, float y2, float x); float interpolate2d(const char *msg, float value, float bin[], float values[], int size); diff --git a/firmware/controllers/idle_thread.cpp b/firmware/controllers/idle_thread.cpp index 5e157bc020..6e5fac9baa 100644 --- a/firmware/controllers/idle_thread.cpp +++ b/firmware/controllers/idle_thread.cpp @@ -54,6 +54,9 @@ static StepperMotor iacMotor; static int adjustedTargetRpm; +static uint32_t lastCrankingCyclesCounter = 0; +static float lastCrankingIacPosition; + /** * that's current position with CLT and IAT corrections */ @@ -213,11 +216,21 @@ static msg_t ivThread(int param) { } else if (!engine->rpmCalculator.isRunning(PASS_ENGINE_PARAMETER_SIGNATURE)) { // during cranking it's always manual mode, PID would make no sence during cranking iacPosition = cltCorrection * engineConfiguration->crankingIACposition; - } else if (engineConfiguration->idleMode == IM_MANUAL) { - // let's re-apply CLT correction - iacPosition = manualIdleController(cltCorrection); + // save cranking position & cycles counter for taper transition + lastCrankingIacPosition = iacPosition; + lastCrankingCyclesCounter = engine->rpmCalculator.getRevolutionCounterSinceStart(); } else { - iacPosition = autoIdle(cltCorrection); + if (engineConfiguration->idleMode == IM_MANUAL) { + // let's re-apply CLT correction + iacPosition = manualIdleController(cltCorrection); + } else { + iacPosition = autoIdle(cltCorrection); + } + // taper transition from cranking to running (uint32_t to float conversion is safe here) + if (engineConfiguration->afterCrankingIACtaperDuration > 0) + iacPosition = interpolateClamped(lastCrankingCyclesCounter, lastCrankingIacPosition, + lastCrankingCyclesCounter + engineConfiguration->afterCrankingIACtaperDuration, iacPosition, + engine->rpmCalculator.getRevolutionCounterSinceStart()); } if (absF(iacPosition - currentIdlePosition) < 1) {