From 5c9a65cff6e0f23cee47cfdf02ee94b4e28fe570 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 14 Dec 2020 17:00:07 +0900 Subject: [PATCH] AP_Proximity: TeraRangerTower uses modified Boundary_3D interface --- libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp index f1532a2e17..4d2bbe7be6 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp @@ -93,14 +93,13 @@ bool AP_Proximity_TeraRangerTower::read_sensor_data() void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_t distance_cm) { // 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); + const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg); if (distance_cm != 0xffff) { - boundary.set_attributes(bnd_loc, angle_deg, ((float) distance_cm) / 1000); + boundary.set_face_attributes(face, angle_deg, ((float) distance_cm) / 1000); // update OA database database_push(angle_deg, ((float) distance_cm) / 1000); + } else { + boundary.reset_face(face); } _last_distance_received_ms = AP_HAL::millis(); - // update boundary used for avoidance - boundary.update_boundary(bnd_loc); }