mathlib, mc_pos_control: rename functions file and add gradual linear function

src/lib/mathlib/math/Functions.hpp

@@ -32,11 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Expo.hpp
+ * @file Functions.hpp
  *
- * So called exponential curve function implementation.
- * It is essentially a linear combination between a linear and a cubic function.
- * It's used in the range [-1,1]
+ * collection of rather simple mathematical functions that get used over and over again
  */
 
 #pragma once
@@ -53,6 +51,11 @@ int sign(T val)
 	return (T(0) < val) - (val < T(0));
 }
 
+/*
+ * So called exponential curve function implementation.
+ * It is essentially a linear combination between a linear and a cubic function.
+ * It's used in the range [-1,1]
+ */
 template<typename _Tp>
 inline const _Tp expo(const _Tp &value, const _Tp &e)
 {
@@ -77,4 +80,31 @@ inline const _Tp expo_deadzone(const _Tp &value, const _Tp &e, const _Tp &dz)
 	return expo(deadzone(value, dz), e);
 }
 
+
+/*
+ * Constant, linear, constant function with the two corner points as parameters
+ * y_high          -------
+ *                /
+ *               /
+ *              /
+ * y_low -------
+ *         x_low   x_high
+ */
+template<typename _Tp>
+inline const _Tp gradual(const _Tp &value, const _Tp &x_low, const _Tp &x_high, const _Tp &y_low, const _Tp &y_high)
+{
+	if (value < x_low) {
+		return y_low;
+
+	} else if (value > x_high) {
+		return y_high;
+
+	} else {
+		/* linear function between the two points */
+		float a = (y_high - y_low) / (x_high - x_low);
+		float b = y_low - a * x_low;
+		return  a * value + b;
+	}
+}
+
 }

src/lib/mathlib/mathlib.h

@@ -45,7 +45,7 @@
 #include "math/Matrix.hpp"
 #include "math/Quaternion.hpp"
 #include "math/Limits.hpp"
-#include "math/Expo.hpp"
+#include "math/Functions.hpp"
 #include "math/matrix_alg.h"
 
 #endif

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -919,17 +919,9 @@ MulticopterPositionControl::slow_land_gradual_velocity_limit()
 	 * for now we use the home altitude and assume that we want to land on a similar absolute altitude.
 	 */
 	float altitude_above_home = -_pos(2) + _home_pos.z;
-	float vel_limit = _params.vel_max_down;
-
-	if (altitude_above_home < _params.slow_land_alt2) {
-		vel_limit = _params.land_speed;
-
-	} else if (altitude_above_home < _params.slow_land_alt1) {
-		/* linear function between the two altitudes */
-		float a = (_params.vel_max_down - _params.land_speed) / (_params.slow_land_alt1 - _params.slow_land_alt2);
-		float b = _params.land_speed - a * _params.slow_land_alt2;
-		vel_limit =  a * altitude_above_home + b;
-	}
+	float vel_limit = math::gradual(altitude_above_home,
+					_params.slow_land_alt2, _params.slow_land_alt1,
+					_params.land_speed, _params.vel_max_down);
 
 	_vel_sp(2) = math::min(_vel_sp(2), vel_limit);
 }