diff --git a/firmware/controllers/core/interpolation.cpp b/firmware/controllers/core/interpolation.cpp index 30fb83d979..b2804ae2b8 100644 --- a/firmware/controllers/core/interpolation.cpp +++ b/firmware/controllers/core/interpolation.cpp @@ -38,7 +38,6 @@ float FastInterpolation::getValue(float x) { return a * x + b; } - /** @brief Linear interpolation by two points * * @param x1 key of the first point @@ -69,6 +68,30 @@ float interpolate(float x1, float y1, float x2, float y2, float x) { return result; } +int findIndex2(const float array[], unsigned size, float value) { + efiAssert(!cisnan(value), "NaN in findIndex", 0); + efiAssert(size > 1, "NaN in findIndex", 0); +// if (size <= 1) +// return size && *array <= value ? 0 : -1; + + signed i = 0; + //unsigned b = 1 << int(log(float(size) - 1) / 0.69314718055994530942); + unsigned b = size >> 1; // in our case size is always a power of 2 + efiAssert(b + b == size, "Size not power of 2", -1); + for (; b; b >>= 1) { + unsigned j = i | b; + /** + * it should be + * "if (j < size && array[j] <= value)" + * but in our case size is always power of 2 thus size is always more then j + */ + // efiAssert(j < size, "size", 0); + if (array[j] <= value) + i = j; + } + return i || *array <= value ? i : -1; +} + /** @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. @@ -143,13 +166,13 @@ float interpolate3d(float x, float xBin[], int xBinSize, float y, float yBin[], int xIndex = findIndex(xBin, xBinSize, x); #if DEBUG_INTERPOLATION if (needInterpolationLogging) - printf("X index=%d\r\n", xIndex); + printf("X index=%d\r\n", xIndex); #endif int yIndex = findIndex(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); + printf("X and Y are smaller than smallest cell in table: %d\r\n", xIndex); #endif return map[0][0]; } @@ -157,7 +180,7 @@ float interpolate3d(float x, float xBin[], int xBinSize, float y, float yBin[], if (xIndex < 0) { #if DEBUG_INTERPOLATION if (needInterpolationLogging) - printf("X is smaller than smallest cell in table: %dr\n", xIndex); + printf("X is smaller than smallest cell in table: %dr\n", xIndex); #endif // no interpolation should be fine here. return map[0][yIndex]; @@ -166,7 +189,7 @@ float interpolate3d(float x, float xBin[], int xBinSize, float y, float yBin[], if (yIndex < 0) { #if DEBUG_INTERPOLATION if (needInterpolationLogging) - printf("Y is smaller than smallest cell in table: %d\r\n", yIndex); + printf("Y is smaller than smallest cell in table: %d\r\n", yIndex); #endif // no interpolation should be fine here. return map[xIndex][0]; @@ -175,7 +198,7 @@ float interpolate3d(float x, float xBin[], int xBinSize, float y, float yBin[], 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); + printf("X and Y are larger than largest cell in table: %d %d\r\n", xIndex, yIndex); #endif return map[xBinSize - 1][yBinSize - 1]; } @@ -183,7 +206,7 @@ float interpolate3d(float x, float xBin[], int xBinSize, float y, float yBin[], if (xIndex == xBinSize - 1) { #if DEBUG_INTERPOLATION if (needInterpolationLogging) - printf("TODO BETTER LOGGING x overflow %d\r\n", yIndex); + printf("TODO BETTER LOGGING x overflow %d\r\n", yIndex); #endif // todo: implement better handling - y interpolation return map[xBinSize - 1][yIndex]; @@ -192,7 +215,7 @@ float interpolate3d(float x, float xBin[], int xBinSize, float y, float yBin[], if (yIndex == yBinSize - 1) { #if DEBUG_INTERPOLATION if (needInterpolationLogging) - printf("Y is larger than largest cell in table: %d\r\n", yIndex); + printf("Y is larger than largest cell in table: %d\r\n", yIndex); #endif // todo: implement better handling - x interpolation return map[xIndex][yBinSize - 1]; diff --git a/firmware/controllers/core/interpolation.h b/firmware/controllers/core/interpolation.h index 91c14e6324..436be02aa9 100644 --- a/firmware/controllers/core/interpolation.h +++ b/firmware/controllers/core/interpolation.h @@ -11,6 +11,7 @@ #define INTERPOLATION_A(x1, y1, x2, y2) ((y1 - y2) / (x1 - x2)) int findIndex(const float array[], int size, float value); +int findIndex2(const float array[], unsigned size, float value); float interpolate(float x1, float y1, float x2, float y2, float x); float interpolate2d(float value, float bin[], float values[], int size); float interpolate3d(float x, float xBin[], int xBinSize, float y, float yBin[], int yBinSize, float* map[]); diff --git a/firmware/controllers/sensors/maf.cpp b/firmware/controllers/sensors/maf.cpp index 97593a83e7..95b4af3dd6 100644 --- a/firmware/controllers/sensors/maf.cpp +++ b/firmware/controllers/sensors/maf.cpp @@ -21,36 +21,51 @@ float getRealMaf(DECLARE_ENGINE_PARAMETER_F) { return engine->mafDecodingLookup[mafAdc >> 4]; } -void setBosch0280218037(engine_configuration_s *engineConfiguration) { - int i = 0; - engineConfiguration->mafDecoding[i] = -34.5; - engineConfiguration->mafDecodingBins[i++] = 0; - - engineConfiguration->mafDecoding[i] = -6; - engineConfiguration->mafDecodingBins[i++] = 0.78125; - - engineConfiguration->mafDecoding[i] = 10.5; - engineConfiguration->mafDecodingBins[i++] = 1.38671875; - - engineConfiguration->mafDecoding[i] = 105.3; - engineConfiguration->mafDecodingBins[i++] = 2.91015625; - - engineConfiguration->mafDecoding[i] = 387.5; - engineConfiguration->mafDecodingBins[i++] = 4.2578125; - - engineConfiguration->mafDecoding[i] = 738; - engineConfiguration->mafDecodingBins[i++] = 4.98046875; - +static void fillTheRest(engine_configuration_s *e, int i) { /** * unrealistic values just to make binary search happy */ while (i < MAF_DECODING_COUNT) { - engineConfiguration->mafDecoding[i] = 200; - engineConfiguration->mafDecodingBins[i] = 10 + i; + e->mafDecoding[i] = 200; + e->mafDecodingBins[i] = 10 + i; i++; } } -void setBosch0280218004(engine_configuration_s *engineConfiguration) { - +static int addMafPoint(engine_configuration_s *e, int i, float kgHrValue, float voltage) { + e->mafDecoding[i] = kgHrValue; + e->mafDecodingBins[i] = voltage; + return i + 1; +} + +void setBosch0280218037(engine_configuration_s *e) { + int i = 0; + i = addMafPoint(e, i, -34.5, 0); + i = addMafPoint(e, i, -6, 0.78125); + + e->mafDecoding[i] = 10.5; + e->mafDecodingBins[i++] = 1.38671875; + + e->mafDecoding[i] = 105.3; + e->mafDecodingBins[i++] = 2.91015625; + + e->mafDecoding[i] = 387.5; + e->mafDecodingBins[i++] = 4.2578125; + + e->mafDecoding[i] = 738; + e->mafDecodingBins[i++] = 4.98046875; + + fillTheRest(e, i); +} + +void setBosch0280218004(engine_configuration_s *e) { + int i = 0; + + fillTheRest(e, i); +} + +void setDensoTODO(engine_configuration_s *e) { + int i = 0; + + fillTheRest(e, i); } diff --git a/firmware/controllers/sensors/maf.h b/firmware/controllers/sensors/maf.h index 16c4e2b761..d99ba235f9 100644 --- a/firmware/controllers/sensors/maf.h +++ b/firmware/controllers/sensors/maf.h @@ -21,4 +21,6 @@ float getRealMaf(DECLARE_ENGINE_PARAMETER_F); void setBosch0280218037(engine_configuration_s *engineConfiguration); void setBosch0280218004(engine_configuration_s *engineConfiguration); +void setDensoTODO(engine_configuration_s *engineConfiguration); + #endif diff --git a/unit_tests/test_basic_math/test_find_index.cpp b/unit_tests/test_basic_math/test_find_index.cpp index 528d498abe..470c6e4149 100644 --- a/unit_tests/test_basic_math/test_find_index.cpp +++ b/unit_tests/test_basic_math/test_find_index.cpp @@ -11,66 +11,59 @@ #include #include "engine_configuration.h" +static int testIndex(const int expected, const float array[], int size, float value) { + assertEquals(expected, findIndex(array, size, value)); + assertEquals(expected, findIndex2(array, size, value)); +} + void testFindIndex(void) { printf("*************************************************** testFindIndex\r\n"); - int result; - +/* { // in real life we only use powers of 2 for sizes float array5[] = { 1, 2, 3, 4, 5 }; const int size5 = 5; printf("To the right5\r\n"); - result = findIndex(array5, size5, 10.0); - assertEquals(4, result); + testIndex(4, array5, size5, 10.0); printf("Middle2\r\n"); - result = findIndex(array5, size5, 4); - assertEquals(3, result); + testIndex(3, array5, size5, 4); printf("Middle2\r\n"); - result = findIndex(array5, size5, 3.1); - assertEquals(2, result); + testIndex(2, array5, size5, 3.1); } +*/ { float array4[] = { 1, 2, 3, 4 }; const int size4 = 4; printf("To the left\r\n"); - result = findIndex(array4, size4, -1.0); - assertEquals(-1, result); + testIndex(-1, array4, size4, -1.0); printf("To the right4\r\n"); - result = findIndex(array4, size4, 10.0); - assertEquals(3, result); + testIndex(3, array4, size4, 10.0); printf("On the edge\r\n"); - result = findIndex(array4, size4, 4.0); - assertEquals(3, result); + testIndex(3, array4, size4, 4.0); printf("Another1\r\n"); - result = findIndex(array4, size4, 3.9); - assertEquals(2, result); + testIndex(2, array4, size4, 3.9); printf("Another2\r\n"); - result = findIndex(array4, size4, 4.1); - assertEquals(3, result); + testIndex(3, array4, size4, 4.1); printf("Another3\r\n"); - result = findIndex(array4, size4, 2); - assertEquals(1, result); + testIndex(1, array4, size4, 2); printf("Left edge1\r\n"); - result = findIndex(array4, size4, 1); - assertEquals(0, result); + testIndex(0, array4, size4, 1); printf("Left edge2\r\n"); - result = findIndex(array4, size4, 1.1); - assertEquals(0, result); + testIndex(0, array4, size4, 1.1); printf("Middle\r\n"); - result = findIndex(array4, size4, 3); - assertEquals(2, result); + testIndex(2, array4, size4, 3); } }