fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

MC pos control: Fix manual yaw handling to not reset yaw in extreme angle conditions

Browse Source
This commit is contained in:
Lorenz Meier 2015-06-27 10:17:15 +02:00
parent 572f1f4637
commit 0fe01f6eb8
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;
}
}