From fbbd13db7507254459dd103fa0a8b1bfc9fcda63 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 17 Dec 2016 12:42:13 +0900
Subject: [PATCH] AP_Proximity: add get_object methods for use with non-GPS
 avoidance

---
 libraries/AP_Proximity/AP_Proximity.cpp       | 21 +++++++++++++++++++
 libraries/AP_Proximity/AP_Proximity.h         |  4 ++++
 .../AP_Proximity/AP_Proximity_Backend.cpp     | 18 ++++++++++++++++
 libraries/AP_Proximity/AP_Proximity_Backend.h |  4 ++++
 4 files changed, 47 insertions(+)

diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp
index 58088444ad..946315fa2a 100644
--- a/libraries/AP_Proximity/AP_Proximity.cpp
+++ b/libraries/AP_Proximity/AP_Proximity.cpp
@@ -307,6 +307,27 @@ bool AP_Proximity::get_closest_object(float& angle_deg, float &distance) const
     return drivers[primary_instance]->get_closest_object(angle_deg, distance);
 }
 
+// get number of objects, used for non-GPS avoidance
+uint8_t AP_Proximity::get_object_count() const
+{
+    if ((drivers[primary_instance] == nullptr) || (_type[primary_instance] == Proximity_Type_None)) {
+        return 0;
+    }
+    // get count from backend
+    return drivers[primary_instance]->get_object_count();
+}
+
+// get an object's angle and distance, used for non-GPS avoidance
+// returns false if no angle or distance could be returned for some reason
+bool AP_Proximity::get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const
+{
+    if ((drivers[primary_instance] == nullptr) || (_type[primary_instance] == Proximity_Type_None)) {
+        return false;
+    }
+    // get angle and distance from backend
+    return drivers[primary_instance]->get_object_angle_and_distance(object_number, angle_deg, distance);
+}
+
 // get distances in 8 directions. used for sending distances to ground station
 bool AP_Proximity::get_distances(Proximity_Distance_Array &prx_dist_array) const
 {
diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h
index 6c4c5f2517..88829b8d0d 100644
--- a/libraries/AP_Proximity/AP_Proximity.h
+++ b/libraries/AP_Proximity/AP_Proximity.h
@@ -80,6 +80,10 @@ public:
     //   returns true on success, false if no valid readings
     bool get_closest_object(float& angle_deg, float &distance) const;
 
+    // get number of objects, angle and distance - used for non-GPS avoidance
+    uint8_t get_object_count() const;
+    bool get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const;
+
     // stucture holding distances in 8 directions
     struct Proximity_Distance_Array {
         uint8_t orientation[8]; // orientation (i.e. rough direction) of the distance (see MAV_SENSOR_ORIENTATION)
diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp
index 79ce14e541..c8801ad72d 100644
--- a/libraries/AP_Proximity/AP_Proximity_Backend.cpp
+++ b/libraries/AP_Proximity/AP_Proximity_Backend.cpp
@@ -65,6 +65,24 @@ bool AP_Proximity_Backend::get_closest_object(float& angle_deg, float &distance)
     return sector_found;
 }
 
+// get number of objects, used for non-GPS avoidance
+uint8_t AP_Proximity_Backend::get_object_count() const
+{
+    return _num_sectors;
+}
+
+// get an object's angle and distance, used for non-GPS avoidance
+// returns false if no angle or distance could be returned for some reason
+bool AP_Proximity_Backend::get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const
+{
+    if (object_number < _num_sectors && _distance_valid[object_number]) {
+        angle_deg = _angle[object_number];
+        distance = _distance[object_number];
+        return true;
+    }
+    return false;
+}
+
 // get distances in 8 directions. used for sending distances to ground station
 bool AP_Proximity_Backend::get_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const
 {
diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h
index f7eda70cc3..8471b8edf2 100644
--- a/libraries/AP_Proximity/AP_Proximity_Backend.h
+++ b/libraries/AP_Proximity/AP_Proximity_Backend.h
@@ -50,6 +50,10 @@ public:
     //   returns true on success, false if no valid readings
     bool get_closest_object(float& angle_deg, float &distance) const;
 
+    // get number of objects, angle and distance - used for non-GPS avoidance
+    uint8_t get_object_count() const;
+    bool get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const;
+
     // get distances in 8 directions. used for sending distances to ground station
     bool get_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const;