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

AP_Proximity: SITL backend uses modified Boundary_3D interface

Browse Source
This commit is contained in:
Randy Mackay 2020-12-14 16:59:53 +09:00
parent 518078c901
commit d4a6ebd273
1 changed files with 13 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_Proximity/AP_Proximity_SITL.cpp
View File

@ -52,20 +52,19 @@ void AP_Proximity_SITL::update(void)
// only called to prompt polyfence to reload fence if required
}
if (AP::fence()->polyfence().inclusion_boundary_available()) {
// update distance in one sector
boundary_location bnd_loc{last_sector};
boundary.reset_sector(bnd_loc);
float fence_distance;
if (get_distance_to_fence(boundary._sector_middle_deg[last_sector], fence_distance)) {
set_status(AP_Proximity::Status::Good);
boundary.set_attributes(bnd_loc, boundary._sector_middle_deg[last_sector], fence_distance);
boundary.update_boundary(bnd_loc);
// update OA database
database_push(boundary._sector_middle_deg[last_sector], fence_distance);
}
last_sector++;
if (last_sector >= PROXIMITY_NUM_SECTORS) {
last_sector = 0;
// update distance in each sector
for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
const float yaw_angle_deg = sector * 45.0f;
AP_Proximity_Boundary_3D::Face face = boundary.get_face(yaw_angle_deg);
float fence_distance;
if (get_distance_to_fence(yaw_angle_deg, fence_distance)) {
set_status(AP_Proximity::Status::Good);
boundary.set_face_attributes(face, yaw_angle_deg, fence_distance);
// update OA database
database_push(yaw_angle_deg, fence_distance);
} else {
boundary.reset_face(face);
}
}
} else {
set_status(AP_Proximity::Status::NoData);