fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 12:23:33 +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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+7 -7
View File
@@ -1392,16 +1392,16 @@ MulticopterPositionControl::task_main()
// do not move yaw while arming // do not move yaw while arming
else if (_manual.z > 0.1f) 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_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_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(_att_sp.yaw_body - _att.yaw); float yaw_offs = _wrap_pi(yaw_target - _att.yaw);
if (yaw_offs < - YAW_OFFSET_MAX) {
_att_sp.yaw_body = _wrap_pi(_att.yaw - YAW_OFFSET_MAX);
} else if (yaw_offs > YAW_OFFSET_MAX) { // If the yaw offset became too big for the system to track stop
_att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX); // shifting it
if (fabsf(yaw_offs) < yaw_offset_max) {
_att_sp.yaw_body = yaw_target;
} }
} }