From a895d2e7c9202542993ec3b2701c3b812572bdb2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 4 Dec 2019 16:16:20 +0900 Subject: [PATCH] AP_OABendyRuler: integrate oadb ekf-offset change --- libraries/AC_Avoidance/AP_OABendyRuler.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.cpp b/libraries/AC_Avoidance/AP_OABendyRuler.cpp index 419a6cd0cc..e1be25294a 100644 --- a/libraries/AC_Avoidance/AP_OABendyRuler.cpp +++ b/libraries/AC_Avoidance/AP_OABendyRuler.cpp @@ -371,16 +371,10 @@ bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, co // check each obstacle's distance from segment float smallest_margin = FLT_MAX; for (uint16_t i=0; idatabase_count(); i++) { - - // convert obstacle's location to offset (in cm) from EKF origin - Vector2f point; const AP_OADatabase::OA_DbItem& item = oaDb->get_item(i); - if (!item.loc.get_vector_xy_from_origin_NE(point)) { - continue; - } - + const Vector2f point_cm = item.pos * 100.0f; // 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 - item.radius; + const float m = Vector2f::closest_distance_between_line_and_point(start_NE, end_NE, point_cm) * 0.01f - item.radius; if (m < smallest_margin) { smallest_margin = m; }