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

Merge pull request #2481 from PX4/mc_yawfix 

MC yawfix
This commit is contained in:
Lorenz Meier 2015-06-28 23:29:57 +02:00
parent abc069dc13 0fe01f6eb8
commit b72ac1428c
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -1392,16 +1392,16 @@ MulticopterPositionControl::task_main()
/* do not move yaw while arming */
else if (_manual.z > 0.1f)
{
const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p;
const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p;
_att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(_att_sp.yaw_body - _att.yaw);
if (yaw_offs < - YAW_OFFSET_MAX) {
_att_sp.yaw_body = _wrap_pi(_att.yaw - YAW_OFFSET_MAX);
float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(yaw_target - _att.yaw);
} else if (yaw_offs > YAW_OFFSET_MAX) {
_att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX);
// If the yaw offset became too big for the system to track stop
// shifting it
if (fabsf(yaw_offs) < yaw_offset_max) {
_att_sp.yaw_body = yaw_target;
}
}