From 75796e95b4232f961b7ccc9e8c8a76433b389eae Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 3 May 2014 19:19:57 +0200 Subject: mc_att_control: MC_YAWRATE_MAX parameter added --- src/modules/mc_att_control/mc_att_control_main.cpp | 23 ++++++++++++++++------ src/modules/mc_att_control/mc_att_control_params.c | 12 +++++++++++ 2 files changed, 29 insertions(+), 6 deletions(-) (limited to 'src/modules/mc_att_control') 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 36d95bf06..900c730e1 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -71,7 +71,7 @@ #include #include #include -#include +#include #include /** @@ -156,6 +156,7 @@ private: param_t yaw_rate_i; param_t yaw_rate_d; param_t yaw_ff; + param_t yaw_rate_max; param_t man_roll_max; param_t man_pitch_max; @@ -168,6 +169,7 @@ private: math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ float yaw_ff; /**< yaw control feed-forward */ + float yaw_rate_max; /**< max yaw rate */ float man_roll_max; float man_pitch_max; @@ -276,6 +278,11 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); + _params.yaw_ff = 0.0f; + _params.yaw_rate_max = 0.0f; + _params.man_roll_max = 0.0f; + _params.man_pitch_max = 0.0f; + _params.man_yaw_max = 0.0f; _rates_prev.zero(); _rates_sp.zero(); @@ -298,7 +305,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); - + _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); @@ -367,15 +374,16 @@ MulticopterAttitudeControl::parameters_update() _params.rate_d(2) = v; param_get(_params_handles.yaw_ff, &_params.yaw_ff); + param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); + _params.yaw_rate_max = math::radians(_params.yaw_rate_max); /* manual control scale */ 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); - - _params.man_roll_max *= M_PI / 180.0; - _params.man_pitch_max *= M_PI / 180.0; - _params.man_yaw_max *= M_PI / 180.0; + _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); return OK; } @@ -626,6 +634,9 @@ MulticopterAttitudeControl::control_attitude(float dt) /* calculate angular rates setpoint */ _rates_sp = _params.att_p.emult(e_R); + /* limit yaw rate */ + _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max); + /* feed forward yaw setpoint rate */ _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; } 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 e52c39368..19134c7b4 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -174,6 +174,18 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); */ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); +/** + * Max yaw rate + * + * Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f); + /** * Max manual roll * -- cgit v1.2.3