From 7455a833d02a82bd96b037b73a6845f268c680fe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 30 Jan 2016 15:03:20 +0100 Subject: [PATCH] MC: Split yaw speed limiting between manual and velocity control modes --- .../mc_att_control/mc_att_control_main.cpp | 20 +++++++++++++++++-- .../mc_att_control/mc_att_control_params.c | 16 +++++++++++++-- 2 files changed, 32 insertions(+), 4 deletions(-) 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 27901dcdff..9668747301 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -190,6 +190,7 @@ private: param_t roll_rate_max; param_t pitch_rate_max; param_t yaw_rate_max; + param_t yaw_auto_max; param_t acro_roll_max; param_t acro_pitch_max; @@ -214,8 +215,9 @@ private: float roll_rate_max; float pitch_rate_max; float yaw_rate_max; + float yaw_auto_max; math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */ - + math::Vector<3> auto_rate_max; /**< attitude rate limits in auto modes */ math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ float rattitude_thres; int vtol_type; /**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */ @@ -347,6 +349,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.pitch_rate_max = 0.0f; _params.yaw_rate_max = 0.0f; _params.mc_rate_max.zero(); + _params.auto_rate_max.zero(); _params.acro_rate_max.zero(); _params.rattitude_thres = 1.0f; _params.vtol_opt_recovery_enabled = false; @@ -379,6 +382,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX"); _params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX"); _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); + _params_handles.yaw_auto_max = param_find("MC_YAWRAUTO_MAX"); _params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX"); _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); @@ -482,6 +486,14 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); _params.mc_rate_max(2) = math::radians(_params.yaw_rate_max); + /* auto angular rate limits */ + param_get(_params_handles.roll_rate_max, &_params.roll_rate_max); + _params.auto_rate_max(0) = math::radians(_params.roll_rate_max); + param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max); + _params.auto_rate_max(1) = math::radians(_params.pitch_rate_max); + param_get(_params_handles.yaw_auto_max, &_params.yaw_auto_max); + _params.auto_rate_max(2) = math::radians(_params.yaw_auto_max); + /* manual rate control scale and auto mode roll/pitch rate limits */ param_get(_params_handles.acro_roll_max, &v); _params.acro_rate_max(0) = math::radians(v); @@ -705,7 +717,11 @@ MulticopterAttitudeControl::control_attitude(float dt) /* limit rates */ for (int i = 0; i < 3; i++) { - _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); + if (_v_control_mode.flag_control_velocity_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)); + } } /* feed forward yaw setpoint rate */ diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index a6e64d49a2..77e990ee2b 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -251,6 +251,18 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 220.0f); /** * Max yaw rate * + * A value of significantly over 120 degrees per second can already lead to mixer saturation. + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f); + +/** + * Max yaw rate in auto mode + * * Limit for yaw rate, has effect for large rotations in autonomous mode, * to avoid large control output and mixer saturation. A value of significantly * over 60 degrees per second can already lead to mixer saturation. @@ -258,10 +270,10 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 220.0f); * * @unit deg/s * @min 0.0 - * @max 360.0 + * @max 120.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 45.0f); +PARAM_DEFINE_FLOAT(MC_YAWRAUTO_MAX, 45.0f); /** * Max acro roll rate