diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index c06259b74c..1d6e9609fc 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -724,32 +724,26 @@ MulticopterAttitudeControl::control_attitude(float dt) /* limit rates */ for (int i = 0; i < 3; i++) { - if (_v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) { + if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) && + !_v_control_mode.flag_control_manual_enabled) { _rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i)); + } else { _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); } } - /* weather-vane mode, dampen yaw rate */ - if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) { - float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale; - _rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max); - // prevent integrator winding up in weathervane mode - _rates_int(2) = 0.0f; - } - /* feed forward yaw setpoint rate */ _rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff; - /* weather-vane mode, scale down yaw rate */ - if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) { + /* weather-vane mode, dampen yaw rate */ + if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) && + _v_att_sp.disable_mc_yaw_control == true && !_v_control_mode.flag_control_manual_enabled) { float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale; _rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max); // prevent integrator winding up in weathervane mode _rates_int(2) = 0.0f; } - } /*