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

mc_att_control: limit max yaw setpoint offset

This commit is contained in:
Anton Babushkin 2014-05-14 14:55:14 +02:00
parent c2f825647e
commit 42a7d80a81
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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;
}