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 fc7861b796..d6ec292f6d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -241,6 +241,7 @@ private: math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */ float _yaw; /**< yaw angle (euler) */ + float _landing_thrust; /** * Update our local parameter cache. @@ -370,7 +371,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _alt_hold_engaged(false), _run_pos_control(true), _run_alt_control(true), - _yaw(0.0f) + _yaw(0.0f), + _landing_thrust(1.0f) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_ctrl_state, 0, sizeof(_ctrl_state)); @@ -1421,6 +1423,7 @@ MulticopterPositionControl::task_main() } float tilt_max = _params.tilt_max_air; + float thr_max = _params.thr_max; /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && @@ -1431,6 +1434,15 @@ MulticopterPositionControl::task_main() if (thr_min < 0.0f) { thr_min = 0.0f; } + + /* don't let it throttle up again during landing */ + if (thrust_sp(2) < 0.0f && thrust_sp(2) < _landing_thrust) { + _landing_thrust = thrust_sp(2); + } + thr_max = -_landing_thrust; + + } else { + _landing_thrust = _params.thr_max; } /* limit min lift */ @@ -1482,19 +1494,19 @@ MulticopterPositionControl::task_main() /* limit max thrust */ float thrust_abs = thrust_sp.length(); - if (thrust_abs > _params.thr_max) { + if (thrust_abs > thr_max) { if (thrust_sp(2) < 0.0f) { - if (-thrust_sp(2) > _params.thr_max) { + if (-thrust_sp(2) > thr_max) { /* thrust Z component is too large, limit it */ thrust_sp(0) = 0.0f; thrust_sp(1) = 0.0f; - thrust_sp(2) = -_params.thr_max; + thrust_sp(2) = -thr_max; saturation_xy = true; saturation_z = true; } else { /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ - float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); + float thrust_xy_max = sqrtf(thr_max * thr_max - thrust_sp(2) * thrust_sp(2)); float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); float k = thrust_xy_max / thrust_xy_abs; thrust_sp(0) *= k; @@ -1504,13 +1516,13 @@ MulticopterPositionControl::task_main() } else { /* Z component is negative, going down, simply limit thrust vector */ - float k = _params.thr_max / thrust_abs; + float k = thr_max / thrust_abs; thrust_sp *= k; saturation_xy = true; saturation_z = true; } - thrust_abs = _params.thr_max; + thrust_abs = thr_max; } /* update integrals */