diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-05-14 14:55:14 +0200 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-05-14 14:55:14 +0200 |
| commit | 42a7d80a8119bd583e40ffda957d195fd7e81b9d (patch) | |
| tree | 522fba2b9a9daa9e99f94940e4bb87dd889bcc67 /src/modules/mc_att_control/mc_att_control_main.cpp | |
| parent | c2f825647e8fd2aa4bf7a37399203c6071239765 (diff) | |
| download | px4-firmware-42a7d80a8119bd583e40ffda957d195fd7e81b9d.tar.gz px4-firmware-42a7d80a8119bd583e40ffda957d195fd7e81b9d.tar.bz2 px4-firmware-42a7d80a8119bd583e40ffda957d195fd7e81b9d.zip | |
mc_att_control: limit max yaw setpoint offset
Diffstat (limited to 'src/modules/mc_att_control/mc_att_control_main.cpp')
| -rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 8 |
1 files changed, 8 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 a69153bf0..e74546f11 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -506,6 +506,14 @@ MulticopterAttitudeControl::control_attitude(float dt) /* move yaw setpoint */ yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); + float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); + float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw); + if (yaw_offs < - yaw_offs_max) { + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs); + + } else if (yaw_offs > yaw_offs_max) { + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs); + } _v_att_sp.R_valid = false; publish_att_sp = true; } |