rusEFI
The most advanced open source ECU
efi_interpolation.h
Go to the documentation of this file.
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 <rusefi/isnan.h>
12 #include <math.h>
13 #include "datalogging.h"
14 #include "obd_error_codes.h"
15 #include "error_handling.h"
16 
17 #include <type_traits>
18 
19 #ifndef DEBUG_INTERPOLATION
20 #define DEBUG_INTERPOLATION FALSE
21 #endif
22 
23 #define INTERPOLATION_A(x1, y1, x2, y2) ((y1 - y2) / (x1 - x2))
24 
25 int findIndex(const float array[], int size, float value);
26 int findIndex2(const float array[], unsigned size, float value);
27 float interpolateClampedWithValidation(float x1, float y1, float x2, float y2, float x);
28 float interpolateClamped(float x1, float y1, float x2, float y2, float x);
29 float interpolateMsg(const char *msg, float x1, float y1, float x2, float y2, float x);
30 
31 // _technically_ and _theoretically_ we can support flat line for both bins and values but I am not sure if
32 // such a rare case is something we want to support
33 // see also: setLinearCurve
34 template<typename TValue, int TSize>
35 void ensureArrayIsAscending(const char* msg, const TValue (&values)[TSize]) {
36  for (size_t i = 0; i < TSize - 1; i++) {
37  float cur = values[i];
38  float next = values[i + 1];
39  if (next <= cur) {
40  firmwareError(ObdCode::CUSTOM_ERR_AXIS_ORDER, "Invalid table axis (must be ascending!): %s %f should be below %f at %d", msg, cur, next, i);
41  }
42  }
43 }
44 
45 template<typename TValue, int TSize>
46 void ensureArrayIsAscendingOrDefault(const char* msg, const TValue (&values)[TSize]) {
47  if (values[1] == 0) {
48  return; // looks like default empty array, do not check
49  }
50  ensureArrayIsAscending(msg, values);
51 }
52 
53 /** @brief Binary search
54  * @returns the highest index within sorted array such that array[i] is greater than or equal to the parameter
55  * @note If the parameter is smaller than the first element of the array, -1 is returned.
56  *
57  * See also ensureArrayIsAscending
58  */
59 template<typename kType>
60 int findIndexMsg(const char *msg, const kType array[], int size, kType value) {
61  float fvalue = (float)value;
62  if (cisnan(fvalue)) {
63  firmwareError(ObdCode::ERROR_NAN_FIND_INDEX, "NaN in findIndex%s", msg);
64  return 0;
65  }
66 
67  if (value < array[0])
68  return -1;
69  int middle;
70 
71  int left = 0;
72  int right = size;
73 
74  // todo: extract binary search as template method?
75  while (true) {
76 #if 0
77  // that's an assertion to make sure we do not loop here
78  size--;
79  efiAssert(ObdCode::CUSTOM_ERR_ASSERT, size > 0, "Unexpected state in binary search", 0);
80 #endif
81 
82  // todo: compare current implementation with
83  // http://eigenjoy.com/2011/01/21/worlds-fastest-binary-search/
84  // ?
85  middle = (left + right) / 2;
86 
87 // print("left=%d middle=%d right=%d: %.2f\r\n", left, middle, right, array[middle]);
88 
89  if (middle == left)
90  break;
91 
92  if (middle != 0 && array[middle - 1] > array[middle]) {
93 #if EFI_UNIT_TEST
94  firmwareError(ObdCode::CUSTOM_ERR_6610, "%s: out of order %.2f %.2f", msg, array[middle - 1], array[middle]);
95 #else
96  warning(ObdCode::CUSTOM_ERR_OUT_OF_ORDER, "%s: out of order %.2f %.2f", msg, array[middle - 1], array[middle]);
97 
98 #endif /* EFI_UNIT_TEST */
99  }
100 
101  if (value < array[middle]) {
102  right = middle;
103  } else if (value > array[middle]) {
104  left = middle;
105  } else {
106  break;
107  }
108  }
109 
110  return middle;
111 }
112 
113 /**
114  * Sets specified value for specified key in a correction curve
115  * see also setLinearCurve()
116  */
117 template<typename VType, typename kType>
118 void setCurveValue(const kType bins[], VType values[], int size, float key, float value) {
119  int index = findIndexMsg("tbVl", bins, size, key);
120  if (index == -1)
121  index = 0;
122  values[index] = value;
123 }
124 
125 void initInterpolation();
Buffered console output stream header.
void initInterpolation()
int findIndex(const float array[], int size, float value)
float interpolateMsg(const char *msg, float x1, float y1, float x2, float y2, float x)
Linear interpolation by two points.
float interpolateClampedWithValidation(float x1, float y1, float x2, float y2, float x)
float interpolateClamped(float x1, float y1, float x2, float y2, float x)
int findIndexMsg(const char *msg, const kType array[], int size, kType value)
Binary search.
void ensureArrayIsAscendingOrDefault(const char *msg, const TValue(&values)[TSize])
void setCurveValue(const kType bins[], VType values[], int size, float key, float value)
int findIndex2(const float array[], unsigned size, float value)
void ensureArrayIsAscending(const char *msg, const TValue(&values)[TSize])
bool warning(ObdCode code, const char *fmt,...)
void firmwareError(ObdCode code, const char *fmt,...)
Standard and custom OBD-II error codes.
@ CUSTOM_ERR_AXIS_ORDER
@ ERROR_NAN_FIND_INDEX
@ CUSTOM_ERR_6610
@ CUSTOM_ERR_ASSERT
@ CUSTOM_ERR_OUT_OF_ORDER
composite packet size