board abstraction

firmware/src/utils/interpolate.cpp

@@ -0,0 +1,163 @@
+/**** Includes ****/
+#include "utils.h"
+#include "interpolate.h"
+
+using namespace util;
+
+/**** Private definitions ****/
+/**** Private constants ****/
+/**** Private variables ****/
+/**** Private function declarations ****/
+/**** Public function definitions ****/
+uint16_t util::interpolate_1d(uint16_t x, uint16_t* x_axis, uint16_t* y_values, uint8_t len_axis)
+{
+	// validate axis length
+	if(len_axis==0) return 0;			// Empty data set
+	if(len_axis==1) return y_values[0];	// Only one data point
+
+	uint16_t y;
+
+	uint8_t i = find_interval_end_index(x, x_axis, len_axis);
+	if(i==0)
+	{
+		//Less then start
+		y = y_values[0];
+	}
+	else if(i==len_axis)
+	{
+		//More than end
+		y = y_values[len_axis-1];
+	}
+	else
+	{
+		// Do interpolate
+		y = interpolate(x, x_axis[i-1], x_axis[i], y_values[i-1], y_values[i]);
+	}
+
+	return y;
+}
+
+uint16_t util::interpolate_2d(uint16_t x, uint16_t y, uint16_t* x_axis, uint8_t len_x_axis, uint16_t* y_axis, uint8_t len_y_axis, uint16_t* z_values)
+{
+	// validate axis length
+	if((len_x_axis==0)&&(len_y_axis==0)) return 0;				// Empty data set
+	if((len_x_axis==1)&&(len_y_axis==1)) return z_values[0];	// Only one data point
+
+	uint8_t ix = find_interval_end_index(x, x_axis, len_x_axis);
+	uint8_t iy = find_interval_end_index(y, y_axis, len_y_axis);
+
+	// Check corners - easy answers
+	if((ix==0)&&(iy==0))
+	{
+		return z_values[0];  //[0][0]  [Y][X]
+	}
+	else if((ix==len_x_axis)&&(iy==0))
+	{
+		return z_values[len_x_axis-1]; //[0][end]
+	}
+	else if((ix==0)&&(iy==len_y_axis))
+	{
+		uint16_t i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
+		return z_values[i]; //[end][0]
+	}
+	else if((ix==len_x_axis)&&(iy==len_y_axis))
+	{
+		uint16_t i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
+		return z_values[i];  //[end][end]
+	};
+
+	// Check boundaries - 1D interpolation
+	if(ix==0)
+    {
+        // On ix=0 line
+        uint16_t i = 0;
+        uint16_t z0 = z_values[i];
+        i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
+        uint16_t z1 = z_values[i];
+        return interpolate(y, y_axis[0], y_axis[len_y_axis-1], z0, z1);
+    }
+    else if(ix==len_x_axis)
+    {
+        // On ix=END line
+        uint16_t i = len_x_axis-1;
+        uint16_t z0 = z_values[i];
+        i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
+        uint16_t z1 = z_values[i];
+        return interpolate(y, y_axis[0], y_axis[len_y_axis-1], z0, z1);
+    }
+    else if(iy==0)
+    {
+        // On iy=0 line
+        uint16_t i = 0;
+        uint16_t z0 = z_values[i];
+        i = len_x_axis-1;
+        uint16_t z1 = z_values[i];
+        return interpolate(x, x_axis[0], x_axis[len_x_axis-1], z0, z1);
+    }
+    else if(iy==len_y_axis)
+    {
+        // On iy=END line
+        uint16_t i = index2d_to_index1d(0, len_y_axis-1, len_x_axis);
+        uint16_t z0 = z_values[i];
+        i = index2d_to_index1d(len_x_axis-1, len_y_axis-1, len_x_axis);
+        uint16_t z1 = z_values[i];
+        return interpolate(x, x_axis[0], x_axis[len_x_axis-1], z0, z1);
+    }
+
+	// Do interpolation
+	// Get axis values
+	uint16_t x0 = x_axis[ix-1];
+	uint16_t x1 = x_axis[ix];
+	uint16_t y0 = y_axis[iy-1];
+	uint16_t y1 = y_axis[iy];
+
+	// Do y0 line calculation
+	// Get z values at x0 and x1 points on y0 line
+	uint16_t i = index2d_to_index1d(ix-1, iy-1, len_x_axis);
+	uint16_t z0 = z_values[i];
+	uint16_t z1 = z_values[i+1];
+	// Interpolate z value on y0 line
+	uint16_t zy0 = interpolate(x, x0, x1, z0, z1);
+
+	// Do y1 line calculation
+	// Get z values at x0 and x1 points on y1 line
+	i = index2d_to_index1d(ix-1, iy, len_x_axis);
+	z0 = z_values[i];
+	z1 = z_values[i+1];
+	// Interpolate z value on y0 line
+	uint16_t zy1 = interpolate(x, x0, x1, z0, z1);
+
+	// Do calculation in y axis on xz line
+	return interpolate(y, y0, y1, zy0, zy1);
+}
+
+uint16_t interpolate(uint16_t x, uint16_t x0, uint16_t x1, uint16_t y0, uint16_t y1)
+{
+	int32_t dy = (int32_t)y1 - (int32_t)y0;
+	int32_t dx = (int32_t)x1 - (int32_t)x0;
+	int32_t d = (int32_t)x - (int32_t)x0;
+
+	int32_t y = dy * d;
+	y /= dx;
+	y += y0;
+
+	return util::sat_cast(y);
+}
+
+uint8_t find_interval_end_index(uint16_t val, uint16_t* axis_values, uint8_t len_axis)
+{
+	for(uint8_t i=0; i<len_axis; i++)
+	{
+		if(val < axis_values[i]) return i;
+		continue;
+	}
+
+	return len_axis;
+}
+
+uint16_t index2d_to_index1d(uint8_t ix, uint8_t iy, uint8_t len_x)
+{
+	return ((uint16_t)len_x * iy) + ix;
+}
+
+/**** Private function definitions ****/