Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: RangeFinder backend uses modified Boundary_3D interface

Browse Source
This commit is contained in:
Randy Mackay 2020-12-14 16:59:28 +09:00
parent 2d77adf720
commit babba91632
1 changed files with 5 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp
View File

@ -42,22 +42,20 @@ void AP_Proximity_RangeFinder::update(void)
// check for horizontal range finders // check for horizontal range finders
if (sensor->orientation() <= ROTATION_YAW_315) { if (sensor->orientation() <= ROTATION_YAW_315) {
const uint8_t sector = (uint8_t)sensor->orientation(); const uint8_t sector = (uint8_t)sensor->orientation();
const boundary_location bnd_loc{sector}; const float angle = sector * 45;
boundary.reset_sector(bnd_loc); const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle);
// distance in meters // distance in meters
const float distance_m = sensor->distance_cm() * 0.01f; const float distance_m = sensor->distance_cm() * 0.01f;
_distance_min = sensor->min_distance_cm() * 0.01f; _distance_min = sensor->min_distance_cm() * 0.01f;
_distance_max = sensor->max_distance_cm() * 0.01f; _distance_max = sensor->max_distance_cm() * 0.01f;
if ((distance_m <= _distance_max) && (distance_m >= _distance_min)) { if ((distance_m <= _distance_max) && (distance_m >= _distance_min)) {
const float angle = sector * 45; boundary.set_face_attributes(face, angle, distance_m);
boundary.set_attributes(bnd_loc, angle, distance_m);
// update OA database // update OA database
database_push(angle, distance_m); database_push(angle, distance_m);
} else {
boundary.reset_face(face);
} }
_last_update_ms = now; _last_update_ms = now;
boundary.update_boundary(bnd_loc);
} }
// check upward facing range finder // check upward facing range finder
if (sensor->orientation() == ROTATION_PITCH_90) { if (sensor->orientation() == ROTATION_PITCH_90) {