diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index 5ae5c0df893d5ea4079d8588605670b178fd0682..02743150aa270dbac59d80e0a0a32918a15a741c 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -535,8 +535,8 @@ void AC_WPNav::get_loiter_position_to_velocity(float dt, float max_speed_cms)
             desired_vel.x = vel_sqrt * dist_error.x/dist_error_total;
             desired_vel.y = vel_sqrt * dist_error.y/dist_error_total;
         }else{
-            desired_vel.x = _pid_pos_lat->get_p(dist_error.x);
-            desired_vel.y = _pid_pos_lon->get_p(dist_error.y);
+            desired_vel.x = _pid_pos_lat->kP() * dist_error.x;
+            desired_vel.y = _pid_pos_lon->kP() * dist_error.y;
         }
 
         // ensure velocity stays within limits