AC_WPNav: update yaw only when track is at least 2m

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
+    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);

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: