diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 5a0d2fe16c..c144224fe6 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -322,7 +322,7 @@ MissionBlock::is_mission_item_reached()
 
 		if ((_navigator->get_vstatus()->is_rotary_wing
 		     || (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT && _mission_item.force_heading))
-		    && PX4_ISFINITE(_mission_item.yaw)) {
+		    && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw))) {
 
 			/* check course if defined only for rotary wing except takeoff */
 			float cog = _navigator->get_vstatus()->is_rotary_wing ? _navigator->get_global_position()->yaw : atan2f(
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 2633522a94..99e70b9e52 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -248,6 +248,16 @@ public:
 	 */
 	float		get_acceptance_radius(float mission_item_radius);
 
+	/**
+	 * Get the yaw acceptance given the current mission item
+	 *
+	 * @param mission_item_yaw the yaw to use in case the controller-derived radius is finite
+	 *
+	 * @return the yaw at which the next waypoint should be used or if the yaw at a waypoint
+	 * should be ignored
+	 */
+	float 		get_yaw_acceptance(float mission_item_yaw);
+
 	orb_advert_t	*get_mavlink_log_pub() { return &_mavlink_log_pub; }
 
 	void		increment_mission_instance_count() { _mission_result.instance_count++; }
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index c55dade689..e3808d7a5a 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -946,6 +946,20 @@ Navigator::get_acceptance_radius(float mission_item_radius)
 	return radius;
 }
 
+float
+Navigator::get_yaw_acceptance(float mission_item_yaw)
+{
+	float yaw = mission_item_yaw;
+
+	const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
+
+	if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) && !PX4_ISFINITE(pos_ctrl_status.yaw_acceptance)) {
+		yaw = pos_ctrl_status.yaw_acceptance;
+	}
+
+	return yaw;
+}
+
 void
 Navigator::load_fence_from_file(const char *filename)
 {