diff --git a/libraries/AC_PID/AC_P_2D.cpp b/libraries/AC_PID/AC_P_2D.cpp
index b09f7f2f3b..a37e2ee9c1 100644
--- a/libraries/AC_PID/AC_P_2D.cpp
+++ b/libraries/AC_PID/AC_P_2D.cpp
@@ -24,12 +24,12 @@ AC_P_2D::AC_P_2D(float initial_p, float dt) :
 
 // update_all - set target and measured inputs to P controller and calculate outputs
 // limit is set true if the target has been moved to limit the maximum position error
-Vector2f AC_P_2D::update_all(float &target_x, float &target_y, const Vector2f &measurement, bool &limit)
+Vector2f AC_P_2D::update_all(postype_t &target_x, postype_t &target_y, const Vector2f &measurement, bool &limit)
 {
     limit = false;
 
     // calculate distance _error
-    _error = Vector2f{target_x, target_y} - measurement;
+    _error = (Vector2p{target_x, target_y} - measurement.topostype()).tofloat();
 
     // Constrain _error and target position
     // Constrain the maximum length of _vel_target to the maximum position correction velocity
diff --git a/libraries/AC_PID/AC_P_2D.h b/libraries/AC_PID/AC_P_2D.h
index ecef569566..d547a07da1 100644
--- a/libraries/AC_PID/AC_P_2D.h
+++ b/libraries/AC_PID/AC_P_2D.h
@@ -20,11 +20,11 @@ public:
     void set_dt(float dt) { _dt = dt; }
 
     // set target and measured inputs to P controller and calculate outputs
-    Vector2f update_all(float &target_x, float &target_y, const Vector2f &measurement, bool &limit) WARN_IF_UNUSED;
+    Vector2f update_all(postype_t &target_x, postype_t &target_y, const Vector2f &measurement, bool &limit) WARN_IF_UNUSED;
 
     // set target and measured inputs to P controller and calculate outputs
     // measurement is provided as 3-axis vector but only x and y are used
-    Vector2f update_all(float &target_x, float &target_y, const Vector3f &measurement, bool &limit) WARN_IF_UNUSED {
+    Vector2f update_all(postype_t &target_x, postype_t &target_y, const Vector3f &measurement, bool &limit) WARN_IF_UNUSED {
         return update_all(target_x, target_y, Vector2f{measurement.x, measurement.y}, limit);
     }