ultimate-nag52-fw/lib/core/tcumap.cpp

134 lines
5.5 KiB
C++

#include "tcumap.h"
#include "string.h"
#include "esp_heap_caps.h"
TcuMap::TcuMap(uint16_t X_Size, uint16_t Y_size, const int16_t* x_ids, const int16_t* y_ids) {
this->x_size = X_Size;
this->y_size = Y_size;
this->alloc_ok = false;
this->x_headers = static_cast<int16_t*>(heap_caps_malloc(x_size * sizeof(int16_t), MALLOC_CAP_SPIRAM));
this->y_headers = static_cast<int16_t*>(heap_caps_malloc(y_size * sizeof(int16_t), MALLOC_CAP_SPIRAM));
this->data = static_cast<int16_t*>(heap_caps_malloc(y_size * x_size * sizeof(int16_t), MALLOC_CAP_SPIRAM));
if (!((nullptr == this->x_headers) || (nullptr == this->y_headers) || (nullptr == this->data))) {
// Allocation succeeded!
(void)memcpy(this->x_headers, x_ids, (sizeof(int16_t) * x_size));
(void)memcpy(this->y_headers, y_ids, (sizeof(int16_t) * y_size));
(void)memset(this->data, 0, y_size * (x_size * sizeof(int16_t)));
this->alloc_ok = true;
}
}
bool TcuMap::add_data(const int16_t* map, const uint16_t size) {
bool result = false;
if (nullptr != map)
{
if (((uint16_t)size) == ((this->x_size) * (this->y_size)))
{
(void)memcpy(this->data, map, size * sizeof(int16_t));
result = true;
}
}
return result;
}
bool TcuMap::allocate_ok(void) const {
return this->alloc_ok;
}
inline void TcuMap::set_indices(const float value, uint16_t* idx_min, uint16_t* idx_max, const int16_t* headers, const uint16_t size) {
// Set minimum index to the first element of the field.
*idx_min = 0u;
// Set maximum index to the last element of the field.
*idx_max = size - 1u;
// Check, if search value is smaller than smallest element of the field.
if (value > (float)headers[0]) {
if (value < (float)headers[*idx_max]) {
// Search value is in between the limits of the smallest and the biggest element of the field.
do {
// Calculate the middle of the remaining list. If the size is odd, it is rounded down.
uint16_t idx_mid = (*idx_min + *idx_max) >> 1;
if (value < (float)headers[idx_mid]) {
// Search value is smaller than the element in the middle of the remaining list.
*idx_max = idx_mid;
}
else if (value > (float)headers[idx_mid]) {
// Search value is bigger than the element in the middle of the remaining list.
*idx_min = idx_mid;
}
else {
// Search value is also an element of the field.
*idx_min = idx_mid;
*idx_max = idx_mid;
}
// Reduce the remaining search area until it is narrowed down to two consecutive elements.
} while (1u < ((*idx_max) - (*idx_min)));
}
else {
// Search value is as big as or bigger then the biggest element in the field.
*idx_min = *idx_max;
}
}
else {
// Search value is as small as or smaller than smallest element of the field.
*idx_max = *idx_min;
}
}
inline float TcuMap::interpolate(const float f_1, const float f_2, const int16_t x_1, const int16_t x_2, const float x) {
// cast values from signed integer values to floating values in order to avoid casting the same value twice
const float x_1_f = (float)x_1;
const float x_2_f = (float)x_2;
// See https://en.wikipedia.org/wiki/Linear_interpolation for details. Return f_1, if x_1 and x_2 are identical.
return (x_1 != x_2) ? f_1 + ((f_2 - f_1) / (x_2_f - x_1_f)) * (x - x_1_f) : f_1;
}
float TcuMap::get_value(float x_value, float y_value) {
uint16_t x_idx_min;
uint16_t x_idx_max;
uint16_t y_idx_min;
uint16_t y_idx_max;
// part 1a - identification of the indices for x-value
set_indices(x_value, &x_idx_min, &x_idx_max, this->x_headers, this->x_size);
// part 1b - identification of the indices for y-value
set_indices(y_value, &y_idx_min, &y_idx_max, this->y_headers, this->y_size);
// part 2: do the interpolation
int16_t x1 = this->x_headers[x_idx_min];
int16_t x2 = this->x_headers[x_idx_max];
int16_t y1 = this->y_headers[y_idx_min];
int16_t y2 = this->y_headers[y_idx_max];
// some precalculations for making the code more readable, although somewhat inefficient
float f_11 = (float)data[(y_idx_min * this->x_size) + x_idx_min];
float f_12 = (float)data[(y_idx_min * this->x_size) + x_idx_max];
float f_21 = (float)data[(y_idx_max * this->x_size) + x_idx_min];
float f_22 = (float)data[(y_idx_max * this->x_size) + x_idx_max];
// interpolation on x-axis for smaller y-index
float f_11f_12_interpolated = interpolate(f_11, f_12, x1, x2, x_value);
// interpolation on x-axis for greater y-index
float f_21f_22_interpolated = interpolate(f_21, f_22, x1, x2, x_value);
// bilinear interpolation, not always efficient, but with more or less constant runtime
// also see https://en.wikipedia.org/wiki/Bilinear_interpolation, https://helloacm.com/cc-function-to-compute-the-bilinear-interpolation/ for mathematical background
return interpolate(f_11f_12_interpolated, f_21f_22_interpolated, y1, y2, y_value);
}
int16_t* TcuMap::get_current_data(void) {
return this->data;
}
void TcuMap::get_x_headers(uint16_t *size, int16_t **headers){
*size = this->x_size;
*headers = this->x_headers;
}
void TcuMap::get_y_headers(uint16_t *size, int16_t **headers){
*size = this->y_size;
*headers = this->y_headers;
}