diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index b4a7ae4180..5c542e9d73 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -297,6 +297,28 @@ void AC_WPNav::update_loiter()
 /// waypoint navigation
 ///
 
+/// wp_and_spline_init - initialise straight line and spline waypoint controllers
+///     updates target roll, pitch targets and I terms based on vehicle lean angles
+///     should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination
+void AC_WPNav::wp_and_spline_init()
+{
+    // check _wp_accel_cms is reasonable
+    if (_wp_accel_cms <= 0) {
+        _wp_accel_cms.set_and_save(WPNAV_ACCELERATION);
+    }
+
+    // initialise position controller
+    _pos_control.init_xy_controller();
+
+    // initialise position controller speed and acceleration
+    _pos_control.set_speed_xy(_wp_speed_cms);
+    _pos_control.set_accel_xy(_wp_accel_cms);
+    _pos_control.set_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms);
+    _pos_control.set_accel_z(_wp_accel_z_cms);
+    _pos_control.calc_leash_length_xy();
+    _pos_control.calc_leash_length_z();
+}
+
 /// set_speed_xy - allows main code to pass target horizontal velocity for wp navigation
 void AC_WPNav::set_speed_xy(float speed_cms)
 {
@@ -347,19 +369,6 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
         _pos_delta_unit = pos_delta/_track_length;
     }
 
-    // check _wp_accel_cms is reasonable
-    if (_wp_accel_cms <= 0) {
-        _wp_accel_cms.set_and_save(WPNAV_ACCELERATION);
-    }
-
-    // initialise position controller speed and acceleration
-    _pos_control.set_speed_xy(_wp_speed_cms);
-    _pos_control.set_accel_xy(_wp_accel_cms);
-    _pos_control.set_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms);
-    _pos_control.set_accel_z(_wp_accel_z_cms);
-    _pos_control.calc_leash_length_xy();
-    _pos_control.calc_leash_length_z();
-
     // calculate leash lengths
     calculate_wp_leash_length();
 
@@ -717,13 +726,6 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V
     _origin = origin;
     _destination = destination;
 
-    // initialise position controller speed and acceleration
-    _pos_control.set_speed_xy(_wp_speed_cms);
-    _pos_control.set_accel_xy(_wp_accel_cms);
-    _pos_control.set_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms);
-    _pos_control.calc_leash_length_xy();
-    _pos_control.calc_leash_length_z();
-
     // calculate slow down distance
     calc_slow_down_distance(_wp_speed_cms, _wp_accel_cms);
 
diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h
index dfae5ee838..487c4f73a3 100644
--- a/libraries/AC_WPNav/AC_WPNav.h
+++ b/libraries/AC_WPNav/AC_WPNav.h
@@ -93,6 +93,11 @@ public:
     /// waypoint controller
     ///
 
+    /// wp_and_spline_init - initialise straight line and spline waypoint controllers
+    ///     updates target roll, pitch targets and I terms based on vehicle lean angles
+    ///     should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination
+    void wp_and_spline_init();
+
     /// set_speed_xy - allows main code to pass target horizontal velocity for wp navigation
     void set_speed_xy(float speed_cms);