diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 7dc6408b1d..78f6614112 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -458,8 +458,8 @@ MissionBlock::is_mission_item_reached_or_completed()
 			time_inside_reached = true;
 		}
 
-		// check if heading for exit is reached (only applies for fixed-wing flight)
-		bool exit_heading_reached = false;
+		// check if course for exit is reached (only applies for fixed-wing flight)
+		bool exit_course_reached = false;
 
 		if (time_inside_reached) {
 
@@ -469,37 +469,34 @@ MissionBlock::is_mission_item_reached_or_completed()
 			const float dist_current_next = get_distance_to_next_waypoint(curr_sp_new->lat, curr_sp_new->lon, next_sp.lat,
 							next_sp.lon);
 
-			/* enforce exit heading if in FW, the next wp is valid, the vehicle is currently loitering and either having force_heading set,
+			/* enforce exit course if in FW, the next wp is valid, the vehicle is currently loitering and either having force_heading set,
 			   or if loitering to achieve altitdue at a NAV_CMD_WAYPOINT */
-			const bool enforce_exit_heading = _navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+			const bool enforce_exit_course = _navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 							  &&
 							  next_sp.valid &&
 							  curr_sp_new->type == position_setpoint_s::SETPOINT_TYPE_LOITER &&
 							  (_mission_item.force_heading || _mission_item.nav_cmd == NAV_CMD_WAYPOINT);
 
-			// can only enforce exit heading if next waypoint is not within loiter radius of current waypoint
-			const bool exit_heading_is_reachable = dist_current_next > 1.2f * curr_sp_new->loiter_radius;
+			// can only enforce exit course if next waypoint is not within loiter radius of current waypoint
+			const bool exit_course_is_reachable = dist_current_next > 1.2f * curr_sp_new->loiter_radius;
 
-			if (enforce_exit_heading && exit_heading_is_reachable) {
-
-				float yaw_err = 0.0f;
+			if (enforce_exit_course && exit_course_is_reachable) {
 
 				// set required yaw from bearing to the next mission item
-				_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
-						    _navigator->get_global_position()->lon,
-						    next_sp.lat, next_sp.lon);
+				const float desired_exit_bearing = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
+								   _navigator->get_global_position()->lon,
+								   next_sp.lat, next_sp.lon);
 				const float cog = atan2f(_navigator->get_local_position()->vy, _navigator->get_local_position()->vx);
-				yaw_err = wrap_pi(_mission_item.yaw - cog);
 
-				exit_heading_reached = fabsf(yaw_err) < _navigator->get_yaw_threshold();
+				exit_course_reached = fabsf(wrap_pi(desired_exit_bearing - cog)) < kLoiterExitCourseReachedThreshold;
 
 			} else {
-				exit_heading_reached = true;
+				exit_course_reached = true;
 			}
 		}
 
 		// set exit flight course to next waypoint
-		if (exit_heading_reached) {
+		if (exit_course_reached) {
 			position_setpoint_s &curr_sp = _navigator->get_position_setpoint_triplet()->current;
 			const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next;
 
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index e9611d8db9..b4bbf0455a 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -54,6 +54,9 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 
+// Maximal course error to exit loiter if exit course is enforced (fixed-wing only)
+static constexpr float kLoiterExitCourseReachedThreshold = 0.087f; // 5 degrees
+
 class Navigator;
 
 class MissionBlock : public NavigatorMode