Piecewise linear interpolation routine (#13798)

* piecewise linear interpolation routines

* haslinghuis review

* triling spaces removed

* KarateBrot review

* ledvinap's review

* minor style fixes

mk/source.mk

@@ -61,6 +61,7 @@ COMMON_SRC = \
             common/maths.c \
             common/printf.c \
             common/printf_serial.c \
+            common/pwl.c \
             common/sdft.c \
             common/sensor_alignment.c \
             common/stopwatch.c \
@@ -401,6 +402,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
             common/encoding.c \
             common/filter.c \
             common/maths.c \
+            common/pwl.c \
             common/sdft.c \
             common/stopwatch.c \
             common/typeconversion.c \

src/main/common/pwl.c

@@ -0,0 +1,63 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "platform.h"
+
+#include "pwl.h"
+
+
+void pwlInitialize(pwl_t *pwl, float *yValues, int numPoints, float xMin, float xMax) {
+    pwl->yValues = yValues;
+    pwl->numPoints = numPoints;
+    pwl->xMin = xMin;
+    pwl->xMax = xMax;
+    pwl->dx = (xMax - xMin) / (numPoints - 1);
+}
+
+void pwlFill(pwl_t *pwl, float (*function)(float, void*), void *args)
+{
+    for (int i = 0; i < pwl->numPoints; ++i) {
+        const float x = pwl->xMin + i * pwl->dx;
+        pwl->yValues[i] = function(x, args);
+    }
+}
+
+float pwlInterpolate(const pwl_t *pwl, float x)
+{
+    if (x <= pwl->xMin) {
+        return pwl->yValues[0];
+    }
+    
+    if (x >= pwl->xMax) {
+        return pwl->yValues[pwl->numPoints - 1];
+    }
+    
+    const int index = (int)((x - pwl->xMin) / pwl->dx);
+    if (index >= pwl->numPoints - 1) {
+        return pwl->yValues[pwl->numPoints - 1];
+    }
+
+    const float x0 = pwl->xMin + index * pwl->dx;
+    const float y0 = pwl->yValues[index];
+    const float y1 = pwl->yValues[index + 1];
+
+    return y0 + (x - x0) * (y1 - y0) / pwl->dx;
+}

src/main/common/pwl.h

@@ -0,0 +1,50 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "utils.h"
+
+
+#define PWL_DECLARE(name, size, xMinV, xMaxV)                           \
+    STATIC_ASSERT((xMinV) < (xMaxV), "xMinV must be less than xMaxV");  \
+    STATIC_ASSERT((size) > 1, "size must be more than 1");              \
+    STATIC_ASSERT((size) < 33, "size must be less than 33");            \
+    float name##_yValues[(size)];                                       \
+    pwl_t name = {                                                      \
+        .yValues = name##_yValues,                                      \
+        .numPoints = (size),                                            \
+        .xMin = (xMinV),                                                \
+        .xMax = (xMaxV),                                                \
+        .dx = ((xMaxV) - (xMinV)) / ((size) - 1)                        \
+    }
+
+typedef struct pwl_s {
+    float *yValues;
+    int numPoints;
+    float xMin;
+    float xMax;
+    float dx;
+} pwl_t;
+
+void pwlInitialize(pwl_t *pwl, float *yValues, int numPoints, float xMin, float xMax);
+void pwlFill(pwl_t *pwl, float (*function)(float, void*), void *arg);
+float pwlInterpolate(const pwl_t *pwl, float x);

src/test/Makefile

@@ -472,6 +472,9 @@ vtx_msp_unittest_DEFINES := \
 		USE_VTX_TABLE= \
 		USE_VTX_MSP=
 
+pwl_unittest_SRC := \
+		$(USER_DIR)/common/pwl.c
+
 # Please tweak the following variable definitions as needed by your
 # project, except GTEST_HEADERS, which you can use in your own targets
 # but shouldn't modify.

src/test/unit/pwl_unittest.cc

@@ -0,0 +1,109 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include <limits.h>
+
+#include <math.h>
+
+extern "C" {
+    #include "common/pwl.h"
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+float xSquared(float x, void *args)
+{
+    UNUSED(args);
+    return x * x;
+}
+
+PWL_DECLARE(pwlXSquared, 21, 0.0f, 10.0f);
+
+TEST(PwlUnittest, TestXSquared21)
+{
+    pwlFill(&pwlXSquared, xSquared, NULL);
+
+    EXPECT_EQ(pwlInterpolate(&pwlXSquared, -1.0f), 0.0f); // outside of X bounds on the left
+    EXPECT_EQ(pwlInterpolate(&pwlXSquared, 11.0f), 100.0f); // outside of X bounds on the right
+    EXPECT_EQ(pwlInterpolate(&pwlXSquared, 0.0f), 0.0f);
+    EXPECT_EQ(pwlInterpolate(&pwlXSquared, 1.0f), 1.0f);
+    EXPECT_EQ(pwlInterpolate(&pwlXSquared, 2.0f), 4.0f);
+    EXPECT_EQ(pwlInterpolate(&pwlXSquared, 9.0f), 81.0f);
+    EXPECT_EQ(pwlInterpolate(&pwlXSquared, 10.0f), 100.0f);
+
+    float x = 0.0f;
+    while (x <= 10.0f) {
+        EXPECT_NEAR(pwlInterpolate(&pwlXSquared, x), xSquared(x, NULL), 0.1f);
+        x += 0.1;
+    }    
+}
+
+
+PWL_DECLARE(pwlXSquaredTwoPoints, 2, 1.0f, 5.0f);
+
+TEST(PwlUnittest, TestXSquared2)
+{
+    pwlFill(&pwlXSquaredTwoPoints, xSquared, NULL);
+
+    EXPECT_EQ(pwlInterpolate(&pwlXSquaredTwoPoints, -1.0f), 1.0f); // outside of X bounds on the left
+    EXPECT_EQ(pwlInterpolate(&pwlXSquaredTwoPoints, 11.0f), 25.0f); // outside of X bounds on the right
+    EXPECT_EQ(pwlInterpolate(&pwlXSquaredTwoPoints, 1.0f), 1.0f);
+    EXPECT_EQ(pwlInterpolate(&pwlXSquaredTwoPoints, 5.0f), 25.0f);
+
+    EXPECT_EQ(pwlInterpolate(&pwlXSquaredTwoPoints, 3.5f), 16.0f);
+}
+
+
+typedef struct additionalArgs_s {
+    float a;
+} additionalArgs_t;
+
+float xSquaredArgs(float x, void *args)
+{
+    additionalArgs_t *addArgs = (additionalArgs_t*)args;
+    return x * x * addArgs->a;
+}
+
+PWL_DECLARE(pwlXSquaredArgs, 21, 0.0f, 10.0f);
+
+TEST(PwlUnittest, TestXSquaredArgs)
+{
+    additionalArgs_t args { .a = 2.0 };
+    pwlFill(&pwlXSquaredArgs, xSquaredArgs, &args);
+
+    EXPECT_EQ(pwlInterpolate(&pwlXSquaredArgs, -1.0f), args.a * 0.0f); // outside of X bounds on the left
+    EXPECT_EQ(pwlInterpolate(&pwlXSquaredArgs, 11.0f), args.a * 100.0f); // outside of X bounds on the right
+    EXPECT_EQ(pwlInterpolate(&pwlXSquaredArgs, 0.0f), args.a * 0.0f);
+    EXPECT_EQ(pwlInterpolate(&pwlXSquaredArgs, 1.0f), args.a * 1.0f);
+    EXPECT_EQ(pwlInterpolate(&pwlXSquaredArgs, 2.0f), args.a * 4.0f);
+    EXPECT_EQ(pwlInterpolate(&pwlXSquaredArgs, 9.0f), args.a * 81.0f);
+    EXPECT_EQ(pwlInterpolate(&pwlXSquaredArgs, 10.0f), args.a * 100.0f);
+
+    float x = 0.0f;
+    while (x <= 10.0f) {
+        EXPECT_NEAR(pwlInterpolate(&pwlXSquaredArgs, x), xSquaredArgs(x, &args), args.a * 0.1f);
+        x += 0.1;
+    }
+}