diff --git a/firmware/controllers/actuators/electronic_throttle.cpp b/firmware/controllers/actuators/electronic_throttle.cpp index f882f75fff..696fe1554c 100644 --- a/firmware/controllers/actuators/electronic_throttle.cpp +++ b/firmware/controllers/actuators/electronic_throttle.cpp @@ -95,6 +95,7 @@ static Pid tuneWorkingPid(&tuneWorkingPidSettings); static PID_AutoTune autoTune; static LoggingWithStorage logger("ETB"); +static pedal2tps_t pedal2tpsMap("Pedal2Tps", 1); EXTERN_ENGINE; @@ -226,7 +227,18 @@ private: } - percent_t targetPosition = getPedalPosition(PASS_ENGINE_PARAMETER_SIGNATURE) + engine->engineState.etbIdleAddition; + percent_t pedalPosition = getPedalPosition(PASS_ENGINE_PARAMETER_SIGNATURE); + + int rpm = GET_RPM(); + float targetFromTable = pedal2tpsMap.getValue(0/*rpm / RPM_1_BYTE_PACKING_MULT*/, 0/*pedalPosition*/); + percent_t targetPosition = targetFromTable + engine->engineState.etbIdleAddition; + + if (engineConfiguration->debugMode == DBG_ETB_LOGIC) { +#if EFI_TUNER_STUDIO + tsOutputChannels.debugFloatField1 = targetFromTable; + tsOutputChannels.debugFloatField2 = engine->engineState.etbIdleAddition; +#endif /* EFI_TUNER_STUDIO */ + } feedForward = interpolate2d("etbb", targetPosition, engineConfiguration->etbBiasBins, engineConfiguration->etbBiasValues, ETB_BIAS_CURVE_LENGTH); @@ -334,7 +346,7 @@ void setEtbOffset(int value) { void setDefaultEtbParameters(DECLARE_ENGINE_PARAMETER_SIGNATURE) { CONFIG(etbIdleThrottleRange) = 5; - setLinearCurveAny(config->pedalToTpsPedalBins, PEDAL_TO_TPS_SIZE, /*from*/0, /*to*/100, 1); + setLinearCurveAny(config->pedalToTpsPedalBins, PEDAL_TO_TPS_SIZE, /*from*/0, /*to*/100, 1); setLinearCurveAny(config->pedalToTpsRpmBins, PEDAL_TO_TPS_SIZE, /*from*/0, /*to*/8000 / RPM_1_BYTE_PACKING_MULT, 1); for (int pedalIndex = 0;pedalIndexpedalToTpsTable, config->pedalToTpsPedalBins, config->pedalToTpsRpmBins); if (!hasPedalPositionSensor()) { return; } diff --git a/firmware/util/math/interpolation.cpp b/firmware/util/math/interpolation.cpp index 6bafa36320..a36fb01963 100644 --- a/firmware/util/math/interpolation.cpp +++ b/firmware/util/math/interpolation.cpp @@ -173,64 +173,6 @@ void ensureArrayIsAscending(const char *msg, const float array[], int size) { } } -/** @brief Binary search - * @returns the highest index within sorted array such that array[i] is greater than or equal to the parameter - * @note If the parameter is smaller than the first element of the array, -1 is returned. - * - * See also ensureArrayIsAscending - */ -int findIndexMsg(const char *msg, const float array[], int size, float value) { - if (cisnan(value)) { - firmwareError(ERROR_NAN_FIND_INDEX, "NaN in findIndex%s", msg); - return 0; - } - - if (value < array[0]) - return -1; - int middle; - - int left = 0; - int right = size; - - // todo: extract binary search as template method? - while (true) { -#if 0 - // that's an assertion to make sure we do not loop here - size--; - efiAssert(CUSTOM_ERR_ASSERT, size > 0, "Unexpected state in binary search", 0); -#endif - - // todo: compare current implementation with - // http://eigenjoy.com/2011/01/21/worlds-fastest-binary-search/ - // ? - middle = (left + right) / 2; - -// print("left=%d middle=%d right=%d: %.2f\r\n", left, middle, right, array[middle]); - - if (middle == left) - break; - - if (middle != 0 && array[middle - 1] > array[middle]) { -#if EFI_UNIT_TEST - firmwareError(CUSTOM_ERR_6610, "%s: out of order %.2f %.2f", msg, array[middle - 1], array[middle]); -#else - warning(CUSTOM_ERR_OUT_OF_ORDER, "%s: out of order %.2f %.2f", msg, array[middle - 1], array[middle]); - -#endif /* EFI_UNIT_TEST */ - } - - if (value < array[middle]) { - right = middle; - } else if (value > array[middle]) { - left = middle; - } else { - break; - } - } - - return middle; -} - int findIndex(const float array[], int size, float value) { return findIndexMsg("", array, size, value); } diff --git a/firmware/util/math/interpolation.h b/firmware/util/math/interpolation.h index 0059756fa5..3114efd389 100644 --- a/firmware/util/math/interpolation.h +++ b/firmware/util/math/interpolation.h @@ -21,7 +21,7 @@ #define INTERPOLATION_A(x1, y1, x2, y2) ((y1 - y2) / (x1 - x2)) int findIndex(const float array[], int size, float value); -int findIndexMsg(const char *msg, const float array[], int size, float value); +#define findIndexMsg(msg, array, size, value) findIndexMsgExt(msg, array, size, value) void ensureArrayIsAscending(const char *msg, const float array[], int size); int findIndex2(const float array[], unsigned size, float value); float interpolateClamped(float x1, float y1, float x2, float y2, float x); @@ -30,6 +30,65 @@ float interpolate2d(const char *msg, float value, const float bin[], const float int needInterpolationLogging(void); +/** @brief Binary search + * @returns the highest index within sorted array such that array[i] is greater than or equal to the parameter + * @note If the parameter is smaller than the first element of the array, -1 is returned. + * + * See also ensureArrayIsAscending + */ +template +int findIndexMsgExt(const char *msg, const kType array[], int size, kType value) { + if (cisnan(value)) { + firmwareError(ERROR_NAN_FIND_INDEX, "NaN in findIndex%s", msg); + return 0; + } + + if (value < array[0]) + return -1; + int middle; + + int left = 0; + int right = size; + + // todo: extract binary search as template method? + while (true) { +#if 0 + // that's an assertion to make sure we do not loop here + size--; + efiAssert(CUSTOM_ERR_ASSERT, size > 0, "Unexpected state in binary search", 0); +#endif + + // todo: compare current implementation with + // http://eigenjoy.com/2011/01/21/worlds-fastest-binary-search/ + // ? + middle = (left + right) / 2; + +// print("left=%d middle=%d right=%d: %.2f\r\n", left, middle, right, array[middle]); + + if (middle == left) + break; + + if (middle != 0 && array[middle - 1] > array[middle]) { +#if EFI_UNIT_TEST + firmwareError(CUSTOM_ERR_6610, "%s: out of order %.2f %.2f", msg, array[middle - 1], array[middle]); +#else + warning(CUSTOM_ERR_OUT_OF_ORDER, "%s: out of order %.2f %.2f", msg, array[middle - 1], array[middle]); + +#endif /* EFI_UNIT_TEST */ + } + + if (value < array[middle]) { + right = middle; + } else if (value > array[middle]) { + left = middle; + } else { + break; + } + } + + return middle; +} + /** * @brief Two-dimensional table lookup with linear interpolation */ @@ -44,12 +103,12 @@ float interpolate3d(float x, const kType xBin[], int xBinSize, float y, const kT return NAN; } - int xIndex = findIndexMsg("x", xBin, xBinSize, x); + int xIndex = findIndexMsgExt("x", xBin, xBinSize, x); #if DEBUG_INTERPOLATION if (needInterpolationLogging()) printf("X index=%d\r\n", xIndex); #endif /* DEBUG_INTERPOLATION */ - int yIndex = findIndexMsg("y", yBin, yBinSize, y); + int yIndex = findIndexMsgExt("y", yBin, yBinSize, y); if (xIndex < 0 && yIndex < 0) { #if DEBUG_INTERPOLATION if (needInterpolationLogging())