AP_Proximity: LightwareSF45B uses modified Boundary_3D interface

libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp

@@ -137,25 +137,21 @@ void AP_Proximity_LightWareSF45B::process_message()
         const float distance_m = _distance_filt.apply((int16_t)UINT16_VALUE(_msg.payload[1], _msg.payload[0])) * 0.01f;
         const float angle_deg = correct_angle_for_orientation((int16_t)UINT16_VALUE(_msg.payload[3], _msg.payload[2]) * 0.01f);
 
-        // if distance is from a new sector then update distance, angle and boundary for previous sector
-        // Get location on 3-D boundary based on angle to the object
-        const boundary_location bnd_loc = boundary.get_sector(angle_deg);
-        if (bnd_loc.sector != _sector) {
-            if (_sector != UINT8_MAX) {
-                // create a location packet
-                const boundary_location loc{_sector};
-                boundary.reset_sector(loc);
-                if (_sector_distance_valid) {
-                    boundary.set_attributes(loc, _sector_angle, _sector_distance);
-                } 
-                // update boundary used for Obstacle Avoidance
-                boundary.update_boundary(loc);
+        // if distance is from a new face then update distance, angle and boundary for previous face
+        // get face from 3D boundary based on yaw angle to the object
+        const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg);
+        if (face != _face) {
+            if (_face_distance_valid) {
+                boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance);
+            } else {
+                // mark previous face invalid
+                boundary.reset_face(_face);
             }
-            // init for new sector
-            _sector = bnd_loc.sector;
-            _sector_angle = 0;
-            _sector_distance = INT16_MAX;
-            _sector_distance_valid = false;
+            // record updated face
+            _face = face;
+            _face_yaw_deg = 0;
+            _face_distance = INT16_MAX;
+            _face_distance_valid = false;
         }
 
         // if distance is from a new minisector then update obstacle database using angle and distance from previous minisector
@@ -173,11 +169,11 @@ void AP_Proximity_LightWareSF45B::process_message()
 
         // check reading is valid
         if (!ignore_reading(angle_deg) && (distance_m >= distance_min()) && (distance_m <= distance_max())) {
-            // update shortest distance for this sector
-            if (distance_m < _sector_distance) {
-                _sector_angle = angle_deg;
-                _sector_distance = distance_m;
-                _sector_distance_valid = true;
+            // update shortest distance for this face
+            if (!_face_distance_valid || (distance_m < _face_distance)) {
+                _face_yaw_deg = angle_deg;
+                _face_distance = distance_m;
+                _face_distance_valid = true;
             }
 
             // update shortest distance for this mini sector

libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h

@@ -80,11 +80,11 @@ private:
     bool _init_complete;                    // true once sensor initialisation is complete
     ModeFilterInt16_Size5 _distance_filt{2};// mode filter to reduce glitches
 
-    // sector (45 degrees) angles and distances (used to build mini fence for simple avoidance)
-    uint8_t _sector = UINT8_MAX;            // sector number (from 0 to 7) of most recently received distance
-    float _sector_distance;                 // shortest distance (in meters) in sector
-    float _sector_angle;                    // angle (in degrees) of shortest distance in sector
-    bool _sector_distance_valid;            // true if sector has at least one valid distance
+    // 3D boundary face and distance for latest readings
+    AP_Proximity_Boundary_3D::Face _face;   // face of most recently received distance
+    float _face_distance;                   // shortest distance (in meters) on face
+    float _face_yaw_deg;                    // yaw angle (in degrees) of shortest distance on face
+    bool _face_distance_valid;              // true if face has at least one valid distance
 
     // mini sector (5 degrees) angles and distances (used to populate obstacle database for path planning)
     uint8_t _minisector = UINT8_MAX;        // mini sector number (from 0 to 71) of most recently received distance