yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #958 from PX4/yaw_offset_limit

Browse Source 
Multirotors: yaw setpoint offset limit
This commit is contained in:
Lorenz Meier
2014-05-15 05:33:08 -07:00
parent a4c4080d63 7ef3c24637
commit c72ec3fd5f
1 changed files with 8 additions and 0 deletions
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

@@ -507,6 +507,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_max);
} else if (yaw_offs > yaw_offs_max) {
_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
}
_v_att_sp.R_valid = false;
publish_att_sp = true;
}