Add linear interpolation method to C_utils

middleware/include/c_utils.h

@@ -52,4 +52,20 @@ void endian_swap(void *ptr, int size);
 /// @return the same byte but wuth the bits reversed
 unsigned char reverse_bits(unsigned char b);
 
+/**
+ * @brief Performs linear interpolation between two points.
+ *
+ * Computes the interpolated y-value corresponding to x, given two
+ * reference points (x1, y1) and (x2, y2).
+ *
+ * @param x   Input value at which to interpolate.
+ * @param x1  First reference x-coordinate.
+ * @param x2  Second reference x-coordinate.
+ * @param y1  y-value at x1.
+ * @param y2  y-value at x2.
+ *
+ * @return Interpolated y-value.
+ */
+float linear_interpolate(float x, float x1, float x2, float y1, float y2);
+
 #endif /* C_UTILS */

middleware/src/c_utils.c

@@ -1,4 +1,9 @@
 #include "c_utils.h"
+#include <assert.h>
+#include <math.h>
+
+/* Epsilon for float comparisons */
+#define FLOAT_EPSILON (0.0001f)
 
 void endian_swap(void *ptr, int size)
 {
@@ -17,4 +22,11 @@ unsigned char reverse_bits(unsigned char b)
 	b = (b & 0xCC) >> 2 | (b & 0x33) << 2;
 	b = (b & 0xAA) >> 1 | (b & 0x55) << 1;
 	return b;
+}
+
+float linear_interpolate(float x, float x1, float x2, float y1, float y2)
+{
+	assert(fabs(x2 - x1) > FLOAT_EPSILON);
+
+	return y1 + ((x - x1) * (y2 - y1) / (x2 - x1));
 }