MulticopterPositionControl: add horizontal margin for saturation cases

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;
 	}
 
-	// Saturate maximal vertical thrust
-	_thr_sp(2) = math::max(_thr_sp(2), -_lim_thr_max);
+	// 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();
 
-	// 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 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), -sqrtf(thrust_z_max_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);