diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index b78aebd1f8..3cb9417d12 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -388,6 +388,7 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
     _flags.fast_waypoint = false;   // default waypoint back to slow
     _flags.slowing_down = false;    // target is not slowing down yet
     _flags.segment_type = SEGMENT_STRAIGHT;
+    _flags.new_wp_destination = true;   // flag new waypoint so we can freeze the pos controller's feed forward and smooth the transition
 
     // initialise the limited speed to current speed along the track
     const Vector3f &curr_vel = _inav.get_velocity();
@@ -504,8 +505,12 @@ void AC_WPNav::advance_wp_target_along_track(float dt)
     	}
     }
 
-    // do not let desired point go past the end of the segment
-    _track_desired = constrain_float(_track_desired, 0, _track_length);
+    // do not let desired point go past the end of the track unless it's a fast waypoint
+    if (!_flags.fast_waypoint) {
+        _track_desired = constrain_float(_track_desired, 0, _track_length);
+    } else {
+        _track_desired = constrain_float(_track_desired, 0, _track_length + WPNAV_WP_FAST_OVERSHOOT_MAX);
+    }
 
     // recalculate the desired position
     _pos_control.set_pos_target(_origin + _pos_delta_unit * _track_desired);
@@ -560,6 +565,10 @@ void AC_WPNav::update_wpnav()
         // advance the target if necessary
         advance_wp_target_along_track(dt);
         _pos_control.trigger_xy();
+        if (_flags.new_wp_destination) {
+            _flags.new_wp_destination = false;
+            _pos_control.freeze_ff_xy();
+        }
         _pos_control.freeze_ff_z();
     }else{
         // run horizontal position controller
@@ -745,6 +754,7 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V
     _pos_control.set_pos_target(origin);
     _flags.reached_destination = false;
     _flags.segment_type = SEGMENT_SPLINE;
+    _flags.new_wp_destination = true;   // flag new waypoint so we can freeze the pos controller's feed forward and smooth the transition
 }
 
 /// update_spline - update spline controller
@@ -771,6 +781,11 @@ void AC_WPNav::update_spline()
         // advance the target if necessary
         advance_spline_target_along_track(dt);
         _pos_control.trigger_xy();
+        if (_flags.new_wp_destination) {
+            _flags.new_wp_destination = false;
+            _pos_control.freeze_ff_xy();
+        }
+        _pos_control.freeze_ff_z();
     }else{
         // run horizontal position controller
         _pos_control.update_xy_controller(false);
diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h
index c95666134b..d65a1256f4 100644
--- a/libraries/AC_WPNav/AC_WPNav.h
+++ b/libraries/AC_WPNav/AC_WPNav.h
@@ -30,6 +30,8 @@
 
 #define WPNAV_LEASH_LENGTH_MIN          100.0f      // minimum leash lengths in cm
 
+#define WPNAV_WP_FAST_OVERSHOOT_MAX     200.0f      // 2m overshoot is allowed during fast waypoints to allow for smooth transitions to next waypoint
+
 #if HAL_CPU_CLASS < HAL_CPU_CLASS_75 || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
  # define WPNAV_LOITER_UPDATE_TIME      0.095f      // 10hz update rate on low speed CPUs (APM1, APM2)
  # define WPNAV_WP_UPDATE_TIME          0.095f      // 10hz update rate on low speed CPUs (APM1, APM2)
@@ -223,6 +225,7 @@ protected:
         uint8_t fast_waypoint           : 1;    // true if we should ignore the waypoint radius and consider the waypoint complete once the intermediate target has reached the waypoint
         uint8_t slowing_down            : 1;    // true when target point is slowing down before reaching the destination
         uint8_t recalc_wp_leash         : 1;    // true if we need to recalculate the leash lengths because of changes in speed or acceleration
+        uint8_t new_wp_destination      : 1;    // true if we have just received a new destination.  allows us to freeze the position controller's xy feed forward
         SegmentType segment_type        : 1;    // active segment is either straight or spline
     } _flags;