From 518078c901c8ee3a44f3e245d9abc095bdfb3482 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 14 Dec 2020 16:59:41 +0900 Subject: [PATCH] AP_Proximity: RPLidarA2 uses modified Boundary_3D interface --- .../AP_Proximity/AP_Proximity_RPLidarA2.cpp | 45 +++++++++---------- .../AP_Proximity/AP_Proximity_RPLidarA2.h | 9 ++-- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp index 7988682199..41869cfa5c 100644 --- a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp +++ b/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; - } - } 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); + const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg); - // initialize the new sector - _last_sector = sector; - _distance_m_last = distance_m; - _angle_deg_last = 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 { + // 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 { diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h index 6238ceca33..c512f2b367 100644 --- a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h +++ b/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