From 76aa0441050596a08e5a523f94ae13f43ed39b07 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 16 Mar 2018 14:48:34 +0100 Subject: [PATCH] navigator mission: add yaw offset to vehicle's yaw for ROI cmds and disabled gimbal yaw --- src/modules/navigator/mission.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index bfe70f123d..688625316f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1058,6 +1058,14 @@ Mission::heading_sp_update() _mission_item.yaw = _wrap_pi(yaw + M_PI_F); pos_sp_triplet->current.yaw = _mission_item.yaw; + } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT + && _navigator->get_vroi().mode == vehicle_roi_s::ROI_WPNEXT && !_param_mnt_yaw_ctl.get()) { + /* if yaw control for the mount is disabled and we have a valid ROI that points to the next + * waypoint, we add the gimbal's yaw offset to the vehicle's yaw */ + yaw += _navigator->get_vroi().yaw_offset; + _mission_item.yaw = yaw; + pos_sp_triplet->current.yaw = _mission_item.yaw; + } else { _mission_item.yaw = yaw; pos_sp_triplet->current.yaw = _mission_item.yaw;