From 5ea5ecf32bfd4b19d8babe94d1ecc02369df3e83 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 Jan 2016 12:35:27 +0100 Subject: [PATCH] Limit manual yaw command properly. Fixes #3600 --- .../mc_pos_control/mc_pos_control_main.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) 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 966a930c2c..f322924e34 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -185,6 +185,7 @@ private: param_t man_roll_max; param_t man_pitch_max; param_t man_yaw_max; + param_t global_yaw_max; param_t mc_att_yaw_p; param_t hold_xy_dz; param_t hold_z_dz; @@ -203,6 +204,7 @@ private: float man_roll_max; float man_pitch_max; float man_yaw_max; + float global_yaw_max; float mc_att_yaw_p; float hold_xy_dz; float hold_z_dz; @@ -432,6 +434,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.man_roll_max = param_find("MPC_MAN_R_MAX"); _params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX"); _params_handles.man_yaw_max = param_find("MPC_MAN_Y_MAX"); + _params_handles.global_yaw_max = param_find("MC_YAWRATE_MAX"); _params_handles.mc_att_yaw_p = param_find("MC_YAW_P"); _params_handles.hold_xy_dz = param_find("MPC_HOLD_XY_DZ"); _params_handles.hold_z_dz = param_find("MPC_HOLD_Z_DZ"); @@ -547,9 +550,12 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.man_roll_max, &_params.man_roll_max); param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); + param_get(_params_handles.global_yaw_max, &_params.global_yaw_max); _params.man_roll_max = math::radians(_params.man_roll_max); _params.man_pitch_max = math::radians(_params.man_pitch_max); _params.man_yaw_max = math::radians(_params.man_yaw_max); + _params.global_yaw_max = math::radians(_params.global_yaw_max); + param_get(_params_handles.mc_att_yaw_p, &v); _params.mc_att_yaw_p = v; } @@ -1766,14 +1772,19 @@ MulticopterPositionControl::task_main() /* do not move yaw while sitting on the ground */ else if (!_vehicle_status.condition_landed && !(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) { - const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p; - _att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max; + /* we want to know the real constraint, and global overrides manual */ + const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max : _params.global_yaw_max; + const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p; + + _att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max; float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); float yaw_offs = _wrap_pi(yaw_target - _yaw); // If the yaw offset became too big for the system to track stop // shifting it + + // XXX this needs inspection - probably requires a clamp, not an if if (fabsf(yaw_offs) < yaw_offset_max) { _att_sp.yaw_body = yaw_target; }