diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index 96d93aacad..9825677a02 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -380,7 +380,12 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
     calculate_wp_leash_length();
 
     // initialise yaw heading
-    _yaw = get_bearing_cd(_origin, _destination);
+    if (_track_length >= WPNAV_YAW_DIST_MIN) {
+        _yaw = get_bearing_cd(_origin, _destination);
+    } else {
+        // set target yaw to current heading.  Alternatively we could pull this from the attitude controller if we had access to it
+        _yaw = _ahrs.yaw_sensor;
+    }
 
     // initialise intermediate point to the origin
     _pos_control.set_pos_target(origin);
diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h
index d65a1256f4..0342889f42 100644
--- a/libraries/AC_WPNav/AC_WPNav.h
+++ b/libraries/AC_WPNav/AC_WPNav.h
@@ -42,6 +42,8 @@
 
 #define WPNAV_LOITER_ACTIVE_TIMEOUT_MS     200      // loiter controller is considered active if it has been called within the past 200ms (0.2 seconds)
 
+#define WPNAV_YAW_DIST_MIN                 200      // minimum track length which will lead to target yaw being updated to point at next waypoint.  Under this distance the yaw target will be frozen at the current heading
+
 class AC_WPNav
 {
 public: