diff options
| author | Mark Whitehorn <kd0aij@gmail.com> | 2015-03-16 10:14:07 -0600 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-18 11:19:06 +0200 |
| commit | a07712bb1e41538de72686562c8295f2688f6880 (patch) | |
| tree | ac2d06f14d4b86f41e236f419c47d8244b183ddd | |
| parent | 9ac350a7d1c5c07a4e4ba7824744930f9110dedc (diff) | |
| download | px4-firmware-a07712bb1e41538de72686562c8295f2688f6880.tar.gz px4-firmware-a07712bb1e41538de72686562c8295f2688f6880.tar.bz2 px4-firmware-a07712bb1e41538de72686562c8295f2688f6880.zip | |
apply roll/pitch acro_rate_max in MC attitude controller
| -rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 9 |
1 files changed, 6 insertions, 3 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 d47731622..6c9048bdb 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -656,11 +656,14 @@ MulticopterAttitudeControl::control_attitude(float dt) /* calculate angular rates setpoint */ _rates_sp = _params.att_p.emult(e_R); - /* 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)); + /* limit roll and pitch rates */ + for (int i = 0; i < 2; i++) { + _rates_sp(i) = math::constrain(_rates_sp(i), -_params.acro_rate_max(i), _params.acro_rate_max(i)); } + /* 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) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff; } |