diff --git a/firmware/util/containers/table_helper.h b/firmware/util/containers/table_helper.h index 555d945c9c..d293efb02d 100644 --- a/firmware/util/containers/table_helper.h +++ b/firmware/util/containers/table_helper.h @@ -12,68 +12,97 @@ #include "interpolation.h" #include "efilib.h" #include "efi_ratio.h" +#include "scaled_channel.h" // popular left edge of CLT-based correction curves #define CLT_CURVE_RANGE_FROM -40 class ValueProvider3D { public: - virtual float getValue(float xRpm, float y) const = 0; + virtual float getValue(float xColumn, float yRow) const = 0; }; /** * this helper class brings together 3D table with two 2D axis curves */ -template> +template> class Map3D : public ValueProvider3D { public: explicit Map3D(const char*name) { - create(name); } - void init(vType table[RPM_BIN_SIZE][LOAD_BIN_SIZE], const kType loadBins[LOAD_BIN_SIZE], const kType rpmBins[RPM_BIN_SIZE]) { - // this method cannot use logger because it's invoked before everything - // that's because this method needs to be invoked before initial configuration processing - // and initial configuration load is done prior to logging initialization - for (int k = 0; k < LOAD_BIN_SIZE; k++) { - pointers[k] = table[k]; - } + template + void init(scaled_channel table[TRowNum][TColNum], const kType rowBins[TRowNum], const kType columnBins[TColNum]) { + static_assert(TValueMultiplier::den == mult); + static_assert(TValueMultiplier::num == 1); - this->loadBins = loadBins; - this->rpmBins = rpmBins; + m_values = reinterpret_cast(&table[0][0]); + + m_rowBins = rowBins; + m_columnBins = columnBins; } - float getValue(float xRpm, float y) const override { - efiAssert(CUSTOM_ERR_ASSERT, loadBins, "map not initialized", NAN); - if (cisnan(y)) { - warning(CUSTOM_PARAM_RANGE, "%s: y is NaN", name); - return NAN; - } + void init(vType table[TRowNum][TColNum], const kType rowBins[TRowNum], const kType columnBins[TColNum]) { + m_values = &table[0][0]; - // todo: we have a bit of a mess: in TunerStudio, RPM is X-axis - return interpolate3d(name, y, loadBins, LOAD_BIN_SIZE, xRpm, rpmBins, RPM_BIN_SIZE, pointers) * TValueMultiplier::asFloat(); + m_rowBins = rowBins; + m_columnBins = columnBins; + } + + float getValue(float xColumn, float yRow) const override { + if (!m_values) { + // not initialized, return 0 + return 0; + } + + auto row = priv::getBinPtr(yRow, m_rowBins); + auto col = priv::getBinPtr(xColumn, m_columnBins); + + // Orient the table such that (0, 0) is the bottom left corner, + // then the following variable names will make sense + float lowerLeft = getValueAtPosition(row.Idx, col.Idx); + float upperLeft = getValueAtPosition(row.Idx + 1, col.Idx); + float lowerRight = getValueAtPosition(row.Idx, col.Idx + 1); + float upperRight = getValueAtPosition(row.Idx + 1, col.Idx + 1); + + // Interpolate each side by itself + float left = priv::linterp(lowerLeft, upperLeft, row.Frac); + float right = priv::linterp(lowerRight, upperRight, row.Frac); + + // Then interpolate between those + float tableValue = priv::linterp(left, right, col.Frac); + + // Correct by the ratio of table units to "world" units + return tableValue * TValueMultiplier::asFloat(); } void setAll(vType value) { - efiAssertVoid(CUSTOM_ERR_6573, loadBins, "map not initialized"); - for (int l = 0; l < LOAD_BIN_SIZE; l++) { - for (int r = 0; r < RPM_BIN_SIZE; r++) { - pointers[l][r] = value / TValueMultiplier::asFloat(); - } + efiAssertVoid(CUSTOM_ERR_6573, m_values, "map not initialized"); + + for (size_t i = 0; i < TRowNum * TColNum; i++) { + m_values[i] = value / TValueMultiplier::asFloat(); } } - vType *pointers[LOAD_BIN_SIZE]; private: - void create(const char* name) { - this->name = name; - memset(&pointers, 0, sizeof(pointers)); + static size_t getIndexForCoordinates(size_t row, size_t column) { + // Index 0 is bottom left corner + // Index TColNum - 1 is bottom right corner + // indicies count right, then up + return row * TColNum + column; } - const kType *loadBins = NULL; - const kType *rpmBins = NULL; - const char *name; + vType getValueAtPosition(size_t row, size_t column) const { + auto idx = getIndexForCoordinates(row, column); + return m_values[idx]; + } + + // TODO: should be const + /*const*/ vType* m_values = nullptr; + + const kType *m_rowBins = nullptr; + const kType *m_columnBins = nullptr; }; typedef Map3D> lambda_Map3D_t; @@ -120,6 +149,15 @@ constexpr void setTable(TElement (&dest)[N][M], const TElement value) { } } +template +constexpr void setTable(scaled_channel (&dest)[N][M], float value) { + for (size_t n = 0; n < N; n++) { + for (size_t m = 0; m < M; m++) { + dest[n][m] = value; + } + } +} + template constexpr void copyTable(TDest (&dest)[N][M], const TSource (&source)[N][M], float multiply = 1.0f) { for (size_t n = 0; n < N; n++) { diff --git a/firmware/util/math/interpolation.cpp b/firmware/util/math/interpolation.cpp index f6809c8a30..078d0ff57b 100644 --- a/firmware/util/math/interpolation.cpp +++ b/firmware/util/math/interpolation.cpp @@ -15,14 +15,6 @@ #include "interpolation.h" -#if EFI_UNIT_TEST -bool needInterpolationLoggingValue = false; - -int needInterpolationLogging(void) { - return needInterpolationLoggingValue; -} -#endif /* EFI_UNIT_TEST */ - #define BINARY_PERF true #if BINARY_PERF && ! EFI_UNIT_TEST diff --git a/firmware/util/math/interpolation.h b/firmware/util/math/interpolation.h index 19e725a2a3..aaa322d884 100644 --- a/firmware/util/math/interpolation.h +++ b/firmware/util/math/interpolation.h @@ -43,7 +43,7 @@ struct BinResult * and how far from (idx) to (idx + 1) the value is located. */ template -BinResult getBin(float value, const TBin (&bins)[TSize]) { +BinResult getBinPtr(float value, const TBin* bins) { // Enforce numeric only (int, float, uintx_t, etc) static_assert(std::is_arithmetic_v, "Table bins must be an arithmetic type"); @@ -86,6 +86,11 @@ BinResult getBin(float value, const TBin (&bins)[TSize]) { return { idx, fraction }; } +template +BinResult getBin(float value, const TBin (&bins)[TSize]) { + return getBinPtr(value, &bins[0]); +} + static float linterp(float low, float high, float frac) { return high * frac + low * (1 - frac); @@ -107,8 +112,6 @@ float interpolate2d(const float value, const TBin (&bin)[TSize], const TValue (& return priv::linterp(low, high, frac); } -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. @@ -169,142 +172,6 @@ int findIndexMsgExt(const char *msg, const kType array[], int size, kType value) return middle; } -/** - * @brief Two-dimensional table lookup with linear interpolation - */ -template -float interpolate3d(const char *msg, float x, const kType xBin[], int xBinSize, float y, const kType yBin[], int yBinSize, const vType* const map[]) { - if (cisnan(x)) { - warning(CUSTOM_INTEPOLATE_ERROR_3, "%.2f: x is NaN in interpolate3d", x); - return NAN; - } - if (cisnan(y)) { - warning(CUSTOM_INTEPOLATE_ERROR_2, "%.2f: y is NaN in interpolate3d", y); - return NAN; - } - - int xIndex = findIndexMsgExt("x", xBin, xBinSize, x); -#if DEBUG_INTERPOLATION - if (needInterpolationLogging()) - printf("X index=%d\r\n", xIndex); -#endif /* DEBUG_INTERPOLATION */ - int yIndex = findIndexMsgExt("y", yBin, yBinSize, y); - if (xIndex < 0 && yIndex < 0) { -#if DEBUG_INTERPOLATION - if (needInterpolationLogging()) - printf("X and Y are smaller than smallest cell in table: %d\r\n", xIndex); -#endif /* DEBUG_INTERPOLATION */ - return map[0][0]; - } - - if (xIndex < 0) { -#if DEBUG_INTERPOLATION - if (needInterpolationLogging()) - printf("X is smaller than smallest cell in table: %dr\n", xIndex); -#endif /* DEBUG_INTERPOLATION */ - if (yIndex == yBinSize - 1) - return map[0][yIndex]; - float keyMin = yBin[yIndex]; - float keyMax = yBin[yIndex + 1]; - float rpmMinValue = map[0][yIndex]; - float rpmMaxValue = map[0][yIndex + 1]; - - return interpolateMsg(msg, keyMin, rpmMinValue, keyMax, rpmMaxValue, y); - } - - if (yIndex < 0) { -#if DEBUG_INTERPOLATION - if (needInterpolationLogging()) - printf("Y is smaller than smallest cell in table: %d\r\n", yIndex); -#endif /* DEBUG_INTERPOLATION */ - if (xIndex == xBinSize - 1) - return map[xIndex][0]; - float key1 = xBin[xIndex]; - float key2 = xBin[xIndex + 1]; - float value1 = map[xIndex][0]; - float value2 = map[xIndex + 1][0]; - - return interpolateMsg(msg, key1, value1, key2, value2, x); - } - - if (xIndex == xBinSize - 1 && yIndex == yBinSize - 1) { -#if DEBUG_INTERPOLATION - if (needInterpolationLogging()) - printf("X and Y are larger than largest cell in table: %d %d\r\n", xIndex, yIndex); -#endif /* DEBUG_INTERPOLATION */ - return map[xBinSize - 1][yBinSize - 1]; - } - - if (xIndex == xBinSize - 1) { -#if DEBUG_INTERPOLATION - if (needInterpolationLogging()) - printf("TODO BETTER LOGGING x overflow %d\r\n", yIndex); -#endif /* DEBUG_INTERPOLATION */ - // here yIndex is less than yBinSize - 1, we've checked that condition already - - float key1 = yBin[yIndex]; - float key2 = yBin[yIndex + 1]; - float value1 = map[xIndex][yIndex]; - float value2 = map[xIndex][yIndex + 1]; - - return interpolateMsg(msg, key1, value1, key2, value2, y); - } - - if (yIndex == yBinSize - 1) { -#if DEBUG_INTERPOLATION - if (needInterpolationLogging()) - printf("Y is larger than largest cell in table: %d\r\n", yIndex); -#endif /* DEBUG_INTERPOLATION */ - // here xIndex is less than xBinSize - 1, we've checked that condition already - - float key1 = xBin[xIndex]; - float key2 = xBin[xIndex + 1]; - float value1 = map[xIndex][yIndex]; - float value2 = map[xIndex + 1][yIndex]; - - return interpolateMsg(msg, key1, value1, key2, value2, x); - } - - /* - * first we find the interpolated value for this RPM - */ - int rpmMaxIndex = xIndex + 1; - - float xMin = xBin[xIndex]; - float xMax = xBin[xIndex + 1]; - float rpmMinKeyMinValue = map[xIndex][yIndex]; - float rpmMaxKeyMinValue = map[xIndex + 1][yIndex]; - - float keyMinValue = interpolateMsg(msg, xMin, rpmMinKeyMinValue, xMax, rpmMaxKeyMinValue, x); - -#if DEBUG_INTERPOLATION - if (needInterpolationLogging()) { - printf("X=%.2f:\r\nrange %.2f - %.2f\r\n", x, xMin, xMax); - printf("X interpolation range %.2f %.2f result %.2f\r\n", rpmMinKeyMinValue, rpmMaxKeyMinValue, keyMinValue); - } -#endif /* DEBUG_INTERPOLATION */ - - int keyMaxIndex = yIndex + 1; - float keyMin = yBin[yIndex]; - float keyMax = yBin[keyMaxIndex]; - float rpmMinKeyMaxValue = map[xIndex][keyMaxIndex]; - float rpmMaxKeyMaxValue = map[rpmMaxIndex][keyMaxIndex]; - - float keyMaxValue = interpolateMsg(msg, xMin, rpmMinKeyMaxValue, xMax, rpmMaxKeyMaxValue, x); - -#if DEBUG_INTERPOLATION - if (needInterpolationLogging()) { - printf("key=%.2f:\r\nrange %.2f - %.2f\r\n", y, keyMin, keyMax); - printf("key interpolation range %.2f %.2f result %.2f\r\n", rpmMinKeyMaxValue, rpmMaxKeyMaxValue, keyMaxValue); - - printf("%f", rpmMinKeyMaxValue); - printf("%f", rpmMaxKeyMaxValue); - printf("%f", keyMaxValue); - } -#endif /* DEBUG_INTERPOLATION */ - - return interpolateMsg(msg, keyMin, keyMinValue, keyMax, keyMaxValue, y); -} void setCurveValue(float bins[], float values[], int size, float key, float value); void initInterpolation(); diff --git a/unit_tests/map_resize.cpp b/unit_tests/map_resize.cpp index a2ff6a3bf2..0fd66179f0 100644 --- a/unit_tests/map_resize.cpp +++ b/unit_tests/map_resize.cpp @@ -94,8 +94,6 @@ static float newKeyBin[newKeySize]; //EngineConfiguration *engineConfiguration; -extern bool needInterpolationLoggingValue; - void resizeMap(void) { // float keyMin = 1.2; // float keyMax = 4.4; @@ -121,8 +119,6 @@ void resizeMap(void) { // engineConfiguration->fuelRpmBins, // FUEL_RPM_COUNT, fuel_ptrs)); - needInterpolationLoggingValue = 0; - // printf("static float ad_maf_table[AD_LOAD_COUNT] = {"); // for (int i = 0; i < newKeySize; i++) { // newKeyBin[i] = interpolate(0, keyMin, newKeySize - 1, keyMax, i); diff --git a/unit_tests/mocks.h b/unit_tests/mocks.h index 3293269ee5..3ce577b6f3 100644 --- a/unit_tests/mocks.h +++ b/unit_tests/mocks.h @@ -39,7 +39,7 @@ public: class MockVp3d : public ValueProvider3D { public: - MOCK_METHOD(float, getValue, (float xRpm, float y), (const, override)); + MOCK_METHOD(float, getValue, (float xColumn, float yRow), (const, override)); }; class MockPwm : public SimplePwm { diff --git a/unit_tests/test_basic_math/test_interpolation_3d.cpp b/unit_tests/test_basic_math/test_interpolation_3d.cpp index 055c98596f..d13f8c7adf 100644 --- a/unit_tests/test_basic_math/test_interpolation_3d.cpp +++ b/unit_tests/test_basic_math/test_interpolation_3d.cpp @@ -14,17 +14,18 @@ float rpmBins[5] = { 100, 200, 300, 400, 500 }; float mafBins[4] = { 1, 2, 3, 4 }; -float map0[4] = { 1, 2, 3, 4 }; -float map1[4] = { 2, 3, 4, 5 }; -float map2[4] = { 3, 4, 200, 300 }; -float map3[4] = { 4, 200, 500, 600 }; -float map4[4] = { 4, 200, 500, 600 }; - -float *map[5] = { map0, map1, map2, map3, map4 }; - +float map[4][5] = { + { 1, 2, 3, 4, 4}, + { 2, 3, 4, 200, 200 }, + { 3, 4, 200, 500, 500 }, + { 4, 5, 300, 600, 600 }, +}; static float getValue(float rpm, float maf) { - return interpolate3d("test", rpm, rpmBins, 5, maf, mafBins, 4, map); + Map3D<5, 4, float, float> x("test"); + x.init(map, mafBins, rpmBins); + + return x.getValue(rpm, maf); } static void newTestToComfirmInterpolation() { @@ -35,7 +36,7 @@ static void newTestToComfirmInterpolation() { //__200_|__3|__4| //______|__2|__3|_LOAD - map2[1] = 10; + map[1][2] = 10; // let's start by testing corners @@ -79,8 +80,6 @@ static void newTestToComfirmInterpolation() { } TEST(misc, testInterpolate3d) { - printf("*************************************************** testInterpolate3d\r\n"); - printf("*** no interpolation here 1\r\n"); ASSERT_FLOAT_EQ(2, getValue(100, 2));