diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.cpp b/libraries/AP_Proximity/AP_Proximity_SITL.cpp index 201d09a462..feec22c898 100644 --- a/libraries/AP_Proximity/AP_Proximity_SITL.cpp +++ b/libraries/AP_Proximity/AP_Proximity_SITL.cpp @@ -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);