AP_Proximity: add boundary points for object avoidance

This returns a fence which can be used for object avoidance by AC_Avoidance

libraries/AP_Proximity/AP_Proximity.cpp

@@ -279,6 +279,23 @@ bool AP_Proximity::get_horizontal_distance(float angle_deg, float &distance) con
     return get_horizontal_distance(primary_instance, angle_deg, distance);
 }
 
+// get boundary points around vehicle for use by avoidance
+//   returns nullptr and sets num_points to zero if no boundary can be returned
+const Vector2f* AP_Proximity::get_boundary_points(uint8_t instance, uint16_t& num_points) const
+{
+    if ((drivers[instance] == nullptr) || (_type[instance] == Proximity_Type_None)) {
+        num_points = 0;
+        return nullptr;
+    }
+    // get boundary from backend
+    return drivers[primary_instance]->get_boundary_points(num_points);
+}
+
+const Vector2f* AP_Proximity::get_boundary_points(uint16_t& num_points) const
+{
+    return get_boundary_points(primary_instance, num_points);
+}
+
 // 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

@@ -71,6 +71,11 @@ 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 boundary points around vehicle for use by avoidance
+    //   returns nullptr and sets num_points to zero if no boundary can be returned
+    const Vector2f* get_boundary_points(uint8_t instance, uint16_t& num_points) const;
+    const Vector2f* get_boundary_points(uint16_t& num_points) 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;

libraries/AP_Proximity/AP_Proximity_Backend.cpp

@@ -65,6 +65,66 @@ bool AP_Proximity_Backend::get_closest_object(float& angle_deg, float &distance)
     return sector_found;
 }
 
+// get boundary points around vehicle for use by avoidance
+//   returns nullptr and sets num_points to zero if no boundary can be returned
+const Vector2f* AP_Proximity_Backend::get_boundary_points(uint16_t& num_points) const
+{
+    // high-level status check
+    if (state.status != AP_Proximity::Proximity_Good) {
+        num_points = 0;
+        return nullptr;
+    }
+
+    // check all sectors have valid data, if not, exit
+    for (uint8_t i=0; i<_num_sectors; i++) {
+        if (!_distance_valid[i]) {
+            num_points = 0;
+            return nullptr;
+        }
+    }
+
+    // return boundary points
+    num_points = _num_sectors;
+    return _boundary_point;
+}
+
+// update boundary points used for object avoidance based on a single sector's distance changing
+//   the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing
+//   the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle
+void AP_Proximity_Backend::update_boundary_for_sector(uint8_t sector)
+{
+    // sanity check
+    if (sector >= _num_sectors) {
+        return;
+    }
+
+    // initialise sector_edge_vector if necessary
+    if (_sector_edge_vector[sector].is_zero()) {
+        float angle_rad = radians((float)_sector_middle_deg[sector]+(float)_sector_width_deg[sector]/2.0f);
+        _sector_edge_vector[sector].x = cosf(angle_rad) * 100.0f;
+        _sector_edge_vector[sector].y = sinf(angle_rad) * 100.0f;
+    }
+
+    // find adjacent sector (clockwise)
+    uint8_t next_sector = sector + 1;
+    if (next_sector >= _num_sectors) {
+        next_sector = 0;
+    }
+
+    // boundary point lies on the line between the two sectors at the shorter distance found in the two sectors
+    if (_distance_valid[sector] && _distance_valid[next_sector]) {
+        float shortest_distance = MIN(_distance[sector], _distance[next_sector]);
+        _boundary_point[sector] = _sector_edge_vector[sector] * shortest_distance;
+    }
+
+    // repeat for edge between sector and previous sector
+    uint8_t prev_sector = (sector == 0) ? _num_sectors-1 : sector-1;
+    if (_distance_valid[prev_sector] && _distance_valid[sector]) {
+        float shortest_distance = MIN(_distance[prev_sector], _distance[sector]);
+        _boundary_point[prev_sector] = _sector_edge_vector[prev_sector] * shortest_distance;
+    }
+}
+
 // 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 boundary points around vehicle for use by avoidance
+    //   returns nullptr and sets num_points to zero if no boundary can be returned
+    const Vector2f* get_boundary_points(uint16_t& num_points) 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;
@@ -49,6 +53,11 @@ protected:
     // find which sector a given angle falls into
     bool convert_angle_to_sector(float angle_degrees, uint8_t &sector) const;
 
+    // update boundary points used for object avoidance based on a single sector's distance changing
+    //   the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing
+    //   the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle
+    void update_boundary_for_sector(uint8_t sector);
+
     // get ignore area info
     uint8_t get_ignore_area_count() const;
     bool get_ignore_area(uint8_t index, uint16_t &angle_deg, uint8_t &width_deg) const;
@@ -66,4 +75,8 @@ protected:
     float _angle[PROXIMITY_SECTORS_MAX];            // angle to closest object within each sector
     float _distance[PROXIMITY_SECTORS_MAX];         // distance to closest object within each sector
     bool _distance_valid[PROXIMITY_SECTORS_MAX];    // true if a valid distance received for each sector
+
+    // 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
 };

libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp

@@ -401,6 +401,8 @@ bool AP_Proximity_LightWareSF40C::process_reply()
                 _distance_valid[sector] = true;
                 _last_distance_received_ms = AP_HAL::millis();
                 success = true;
+                // update boundary used for avoidance
+                update_boundary_for_sector(sector);
             }
             break;
         }