Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_OABendyRuler: integrate static object radius

This commit is contained in:
Randy Mackay 2019-11-14 17:10:51 +09:00
parent 52c1f9f798
commit c9c28abac1
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AC_Avoidance/AP_OABendyRuler.cpp
View File

@ -374,12 +374,13 @@ bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, co
// convert obstacle's location to offset (in cm) from EKF origin
Vector2f point;
if (!oaDb->get_item(i).loc.get_vector_xy_from_origin_NE(point)) {
const AP_OADatabase::OA_DbItem& item = oaDb->get_item(i);
if (!item.loc.get_vector_xy_from_origin_NE(point)) {
continue;
}
// margin is distance between line segment and obstacle minus obstacle's radius
const float m = Vector2f::closest_distance_between_line_and_point(start_NE, end_NE, point) * 0.01f - oaDb->get_accuracy();
const float m = Vector2f::closest_distance_between_line_and_point(start_NE, end_NE, point) * 0.01f - item.radius;
if (m < smallest_margin) {
smallest_margin = m;
}