diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index 8926fee165..3aeca5e242 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -371,11 +371,6 @@ bool AC_WPNav::set_wp_destination_next_NED(const Vector3f& destination_NED)
 ///     relies on set_wp_destination or set_wp_origin_and_destination having been called first
 void AC_WPNav::shift_wp_origin_to_current_pos()
 {
-    // return immediately if vehicle is not at the origin
-    if (_track_desired > 0.0f) {
-        return;
-    }
-
     // get current and target locations
     const Vector3f &curr_pos = _inav.get_position();
     const Vector3f pos_target = _pos_control.get_pos_target();
diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h
index c4ba53ed9d..b62e26ec64 100644
--- a/libraries/AC_WPNav/AC_WPNav.h
+++ b/libraries/AC_WPNav/AC_WPNav.h
@@ -287,7 +287,6 @@ protected:
     Vector3f    _origin;                // starting point of trip to next waypoint in cm from ekf origin
     Vector3f    _destination;           // target destination in cm from ekf origin
     float       _track_error_xy;        // horizontal error of the actual position vs the desired position
-    float       _track_desired;         // our desired distance along the track in cm
     float       _track_scalar_dt;       // time compression multiplier to slow the progress along the track
     float       _yaw;                   // current yaw heading in centi-degrees based on track direction
     float       _yaw_rate_cds;          // current yaw rate in centi-degrees/second based on track curvature