MulticopterPositionControl: add horizontal margin for saturation cases

src/lib/mathlib/math/Functions.hpp

@@ -54,6 +54,12 @@ int signNoZero(T val)
 	return (T(0) <= val) - (val < T(0));
 }
 
+template<typename T>
+T sq(T val)
+{
+	return val * val;
+}
+
 /*
  * So called exponential curve function implementation.
  * It is essentially a linear combination between a linear and a cubic function.

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -152,6 +152,7 @@ int MulticopterPositionControl::parameters_update(bool force)
 			Vector3f(_param_mpc_xy_vel_p_acc.get(), _param_mpc_xy_vel_p_acc.get(), _param_mpc_z_vel_p_acc.get()),
 			Vector3f(_param_mpc_xy_vel_i_acc.get(), _param_mpc_xy_vel_i_acc.get(), _param_mpc_z_vel_i_acc.get()),
 			Vector3f(_param_mpc_xy_vel_d_acc.get(), _param_mpc_xy_vel_d_acc.get(), _param_mpc_z_vel_d_acc.get()));
+		_control.setHorizontalThrustMargin(_param_mpc_thr_xy_marg.get());
 
 		// Check that the design parameters are inside the absolute maximum constraints
 		if (_param_mpc_xy_cruise.get() > _param_mpc_xy_vel_max.get()) {

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -161,6 +161,7 @@ private:
 		(ParamFloat<px4::params::MPC_TILTMAX_LND>)  _param_mpc_tiltmax_lnd,  /**< maximum tilt for landing and smooth takeoff */
 		(ParamFloat<px4::params::MPC_THR_MIN>)      _param_mpc_thr_min,
 		(ParamFloat<px4::params::MPC_THR_MAX>)      _param_mpc_thr_max,
+		(ParamFloat<px4::params::MPC_THR_XY_MARG>)  _param_mpc_thr_xy_marg,
 
 		(ParamFloat<px4::params::SYS_VEHICLE_RESP>) _param_sys_vehicle_resp,
 		(ParamFloat<px4::params::MPC_ACC_HOR>)      _param_mpc_acc_hor,

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -65,6 +65,11 @@ void PositionControl::setThrustLimits(const float min, const float max)
 	_lim_thr_max = max;
 }
 
+void PositionControl::setHorizontalThrustMargin(const float margin)
+{
+	_lim_thr_xy_margin = margin;
+}
+
 void PositionControl::updateHoverThrust(const float hover_thrust_new)
 {
 	_vel_int(2) += (hover_thrust_new - _hover_thrust) * (CONSTANTS_ONE_G / hover_thrust_new);
@@ -137,13 +142,19 @@ void PositionControl::_velocityControl(const float dt)
 		vel_error(2) = 0.f;
 	}
 
+	// Prioritize vertical control while keeping a horizontal margin
+	const Vector2f thrust_sp_xy(_thr_sp);
+	const float thrust_sp_xy_norm = thrust_sp_xy.norm();
+
+	// Determine how much vertical thrust is left keeping horizontal margin
+	const float allocated_horizontal_thrust = math::min(thrust_sp_xy_norm, _lim_thr_xy_margin);
+	const float thrust_z_max_squared = math::sq(_lim_thr_max) - math::sq(allocated_horizontal_thrust);
+
 	// Saturate maximal vertical thrust
-	_thr_sp(2) = math::max(_thr_sp(2), -_lim_thr_max);
+	_thr_sp(2) = math::max(_thr_sp(2), -sqrtf(thrust_z_max_squared));
 
-	// Get allowed horizontal thrust after prioritizing vertical control
-	const float thrust_max_squared = _lim_thr_max * _lim_thr_max;
-	const float thrust_z_squared = _thr_sp(2) * _thr_sp(2);
-	const float thrust_max_xy_squared = thrust_max_squared - thrust_z_squared;
+	// Determine how much horizontal thrust is left after prioritizing vertical control
+	const float thrust_max_xy_squared = thrust_z_max_squared - math::sq(_thr_sp(2));
 	float thrust_max_xy = 0;
 
 	if (thrust_max_xy_squared > 0) {
@@ -151,9 +162,6 @@ void PositionControl::_velocityControl(const float dt)
 	}
 
 	// Saturate thrust in horizontal direction
-	const Vector2f thrust_sp_xy(_thr_sp);
-	const float thrust_sp_xy_norm = thrust_sp_xy.norm();
-
 	if (thrust_sp_xy_norm > thrust_max_xy) {
 		_thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy;
 	}

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -107,6 +107,12 @@ public:
 	 */
 	void setThrustLimits(const float min, const float max);
 
+	/**
+	 * Set margin that is kept for horizontal control when prioritizing vertical thrust
+	 * @param margin of normalized thrust that is kept for horizontal control e.g. 0.3
+	 */
+	void setHorizontalThrustMargin(const float margin);
+
 	/**
 	 * Set the maximum tilt angle in radians the output attitude is allowed to have
 	 * @param tilt angle in radians from level orientation
@@ -191,6 +197,7 @@ private:
 	float _lim_vel_down{}; ///< Downwards velocity limit with feed forward and position control
 	float _lim_thr_min{}; ///< Minimum collective thrust allowed as output [-1,0] e.g. -0.9
 	float _lim_thr_max{}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1
+	float _lim_thr_xy_margin{}; ///< Margin to keep for horizontal control when saturating prioritized vertical thrust
 	float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have
 
 	float _hover_thrust{}; ///< Thrust [0.1, 0.9] with which the vehicle hovers not accelerating down or up with level orientation

src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp

@@ -79,6 +79,7 @@ public:
 		_position_control.setVelocityGains(Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f));
 		_position_control.setVelocityLimits(1.f, 1.f, 1.f);
 		_position_control.setThrustLimits(0.1f, 0.9f);
+		_position_control.setHorizontalThrustMargin(0.3f);
 		_position_control.setTiltLimit(1.f);
 		_position_control.setHoverThrust(.5f);
 
@@ -193,11 +194,13 @@ TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit)
 	Vector3f thrust(_output_setpoint.thrust);
 	EXPECT_FLOAT_EQ(thrust(0), 0.f);
 	EXPECT_FLOAT_EQ(thrust(1), 0.f);
-	EXPECT_FLOAT_EQ(thrust(2), -0.9f);
+	// Expect the remaining vertical thrust after allocating the horizontal margin
+	// sqrt(0.9^2 - 0.3^2) = 0.8485
+	EXPECT_FLOAT_EQ(thrust(2), -0.848528137423857f);
 
 	EXPECT_EQ(_attitude.thrust_body[0], 0.f);
 	EXPECT_EQ(_attitude.thrust_body[1], 0.f);
-	EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.9f);
+	EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.848528137423857f);
 
 	EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f);
 	EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f);

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -39,9 +39,10 @@
  */
 
 /**
- * Minimum thrust in auto thrust control
+ * Minimum collective thrust in auto thrust control
  *
  * It's recommended to set it > 0 to avoid free fall with zero thrust.
+ * Note: Without airmode zero thrust leads to zero roll/pitch control authority. (see MC_AIRMODE)
  *
  * @unit norm
  * @min 0.05
@@ -106,6 +107,21 @@ PARAM_DEFINE_INT32(MPC_USE_HTE, 1);
  */
 PARAM_DEFINE_INT32(MPC_THR_CURVE, 0);
 
+/**
+ * Horizontal thrust margin
+ *
+ * Margin that is kept for horizontal control when prioritizing vertical thrust.
+ * To avoid completely starving horizontal control with high vertical error.
+ *
+ * @unit norm
+ * @min 0.0
+ * @max 0.5
+ * @decimal 2
+ * @increment 0.01
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_THR_XY_MARG, 0.3f);
+
 /**
  * Maximum thrust in auto thrust control
  *