| Line | Branch | Decision | Exec | Source |
|---|---|---|---|---|
| 1 | /** | |||
| 2 | * @file efi_interpolation.h | |||
| 3 | * See also libfirmware interpolation.h | |||
| 4 | * | |||
| 5 | * @date Oct 17, 2013 | |||
| 6 | * @author Andrey Belomutskiy, (c) 2012-2020 | |||
| 7 | */ | |||
| 8 | ||||
| 9 | #pragma once | |||
| 10 | ||||
| 11 | #include <cmath> | |||
| 12 | #include "datalogging.h" | |||
| 13 | #include "obd_error_codes.h" | |||
| 14 | #include "error_handling.h" | |||
| 15 | ||||
| 16 | #include <type_traits> | |||
| 17 | ||||
| 18 | #ifndef DEBUG_INTERPOLATION | |||
| 19 | #define DEBUG_INTERPOLATION FALSE | |||
| 20 | #endif | |||
| 21 | ||||
| 22 | #define INTERPOLATION_A(x1, y1, x2, y2) ((y1 - y2) / (x1 - x2)) | |||
| 23 | ||||
| 24 | float interpolateClampedWithValidation(float x1, float y1, float x2, float y2, float x); | |||
| 25 | float interpolateClamped(float x1, float y1, float x2, float y2, float x); | |||
| 26 | float interpolateMsg(const char *msg, float x1, float y1, float x2, float y2, float x); | |||
| 27 | ||||
| 28 | // _technically_ and _theoretically_ we can support flat line for both bins and values but I am not sure if | |||
| 29 | // such a rare case is something we want to support | |||
| 30 | // see also: setLinearCurve | |||
| 31 | template<typename TValue, int TSize> | |||
| 32 | 33369 | void ensureArrayIsAscending(const char* msg, const TValue (&values)[TSize]) { | ||
| 33 |
58/66void ensureArrayIsAscending<scaled_channel<short, 1, 1>, 5>(char const*, scaled_channel<short, 1, 1> const (&) [5]):
✓ Branch 0 taken 2340 times.
✓ Branch 1 taken 585 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 50>, 6>(char const*, scaled_channel<unsigned char, 1, 50> const (&) [6]):
✗ Branch 0 not taken.
✗ Branch 1 not taken.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 100>, 2>(char const*, scaled_channel<unsigned char, 1, 100> const (&) [2]):
✓ Branch 0 taken 585 times.
✓ Branch 1 taken 585 times.
void ensureArrayIsAscending<scaled_channel<short, 1, 1>, 16>(char const*, scaled_channel<short, 1, 1> const (&) [16]):
✓ Branch 0 taken 8775 times.
✓ Branch 1 taken 585 times.
void ensureArrayIsAscending<short, 4>(char const*, short const (&) [4]):
✓ Branch 0 taken 1755 times.
✓ Branch 1 taken 585 times.
void ensureArrayIsAscending<float, 32>(char const*, float const (&) [32]):
✓ Branch 0 taken 18135 times.
✓ Branch 1 taken 585 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 5>, 8>(char const*, scaled_channel<unsigned char, 1, 5> const (&) [8]):
✓ Branch 0 taken 4095 times.
✓ Branch 1 taken 585 times.
void ensureArrayIsAscending<signed char, 8>(char const*, signed char const (&) [8]):
✓ Branch 0 taken 4095 times.
✓ Branch 1 taken 585 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 5>, 5>(char const*, scaled_channel<unsigned char, 1, 5> const (&) [5]):
✓ Branch 0 taken 2340 times.
✓ Branch 1 taken 585 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 100>, 8>(char const*, scaled_channel<unsigned char, 1, 100> const (&) [8]):
✓ Branch 0 taken 8190 times.
✓ Branch 1 taken 1170 times.
void ensureArrayIsAscending<unsigned short, 8>(char const*, unsigned short const (&) [8]):
✓ Branch 0 taken 12355 times.
✓ Branch 1 taken 1765 times.
void ensureArrayIsAscending<float, 8>(char const*, float const (&) [8]):
✓ Branch 0 taken 40950 times.
✓ Branch 1 taken 5850 times.
void ensureArrayIsAscending<unsigned short, 6>(char const*, unsigned short const (&) [6]):
✓ Branch 0 taken 14625 times.
✓ Branch 1 taken 2925 times.
void ensureArrayIsAscending<float, 16>(char const*, float const (&) [16]):
✓ Branch 0 taken 35100 times.
✓ Branch 1 taken 2340 times.
void ensureArrayIsAscending<unsigned short, 16>(char const*, unsigned short const (&) [16]):
✓ Branch 0 taken 52650 times.
✓ Branch 1 taken 3510 times.
void ensureArrayIsAscending<scaled_channel<unsigned int, 10, 1>, 2>(char const*, scaled_channel<unsigned int, 10, 1> const (&) [2]):
✓ Branch 0 taken 585 times.
✓ Branch 1 taken 585 times.
void ensureArrayIsAscending<scaled_channel<short, 100, 1>, 8>(char const*, scaled_channel<short, 100, 1> const (&) [8]):
✓ Branch 0 taken 4095 times.
✓ Branch 1 taken 585 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 50>, 8>(char const*, scaled_channel<unsigned char, 1, 50> const (&) [8]):
✗ Branch 0 not taken.
✗ Branch 1 not taken.
void ensureArrayIsAscending<unsigned char, 8>(char const*, unsigned char const (&) [8]):
✓ Branch 0 taken 4109 times.
✓ Branch 1 taken 587 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 50>, 10>(char const*, scaled_channel<unsigned char, 1, 50> const (&) [10]):
✓ Branch 0 taken 36 times.
✓ Branch 1 taken 4 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 2, 1>, 16>(char const*, scaled_channel<unsigned char, 2, 1> const (&) [16]):
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 2 times.
void ensureArrayIsAscending<scaled_channel<unsigned short, 10, 1>, 10>(char const*, scaled_channel<unsigned short, 10, 1> const (&) [10]):
✓ Branch 0 taken 18 times.
✓ Branch 1 taken 2 times.
void ensureArrayIsAscending<scaled_channel<unsigned short, 10, 1>, 8>(char const*, scaled_channel<unsigned short, 10, 1> const (&) [8]):
✓ Branch 0 taken 14 times.
✓ Branch 1 taken 2 times.
void ensureArrayIsAscending<scaled_channel<unsigned short, 100, 1>, 8>(char const*, scaled_channel<unsigned short, 100, 1> const (&) [8]):
✓ Branch 0 taken 14 times.
✓ Branch 1 taken 2 times.
void ensureArrayIsAscending<float, 5>(char const*, float const (&) [5]):
✓ Branch 0 taken 4680 times.
✓ Branch 1 taken 1170 times.
void ensureArrayIsAscending<scaled_channel<unsigned short, 1000, 1>, 8>(char const*, scaled_channel<unsigned short, 1000, 1> const (&) [8]):
✓ Branch 0 taken 4095 times.
✓ Branch 1 taken 585 times.
void ensureArrayIsAscending<unsigned short, 4>(char const*, unsigned short const (&) [4]):
✓ Branch 0 taken 14040 times.
✓ Branch 1 taken 4680 times.
void ensureArrayIsAscending<scaled_channel<unsigned short, 100, 1>, 6>(char const*, scaled_channel<unsigned short, 100, 1> const (&) [6]):
✓ Branch 0 taken 5850 times.
✓ Branch 1 taken 1170 times.
void ensureArrayIsAscending<unsigned char, 6>(char const*, unsigned char const (&) [6]):
✓ Branch 0 taken 2925 times.
✓ Branch 1 taken 585 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 50>, 4>(char const*, scaled_channel<unsigned char, 1, 50> const (&) [4]):
✓ Branch 0 taken 1755 times.
✓ Branch 1 taken 585 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 100>, 16>(char const*, scaled_channel<unsigned char, 1, 100> const (&) [16]):
✓ Branch 0 taken 8775 times.
✓ Branch 1 taken 585 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 10>, 4>(char const*, scaled_channel<unsigned char, 1, 10> const (&) [4]):
✗ Branch 0 not taken.
✗ Branch 1 not taken.
void ensureArrayIsAscending<unsigned char, 4>(char const*, unsigned char const (&) [4]):
✗ Branch 0 not taken.
✗ Branch 1 not taken.
|
290380 | for (size_t i = 0; i < TSize - 1; i++) { | |
| 34 | 257011 | float cur = values[i]; | ||
| 35 | 257011 | float next = values[i + 1]; | ||
| 36 |
29/66void ensureArrayIsAscending<scaled_channel<short, 1, 1>, 5>(char const*, scaled_channel<short, 1, 1> const (&) [5]):
✗ Branch 0 not taken.
✓ Branch 1 taken 2340 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 50>, 6>(char const*, scaled_channel<unsigned char, 1, 50> const (&) [6]):
✗ Branch 0 not taken.
✗ Branch 1 not taken.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 100>, 2>(char const*, scaled_channel<unsigned char, 1, 100> const (&) [2]):
✗ Branch 0 not taken.
✓ Branch 1 taken 585 times.
void ensureArrayIsAscending<scaled_channel<short, 1, 1>, 16>(char const*, scaled_channel<short, 1, 1> const (&) [16]):
✗ Branch 0 not taken.
✓ Branch 1 taken 8775 times.
void ensureArrayIsAscending<short, 4>(char const*, short const (&) [4]):
✗ Branch 0 not taken.
✓ Branch 1 taken 1755 times.
void ensureArrayIsAscending<float, 32>(char const*, float const (&) [32]):
✗ Branch 0 not taken.
✓ Branch 1 taken 18135 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 5>, 8>(char const*, scaled_channel<unsigned char, 1, 5> const (&) [8]):
✗ Branch 0 not taken.
✓ Branch 1 taken 4095 times.
void ensureArrayIsAscending<signed char, 8>(char const*, signed char const (&) [8]):
✗ Branch 0 not taken.
✓ Branch 1 taken 4095 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 5>, 5>(char const*, scaled_channel<unsigned char, 1, 5> const (&) [5]):
✗ Branch 0 not taken.
✓ Branch 1 taken 2340 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 100>, 8>(char const*, scaled_channel<unsigned char, 1, 100> const (&) [8]):
✗ Branch 0 not taken.
✓ Branch 1 taken 8190 times.
void ensureArrayIsAscending<unsigned short, 8>(char const*, unsigned short const (&) [8]):
✗ Branch 0 not taken.
✓ Branch 1 taken 12355 times.
void ensureArrayIsAscending<float, 8>(char const*, float const (&) [8]):
✗ Branch 0 not taken.
✓ Branch 1 taken 40950 times.
void ensureArrayIsAscending<unsigned short, 6>(char const*, unsigned short const (&) [6]):
✗ Branch 0 not taken.
✓ Branch 1 taken 14625 times.
void ensureArrayIsAscending<float, 16>(char const*, float const (&) [16]):
✗ Branch 0 not taken.
✓ Branch 1 taken 35100 times.
void ensureArrayIsAscending<unsigned short, 16>(char const*, unsigned short const (&) [16]):
✗ Branch 0 not taken.
✓ Branch 1 taken 52650 times.
void ensureArrayIsAscending<scaled_channel<unsigned int, 10, 1>, 2>(char const*, scaled_channel<unsigned int, 10, 1> const (&) [2]):
✗ Branch 0 not taken.
✓ Branch 1 taken 585 times.
void ensureArrayIsAscending<scaled_channel<short, 100, 1>, 8>(char const*, scaled_channel<short, 100, 1> const (&) [8]):
✗ Branch 0 not taken.
✓ Branch 1 taken 4095 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 50>, 8>(char const*, scaled_channel<unsigned char, 1, 50> const (&) [8]):
✗ Branch 0 not taken.
✗ Branch 1 not taken.
void ensureArrayIsAscending<unsigned char, 8>(char const*, unsigned char const (&) [8]):
✗ Branch 0 not taken.
✓ Branch 1 taken 4109 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 50>, 10>(char const*, scaled_channel<unsigned char, 1, 50> const (&) [10]):
✗ Branch 0 not taken.
✓ Branch 1 taken 36 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 2, 1>, 16>(char const*, scaled_channel<unsigned char, 2, 1> const (&) [16]):
✗ Branch 0 not taken.
✓ Branch 1 taken 30 times.
void ensureArrayIsAscending<scaled_channel<unsigned short, 10, 1>, 10>(char const*, scaled_channel<unsigned short, 10, 1> const (&) [10]):
✗ Branch 0 not taken.
✓ Branch 1 taken 18 times.
void ensureArrayIsAscending<scaled_channel<unsigned short, 10, 1>, 8>(char const*, scaled_channel<unsigned short, 10, 1> const (&) [8]):
✗ Branch 0 not taken.
✓ Branch 1 taken 14 times.
void ensureArrayIsAscending<scaled_channel<unsigned short, 100, 1>, 8>(char const*, scaled_channel<unsigned short, 100, 1> const (&) [8]):
✗ Branch 0 not taken.
✓ Branch 1 taken 14 times.
void ensureArrayIsAscending<float, 5>(char const*, float const (&) [5]):
✗ Branch 0 not taken.
✓ Branch 1 taken 4680 times.
void ensureArrayIsAscending<scaled_channel<unsigned short, 1000, 1>, 8>(char const*, scaled_channel<unsigned short, 1000, 1> const (&) [8]):
✗ Branch 0 not taken.
✓ Branch 1 taken 4095 times.
void ensureArrayIsAscending<unsigned short, 4>(char const*, unsigned short const (&) [4]):
✗ Branch 0 not taken.
✓ Branch 1 taken 14040 times.
void ensureArrayIsAscending<scaled_channel<unsigned short, 100, 1>, 6>(char const*, scaled_channel<unsigned short, 100, 1> const (&) [6]):
✗ Branch 0 not taken.
✓ Branch 1 taken 5850 times.
void ensureArrayIsAscending<unsigned char, 6>(char const*, unsigned char const (&) [6]):
✗ Branch 0 not taken.
✓ Branch 1 taken 2925 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 50>, 4>(char const*, scaled_channel<unsigned char, 1, 50> const (&) [4]):
✗ Branch 0 not taken.
✓ Branch 1 taken 1755 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 100>, 16>(char const*, scaled_channel<unsigned char, 1, 100> const (&) [16]):
✗ Branch 0 not taken.
✓ Branch 1 taken 8775 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 10>, 4>(char const*, scaled_channel<unsigned char, 1, 10> const (&) [4]):
✗ Branch 0 not taken.
✗ Branch 1 not taken.
void ensureArrayIsAscending<unsigned char, 4>(char const*, unsigned char const (&) [4]):
✗ Branch 0 not taken.
✗ Branch 1 not taken.
|
257011 | if (next <= cur) { | |
| 37 | ✗ | firmwareError(ObdCode::CUSTOM_ERR_AXIS_ORDER, "Invalid table axis (must be ascending!): %s %f should be below %f at %d", msg, cur, next, i); | ||
| 38 | } | |||
| 39 | } | |||
| 40 | 33369 | } | ||
| 41 | ||||
| 42 | template<typename TValue, int TSize> | |||
| 43 | void ensureArrayIsAscendingOrDefault(const char* msg, const TValue (&values)[TSize]) { | |||
| 44 | if (values[1] == 0) { | |||
| 45 | return; // looks like default empty array, do not check | |||
| 46 | } | |||
| 47 | ensureArrayIsAscending(msg, values); | |||
| 48 | } | |||
| 49 | ||||
| 50 | /** @brief Binary search | |||
| 51 | * @returns the highest index within sorted array such that array[i] is greater than or equal to the parameter | |||
| 52 | * @note If the parameter is smaller than the first element of the array, -1 is returned. | |||
| 53 | * | |||
| 54 | * See also ensureArrayIsAscending | |||
| 55 | */ | |||
| 56 | template<typename kType> | |||
| 57 | 7010 | int findIndexMsg(const char *msg, const kType array[], int size, kType value) { | ||
| 58 | 7010 | float fvalue = (float)value; | ||
| 59 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 7010 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 7010 times.
|
7010 | if (std::isnan(fvalue)) { |
| 60 | ✗ | firmwareError(ObdCode::ERROR_NAN_FIND_INDEX, "NaN in findIndex%s", msg); | ||
| 61 | ✗ | return 0; | ||
| 62 | } | |||
| 63 | ||||
| 64 |
2/2✓ Branch 0 taken 2337 times.
✓ Branch 1 taken 4673 times.
|
2/2✓ Decision 'true' taken 2337 times.
✓ Decision 'false' taken 4673 times.
|
7010 | if (value < array[0]) |
| 65 | 2337 | return -1; | ||
| 66 | int middle; | |||
| 67 | ||||
| 68 | 4673 | int left = 0; | ||
| 69 | 4673 | int right = size; | ||
| 70 | ||||
| 71 | // todo: extract binary search as template method? | |||
| 72 | while (true) { | |||
| 73 | #if 0 | |||
| 74 | // that's an assertion to make sure we do not loop here | |||
| 75 | size--; | |||
| 76 | efiAssert(ObdCode::CUSTOM_ERR_ASSERT, size > 0, "Unexpected state in binary search", 0); | |||
| 77 | #endif | |||
| 78 | ||||
| 79 | // todo: compare current implementation with | |||
| 80 | // http://eigenjoy.com/2011/01/21/worlds-fastest-binary-search/ | |||
| 81 | // ? | |||
| 82 | 21029 | middle = (left + right) / 2; | ||
| 83 | ||||
| 84 | // print("left=%d middle=%d right=%d: %.2f\r\n", left, middle, right, array[middle]); | |||
| 85 | ||||
| 86 |
2/2✓ Branch 0 taken 4089 times.
✓ Branch 1 taken 16940 times.
|
2/2✓ Decision 'true' taken 4089 times.
✓ Decision 'false' taken 16940 times.
|
21029 | if (middle == left) |
| 87 | 4089 | break; | ||
| 88 | ||||
| 89 |
2/4✓ Branch 0 taken 16940 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 16940 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 16940 times.
|
16940 | if (middle != 0 && array[middle - 1] > array[middle]) { |
| 90 | #if EFI_UNIT_TEST | |||
| 91 | ✗ | firmwareError(ObdCode::CUSTOM_ERR_6610, "%s: out of order %.2f %.2f", msg, array[middle - 1], array[middle]); | ||
| 92 | #else | |||
| 93 | warning(ObdCode::CUSTOM_ERR_OUT_OF_ORDER, "%s: out of order %.2f %.2f", msg, array[middle - 1], array[middle]); | |||
| 94 | ||||
| 95 | #endif /* EFI_UNIT_TEST */ | |||
| 96 | } | |||
| 97 | ||||
| 98 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 16936 times.
|
2/2✓ Decision 'true' taken 4 times.
✓ Decision 'false' taken 16936 times.
|
16940 | if (value < array[middle]) { |
| 99 | 4 | right = middle; | ||
| 100 |
2/2✓ Branch 0 taken 16352 times.
✓ Branch 1 taken 584 times.
|
2/2✓ Decision 'true' taken 16352 times.
✓ Decision 'false' taken 584 times.
|
16936 | } else if (value > array[middle]) { |
| 101 | 16352 | left = middle; | ||
| 102 | } else { | |||
| 103 | 584 | break; | ||
| 104 | } | |||
| 105 | } | |||
| 106 | ||||
| 107 | 4673 | return middle; | ||
| 108 | } | |||
| 109 | ||||
| 110 | /** | |||
| 111 | * Sets specified value for specified key in a correction curve | |||
| 112 | * see also setLinearCurve() | |||
| 113 | */ | |||
| 114 | template<typename VType, typename kType> | |||
| 115 | 7010 | void setCurveValue(const kType bins[], VType values[], int size, float key, float value) { | ||
| 116 | 7010 | int index = findIndexMsg("tbVl", bins, size, key); | ||
| 117 |
2/2✓ Branch 0 taken 2337 times.
✓ Branch 1 taken 4673 times.
|
2/2✓ Decision 'true' taken 2337 times.
✓ Decision 'false' taken 4673 times.
|
7010 | if (index == -1) |
| 118 | 2337 | index = 0; | ||
| 119 | 7010 | values[index] = value; | ||
| 120 | 7010 | } | ||
| 121 |