AP_Proximity: Convert Proximity Backend drivers to use 3D Boundary methods

libraries/AP_Proximity/AP_Proximity.cpp

@@ -359,21 +359,32 @@ bool AP_Proximity::get_horizontal_distances(Proximity_Distance_Array &prx_dist_a
     return drivers[primary_instance]->get_horizontal_distances(prx_dist_array);
 }
 
-// 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
+// get total number of obstacles, used in GPS based Simple Avoidance
+uint8_t AP_Proximity::get_obstacle_count() const
+{   
+    if (!valid_instance(primary_instance)) {
+        return 0;
+    }
+    return drivers[primary_instance]->get_obstacle_count();
+}
+
+// get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance
+void AP_Proximity::get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const
 {
-    if (!valid_instance(instance)) {
-        num_points = 0;
-        return nullptr;
+    if (!valid_instance(primary_instance)) {
+        return;
     }
-    // get boundary from backend
-    return drivers[instance]->get_boundary_points(num_points);
+    return drivers[primary_instance]->get_obstacle(obstacle_num, vec_to_obstacle);
 }
 
-const Vector2f* AP_Proximity::get_boundary_points(uint16_t& num_points) const
+// returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
+// used in GPS based Simple Avoidance
+float AP_Proximity::distance_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const
 {
-    return get_boundary_points(primary_instance, num_points);
+    if (!valid_instance(primary_instance)) {
+        return FLT_MAX;
+    }
+    return drivers[primary_instance]->distance_to_obstacle(obstacle_num, seg_start, seg_end, closest_point);
 }
 
 // get distance and angle to closest object (used for pre-arm check)
@@ -394,7 +405,7 @@ uint8_t AP_Proximity::get_object_count() const
         return 0;
     }
     // get count from backend
-    return drivers[primary_instance]->get_object_count();
+    return drivers[primary_instance]->get_horizontal_object_count();
 }
 
 // get an object's angle and distance, used for non-GPS avoidance
@@ -405,7 +416,7 @@ bool AP_Proximity::get_object_angle_and_distance(uint8_t object_number, float& a
         return false;
     }
     // get angle and distance from backend
-    return drivers[primary_instance]->get_object_angle_and_distance(object_number, angle_deg, distance);
+    return drivers[primary_instance]->get_horizontal_object_angle_and_distance(object_number, angle_deg, distance);
 }
 
 // get maximum and minimum distances (in meters) of primary sensor

libraries/AP_Proximity/AP_Proximity.h

@@ -24,6 +24,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_NUM_LAYERS 5
 
 class AP_Proximity_Backend;
 
@@ -88,10 +89,14 @@ public:
     // get distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
     bool get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) 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 total number of obstacles, used in GPS based Simple Avoidance
+    uint8_t get_obstacle_count() const;
+    
+    // get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance
+    void get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const;
+    
+    // returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
+    float distance_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const;
 
     // get distance and angle to closest object (used for pre-arm check)
     //   returns true on success, false if no valid readings

libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp

@@ -33,8 +33,8 @@ void AP_Proximity_AirSimSITL::update(void)
     }
 
     set_status(AP_Proximity::Status::Good);
-
-    memset(_distance_valid, 0, sizeof(_distance_valid));
+    // reset all horizontal sectors to default, so that it can be filled with the fresh lidar data
+    boundary.reset_all_horizontal_sectors();
 
     for (uint16_t i=0; i<points.length; i++) {
         Vector3f &point = points.data[i];
@@ -42,42 +42,37 @@ void AP_Proximity_AirSimSITL::update(void)
             continue;
         }
         const float angle_deg = wrap_360(degrees(atan2f(point.y, point.x)));
-        const uint8_t sector = convert_angle_to_sector(angle_deg);
-
+        // Get location on 3-D boundary based on angle to the object
+        const boundary_location bnd_loc = boundary.get_sector(angle_deg);
         const Vector2f v = Vector2f(point.x, point.y);
         const float distance_m = v.length();
 
         if (distance_m > distance_min()) {
-            if (_last_sector == sector) {
+            if (_last_sector == bnd_loc.sector) {
                 if (_distance_m_last > distance_m) {
                     _distance_m_last = distance_m;
                     _angle_deg_last = angle_deg;
                 }
             } else {
                 // new sector started, previous one can be updated
-                _distance_valid[_last_sector] = true;
-                _angle[_last_sector] = _angle_deg_last;
-                _distance[_last_sector] = _distance_m_last;
-
+                // create a boundary location object
+                const boundary_location set_loc{_last_sector};
+                boundary.set_attributes(set_loc, _angle_deg_last, _distance_m_last);
                 // update boundary
-                update_boundary_for_sector(_last_sector, true);
+                boundary.update_boundary(set_loc);
+                // update OA database
+                database_push(_angle_deg_last, _distance_m_last);
 
                 // initialize new sector
-                _last_sector = sector;
+                _last_sector = bnd_loc.sector;
                 _distance_m_last = INT16_MAX;
                 _angle_deg_last = angle_deg;
             }
         } else {
-            _distance_valid[sector] = false;
+            // reset data back to defaults at this location
+            boundary.reset_sector(bnd_loc);
         }
     }
-
-#if 0
-    printf("npoints=%u\n", points.length);
-    for (uint16_t i=0; i<PROXIMITY_NUM_SECTORS; i++) {
-        printf("sector[%u] ang=%.1f dist=%.1f\n", i, _angle[i], _distance[i]);
-    }
-#endif
 }
 
 // get maximum and minimum distances (in meters) of primary sensor

libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp

@@ -48,9 +48,6 @@ void AP_Proximity_LightWareSF40C::update(void)
 // initialise sensor
 void AP_Proximity_LightWareSF40C::initialise()
 {
-    // initialise boundary
-    init_boundary();
-
     // exit immediately if we've sent initialisation requests in the last second
     uint32_t now_ms = AP_HAL::millis();
     if ((now_ms - _last_request_ms) < 1000) {
@@ -321,18 +318,20 @@ void AP_Proximity_LightWareSF40C::process_message()
             const uint16_t idx = 14 + (i * 2);
             const int16_t dist_cm = (int16_t)buff_to_uint16(_msg.payload[idx], _msg.payload[idx+1]);
             const float angle_deg = wrap_360((point_start_index + i) * angle_inc_deg * angle_sign + angle_correction);
-            const uint8_t sector = convert_angle_to_sector(angle_deg);
-
+            // Get location on 3-D boundary based on angle to the object
+            const boundary_location bnd_loc = boundary.get_sector(angle_deg);
+            const uint8_t sector = bnd_loc.sector;
             // if we've entered a new sector then finish off previous sector
             if (sector != _last_sector) {
                 // update boundary used for avoidance
                 if (_last_sector != UINT8_MAX) {
-                    update_boundary_for_sector(_last_sector, false);
+                    // create a location package
+                    const boundary_location last_loc{_last_sector};
+                    boundary.update_boundary(last_loc);
                 }
                 // init for new sector
                 _last_sector = sector;
-                _distance[sector] = INT16_MAX;
-                _distance_valid[sector] = false;
+                boundary.reset_sector(bnd_loc);
             }
 
             // check reading is not within an ignore zone
@@ -342,10 +341,8 @@ void AP_Proximity_LightWareSF40C::process_message()
                     const float dist_m = dist_cm * 0.01f;
 
                     // update shortest distance for this sector
-                    if (dist_m < _distance[sector]) {
-                        _angle[sector] = angle_deg;
-                        _distance[sector] = dist_m;
-                        _distance_valid[sector] = true;
+                    if (dist_m < boundary.get_distance(bnd_loc)) {
+                        boundary.set_attributes(bnd_loc, angle_deg, dist_m);
                     }
 
                     // calculate shortest of last few readings

libraries/AP_Proximity/AP_Proximity_LightWareSF40C_v09.cpp

@@ -79,9 +79,6 @@ bool AP_Proximity_LightWareSF40C_v09::initialise()
         return false;
     }
 
-    // initialise sectors
-    init_boundary();
-
     return true;
 }
 
@@ -205,7 +202,7 @@ bool AP_Proximity_LightWareSF40C_v09::send_request_for_distance()
     char request_str[16];
     snprintf(request_str, sizeof(request_str), "?TS,%u,%u\r\n",
              (unsigned int)PROXIMITY_SECTOR_WIDTH_DEG,
-             _sector_middle_deg[_last_sector]);
+             boundary._sector_middle_deg[_last_sector]);
     _uart->write(request_str);
 
 
@@ -327,14 +324,19 @@ bool AP_Proximity_LightWareSF40C_v09::process_reply()
             float angle_deg = strtof(element_buf[0], NULL);
             float distance_m = strtof(element_buf[1], NULL);
             if (!ignore_reading(angle_deg)) {
-                const uint8_t sector = convert_angle_to_sector(angle_deg);
-                _angle[sector] = angle_deg;
-                _distance[sector] = distance_m;
-                _distance_valid[sector] = is_positive(distance_m);
                 _last_distance_received_ms = AP_HAL::millis();
                 success = true;
+                // Get location on 3-D boundary based on angle to the object
+                const boundary_location bnd_loc = boundary.get_sector(angle_deg);
+                // reset this sector back to default, so that it can be filled with the fresh sensor data
+                boundary.reset_sector(bnd_loc);
+                if (is_positive(distance_m)) {
+                    boundary.set_attributes(bnd_loc, angle_deg, distance_m);
+                    // update OA database
+                    database_push(angle_deg, distance_m);
+                }
                 // update boundary used for avoidance
-                update_boundary_for_sector(sector, true);
+                boundary.update_boundary(bnd_loc);
             }
             break;
         }

libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp

@@ -69,9 +69,6 @@ void AP_Proximity_LightWareSF45B::initialise()
 
     // request stream rate and contents
     request_stream_start();
-
-    // initialise boundary
-    init_boundary();
 }
 
 // request start of streaming of distances
@@ -141,16 +138,21 @@ void AP_Proximity_LightWareSF45B::process_message()
         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
-        const uint8_t sector = convert_angle_to_sector(angle_deg);
-        if (sector != _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) {
-                _angle[_sector] = _sector_angle;
-                _distance[_sector] = _sector_distance;
-                _distance_valid[_sector] = _sector_distance_valid;
-                update_boundary_for_sector(_sector, false);
+                // 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);
             }
             // init for new sector
-            _sector = sector;
+            _sector = bnd_loc.sector;
             _sector_angle = 0;
             _sector_distance = INT16_MAX;
             _sector_distance_valid = false;

libraries/AP_Proximity/AP_Proximity_MAV.cpp

@@ -21,7 +21,7 @@
 extern const AP_HAL::HAL& hal;
 
 #define PROXIMITY_MAV_TIMEOUT_MS    500 // distance messages must arrive within this many milliseconds
-
+#define PROXIMITY_3D_MSG_TIMEOUT_MS  50  // mini-fence will be cleared if OBSTACLE_DISTANCE_3D message does not arrive within this many milliseconds
 // update the state of the sensor
 void AP_Proximity_MAV::update(void)
 {
@@ -53,14 +53,25 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
 
         // store distance to appropriate sector based on orientation field
         if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) {
-            uint8_t sector = packet.orientation;
-            _angle[sector] = sector * 45;
-            _distance[sector] = packet.current_distance * 0.01f;
+            const uint8_t sector = packet.orientation;
+            // create a boundary location object based on this sector
+            const boundary_location bnd_loc{sector};
+            // store in meters
+            const uint16_t distance = packet.current_distance * 0.01f;
             _distance_min = packet.min_distance * 0.01f;
             _distance_max = packet.max_distance * 0.01f;
-            _distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max);
+            
+            // reset data on this sector, to be filled with new data
+            boundary.reset_sector(bnd_loc);
+            if (distance <= _distance_max && distance >= _distance_max) {
+                boundary.set_attributes(bnd_loc, sector * 45, distance);
+                // update OA database
+                database_push(boundary.get_angle(bnd_loc), distance);
+            }
+            
             _last_update_ms = AP_HAL::millis();
-            update_boundary_for_sector(sector, true);
+            // update boundary used for Obstacle Avoidance
+            boundary.update_boundary(bnd_loc);
         }
 
         // store upward distance
@@ -88,7 +99,6 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
             return;
         }
 
-        const float MAX_DISTANCE = 9999.0f;
         const uint8_t total_distances = MIN(((360.0f / fabsf(increment)) + 0.5f), MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN); // usually 72
 
         // set distance min and max
@@ -109,11 +119,8 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
 
         // initialise updated array and proximity sector angles (to closest object) and distances
         bool sector_updated[PROXIMITY_NUM_SECTORS];
-        for (uint8_t i = 0; i < PROXIMITY_NUM_SECTORS; i++) {
-            sector_updated[i] = false;
-            _angle[i] = _sector_middle_deg[i];
-            _distance[i] = MAX_DISTANCE;
-        }
+        memset(sector_updated, 0, sizeof(sector_updated));
+        boundary.reset_all_horizontal_sectors();
 
         // iterate over message's sectors
         for (uint8_t j = 0; j < total_distances; j++) {
@@ -133,7 +140,7 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
             // iterate over proximity sectors
             for (uint8_t i = 0; i < PROXIMITY_NUM_SECTORS; i++) {
                 // update distance array sector with shortest distance from message
-                const float angle_diff = wrap_180(_sector_middle_deg[i] - mid_angle);
+                const float angle_diff = wrap_180(boundary._sector_middle_deg[i] - mid_angle);
                 if (fabsf(angle_diff) > PROXIMITY_SECTOR_WIDTH_DEG*0.5f) {
                     // not even in this sector
                     continue;
@@ -146,15 +153,16 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
                     // safe.
                     continue;
                 }
-                if (packet_distance_m >= _distance[i]) {
+                if (packet_distance_m >= boundary.get_distance(i)) {
                     // this is no closer than a previous distance
-                    // found from the pakcet
+                    // found from the packet
                     continue;
                 }
 
                 // this is the shortest distance we've found in the packet so far
-                _distance[i] = packet_distance_m;
-                _angle[i] = mid_angle;
+                // create a location packet
+                const boundary_location bnd_loc{i};
+                boundary.set_attributes(bnd_loc, mid_angle, packet_distance_m);
                 sector_updated[i] = true;
             }
 
@@ -166,9 +174,9 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
 
         // update proximity sectors validity and boundary point
         for (uint8_t i = 0; i < PROXIMITY_NUM_SECTORS; i++) {
-            _distance_valid[i] = (_distance[i] >= _distance_min) && (_distance[i] <= _distance_max);
             if (sector_updated[i]) {
-                update_boundary_for_sector(i, false);
+                const boundary_location bnd_loc{i};
+                boundary.update_boundary(bnd_loc);
             }
         }
     }

libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp

@@ -103,9 +103,6 @@ float AP_Proximity_RPLidarA2::distance_min() const
 
 bool AP_Proximity_RPLidarA2::initialise()
 {
-    // initialise boundary
-    init_boundary();
-
     if (!_initialised) {
         reset_rplidar();            // set to a known state
         Debug(1, "LIDAR initialised");
@@ -315,7 +312,8 @@ void AP_Proximity_RPLidarA2::parse_response_data()
 #endif
                 _last_distance_received_ms = AP_HAL::millis();
                 if (!ignore_reading(angle_deg)) {
-                    const uint8_t sector = convert_angle_to_sector(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) {
@@ -324,18 +322,21 @@ void AP_Proximity_RPLidarA2::parse_response_data()
                             }
                         } else {
                             // a new sector started, the previous one can be updated now
-                            _angle[_last_sector] = _angle_deg_last;
-                            _distance[_last_sector] = _distance_m_last;
-                            _distance_valid[_last_sector] = true;
+                            // 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
-                            update_boundary_for_sector(_last_sector, true);
+                            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;
                         }
                     } else {
-                        _distance_valid[sector] = false;
+                        boundary.reset_sector(bnd_loc);
                     }
                 }
             } else {

libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp

@@ -41,14 +41,23 @@ void AP_Proximity_RangeFinder::update(void)
         if (sensor->has_data()) {
             // check for horizontal range finders
             if (sensor->orientation() <= ROTATION_YAW_315) {
-                uint8_t sector = (uint8_t)sensor->orientation();
-                _angle[sector] = sector * 45;
-                _distance[sector] = sensor->distance_cm() * 0.01f;
+                const uint8_t sector = (uint8_t)sensor->orientation();
+                const boundary_location bnd_loc{sector};
+                boundary.reset_sector(bnd_loc);
+                // distance in meters
+                const float distance_m = sensor->distance_cm() * 0.01f;
                 _distance_min = sensor->min_distance_cm() * 0.01f;
                 _distance_max = sensor->max_distance_cm() * 0.01f;
-                _distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max);
+
+                if ((distance_m <= _distance_max) && (distance_m >= _distance_min)) {
+                    const float angle = sector * 45;
+                    boundary.set_attributes(bnd_loc, angle, distance_m);
+                    // update OA database
+                    database_push(angle, distance_m);
+                }
+                
                 _last_update_ms = now;
-                update_boundary_for_sector(sector, true);
+                boundary.update_boundary(bnd_loc);
             }
             // check upward facing range finder
             if (sensor->orientation() == ROTATION_PITCH_90) {

libraries/AP_Proximity/AP_Proximity_SITL.cpp

@@ -53,13 +53,15 @@ void AP_Proximity_SITL::update(void)
     }
     if (AP::fence()->polyfence().inclusion_boundary_available()) {
         // update distance in one sector
-        if (get_distance_to_fence(_sector_middle_deg[last_sector], _distance[last_sector])) {
+        boundary_location bnd_loc{last_sector};
+        boundary.reset_sector(bnd_loc);
+        float fence_distance;
+        if (get_distance_to_fence(boundary._sector_middle_deg[last_sector], fence_distance)) {
             set_status(AP_Proximity::Status::Good);
-            _distance_valid[last_sector] = true;
-            _angle[last_sector] = _sector_middle_deg[last_sector];
-            update_boundary_for_sector(last_sector, true);
-        } else {
-            _distance_valid[last_sector] = false;
+            boundary.set_attributes(bnd_loc, boundary._sector_middle_deg[last_sector], fence_distance);
+            boundary.update_boundary(bnd_loc);
+            // update OA database
+            database_push(boundary._sector_middle_deg[last_sector], fence_distance);
         }
         last_sector++;
         if (last_sector >= PROXIMITY_NUM_SECTORS) {

libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp

@@ -91,12 +91,16 @@ bool AP_Proximity_TeraRangerTower::read_sensor_data()
 
 // process reply
 void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
-{
-    const uint8_t sector = convert_angle_to_sector(angle_deg);
-    _angle[sector] = angle_deg;
-    _distance[sector] = ((float) distance_cm) / 1000;
-    _distance_valid[sector] = distance_cm != 0xffff;
+{   
+    // Get location on 3-D boundary based on angle to the object
+    const boundary_location bnd_loc = boundary.get_sector(angle_deg);
+    boundary.reset_sector(bnd_loc);
+    if (distance_cm != 0xffff) {
+        boundary.set_attributes(bnd_loc, angle_deg, ((float) distance_cm) / 1000);
+        // update OA database
+        database_push(angle_deg, ((float) distance_cm) / 1000);
+    }
     _last_distance_received_ms = AP_HAL::millis();
     // update boundary used for avoidance
-    update_boundary_for_sector(sector, true);
+    boundary.update_boundary(bnd_loc);  
 }

libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp

@@ -144,14 +144,18 @@ bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
 
 // process reply
 void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
-{
-    const uint8_t sector = convert_angle_to_sector(angle_deg);
-    _angle[sector] = angle_deg;
-    _distance[sector] = ((float) distance_cm) / 1000;
-
+{   
+    // Get location on 3-D boundary based on angle to the object
+    const boundary_location bnd_loc = boundary.get_sector(angle_deg);
     //check for target too far, target too close and sensor not connected
-    _distance_valid[sector] = distance_cm != 0xffff && distance_cm != 0x0000 && distance_cm != 0x0001;
+    const bool valid = distance_cm != 0xffff && distance_cm != 0x0000 && distance_cm != 0x0001;
+    boundary.reset_sector(bnd_loc);
+    if (valid) {
+        boundary.set_attributes(bnd_loc, angle_deg, ((float) distance_cm) / 1000);
+        // update OA database
+        database_push(angle_deg, ((float) distance_cm) / 1000);
+    }
     _last_distance_received_ms = AP_HAL::millis();
     // update boundary used for avoidance
-    update_boundary_for_sector(sector, true);
+    boundary.update_boundary(bnd_loc);
 }