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

Fix & Update Obstacle Margin Check 

Fixed error in margin checking with object point vector
Update obstacle check for better performance
This commit is contained in:
Seunghwan Jo 2023-08-02 13:01:49 +09:00 committed by GitHub
parent d503222ff4
commit a7e4465a1d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 14 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AC_Avoidance/AP_OABendyRuler.cpp
View File

@ -688,7 +688,7 @@ bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, co
return false;
}
// convert cm to m without division
// // convert cm to m without division
start_NEU *= 0.01f;
end_NEU *= 0.01f;
@ -697,19 +697,25 @@ bool AP_OABendyRuler::calc_margin_from_object_database(const Location &start, co
// this is based on the explanation given here: www.fundza.com/vectors/point2line/index.html
Vector3f line_vec = end_NEU - start_NEU;
float line_length = line_vec.length();
float scale = 1.0f / line_length;
bool length_ok = !is_zero(line_length);
// protection against divide by zero
float scale = length_ok ? 1.0f / line_length : 0.0f;
const Vector3f line_unit_vec = line_vec * scale;
// check each obstacle's distance from segment
float smallest_margin = FLT_MAX;
for (uint16_t i=0; i<oaDb->database_count(); ++i) {
const AP_OADatabase::OA_DbItem& item = oaDb->get_item(i);
// declare point_vec with protection against divide by zero
const Vector3f point_vec = ::is_zero(line_length) ? Vector3f(0.0f, 0.0f, 0.0f): item.pos - start_NEU;
const Vector3f scaled_point_vec = point_vec * scale;
const float dot_product = constrain_float(scaled_point_vec.dot(line_unit_vec), 0.0f, 1.0f);
const Vector3f closest_point = line_vec * dot_product;
const Vector3f point_vec = item.pos - start_NEU;
Vector3f closest_point = start_NEU;
// protection against divide by zero
if (length_ok) {
const Vector3f scaled_point_vec = point_vec * scale;
float dot_product = constrain_ftype(line_unit_vec * scaled_point_vec, 0.0f, 1.0f);
closest_point += line_vec * dot_product;
}
// margin is distance between line segment and obstacle minus obstacle's radius
const float m = (point_vec - closest_point).length() - item.radius;
const float m = (closest_point - item.pos).length() - item.radius;
if (m < smallest_margin) {
smallest_margin = m;
}