navigator mission: add yaw offset to vehicle's yaw for ROI cmds and disabled gimbal yaw

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;