diff --git a/src/lib/mathlib/math/Functions.hpp b/src/lib/mathlib/math/Functions.hpp index dafa172701..d3ac322a23 100644 --- a/src/lib/mathlib/math/Functions.hpp +++ b/src/lib/mathlib/math/Functions.hpp @@ -54,6 +54,12 @@ int signNoZero(T val) return (T(0) <= val) - (val < T(0)); } +template +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. diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 9d9d6a63aa..5308c87f3e 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/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()) { diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 9d5f11be36..8cb56f6c7b 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -161,6 +161,7 @@ private: (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ (ParamFloat) _param_mpc_thr_min, (ParamFloat) _param_mpc_thr_max, + (ParamFloat) _param_mpc_thr_xy_marg, (ParamFloat) _param_sys_vehicle_resp, (ParamFloat) _param_mpc_acc_hor, diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index fa36896cb6..c370668c4c 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/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; } diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index c486499e54..996c662120 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/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 diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index f2bb5cf8e1..3c98d5da85 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/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); diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 46c5fb3946..3cb28c0b3b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/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 *