diff options
| -rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 4 |
1 files changed, 2 insertions, 2 deletions
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index e74546f11..ee457c120 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -509,10 +509,10 @@ MulticopterAttitudeControl::control_attitude(float 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); + _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); + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); } _v_att_sp.R_valid = false; publish_att_sp = true; |