diff --git a/mk/source.mk b/mk/source.mk index 4200ed27b1..38d1063b0b 100644 --- a/mk/source.mk +++ b/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 \ diff --git a/src/main/common/pwl.c b/src/main/common/pwl.c new file mode 100644 index 0000000000..eb06cda5a4 --- /dev/null +++ b/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 . + */ + +#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; +} diff --git a/src/main/common/pwl.h b/src/main/common/pwl.h new file mode 100644 index 0000000000..458d213213 --- /dev/null +++ b/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 . + */ + +#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); diff --git a/src/test/Makefile b/src/test/Makefile index f5dbca94dd..b256f3fff7 100644 --- a/src/test/Makefile +++ b/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. diff --git a/src/test/unit/pwl_unittest.cc b/src/test/unit/pwl_unittest.cc new file mode 100644 index 0000000000..9cabce77fa --- /dev/null +++ b/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 . + */ + +#include +#include + +#include + +#include + +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; + } +}