mc_att_control: add gradual3 function to cover hover thrust rescaling

src/lib/mathlib/math/Functions.hpp

@@ -147,4 +147,29 @@ const T gradual(const T &value, const T &x_low, const T &x_high, const T &y_low,
 	}
 }
 
+/*
+ * Constant, linear, linear, constant function with the three corner points as parameters
+ *  y_high               -------
+ *                      /
+ *                    /
+ *  y_middle        /
+ *                /
+ *               /
+ *              /
+ * 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)
+{
+	if (value < x_middle) {
+		return gradual(value, x_low, x_middle, y_low, y_middle);
+
+	} else {
+		return gradual(value, x_middle, x_high, y_middle, y_high);
+	}
+}
+
 } /* namespace math */

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -107,7 +107,7 @@ MulticopterAttitudeControl::parameters_updated()
 float
 MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
 {
-	float throttle_min = _vehicle_land_detected.landed ? 0.0f : _param_mpc_manthr_min.get();
+	const float throttle_min = _vehicle_land_detected.landed ? 0.0f : _param_mpc_manthr_min.get();
 
 	// throttle_stick_input is in range [0, 1]
 	switch (_param_mpc_thr_curve.get()) {
@@ -115,14 +115,9 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
 		return throttle_min + throttle_stick_input * (_param_mpc_thr_max.get() - throttle_min);
 
 	default: // 0 or other: rescale to hover throttle at 0.5 stick
-		if (throttle_stick_input < 0.5f) {
+		return math::gradual3(throttle_stick_input,
-			return (_param_mpc_thr_hover.get() - throttle_min) / 0.5f * throttle_stick_input +
+				      0.f, .5f, 1.f,
-			       throttle_min;
+				      throttle_min, _param_mpc_thr_hover.get(), _param_mpc_thr_max.get());
-
-		} else {
-			return (_param_mpc_thr_max.get() - _param_mpc_thr_hover.get()) / 0.5f * (throttle_stick_input - 1.0f) +
-			       _param_mpc_thr_max.get();
-		}
 	}
 }