From 9c3812e3841f833aad7a6a139fe992509272f135 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 29 May 2019 15:20:48 +0900
Subject: [PATCH] AP_Proximity: move copy_locations to backend

also remove unused get_locations
also fix comment for copy_locations method
---
 libraries/AP_Proximity/AP_Proximity.cpp       | 64 ++-----------------
 libraries/AP_Proximity/AP_Proximity.h         | 13 +---
 .../AP_Proximity/AP_Proximity_Backend.cpp     | 64 ++++++++++++++++++-
 libraries/AP_Proximity/AP_Proximity_Backend.h | 14 ++++
 4 files changed, 84 insertions(+), 71 deletions(-)

diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp
index bf8035b435..8d09d4abf2 100644
--- a/libraries/AP_Proximity/AP_Proximity.cpp
+++ b/libraries/AP_Proximity/AP_Proximity.cpp
@@ -379,76 +379,24 @@ const Vector2f* AP_Proximity::get_boundary_points(uint16_t& num_points) const
     return get_boundary_points(primary_instance, num_points);
 }
 
-const AP_Proximity::Proximity_Location* AP_Proximity::get_locations(uint16_t& location_count) const
-{
-    if (_location_count == 0) {
-        return nullptr;
-    }
-
-    location_count = _location_count;
-    return _locations;
-}
-
 // copy location points around vehicle into a buffer owned by the caller
 // caller should provide the buff_size which is the maximum number of locations the buffer can hold (normally PROXIMITY_MAX_DIRECTION)
 // num_copied is updated with the number of locations copied into the buffer
 // returns true on success, false on failure (should only happen if there is a semaphore conflict)
-bool AP_Proximity::copy_locations(Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied)
+bool AP_Proximity::copy_locations(uint8_t instance, Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied)
 {
-    // sanity check buffer
-    if (buff == nullptr) {
+    if ((drivers[instance] == nullptr) || (_type[instance] == Proximity_Type_None)) {
         num_copied = 0;
         return false;
     }
 
-    WITH_SEMAPHORE(_rsem);
-
-    // copy locations into caller's buffer
-    num_copied = MIN(_location_count, buff_size);
-    for (uint16_t i=0; i<num_copied; i++) {
-        buff[i] = _locations[i];
-    }
-
-    return true;
+    // call backend copy_locations
+    return drivers[instance]->copy_locations(buff, buff_size, num_copied);
 }
 
-void AP_Proximity::locations_update()
+bool AP_Proximity::copy_locations(Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied)
 {
-    // exit immediately if primary sensor is disabled
-    if ((drivers[primary_instance] == nullptr) || (_type[primary_instance] == Proximity_Type_None)) {
-        return;
-    }
-
-    WITH_SEMAPHORE(_rsem);
-
-    // get number of objects, return early if none
-    uint8_t num_objects = get_object_count();
-    if (num_objects == 0) {
-        _location_count = 0;
-        return;
-    }
-
-    // get vehicle location and heading in degrees
-    Location my_loc;
-    if (!AP::ahrs().get_position(my_loc)) {
-        return;
-    }
-    const float veh_bearing = AP::ahrs().yaw_sensor * 0.01f;
-
-    // convert offset from vehicle position to earth frame location
-    const uint32_t now_ms = AP_HAL::millis();
-    _location_count = 0;
-    for (uint8_t i=0; i<num_objects; i++) {
-        float ang_deg, dist_m;
-        if (get_object_angle_and_distance(i, ang_deg, dist_m)) {
-            Location temp_loc = my_loc;
-            temp_loc.offset_bearing(wrap_180(veh_bearing + ang_deg), dist_m);
-            _locations[_location_count].loc = temp_loc;
-            _locations[_location_count].radius_m = 1;
-            _locations[_location_count].last_update_ms = now_ms;
-            _location_count++;
-        }
-    }
+    return copy_locations(primary_instance, buff, buff_size, num_copied);
 }
 
 // get distance and angle to closest object (used for pre-arm check)
diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h
index 09edb5cbad..c59086cea4 100644
--- a/libraries/AP_Proximity/AP_Proximity.h
+++ b/libraries/AP_Proximity/AP_Proximity.h
@@ -26,7 +26,7 @@
 #define PROXIMITY_MAX_IGNORE                6   // up to six areas can be ignored
 #define PROXIMITY_MAX_DIRECTION 8
 #define PROXIMITY_SENSOR_ID_START 10
-#define PROXIMITY_LOCATION_TIMEOUT_MS       3000 // locations (provided by get_locations method) are valid for this many milliseconds
+#define PROXIMITY_LOCATION_TIMEOUT_MS       3000 // locations (provided by copy_locations method) are valid for this many milliseconds
 
 class AP_Proximity_Backend;
 
@@ -108,14 +108,11 @@ public:
     const Vector2f* get_boundary_points(uint8_t instance, uint16_t& num_points) const;
     const Vector2f* get_boundary_points(uint16_t& num_points) const;
 
-    // get Location points around vehicle for use by avoidance in earth-frame
-    //   returns nullptr and sets num_points to zero if no boundary can be returned
-    const Proximity_Location* get_locations(uint16_t& location_count) const;
-
     // copy location points around vehicle into a buffer owned by the caller
     // caller should provide the buff_size which is the maximum number of locations the buffer can hold (normally PROXIMITY_MAX_DIRECTION)
     // num_copied is updated with the number of locations copied into the buffer
     // returns true on success, false on failure (should only happen if there is a semaphore conflict)
+    bool copy_locations(uint8_t instance, Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied);
     bool copy_locations(Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied);
 
     // get distance and angle to closest object (used for pre-arm check)
@@ -177,12 +174,6 @@ private:
     AP_Int8 _ignore_width_deg[PROXIMITY_MAX_IGNORE];    // width of beam (in degrees) that should be ignored
 
     void detect_instance(uint8_t instance);
-
-    // earth frame objects
-    void locations_update();
-    uint16_t _location_count;                               // number of locations held in _locations buffer
-    Proximity_Location _locations[PROXIMITY_MAX_DIRECTION]; // buffer of locations
-    HAL_Semaphore_Recursive _rsem;                          // semaphore for access to _locations and _location_count
 };
 
 namespace AP {
diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp
index 768b8c0e3d..3df9aa64b9 100644
--- a/libraries/AP_Proximity/AP_Proximity_Backend.cpp
+++ b/libraries/AP_Proximity/AP_Proximity_Backend.cpp
@@ -14,6 +14,8 @@
  */
 
 #include <AP_Common/AP_Common.h>
+#include <AP_Common/Location.h>
+#include <AP_AHRS/AP_AHRS.h>
 #include <AP_HAL/AP_HAL.h>
 #include "AP_Proximity.h"
 #include "AP_Proximity_Backend.h"
@@ -172,7 +174,7 @@ void AP_Proximity_Backend::init_boundary()
         _sector_edge_vector[sector].y = sinf(angle_rad) * 100.0f;
         _boundary_point[sector] = _sector_edge_vector[sector] * PROXIMITY_BOUNDARY_DIST_DEFAULT;
     }
-    frontend.locations_update();
+    update_locations();
 }
 
 // update boundary points used for object avoidance based on a single sector's distance changing
@@ -227,7 +229,27 @@ void AP_Proximity_Backend::update_boundary_for_sector(uint8_t sector)
     if (!_distance_valid[prev_sector_ccw]) {
         _boundary_point[prev_sector_ccw] = _sector_edge_vector[prev_sector_ccw] * shortest_distance;
     }
-    frontend.locations_update();
+    update_locations();
+}
+
+// copy location points around vehicle into a buffer owned by the caller
+// caller should provide the buff_size which is the maximum number of locations the buffer can hold (normally PROXIMITY_MAX_DIRECTION)
+// num_copied is updated with the number of locations copied into the buffer
+// returns true on success, false on failure which should only happen if buff is nullptr
+bool AP_Proximity_Backend::copy_locations(AP_Proximity::Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied)
+{
+    // sanity check buffer
+    if (buff == nullptr) {
+        num_copied = 0;
+        return false;
+    }
+
+    WITH_SEMAPHORE(_rsem);
+
+    // copy locations into caller's buffer
+    num_copied = MIN(_location_count, buff_size);
+    memcpy(buff, _locations, num_copied * sizeof(AP_Proximity::Proximity_Location));
+    return true;
 }
 
 // set status and update valid count
@@ -328,3 +350,41 @@ bool AP_Proximity_Backend::get_next_ignore_start_or_end(uint8_t start_or_end, in
     }
     return found;
 }
+
+void AP_Proximity_Backend::update_locations()
+{
+    WITH_SEMAPHORE(_rsem);
+
+    // get number of objects, return early if none
+    const uint8_t num_objects = get_object_count();
+    if (num_objects == 0) {
+        _location_count = 0;
+        return;
+    }
+
+    // get vehicle location and heading in degrees
+    float veh_bearing;
+    Location my_loc;
+    {
+        WITH_SEMAPHORE(AP::ahrs().get_semaphore());
+        if (!AP::ahrs().get_position(my_loc)) {
+            return;
+        }
+        veh_bearing = AP::ahrs().yaw_sensor * 0.01f;
+    }
+
+    // convert offset from vehicle position to earth frame location
+    const uint32_t now_ms = AP_HAL::millis();
+    _location_count = 0;
+    for (uint8_t i=0; i<num_objects; i++) {
+        float ang_deg, dist_m;
+        if (get_object_angle_and_distance(i, ang_deg, dist_m)) {
+            Location temp_loc = my_loc;
+            temp_loc.offset_bearing(wrap_180(veh_bearing + ang_deg), dist_m);
+            _locations[_location_count].loc = temp_loc;
+            _locations[_location_count].radius_m = 1;
+            _locations[_location_count].last_update_ms = now_ms;
+            _location_count++;
+        }
+    }
+}
diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h
index 17e4f5f51e..da2ef80ea4 100644
--- a/libraries/AP_Proximity/AP_Proximity_Backend.h
+++ b/libraries/AP_Proximity/AP_Proximity_Backend.h
@@ -64,6 +64,12 @@ public:
     // get distances in 8 directions. used for sending distances to ground station
     bool get_horizontal_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const;
 
+    // copy location points around vehicle into a buffer owned by the caller
+    // caller should provide the buff_size which is the maximum number of locations the buffer can hold (normally PROXIMITY_MAX_DIRECTION)
+    // num_copied is updated with the number of locations copied into the buffer
+    // returns true on success, false on failure which should only happen if buff is nullptr
+    bool copy_locations(AP_Proximity::Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied);
+
 protected:
 
     // set status and update valid_count
@@ -86,6 +92,9 @@ protected:
     bool get_ignore_area(uint8_t index, uint16_t &angle_deg, uint8_t &width_deg) const;
     bool get_next_ignore_start_or_end(uint8_t start_or_end, int16_t start_angle, int16_t &ignore_start) const;
 
+    // earth frame objects
+    void update_locations();
+
     AP_Proximity &frontend;
     AP_Proximity::Proximity_State &state;   // reference to this instances state
 
@@ -102,4 +111,9 @@ protected:
     // fence boundary
     Vector2f _sector_edge_vector[PROXIMITY_SECTORS_MAX];    // vector for right-edge of each sector, used to speed up calculation of boundary
     Vector2f _boundary_point[PROXIMITY_SECTORS_MAX];        // bounding polygon around the vehicle calculated conservatively for object avoidance
+
+    // earth frame locations (i.e. detected obstacles stored as lat/lon points)
+    uint16_t _location_count;                               // number of locations held in _locations buffer
+    AP_Proximity::Proximity_Location _locations[PROXIMITY_SECTORS_MAX];   // buffer of locations
+    HAL_Semaphore_Recursive _rsem;                          // semaphore for access to _locations and _location_count
 };