diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 8aa4f2d8fc..f5e850381b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -199,6 +199,8 @@ private: /** During smooth-takeoff, below ALTITUDE_THRESHOLD the yaw-control is turned off ant tilt is limited */ static constexpr float ALTITUDE_THRESHOLD = 0.3f; + static constexpr float MAX_SAFE_TILT_DEG = 89.f; // Numerical issues above this value due to tanf + systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */ WeatherVane *_wv_controller{nullptr}; @@ -343,6 +345,18 @@ MulticopterPositionControl::parameters_update(bool force) ModuleParams::updateParams(); SuperBlock::updateParams(); + if (_param_mpc_tiltmax_air.get() > MAX_SAFE_TILT_DEG) { + _param_mpc_tiltmax_air.set(MAX_SAFE_TILT_DEG); + _param_mpc_tiltmax_air.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Tilt constrained to safe value"); + } + + if (_param_mpc_tiltmax_lnd.get() > _param_mpc_tiltmax_air.get()) { + _param_mpc_tiltmax_lnd.set(_param_mpc_tiltmax_air.get()); + _param_mpc_tiltmax_lnd.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Land tilt has been constrained by max tilt"); + } + _control.setPositionGains(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(), _param_mpc_z_p.get())); _control.setVelocityGains(Vector3f(_param_mpc_xy_vel_p.get(), _param_mpc_xy_vel_p.get(), _param_mpc_z_vel_p.get()), Vector3f(_param_mpc_xy_vel_i.get(), _param_mpc_xy_vel_i.get(), _param_mpc_z_vel_i.get()), 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 d6dfc661f1..ead42d1cbb 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -306,7 +306,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 12.0f); * * @unit deg * @min 20.0 - * @max 180.0 + * @max 89.0 * @decimal 1 * @group Multicopter Position Control */ @@ -319,7 +319,7 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f); * * @unit deg * @min 10.0 - * @max 90.0 + * @max 89.0 * @decimal 1 * @group Multicopter Position Control */