dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_att_control: yaw offset limiting bug fixed

This commit is contained in:
Anton Babushkin 2014-05-15 10:22:42 +02:00
parent 42a7d80a81
commit 8e43db7bc0
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -509,10 +509,10 @@ MulticopterAttitudeControl::control_attitude(float 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);
_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
} else if (yaw_offs > yaw_offs_max) {
_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs);
_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
}
_v_att_sp.R_valid = false;
publish_att_sp = true;