AP_Proximity: add Location* object and accessors

libraries/AP_Proximity/AP_Proximity.cpp

@@ -22,6 +22,7 @@
 #include "AP_Proximity_MAV.h"
 #include "AP_Proximity_SITL.h"
 #include "AP_Proximity_MorseSITL.h"
+#include <AP_AHRS/AP_AHRS.h>
 
 extern const AP_HAL::HAL &hal;
 
@@ -378,6 +379,78 @@ 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)
+{
+    // 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);
+    for (uint16_t i=0; i<num_copied; i++) {
+        buff[i] = _locations[i];
+    }
+
+    return true;
+}
+
+void AP_Proximity::locations_update()
+{
+    // 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++;
+        }
+    }
+}
+
 // 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

libraries/AP_Proximity/AP_Proximity.h

@@ -20,11 +20,13 @@
 #include <AP_Math/AP_Math.h>
 #include <AP_SerialManager/AP_SerialManager.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>
+#include <AP_Common/Location.h>
 
 #define PROXIMITY_MAX_INSTANCES             1   // Maximum number of proximity sensor instances available on this platform
 #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
 
 class AP_Proximity_Backend;
 
@@ -63,6 +65,13 @@ public:
         float distance[PROXIMITY_MAX_DIRECTION];      // distance in meters
     };
 
+    // structure holding locations of detected objects in earth frame
+    struct Proximity_Location {
+        float radius_m; // radius of object in meters
+        Location loc;
+        uint32_t last_update_ms;
+    };
+
     // detect and initialise any available proximity sensors
     void init(void);
 
@@ -99,6 +108,16 @@ 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(Proximity_Location* buff, uint16_t buff_size, uint16_t& num_copied);
+
     // 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;
@@ -158,6 +177,12 @@ 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 {

libraries/AP_Proximity/AP_Proximity_Backend.cpp

@@ -172,6 +172,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 boundary points used for object avoidance based on a single sector's distance changing
@@ -226,6 +227,7 @@ 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();
 }
 
 // set status and update valid count