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

Functions: add gradual function with arbitrary number of corner points

Browse Source
This commit is contained in:
Matthias Grob
2022-08-11 14:32:03 +02:00
committed by Beat Küng
parent 6a9a049f1e
commit 2edb35b1b5
3 changed files with 50 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/Functions.hpp
+36 -17
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2017-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -166,28 +166,47 @@ const T interpolate(const T &value, const T &x_low, const T &x_high, const T &y_
}
/*
* Constant, linear, linear, constant function with the three corner points as parameters
* y_high -------
* /
* /
* y_middle /
* /
* Constant, piecewise linear, constant function with 1/N size intervalls and N corner points as parameters
* y[N] -------
* /
* /
* y[1] /
* /
* /
* y_low -------
* x_low x_middle x_high
* /
* y[0] -------
* 0 1/(N-1) 2/(N-1) ... 1
*/
template<typename T>
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)
template<typename T, size_t N>
const T interpolateN(const T &value, const T(&y)[N])
{
if (value < x_middle) {
return interpolate(value, x_low, x_middle, y_low, y_middle);
size_t index = constrain((int)(value * (N - 1)), 0, (int)(N - 2));
return interpolate(value, (T)index / (T)(N - 1), (T)(index + 1) / (T)(N - 1), y[index], y[index + 1]);
}
} else {
return interpolate(value, x_middle, x_high, y_middle, y_high);
/*
* Constant, piecewise linear, constant function with N corner points as parameters
* y[N] -------
* /
* /
* y[1] /
* /
* /
* /
* y[0] -------
* x[0] x[1] ... x[N]
* Note: x[N] corner coordinates have to be sorted in ascending order
*/
template<typename T, size_t N>
const T interpolateNXY(const T &value, const T(&x)[N], const T(&y)[N])
{
size_t index = 0;
while (value > x[index + 1] && index < N) {
index++;
}
return interpolate(value, x[index], x[index + 1], y[index], y[index + 1]);
}
/*
src/lib/mathlib/math/FunctionsTest.cpp
+13 -28
View File
@@ -176,36 +176,21 @@ TEST(FunctionsTest, interpolate)
EXPECT_FLOAT_EQ(interpolate(0.f, 0.f, 0.f, 0.f, 0.f), 0.f);
}
TEST(FunctionsTest, interpolate3)
TEST(FunctionsTest, interpolateNXY)
{
float x[3] = {0.f, .5f, 1.5f};
float y[3] = {1.f, 2.f, 3.f};
// factor of *2, offset +1
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);
EXPECT_FLOAT_EQ(interpolateNXY(-12.f, x, y), 1.f);
EXPECT_FLOAT_EQ(interpolateNXY(0.f, x, y), 1.f);
EXPECT_FLOAT_EQ(interpolateNXY(.25f, x, y), 1.5f);
EXPECT_FLOAT_EQ(interpolateNXY(.5f, x, y), 2.f);
EXPECT_FLOAT_EQ(interpolateNXY(.75f, x, y), 2.25f);
EXPECT_FLOAT_EQ(interpolateNXY(1.f, x, y), 2.5f);
EXPECT_FLOAT_EQ(interpolateNXY(1.25f, x, y), 2.75f);
EXPECT_FLOAT_EQ(interpolateNXY(1.5f, x, y), 3.f);
EXPECT_FLOAT_EQ(interpolateNXY(12.f, x, y), 3.f);
}
TEST(FunctionsTest, sqrt_linear)
src/modules/mc_att_control/mc_att_control_main.cpp
+1 -3
View File
@@ -102,9 +102,7 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
return math::interpolate(throttle_stick_input, 0.f, 1.f, _param_mpc_manthr_min.get(), _param_mpc_thr_max.get());
default: // 0 or other: rescale to hover throttle at 0.5 stick
return math::interpolate3(throttle_stick_input,
0.f, .5f, 1.f,
_param_mpc_manthr_min.get(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get());
return math::interpolateN(throttle_stick_input, {_param_mpc_manthr_min.get(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()});
}
}