diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp
index 2c86bfa7e5..0a12bdf486 100644
--- a/libraries/AC_Avoidance/AC_Avoid.cpp
+++ b/libraries/AC_Avoidance/AC_Avoid.cpp
@@ -108,11 +108,46 @@ void AC_Avoid::adjust_velocity_polygon_fence(float kP, float accel_cmss, Vector2
         return;
     }
 
+    // exit immediately if no desired velocity
+    if (desired_vel.is_zero()) {
+        return;
+    }
+
     // get polygon boundary
     // Note: first point in list is the return-point (which copter does not use)
     uint16_t num_points;
     Vector2f* boundary = _fence.get_polygon_points(num_points);
 
+    // adjust velocity using polygon
+    adjust_velocity_polygon(kP, accel_cmss, desired_vel, boundary, num_points);
+}
+
+/*
+ * Adjusts the desired velocity based on output from the proximity sensor
+ */
+void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector2f &desired_vel)
+{
+    // exit immediately if proximity sensor is not present
+    if (_proximity.get_status() != AP_Proximity::Proximity_Good) {
+        return;
+    }
+
+    // exit immediately if no desired velocity
+    if (desired_vel.is_zero()) {
+        return;
+    }
+
+    // get boundary from proximity sensor
+    uint16_t num_points;
+    const Vector2f *boundary = _proximity.get_boundary_points(num_points);
+    adjust_velocity_polygon(kP, accel_cmss, desired_vel, boundary, num_points);
+}
+
+/*
+ * Adjusts the desired velocity for the polygon fence.
+ */
+void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f* boundary, uint16_t num_points)
+{
     // exit if there are no points
     if (boundary == nullptr || num_points == 0) {
         return;
@@ -154,37 +189,6 @@ void AC_Avoid::adjust_velocity_polygon_fence(float kP, float accel_cmss, Vector2
     desired_vel = safe_vel;
 }
 
-/*
- * Adjusts the desired velocity based on output from the proximity sensor
- */
-void AC_Avoid::adjust_velocity_proximity(const float kP, const float accel_cmss, Vector2f &desired_vel)
-{
-    // exit immediately if proximity sensor is not present
-    if (_proximity.get_status() != AP_Proximity::Proximity_Good) {
-        return;
-    }
-
-    // exit immediately if no desired velocity
-    if (desired_vel.is_zero()) {
-        return;
-    }
-
-    // normalise desired velocity vector
-    Vector2f vel_dir = desired_vel.normalized();
-
-    // get angle of desired velocity
-    float heading_rad = atan2f(vel_dir.y, vel_dir.x);
-
-    // rotate desired velocity angle into body-frame angle
-    float heading_bf_rad = wrap_PI(heading_rad - _ahrs.yaw);
-
-    // get nearest object using body-frame angle and shorten desired velocity (which must remain in earth-frame)
-    float distance_m;
-    if (_proximity.get_horizontal_distance(degrees(heading_bf_rad), distance_m)) {
-        limit_velocity(kP, accel_cmss, desired_vel, vel_dir, MAX(distance_m*100.0f - 200.0f, 0.0f));
-    }
-}
-
 /*
  * Limits the component of desired_vel in the direction of the unit vector
  * limit_direction to be at most the maximum speed permitted by the limit_distance.
diff --git a/libraries/AC_Avoidance/AC_Avoid.h b/libraries/AC_Avoidance/AC_Avoid.h
index 1a47b0549d..1fc7210ea3 100644
--- a/libraries/AC_Avoidance/AC_Avoid.h
+++ b/libraries/AC_Avoidance/AC_Avoid.h
@@ -54,6 +54,11 @@ private:
      */
     void adjust_velocity_proximity(float kP, float accel_cmss, Vector2f &desired_vel);
 
+    /*
+     * Adjusts the desired velocity given an array of boundary points
+     */
+    void adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel, const Vector2f* boundary, uint16_t num_points);
+
     /*
      * Limits the component of desired_vel in the direction of the unit vector
      * limit_direction to be at most the maximum speed permitted by the limit_distance.