From 71d928aab9793971037581b63fe0e267a9f9f076 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 14 Dec 2020 17:00:17 +0900 Subject: [PATCH] AP_Proximity: TeraRangerTowerEvo uses modified Boundary_3D interface --- .../AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp b/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp index 0ac71d4562..20c67329cf 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp @@ -144,18 +144,17 @@ bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data() // process reply void AP_Proximity_TeraRangerTowerEvo::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); + const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg); //check for target too far, target too close and sensor not connected 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); + 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); }