From 87dea46f5d85e944bd21400501a3a9dd9b373b76 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 15 Nov 2016 16:15:46 +0900
Subject: [PATCH] AP_Proximity: add get_closet_object for use in pre-arm checks

---
 libraries/AP_Proximity/AP_Proximity.cpp       | 11 +++++++++
 libraries/AP_Proximity/AP_Proximity.h         |  4 ++++
 .../AP_Proximity/AP_Proximity_Backend.cpp     | 24 +++++++++++++++++++
 libraries/AP_Proximity/AP_Proximity_Backend.h |  4 ++++
 4 files changed, 43 insertions(+)

diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp
index f5a11db804..d852a2616b 100644
--- a/libraries/AP_Proximity/AP_Proximity.cpp
+++ b/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);
+}
diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h
index 7984051226..db09425cf0 100644
--- a/libraries/AP_Proximity/AP_Proximity.h
+++ b/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
diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp
index 796da5010a..2563adfcd2 100644
--- a/libraries/AP_Proximity/AP_Proximity_Backend.cpp
+++ b/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)
 {
diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h
index c49d310551..6ff3744586 100644
--- a/libraries/AP_Proximity/AP_Proximity_Backend.h
+++ b/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