diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index f87f120ade..229f3d3099 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -709,7 +709,6 @@ private:
     void send_hwstatus(mavlink_channel_t chan);
     void send_vfr_hud(mavlink_channel_t chan);
     void send_current_waypoint(mavlink_channel_t chan);
-    void send_proximity(mavlink_channel_t chan, uint16_t count_max);
     void send_rpm(mavlink_channel_t chan);
     void rpm_update();
     void button_update();
diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
index 0e474a7bfe..14b93bc068 100644
--- a/ArduCopter/GCS_Mavlink.cpp
+++ b/ArduCopter/GCS_Mavlink.cpp
@@ -207,58 +207,6 @@ void NOINLINE Copter::send_current_waypoint(mavlink_channel_t chan)
     mavlink_msg_mission_current_send(chan, mission.get_current_nav_index());
 }
 
-void NOINLINE Copter::send_proximity(mavlink_channel_t chan, uint16_t count_max)
-{
-#if PROXIMITY_ENABLED == ENABLED
-    // return immediately if no proximity sensor is present
-    if (g2.proximity.get_status() == AP_Proximity::Proximity_NotConnected) {
-        return;
-    }
-
-    // return immediately if no tx buffer room to send messages
-    if (count_max == 0) {
-        return;
-    }
-
-    bool send_upwards = true;
-
-    // send horizontal distances
-    AP_Proximity::Proximity_Distance_Array dist_array;
-    const uint8_t horiz_count = MIN(count_max, PROXIMITY_MAX_DIRECTION);  // send at most PROXIMITY_MAX_DIRECTION horizontal distances
-    if (g2.proximity.get_horizontal_distances(dist_array)) {
-        for (uint8_t i = 0; i < horiz_count; i++) {
-            mavlink_msg_distance_sensor_send(
-                chan,
-                AP_HAL::millis(),                               //  time since system boot
-                (uint16_t)(g2.proximity.distance_min() * 100),  // minimum distance the sensor can measure in centimeters
-                (uint16_t)(g2.proximity.distance_max() * 100),  // maximum distance the sensor can measure in centimeters
-                (uint16_t)(dist_array.distance[i] * 100),       // current distance reading (in cm?)
-                MAV_DISTANCE_SENSOR_LASER,                      // type from MAV_DISTANCE_SENSOR enum
-                PROXIMITY_SENSOR_ID_START + i,                  // onboard ID of the sensor
-                dist_array.orientation[i],                      //  direction the sensor faces from MAV_SENSOR_ORIENTATION enum
-                0);                                             // Measurement covariance in centimeters, 0 for unknown / invalid readings
-        }
-        // check if we still have room to send upwards distance
-        send_upwards = (count_max > 8);
-    }
-
-    // send upward distance
-    float dist_up;
-    if (send_upwards && g2.proximity.get_upward_distance(dist_up)) {
-        mavlink_msg_distance_sensor_send(
-            chan,
-            AP_HAL::millis(),                                        //  time since system boot
-            (uint16_t)(g2.proximity.distance_min() * 100),           // minimum distance the sensor can measure in centimeters
-            (uint16_t)(g2.proximity.distance_max() * 100),           // maximum distance the sensor can measure in centimeters
-            (uint16_t)(dist_up * 100),                               // current distance reading
-            MAV_DISTANCE_SENSOR_LASER,                               // type from MAV_DISTANCE_SENSOR enum
-            PROXIMITY_SENSOR_ID_START + PROXIMITY_MAX_DIRECTION +1,  // onboard ID of the sensor
-            MAV_SENSOR_ROTATION_PITCH_90,                            // direction upwards
-            0);                                                      // Measurement covariance in centimeters, 0 for unknown / invalid readings
-    }
-#endif
-}
-
 /*
   send RPM packet
  */
@@ -467,8 +415,10 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
         CHECK_PAYLOAD_SIZE(DISTANCE_SENSOR);
         send_distance_sensor_downward(copter.rangefinder);
 #endif
+#if PROXIMITY_ENABLED == ENABLED
         CHECK_PAYLOAD_SIZE(DISTANCE_SENSOR);
-        copter.send_proximity(chan, comm_get_txspace(chan) / (packet_overhead()+9));
+        send_proximity(copter.g2.proximity, comm_get_txspace(chan) / (packet_overhead()+9));
+#endif
         break;
 
     case MSG_RPM: