diff options
| author | Mark Whitehorn <kd0aij@gmail.com> | 2015-03-16 10:14:07 -0600 |
|---|---|---|
| committer | Mark Whitehorn <kd0aij@gmail.com> | 2015-04-11 08:04:37 -0600 |
| commit | 6b26594697a305477b81aefa4788e1521de63bc5 (patch) | |
| tree | a7d39785a09c5c7ae8e24fadc00cfb32ac1f2b94 | |
| parent | d4ae721bc0a96509cfbdb004bbe302694e143ee7 (diff) | |
| download | px4-firmware-6b26594697a305477b81aefa4788e1521de63bc5.tar.gz px4-firmware-6b26594697a305477b81aefa4788e1521de63bc5.tar.bz2 px4-firmware-6b26594697a305477b81aefa4788e1521de63bc5.zip | |
apply roll/pitch acro_rate_max in MC attitude controller
| -rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 5 |
1 files changed, 5 insertions, 0 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 85a0dda9e..987fb0314 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -639,6 +639,11 @@ MulticopterAttitudeControl::control_attitude(float dt) /* calculate angular rates setpoint */ _rates_sp = _params.att_p.emult(e_R); + /* 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); |