diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-05-03 19:19:57 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-05-03 19:19:57 +0200 |
commit | 75796e95b4232f961b7ccc9e8c8a76433b389eae (patch) | |
tree | 56e68cf08772c220f8c56a107cdcfe6afc73d73b | |
parent | 55cae08cf37c2a67f1ef357f205d2ee5e3208655 (diff) | |
download | px4-firmware-75796e95b4232f961b7ccc9e8c8a76433b389eae.tar.gz px4-firmware-75796e95b4232f961b7ccc9e8c8a76433b389eae.tar.bz2 px4-firmware-75796e95b4232f961b7ccc9e8c8a76433b389eae.zip |
mc_att_control: MC_YAWRATE_MAX parameter added
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 23 | ||||
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_params.c | 12 |
2 files changed, 29 insertions, 6 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 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 <systemlib/err.h> #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> -#include <mathlib/mathlib.h> +#include <lib/mathlib/mathlib.h> #include <lib/geo/geo.h> /** @@ -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 @@ -175,6 +175,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 * * @unit deg |