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 | 33312 | 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 2336 times.
✓ Branch 1 taken 584 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 584 times.
✓ Branch 1 taken 584 times.
void ensureArrayIsAscending<scaled_channel<short, 1, 1>, 16>(char const*, scaled_channel<short, 1, 1> const (&) [16]):
✓ Branch 0 taken 8760 times.
✓ Branch 1 taken 584 times.
void ensureArrayIsAscending<short, 4>(char const*, short const (&) [4]):
✓ Branch 0 taken 1752 times.
✓ Branch 1 taken 584 times.
void ensureArrayIsAscending<float, 32>(char const*, float const (&) [32]):
✓ Branch 0 taken 18104 times.
✓ Branch 1 taken 584 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 5>, 8>(char const*, scaled_channel<unsigned char, 1, 5> const (&) [8]):
✓ Branch 0 taken 4088 times.
✓ Branch 1 taken 584 times.
void ensureArrayIsAscending<signed char, 8>(char const*, signed char const (&) [8]):
✓ Branch 0 taken 4088 times.
✓ Branch 1 taken 584 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 5>, 5>(char const*, scaled_channel<unsigned char, 1, 5> const (&) [5]):
✓ Branch 0 taken 2336 times.
✓ Branch 1 taken 584 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 100>, 8>(char const*, scaled_channel<unsigned char, 1, 100> const (&) [8]):
✓ Branch 0 taken 8176 times.
✓ Branch 1 taken 1168 times.
void ensureArrayIsAscending<unsigned short, 8>(char const*, unsigned short const (&) [8]):
✓ Branch 0 taken 12334 times.
✓ Branch 1 taken 1762 times.
void ensureArrayIsAscending<float, 8>(char const*, float const (&) [8]):
✓ Branch 0 taken 40880 times.
✓ Branch 1 taken 5840 times.
void ensureArrayIsAscending<unsigned short, 6>(char const*, unsigned short const (&) [6]):
✓ Branch 0 taken 14600 times.
✓ Branch 1 taken 2920 times.
void ensureArrayIsAscending<float, 16>(char const*, float const (&) [16]):
✓ Branch 0 taken 35040 times.
✓ Branch 1 taken 2336 times.
void ensureArrayIsAscending<unsigned short, 16>(char const*, unsigned short const (&) [16]):
✓ Branch 0 taken 52560 times.
✓ Branch 1 taken 3504 times.
void ensureArrayIsAscending<scaled_channel<unsigned int, 10, 1>, 2>(char const*, scaled_channel<unsigned int, 10, 1> const (&) [2]):
✓ Branch 0 taken 584 times.
✓ Branch 1 taken 584 times.
void ensureArrayIsAscending<scaled_channel<short, 100, 1>, 8>(char const*, scaled_channel<short, 100, 1> const (&) [8]):
✓ Branch 0 taken 4088 times.
✓ Branch 1 taken 584 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 4102 times.
✓ Branch 1 taken 586 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 4672 times.
✓ Branch 1 taken 1168 times.
void ensureArrayIsAscending<scaled_channel<unsigned short, 1000, 1>, 8>(char const*, scaled_channel<unsigned short, 1000, 1> const (&) [8]):
✓ Branch 0 taken 4088 times.
✓ Branch 1 taken 584 times.
void ensureArrayIsAscending<unsigned short, 4>(char const*, unsigned short const (&) [4]):
✓ Branch 0 taken 14016 times.
✓ Branch 1 taken 4672 times.
void ensureArrayIsAscending<scaled_channel<unsigned short, 100, 1>, 6>(char const*, scaled_channel<unsigned short, 100, 1> const (&) [6]):
✓ Branch 0 taken 5840 times.
✓ Branch 1 taken 1168 times.
void ensureArrayIsAscending<unsigned char, 6>(char const*, unsigned char const (&) [6]):
✓ Branch 0 taken 2920 times.
✓ Branch 1 taken 584 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 50>, 4>(char const*, scaled_channel<unsigned char, 1, 50> const (&) [4]):
✓ Branch 0 taken 1752 times.
✓ Branch 1 taken 584 times.
void ensureArrayIsAscending<scaled_channel<unsigned char, 1, 100>, 16>(char const*, scaled_channel<unsigned char, 1, 100> const (&) [16]):
✓ Branch 0 taken 8760 times.
✓ Branch 1 taken 584 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.
|
289884 | for (size_t i = 0; i < TSize - 1; i++) { | |
34 | 256572 | float cur = values[i]; | ||
35 | 256572 | 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 2336 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 584 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 8760 times.
void ensureArrayIsAscending<short, 4>(char const*, short const (&) [4]):
✗ Branch 0 not taken.
✓ Branch 1 taken 1752 times.
void ensureArrayIsAscending<float, 32>(char const*, float const (&) [32]):
✗ Branch 0 not taken.
✓ Branch 1 taken 18104 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 4088 times.
void ensureArrayIsAscending<signed char, 8>(char const*, signed char const (&) [8]):
✗ Branch 0 not taken.
✓ Branch 1 taken 4088 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 2336 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 8176 times.
void ensureArrayIsAscending<unsigned short, 8>(char const*, unsigned short const (&) [8]):
✗ Branch 0 not taken.
✓ Branch 1 taken 12334 times.
void ensureArrayIsAscending<float, 8>(char const*, float const (&) [8]):
✗ Branch 0 not taken.
✓ Branch 1 taken 40880 times.
void ensureArrayIsAscending<unsigned short, 6>(char const*, unsigned short const (&) [6]):
✗ Branch 0 not taken.
✓ Branch 1 taken 14600 times.
void ensureArrayIsAscending<float, 16>(char const*, float const (&) [16]):
✗ Branch 0 not taken.
✓ Branch 1 taken 35040 times.
void ensureArrayIsAscending<unsigned short, 16>(char const*, unsigned short const (&) [16]):
✗ Branch 0 not taken.
✓ Branch 1 taken 52560 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 584 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 4088 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 4102 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 4672 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 4088 times.
void ensureArrayIsAscending<unsigned short, 4>(char const*, unsigned short const (&) [4]):
✗ Branch 0 not taken.
✓ Branch 1 taken 14016 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 5840 times.
void ensureArrayIsAscending<unsigned char, 6>(char const*, unsigned char const (&) [6]):
✗ Branch 0 not taken.
✓ Branch 1 taken 2920 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 1752 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 8760 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.
|
256572 | 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 | 33312 | } | ||
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 | 6998 | int findIndexMsg(const char *msg, const kType array[], int size, kType value) { | ||
58 | 6998 | float fvalue = (float)value; | ||
59 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 6998 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 6998 times.
|
6998 | 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 2333 times.
✓ Branch 1 taken 4665 times.
|
2/2✓ Decision 'true' taken 2333 times.
✓ Decision 'false' taken 4665 times.
|
6998 | if (value < array[0]) |
65 | 2333 | return -1; | ||
66 | int middle; | |||
67 | ||||
68 | 4665 | int left = 0; | ||
69 | 4665 | 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 | 20993 | 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 4082 times.
✓ Branch 1 taken 16911 times.
|
2/2✓ Decision 'true' taken 4082 times.
✓ Decision 'false' taken 16911 times.
|
20993 | if (middle == left) |
87 | 4082 | break; | ||
88 | ||||
89 |
2/4✓ Branch 0 taken 16911 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 16911 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 16911 times.
|
16911 | 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 16907 times.
|
2/2✓ Decision 'true' taken 4 times.
✓ Decision 'false' taken 16907 times.
|
16911 | if (value < array[middle]) { |
99 | 4 | right = middle; | ||
100 |
2/2✓ Branch 0 taken 16324 times.
✓ Branch 1 taken 583 times.
|
2/2✓ Decision 'true' taken 16324 times.
✓ Decision 'false' taken 583 times.
|
16907 | } else if (value > array[middle]) { |
101 | 16324 | left = middle; | ||
102 | } else { | |||
103 | 583 | break; | ||
104 | } | |||
105 | } | |||
106 | ||||
107 | 4665 | 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 | 6998 | void setCurveValue(const kType bins[], VType values[], int size, float key, float value) { | ||
116 | 6998 | int index = findIndexMsg("tbVl", bins, size, key); | ||
117 |
2/2✓ Branch 0 taken 2333 times.
✓ Branch 1 taken 4665 times.
|
2/2✓ Decision 'true' taken 2333 times.
✓ Decision 'false' taken 4665 times.
|
6998 | if (index == -1) |
118 | 2333 | index = 0; | ||
119 | 6998 | values[index] = value; | ||
120 | 6998 | } | ||
121 |