diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index ca8360c804..9092892183 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -167,6 +167,7 @@ void MulticopterPositionControl::parameters_update(bool force) 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()); + _control.decoupleHorizontalAndVecticalAcceleration(_param_mpc_acc_decouple.get()); _goto_control.setParamMpcAccHor(_param_mpc_acc_hor.get()); _goto_control.setParamMpcAccDownMax(_param_mpc_acc_down_max.get()); _goto_control.setParamMpcAccUpMax(_param_mpc_acc_up_max.get()); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index b1f88fde57..3dac59bc1b 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -146,6 +146,7 @@ private: (ParamFloat) _param_mpc_tiltmax_air, (ParamFloat) _param_mpc_thr_hover, (ParamBool) _param_mpc_use_hte, + (ParamBool) _param_mpc_acc_decouple, // Takeoff / Land (ParamFloat) _param_com_spoolup_time, /**< time to let motors spool up after arming */ diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 955f3f4b44..0cf632c043 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -204,13 +204,20 @@ void PositionControl::_velocityControl(const float dt) void PositionControl::_accelerationControl() { // Assume standard acceleration due to gravity in vertical direction for attitude generation - Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized(); + float z_specific_force = -CONSTANTS_ONE_G; + + if (!_decouple_horizontal_and_vertical_acceleration) { + // Include vertical acceleration setpoint for better horizontal acceleration tracking + z_specific_force += _acc_sp(2); + } + + Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), -z_specific_force).normalized(); ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt); - // Scale thrust assuming hover thrust produces standard gravity - float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust; + // Convert to thrust assuming hover thrust produces standard gravity + const float thrust_ned_z = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust; // Project thrust to planned body attitude - collective_thrust /= (Vector3f(0, 0, 1).dot(body_z)); - collective_thrust = math::min(collective_thrust, -_lim_thr_min); + const float cos_ned_body = (Vector3f(0, 0, 1).dot(body_z)); + const float collective_thrust = math::min(thrust_ned_z / cos_ned_body, -_lim_thr_min); _thr_sp = body_z * collective_thrust; } diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index 4eb909e1fa..7575409bbc 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -163,6 +163,11 @@ public: */ void resetIntegral() { _vel_int.setZero(); } + /** + * If set, the tilt setpoint is computed by assuming no vertical acceleration + */ + void decoupleHorizontalAndVecticalAcceleration(bool val) { _decouple_horizontal_and_vertical_acceleration = val; } + /** * Get the controllers output local position setpoint * These setpoints are the ones which were executed on including PID output and feed-forward. @@ -211,6 +216,7 @@ private: float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have float _hover_thrust{}; ///< Thrust [HOVER_THRUST_MIN, HOVER_THRUST_MAX] with which the vehicle hovers not accelerating down or up with level orientation + bool _decouple_horizontal_and_vertical_acceleration{false}; ///< Ignore vertical acceleration setpoint to remove its effect on the tilt setpoint // States matrix::Vector3f _pos; /**< current position */ diff --git a/src/modules/mc_pos_control/multicopter_position_control_limits_params.c b/src/modules/mc_pos_control/multicopter_position_control_limits_params.c index a60732cfa8..91039f04ea 100644 --- a/src/modules/mc_pos_control/multicopter_position_control_limits_params.c +++ b/src/modules/mc_pos_control/multicopter_position_control_limits_params.c @@ -138,3 +138,13 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.f); + +/** + * Acceleration to tilt coupling + * + * Set to decouple tilt from vertical acceleration. + * + * @boolean + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_ACC_DECOUPLE, 1);