fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 17:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rename math::gradual() to math::interpolate() and add unit tests to cover additional corner cases.

Browse Source
This commit is contained in:
mcsauder
2022-08-18 14:18:02 +02:00
committed by Mathieu Bresciani
parent 021b23826d
commit 87a5705960
8 changed files with 93 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/Functions.hpp
+6 -6
View File
@@ -149,7 +149,7 @@ const T expo_deadzone(const T &value, const T &e, const T &dz)
* x_low x_high
*/
template<typename T>
const T gradual(const T &value, const T &x_low, const T &x_high, const T &y_low, const T &y_high)
const T interpolate(const T &value, const T &x_low, const T &x_high, const T &y_low, const T &y_high)
{
if (value <= x_low) {
return y_low;
@@ -178,15 +178,15 @@ const T gradual(const T &value, const T &x_low, const T &x_high, const T &y_low,
* x_low x_middle x_high
*/
template<typename T>
const T gradual3(const T &value,
const T &x_low, const T &x_middle, const T &x_high,
const T &y_low, const T &y_middle, const T &y_high)
const T interpolate3(const T &value,
const T &x_low, const T &x_middle, const T &x_high,
const T &y_low, const T &y_middle, const T &y_high)
{
if (value < x_middle) {
return gradual(value, x_low, x_middle, y_low, y_middle);
return interpolate(value, x_low, x_middle, y_low, y_middle);
} else {
return gradual(value, x_middle, x_high, y_middle, y_high);
return interpolate(value, x_middle, x_high, y_middle, y_high);
}
}
src/lib/mathlib/math/FunctionsTest.cpp
+54 -45
View File
@@ -140,63 +140,72 @@ TEST(FunctionsTest, expo_deadzone)
EXPECT_FLOAT_EQ(expo_deadzone(-.5f, .9f, .4f), -.020833336f);
}
TEST(FunctionsTest, gradual)
TEST(FunctionsTest, interpolate)
{
// factor of *2, offset +1
EXPECT_FLOAT_EQ(gradual(-12.f, 0.f, 1.f, 1.f, 3.f), 1.f);
EXPECT_FLOAT_EQ(gradual(0.f, 0.f, 1.f, 1.f, 3.f), 1.f);
EXPECT_FLOAT_EQ(gradual(.25f, 0.f, 1.f, 1.f, 3.f), 1.5f);
EXPECT_FLOAT_EQ(gradual(.5f, 0.f, 1.f, 1.f, 3.f), 2.f);
EXPECT_FLOAT_EQ(gradual(.75f, 0.f, 1.f, 1.f, 3.f), 2.5f);
EXPECT_FLOAT_EQ(gradual(1.f, 0.f, 1.f, 1.f, 3.f), 3.f);
EXPECT_FLOAT_EQ(gradual(12.f, 0.f, 1.f, 1.f, 3.f), 3.f);
EXPECT_FLOAT_EQ(interpolate(-12.f, 0.f, 1.f, 1.f, 3.f), 1.f);
EXPECT_FLOAT_EQ(interpolate(0.f, 0.f, 1.f, 1.f, 3.f), 1.f);
EXPECT_FLOAT_EQ(interpolate(.25f, 0.f, 1.f, 1.f, 3.f), 1.5f);
EXPECT_FLOAT_EQ(interpolate(.5f, 0.f, 1.f, 1.f, 3.f), 2.f);
EXPECT_FLOAT_EQ(interpolate(.75f, 0.f, 1.f, 1.f, 3.f), 2.5f);
EXPECT_FLOAT_EQ(interpolate(1.f, 0.f, 1.f, 1.f, 3.f), 3.f);
EXPECT_FLOAT_EQ(interpolate(12.f, 0.f, 1.f, 1.f, 3.f), 3.f);
// factor of *2, offset +3
EXPECT_FLOAT_EQ(gradual(-12.f, 1.f, 2.f, 4.f, 6.f), 4.f);
EXPECT_FLOAT_EQ(gradual(1.f, 1.f, 2.f, 4.f, 6.f), 4.f);
EXPECT_FLOAT_EQ(gradual(1.25f, 1.f, 2.f, 4.f, 6.f), 4.5f);
EXPECT_FLOAT_EQ(gradual(1.5f, 1.f, 2.f, 4.f, 6.f), 5.f);
EXPECT_FLOAT_EQ(gradual(1.75f, 1.f, 2.f, 4.f, 6.f), 5.5f);
EXPECT_FLOAT_EQ(gradual(2.f, 1.f, 2.f, 4.f, 6.f), 6.f);
EXPECT_FLOAT_EQ(gradual(12.f, 1.f, 2.f, 4.f, 6.f), 6.f);
EXPECT_FLOAT_EQ(interpolate(-12.f, 1.f, 2.f, 4.f, 6.f), 4.f);
EXPECT_FLOAT_EQ(interpolate(1.f, 1.f, 2.f, 4.f, 6.f), 4.f);
EXPECT_FLOAT_EQ(interpolate(1.25f, 1.f, 2.f, 4.f, 6.f), 4.5f);
EXPECT_FLOAT_EQ(interpolate(1.5f, 1.f, 2.f, 4.f, 6.f), 5.f);
EXPECT_FLOAT_EQ(interpolate(1.75f, 1.f, 2.f, 4.f, 6.f), 5.5f);
EXPECT_FLOAT_EQ(interpolate(2.f, 1.f, 2.f, 4.f, 6.f), 6.f);
EXPECT_FLOAT_EQ(interpolate(12.f, 1.f, 2.f, 4.f, 6.f), 6.f);
// corner case when x_low == x_high == value
EXPECT_FLOAT_EQ(gradual(1.f, 1.f, 1.f, 4.f, 6.f), 4.f);
EXPECT_FLOAT_EQ(interpolate(1.f, 1.f, 1.f, 4.f, 6.f), 4.f);
// corner case when x_low > x_high
EXPECT_FLOAT_EQ(gradual(1.05f, 1.1f, 1.f, 4.f, 6.f), 4.f);
EXPECT_FLOAT_EQ(interpolate(1.05f, 1.1f, 1.f, 4.f, 6.f), 4.f);
// corner case when y_low == y_high == value
EXPECT_FLOAT_EQ(interpolate(1.f, 1.f, 1.f, 4.f, 6.f), 4.f);
// corner case when y_low > y_high
EXPECT_FLOAT_EQ(interpolate(1.5f, 1.f, 2.f, 6.f, 4.f), 5.f);
// corner case when x_low == x_high == y_low == y_high == 0.f
EXPECT_FLOAT_EQ(interpolate(0.f, 0.f, 0.f, 0.f, 0.f), 0.f);
}
TEST(FunctionsTest, gradual3)
TEST(FunctionsTest, interpolate3)
{
// factor of *2, offset +1
EXPECT_FLOAT_EQ(gradual3(-12.f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 1.f);
EXPECT_FLOAT_EQ(gradual3(0.f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 1.f);
EXPECT_FLOAT_EQ(gradual3(.25f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 1.5f);
EXPECT_FLOAT_EQ(gradual3(.5f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 2.f);
EXPECT_FLOAT_EQ(gradual3(.75f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 2.25f);
EXPECT_FLOAT_EQ(gradual3(1.f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 2.5f);
EXPECT_FLOAT_EQ(gradual3(1.25f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 2.75f);
EXPECT_FLOAT_EQ(gradual3(1.5f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 3.f);
EXPECT_FLOAT_EQ(gradual3(12.f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 3.f);
EXPECT_FLOAT_EQ(interpolate3(-12.f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 1.f);
EXPECT_FLOAT_EQ(interpolate3(0.f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 1.f);
EXPECT_FLOAT_EQ(interpolate3(.25f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 1.5f);
EXPECT_FLOAT_EQ(interpolate3(.5f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 2.f);
EXPECT_FLOAT_EQ(interpolate3(.75f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 2.25f);
EXPECT_FLOAT_EQ(interpolate3(1.f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 2.5f);
EXPECT_FLOAT_EQ(interpolate3(1.25f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 2.75f);
EXPECT_FLOAT_EQ(interpolate3(1.5f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 3.f);
EXPECT_FLOAT_EQ(interpolate3(12.f,
0.f, .5f, 1.5f,
1.f, 2.f, 3.f), 3.f);
}
TEST(FunctionsTest, sqrt_linear)