Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: LightwareSF40C uses modified Boundary_3D interface

Browse Source
This commit is contained in:
Randy Mackay 2020-12-14 16:57:27 +09:00
parent ad899de78c
commit 4fda6b38f7
2 changed files with 20 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp
View File

@ -318,20 +318,20 @@ void AP_Proximity_LightWareSF40C::process_message()
const uint16_t idx = 14 + (i * 2);
const int16_t dist_cm = (int16_t)buff_to_uint16(_msg.payload[idx], _msg.payload[idx+1]);
const float angle_deg = wrap_360((point_start_index + i) * angle_inc_deg * angle_sign + angle_correction);
// Get location on 3-D boundary based on angle to the object
const boundary_location bnd_loc = boundary.get_sector(angle_deg);
const uint8_t sector = bnd_loc.sector;
// if we've entered a new sector then finish off previous sector
if (sector != _last_sector) {
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg);
// if point is on a new face then finish off previous face
if (face != _face) {
// update boundary used for avoidance
if (_last_sector != UINT8_MAX) {
// create a location package
const boundary_location last_loc{_last_sector};
boundary.update_boundary(last_loc);
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
_last_sector = sector;
boundary.reset_sector(bnd_loc);
// init for new face
_face = face;
_face_distance_valid = false;
}
// check reading is not within an ignore zone
@ -340,9 +340,10 @@ void AP_Proximity_LightWareSF40C::process_message()
if ((dist_cm >= dist_min_cm) && (dist_cm <= dist_max_cm)) {
const float dist_m = dist_cm * 0.01f;
// update shortest distance for this sector
if (dist_m < boundary.get_distance(bnd_loc)) {
boundary.set_attributes(bnd_loc, angle_deg, dist_m);
// update shortest distance for this face
if (!_face_distance_valid || dist_m < _face_distance) {
_face_distance = dist_m;
_face_distance_valid = true;
}
// calculate shortest of last few readings

5
libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h
View File

@ -109,7 +109,10 @@ private:
uint32_t _last_reply_ms; // system time of last valid reply
uint32_t _last_restart_ms; // system time we restarted the sensor
uint32_t _last_distance_received_ms; // system time of last distance measurement received from sensor
uint8_t _last_sector = UINT8_MAX; // sector of last distance_cm
AP_Proximity_Boundary_3D::Face _face; // face of _face_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
// state of sensor
struct {