GCS_MAVLink: move send_proximity to GCS common code (NFC)

Also clean two comments

libraries/GCS_MAVLink/GCS.h

@@ -14,6 +14,7 @@
 #include <AP_SerialManager/AP_SerialManager.h>
 #include <AP_Mount/AP_Mount.h>
 #include <AP_Avoidance/AP_Avoidance.h>
+#include <AP_Proximity/AP_Proximity.h>
 #include <AP_HAL/utility/RingBuffer.h>
 #include <AP_Frsky_Telem/AP_Frsky_Telem.h>
 #include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
@@ -155,6 +156,7 @@ public:
     bool send_distance_sensor(const RangeFinder &rangefinder) const;
     void send_distance_sensor_downward(const RangeFinder &rangefinder) const;
     void send_rangefinder_downward(const RangeFinder &rangefinder) const;
+    void send_proximity(const AP_Proximity &proximity, uint16_t count_max) const;
     void send_ahrs2(AP_AHRS &ahrs);
     bool send_gps_raw(AP_GPS &gps);
     void send_system_time(AP_GPS &gps);

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -228,7 +228,7 @@ void GCS_MAVLINK::send_distance_sensor(const RangeFinder &rangefinder, const uin
                 AP_HAL::millis(),                       // time since system boot TODO: take time of measurement
                 rangefinder.min_distance_cm(instance),  // minimum distance the sensor can measure in centimeters
                 rangefinder.max_distance_cm(instance),  // maximum distance the sensor can measure in centimeters
-                rangefinder.distance_cm(instance),      // current distance reading (in cm?)
+                rangefinder.distance_cm(instance),      // current distance reading
                 rangefinder.get_sensor_type(instance),  // type from MAV_DISTANCE_SENSOR enum
                 instance,                               // onboard ID of the sensor == instance
                 rangefinder.get_orientation(instance),  // direction the sensor faces from MAV_SENSOR_ORIENTATION enum
@@ -269,6 +269,56 @@ void GCS_MAVLINK::send_rangefinder_downward(const RangeFinder &rangefinder) cons
             rangefinder.voltage_mv_orient(ROTATION_PITCH_270) * 0.001f);
 }
 
+void GCS_MAVLINK::send_proximity(const AP_Proximity &proximity, uint16_t count_max) const
+{
+    // return immediately if no proximity sensor is present
+    if (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 (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)(proximity.distance_min() * 100.0f),  // minimum distance the sensor can measure in centimeters
+                    (uint16_t)(proximity.distance_max() * 100.0f),  // maximum distance the sensor can measure in centimeters
+                    (uint16_t)(dist_array.distance[i] * 100.0f),    // current distance reading
+                    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 && proximity.get_upward_distance(dist_up)) {
+        mavlink_msg_distance_sensor_send(
+                chan,
+                AP_HAL::millis(),                                         // time since system boot
+                (uint16_t)(proximity.distance_min() * 100.0f),            // minimum distance the sensor can measure in centimeters
+                (uint16_t)(proximity.distance_max() * 100.0f),            // maximum distance the sensor can measure in centimeters
+                (uint16_t)(dist_up * 100.0f),                             // 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
+    }
+}
+
 // report AHRS2 state
 void GCS_MAVLINK::send_ahrs2(AP_AHRS &ahrs)
 {