aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Proximity: LightwareSF45B uses modified Boundary_3D interface

Browse Source
This commit is contained in:
Randy Mackay 2020-12-14 16:58:39 +09:00
parent 623bd6923e
commit a65e0affe4
2 changed files with 24 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp
View File

@ -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

10
libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h
View File

@ -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