Functions: add gradual function with arbitrary number of corner points

src/lib/mathlib/math/Functions.hpp

@@ -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
+ * Constant, piecewise linear, constant function with 1/N size intervalls and N corner points as parameters
- *  y_high               -------
+ * y[N]                 -------
  *                     /
  *                   /
- *  y_middle        /
+ * y[1]            /
  *               /
  *              /
  *             /
- * y_low -------
+ * y[0] -------
- *         x_low x_middle x_high
+ *        0 1/(N-1) 2/(N-1) ... 1
  */
-template<typename T>
+template<typename T, size_t N>
-const T interpolate3(const T &value,
+const T interpolateN(const T &value, const T(&y)[N])
-		     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) {
+	size_t index = constrain((int)(value * (N - 1)), 0, (int)(N - 2));
-		return interpolate(value, x_low, x_middle, y_low, y_middle);
+	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

@@ -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,
+	EXPECT_FLOAT_EQ(interpolateNXY(-12.f, x, y), 1.f);
-				     0.f, .5f, 1.5f,
+	EXPECT_FLOAT_EQ(interpolateNXY(0.f, x, y), 1.f);
-				     1.f, 2.f, 3.f), 1.f);
+	EXPECT_FLOAT_EQ(interpolateNXY(.25f, x, y), 1.5f);
-	EXPECT_FLOAT_EQ(interpolate3(0.f,
+	EXPECT_FLOAT_EQ(interpolateNXY(.5f, x, y), 2.f);
-				     0.f, .5f, 1.5f,
+	EXPECT_FLOAT_EQ(interpolateNXY(.75f, x, y), 2.25f);
-				     1.f, 2.f, 3.f), 1.f);
+	EXPECT_FLOAT_EQ(interpolateNXY(1.f, x, y), 2.5f);
-	EXPECT_FLOAT_EQ(interpolate3(.25f,
+	EXPECT_FLOAT_EQ(interpolateNXY(1.25f, x, y), 2.75f);
-				     0.f, .5f, 1.5f,
+	EXPECT_FLOAT_EQ(interpolateNXY(1.5f, x, y), 3.f);
-				     1.f, 2.f, 3.f), 1.5f);
+	EXPECT_FLOAT_EQ(interpolateNXY(12.f, x, y), 3.f);
-	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)

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -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,
+		return math::interpolateN(throttle_stick_input, {_param_mpc_manthr_min.get(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()});
-					  0.f, .5f, 1.f,
-					  _param_mpc_manthr_min.get(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get());
 	}
 }