diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 90ff162b39..ddbd24c9b7 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -246,15 +246,23 @@ Mission::on_active()
 	}
 
 	/* see if we need to update the current yaw heading */
-	if (_navigator->get_vroi().mode == vehicle_roi_s::ROI_LOCATION
-	    || (_param_yawmode.get() != MISSION_YAWMODE_NONE
-		&& _param_yawmode.get() < MISSION_YAWMODE_MAX
-		&& _mission_type != MISSION_TYPE_NONE)
-	    || _navigator->get_vstatus()->is_vtol) {
-
+	if (!_param_mnt_yaw_ctl.get() && (_navigator->get_vstatus()->is_rotary_wing)
+	    && (_navigator->get_vroi().mode != vehicle_roi_s::ROI_NONE)
+	    && !(_mission_item.nav_cmd == NAV_CMD_TAKEOFF
+		 || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
+		 || _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
+		 || _mission_item.nav_cmd == NAV_CMD_LAND
+		 || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
+		 || _work_item_type == WORK_ITEM_TYPE_ALIGN)) {
+		// Mount control is disabled If the vehicle is in ROI-mode, the vehicle
+		// needs to rotate such that ROI is in the field of view.
+		// ROI only makes sense for multicopters.
 		heading_sp_update();
 	}
 
+	// TODO: Add vtol heading update method if required.
+	// Question: Why does vtol ever have to update heading?
+
 	/* check if landing needs to be aborted */
 	if ((_mission_item.nav_cmd == NAV_CMD_LAND)
 	    && (_navigator->abort_landing())) {
@@ -1170,112 +1178,67 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item)
 void
 Mission::heading_sp_update()
 {
-	/* we don't want to be yawing during takeoff, landing or aligning for a transition */
-	if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
-	    || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
-	    || _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
-	    || _mission_item.nav_cmd == NAV_CMD_LAND
-	    || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
-	    || _work_item_type == WORK_ITEM_TYPE_ALIGN) {
-
-		return;
-	}
-
-	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
-	/* Don't change setpoint if last and current waypoint are not valid */
-	if (!pos_sp_triplet->current.valid) {
-		return;
-	}
-
-	/* Calculate direction the vehicle should point to. */
-
-	double point_from_latlon[2];
-	double point_to_latlon[2];
+	struct position_setpoint_triplet_s *pos_sp_triplet =
+		_navigator->get_position_setpoint_triplet();
+
+	// Only update if current triplet is valid
+	if (pos_sp_triplet->current.valid) {
+
+		double point_from_latlon[2] = { _navigator->get_global_position()->lat,
+						_navigator->get_global_position()->lon
+					      };
+		double point_to_latlon[2] = { _navigator->get_global_position()->lat,
+					      _navigator->get_global_position()->lon
+					    };
+		float yaw_offset = 0.0f;
+
+		// Depending on ROI-mode, update heading
+		switch (_navigator->get_vroi().mode) {
+		case vehicle_roi_s::ROI_LOCATION: {
+				// ROI is a fixed location. Vehicle needs to point towards that location
+				point_to_latlon[0] = _navigator->get_vroi().lat;
+				point_to_latlon[1] = _navigator->get_vroi().lon;
+				// No yaw offset required
+				yaw_offset = 0.0f;
+				break;
+			}
 
-	point_from_latlon[0] = _navigator->get_global_position()->lat;
-	point_from_latlon[1] = _navigator->get_global_position()->lon;
+		case vehicle_roi_s::ROI_WPNEXT: {
+				// ROI is current waypoint. Vehcile needs to point towards current waypoint
+				point_to_latlon[0] = pos_sp_triplet->current.lat;
+				point_to_latlon[1] = pos_sp_triplet->current.lon;
+				// Add the gimbal's yaw offset
+				yaw_offset = _navigator->get_vroi().yaw_offset;
+				break;
+			}
 
-	if (_navigator->get_vroi().mode == vehicle_roi_s::ROI_LOCATION && !_param_mnt_yaw_ctl.get()) {
-		point_to_latlon[0] = _navigator->get_vroi().lat;
-		point_to_latlon[1] = _navigator->get_vroi().lon;
+		case vehicle_roi_s::ROI_NONE:
+		case vehicle_roi_s::ROI_WPINDEX:
+		case vehicle_roi_s::ROI_TARGET:
+		case vehicle_roi_s::ROI_ENUM_END:
+		default: {
+			}
+		}
 
-		/* stop if positions are close together to prevent excessive yawing */
-		float d_current = get_distance_to_next_waypoint(
-					  point_from_latlon[0], point_from_latlon[1],
-					  point_to_latlon[0], point_to_latlon[1]);
+		// Get desired heading and update it.
+		// However, only update if distance to desired heading is
+		// larger than acceptance radius to prevent excessive yawing
+		float d_current = get_distance_to_next_waypoint(point_from_latlon[0],
+				  point_from_latlon[1], point_to_latlon[0], point_to_latlon[1]);
 
 		if (d_current > _navigator->get_acceptance_radius()) {
-			float yaw = get_bearing_to_next_waypoint(
-					    point_from_latlon[0], point_from_latlon[1],
-					    point_to_latlon[0], point_to_latlon[1]);
+			float yaw = _wrap_pi(
+					    get_bearing_to_next_waypoint(point_from_latlon[0],
+							    point_from_latlon[1], point_to_latlon[0],
+							    point_to_latlon[1]) + yaw_offset);
 
 			_mission_item.yaw = yaw;
 			pos_sp_triplet->current.yaw = _mission_item.yaw;
 		}
 
-	} else {
-		/* set yaw angle for the waypoint if a loiter time has been specified */
-		if (_waypoint_position_reached && get_time_inside(_mission_item) > FLT_EPSILON) {
-			// XXX: should actually be param4 from mission item
-			// at the moment it will just keep the heading it has
-			//_mission_item.yaw = _on_arrival_yaw;
-			//pos_sp_triplet->current.yaw = _mission_item.yaw;
-
-		} else {
-			/* target location is home */
-			if ((_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME
-			     || _param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME)
-			    // need to be rotary wing for this but not in a transition
-			    // in VTOL mode this will prevent updating yaw during FW flight
-			    // (which would result in a wrong yaw setpoint spike during back transition)
-			    && _navigator->get_vstatus()->is_rotary_wing
-			    && !(_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION || _navigator->get_vstatus()->in_transition_mode)) {
-
-				point_to_latlon[0] = _navigator->get_home_position()->lat;
-				point_to_latlon[1] = _navigator->get_home_position()->lon;
-
-			} else {
-				/* target location is next (current) waypoint */
-				point_to_latlon[0] = pos_sp_triplet->current.lat;
-				point_to_latlon[1] = pos_sp_triplet->current.lon;
-			}
-
-			float d_current = get_distance_to_next_waypoint(
-						  point_from_latlon[0], point_from_latlon[1],
-						  point_to_latlon[0], point_to_latlon[1]);
-
-			/* stop if positions are close together to prevent excessive yawing */
-			if (d_current > _navigator->get_acceptance_radius()) {
-				float yaw = get_bearing_to_next_waypoint(
-						    point_from_latlon[0],
-						    point_from_latlon[1],
-						    point_to_latlon[0],
-						    point_to_latlon[1]);
-
-				/* always keep the back of the rotary wing pointing towards home */
-				if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
-					_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;
-				}
-			}
-		}
+		// we set yaw directly so we can run this in parallel to the FOH update
+		_navigator->set_position_setpoint_triplet_updated();
 	}
-
-	// we set yaw directly so we can run this in parallel to the FOH update
-	_navigator->set_position_setpoint_triplet_updated();
 }
 
 void