AP_Proximity: RPLidarA2 uses modified Boundary_3D interface

libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp

@@ -312,31 +312,30 @@ void AP_Proximity_RPLidarA2::parse_response_data()
 #endif
                 _last_distance_received_ms = AP_HAL::millis();
                 if (!ignore_reading(angle_deg)) {
-                    const boundary_location bnd_loc = boundary.get_sector(angle_deg);
-                    const uint8_t sector = bnd_loc.sector;
-                    if (distance_m > distance_min()) {
-                        if (_last_sector == sector) {
-                            if (_distance_m_last > distance_m) {
-                                _distance_m_last = distance_m;
-                                _angle_deg_last  = angle_deg;
-                            }
+                    const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg);
+
+                    if (face != _last_face) {
+                        // distance is for a new face, the previous one can be updated now
+                        if (_last_distance_valid) {
+                            boundary.set_face_attributes(face, _last_angle_deg, _last_distance_m);
                         } else {
-                            // a new sector started, the previous one can be updated now
-                            // create a location packet
-                            const boundary_location loc{_last_sector};
-                            boundary.set_attributes(loc, _angle_deg_last, _distance_m_last);
-                            // update boundary used for avoidance
-                            boundary.update_boundary(loc);
-                            // update OA database
-                            database_push(_angle_deg_last, _distance_m_last);
-
-                            // initialize the new sector
-                            _last_sector     = sector;
-                            _distance_m_last = distance_m;
-                            _angle_deg_last  = angle_deg;
+                            // reset distance from last face
+                            boundary.reset_face(face);
                         }
-                    } else {
-                        boundary.reset_sector(bnd_loc);
+
+                        // initialize the new face
+                        _last_face = face;
+                        _last_distance_valid = false;
+                    }
+                    if (distance_m > distance_min()) {
+                        // update shortest distance
+                        if (!_last_distance_valid || (distance_m < _last_distance_m)) {
+                            _last_distance_m = distance_m;
+                            _last_distance_valid = true;
+                            _last_angle_deg = angle_deg;
+                        }
+                        // update OA database
+                        database_push(_last_angle_deg, _last_distance_m);
                     }
                 }
             } else {

libraries/AP_Proximity/AP_Proximity_RPLidarA2.h

@@ -91,14 +91,15 @@ private:
     // request related variables
     enum ResponseType _response_type;         ///< response from the lidar
     enum rp_state _rp_state;
-    uint8_t   _last_sector;                   ///< last sector requested
     uint32_t  _last_request_ms;               ///< system time of last request
     uint32_t  _last_distance_received_ms;     ///< system time of last distance measurement received from sensor
     uint32_t  _last_reset_ms;
 
-    // sector related variables
-    float _angle_deg_last;
-    float _distance_m_last;
+    // face related variables
+    AP_Proximity_Boundary_3D::Face _last_face;///< last face requested
+    float _last_angle_deg;                    ///< yaw angle (in degrees) of _last_distance_m
+    float _last_distance_m;                   ///< shortest distance for _last_face
+    bool _last_distance_valid;                ///< true if _last_distance_m is valid
 
     struct PACKED _sensor_scan {
         uint8_t startbit      : 1;            ///< on the first revolution 1 else 0