diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index e56355393d..f1af2fbbdd 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -121,12 +121,17 @@ Loiter::reposition()
 
 		// set yaw
 
-		// calculate direction the vehicle should point to.
-		pos_sp_triplet->current.yaw = get_bearing_to_next_waypoint(
-				_navigator->get_global_position()->lat,
-				_navigator->get_global_position()->lon,
-				pos_sp_triplet->current.lat,
-				pos_sp_triplet->current.lon);
+		float travel_dist = get_distance_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+								pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
+
+		if (travel_dist > 1.0f) {
+			// calculate direction the vehicle should point to.
+			pos_sp_triplet->current.yaw = get_bearing_to_next_waypoint(
+					_navigator->get_global_position()->lat,
+					_navigator->get_global_position()->lon,
+					pos_sp_triplet->current.lat,
+					pos_sp_triplet->current.lon);
+		}
 
 		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);