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

AP_Proximity: TeraRangerTowerEvo uses modified Boundary_3D interface

Browse Source
This commit is contained in:
Randy Mackay 2020-12-14 17:00:17 +09:00
parent 5c9a65cff6
commit 71d928aab9
1 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp
View File

@ -146,16 +146,15 @@ bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint16_t distance_cm) void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
{ {
// Get location on 3-D boundary based on angle to the object // Get location on 3-D boundary based on angle to the object
const boundary_location bnd_loc = boundary.get_sector(angle_deg); const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg);
//check for target too far, target too close and sensor not connected //check for target too far, target too close and sensor not connected
const bool valid = distance_cm != 0xffff && distance_cm != 0x0000 && distance_cm != 0x0001; const bool valid = distance_cm != 0xffff && distance_cm != 0x0000 && distance_cm != 0x0001;
boundary.reset_sector(bnd_loc);
if (valid) { if (valid) {
boundary.set_attributes(bnd_loc, angle_deg, ((float) distance_cm) / 1000); boundary.set_face_attributes(face, angle_deg, ((float) distance_cm) / 1000);
// update OA database // update OA database
database_push(angle_deg, ((float) distance_cm) / 1000); database_push(angle_deg, ((float) distance_cm) / 1000);
} else {
boundary.reset_face(face);
} }
_last_distance_received_ms = AP_HAL::millis(); _last_distance_received_ms = AP_HAL::millis();
// update boundary used for avoidance
boundary.update_boundary(bnd_loc);
} }