From a65e0affe4ad8322c8352ce18587a3a5318bf7b9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 14 Dec 2020 16:58:39 +0900 Subject: [PATCH] AP_Proximity: LightwareSF45B uses modified Boundary_3D interface --- .../AP_Proximity_LightWareSF45B.cpp | 42 +++++++++---------- .../AP_Proximity_LightWareSF45B.h | 10 ++--- 2 files changed, 24 insertions(+), 28 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp index 01c0d6070c..88fe5f158d 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp +++ b/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 diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h index c900eed946..725c3e6af6 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h +++ b/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