diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp
index afb6f4956c..1ab6c817d8 100644
--- a/libraries/AR_WPNav/AR_WPNav.cpp
+++ b/libraries/AR_WPNav/AR_WPNav.cpp
@@ -505,6 +505,10 @@ void AR_WPNav::update_steering_and_speed(const Location &current_loc, float dt)
 {
     _cross_track_error = calc_crosstrack_error(current_loc);
 
+    // update position controller
+    _pos_control.set_reversed(_reversed);
+    _pos_control.update(dt);
+
     // handle pivot turns
     if (_pivot.active()) {
         // decelerate to zero
@@ -512,14 +516,11 @@ void AR_WPNav::update_steering_and_speed(const Location &current_loc, float dt)
         _desired_heading_cd = _reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd();
         _desired_turn_rate_rads = is_zero(_desired_speed_limited) ? _pivot.get_turn_rate_rads(_desired_heading_cd * 0.01, dt) : 0;
         _desired_lat_accel = 0.0f;
-        return;
+    } else {
+        _desired_speed_limited = _pos_control.get_desired_speed();
+        _desired_turn_rate_rads = _pos_control.get_desired_turn_rate_rads();
+        _desired_lat_accel = _pos_control.get_desired_lat_accel();
     }
-
-    _pos_control.set_reversed(_reversed);
-    _pos_control.update(dt);
-    _desired_speed_limited = _pos_control.get_desired_speed();
-    _desired_turn_rate_rads = _pos_control.get_desired_turn_rate_rads();
-    _desired_lat_accel = _pos_control.get_desired_lat_accel();
 }
 
 // settor to allow vehicle code to provide turn related param values to this library (should be updated regularly)