AP_Proximity: add get_closet_object for use in pre-arm checks

libraries/AP_Proximity/AP_Proximity.cpp

@@ -194,3 +194,14 @@ bool AP_Proximity::get_horizontal_distance(float angle_deg, float &distance) con
 {
     return get_horizontal_distance(primary_instance, angle_deg, distance);
 }
+
+// get distance and angle to closest object (used for pre-arm check)
+//   returns true on success, false if no valid readings
+bool AP_Proximity::get_closest_object(float& angle_deg, float &distance) const
+{
+    if ((drivers[primary_instance] == nullptr) || (_type[primary_instance] == Proximity_Type_None)) {
+        return false;
+    }
+    // get closest object from backend
+    return drivers[primary_instance]->get_closest_object(angle_deg, distance);
+}

libraries/AP_Proximity/AP_Proximity.h

@@ -70,6 +70,10 @@ public:
     bool get_horizontal_distance(uint8_t instance, float angle_deg, float &distance) const;
     bool get_horizontal_distance(float angle_deg, float &distance) const;
 
+    // get distance and angle to closest object (used for pre-arm check)
+    //   returns true on success, false if no valid readings
+    bool get_closest_object(float& angle_deg, float &distance) const;
+
     // The Proximity_State structure is filled in by the backend driver
     struct Proximity_State {
         uint8_t                 instance;   // the instance number of this proximity sensor

libraries/AP_Proximity/AP_Proximity_Backend.cpp

@@ -41,6 +41,30 @@ bool AP_Proximity_Backend::get_horizontal_distance(float angle_deg, float &dista
     return false;
 }
 
+// get distance and angle to closest object (used for pre-arm check)
+//   returns true on success, false if no valid readings
+bool AP_Proximity_Backend::get_closest_object(float& angle_deg, float &distance) const
+{
+    bool sector_found = false;
+    uint8_t sector = 0;
+
+    // check all sectors for shorter distance
+    for (uint8_t i=0; i<_num_sectors; i++) {
+        if (_distance_valid[i]) {
+            if (!sector_found || (_distance[i] < _distance[sector])) {
+                sector = i;
+                sector_found = true;
+            }
+        }
+    }
+
+    if (sector_found) {
+        angle_deg = _angle[sector];
+        distance = _distance[sector];
+    }
+    return sector_found;
+}
+
 // set status and update valid count
 void AP_Proximity_Backend::set_status(AP_Proximity::Proximity_Status status)
 {

libraries/AP_Proximity/AP_Proximity_Backend.h

@@ -37,6 +37,10 @@ public:
     // returns true on successful read and places distance in distance
     bool get_horizontal_distance(float angle_deg, float &distance) const;
 
+    // get distance and angle to closest object (used for pre-arm check)
+    //   returns true on success, false if no valid readings
+    bool get_closest_object(float& angle_deg, float &distance) const;
+
 protected:
 
     // set status and update valid_count