From 0fe01f6eb8f63a1487f1b6c19d193a1456ebf3b0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Jun 2015 10:17:15 +0200 Subject: [PATCH] MC pos control: Fix manual yaw handling to not reset yaw in extreme angle conditions --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index ccae79c11d..9f275cd8fe 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -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; } }