|
|
|
@ -456,7 +456,7 @@ float AP_OABendyRuler::calc_avoidance_margin(const Location &start, const Locati
@@ -456,7 +456,7 @@ float AP_OABendyRuler::calc_avoidance_margin(const Location &start, const Locati
|
|
|
|
|
// on success returns true and updates margin
|
|
|
|
|
bool AP_OABendyRuler::calc_margin_from_circular_fence(const Location &start, const Location &end, float &margin) const |
|
|
|
|
{ |
|
|
|
|
#if AC_FENCE |
|
|
|
|
#if AP_FENCE_ENABLED |
|
|
|
|
// exit immediately if polygon fence is not enabled
|
|
|
|
|
const AC_Fence *fence = AC_Fence::get_singleton(); |
|
|
|
|
if (fence == nullptr) { |
|
|
|
@ -479,14 +479,14 @@ bool AP_OABendyRuler::calc_margin_from_circular_fence(const Location &start, con
@@ -479,14 +479,14 @@ bool AP_OABendyRuler::calc_margin_from_circular_fence(const Location &start, con
|
|
|
|
|
return true; |
|
|
|
|
#else |
|
|
|
|
return false; |
|
|
|
|
#endif // AC_FENCE
|
|
|
|
|
#endif // AP_FENCE_ENABLED
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// calculate minimum distance between a path and the altitude fence
|
|
|
|
|
// on success returns true and updates margin
|
|
|
|
|
bool AP_OABendyRuler::calc_margin_from_alt_fence(const Location &start, const Location &end, float &margin) const |
|
|
|
|
{ |
|
|
|
|
#if AC_FENCE |
|
|
|
|
#if AP_FENCE_ENABLED |
|
|
|
|
// exit immediately if polygon fence is not enabled
|
|
|
|
|
const AC_Fence *fence = AC_Fence::get_singleton(); |
|
|
|
|
if (fence == nullptr) { |
|
|
|
@ -515,14 +515,14 @@ bool AP_OABendyRuler::calc_margin_from_alt_fence(const Location &start, const Lo
@@ -515,14 +515,14 @@ bool AP_OABendyRuler::calc_margin_from_alt_fence(const Location &start, const Lo
|
|
|
|
|
return true; |
|
|
|
|
#else |
|
|
|
|
return false; |
|
|
|
|
#endif // AC_FENCE
|
|
|
|
|
#endif // AP_FENCE_ENABLED
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// calculate minimum distance between a path and all inclusion and exclusion polygons
|
|
|
|
|
// on success returns true and updates margin
|
|
|
|
|
bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Location &start, const Location &end, float &margin) const |
|
|
|
|
{ |
|
|
|
|
#if AC_FENCE |
|
|
|
|
#if AP_FENCE_ENABLED |
|
|
|
|
const AC_Fence *fence = AC_Fence::get_singleton(); |
|
|
|
|
if (fence == nullptr) { |
|
|
|
|
return false; |
|
|
|
@ -585,14 +585,14 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Lo
@@ -585,14 +585,14 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Lo
|
|
|
|
|
return margin_updated; |
|
|
|
|
#else |
|
|
|
|
return false; |
|
|
|
|
#endif // AC_FENCE
|
|
|
|
|
#endif // AP_FENCE_ENABLED
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// calculate minimum distance between a path and all inclusion and exclusion circles
|
|
|
|
|
// on success returns true and updates margin
|
|
|
|
|
bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_circles(const Location &start, const Location &end, float &margin) const |
|
|
|
|
{ |
|
|
|
|
#if AC_FENCE |
|
|
|
|
#if AP_FENCE_ENABLED |
|
|
|
|
// exit immediately if fence is not enabled
|
|
|
|
|
const AC_Fence *fence = AC_Fence::get_singleton(); |
|
|
|
|
if (fence == nullptr) { |
|
|
|
@ -665,7 +665,7 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_circles(const Loc
@@ -665,7 +665,7 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_circles(const Loc
|
|
|
|
|
return margin_updated; |
|
|
|
|
#else |
|
|
|
|
return false; |
|
|
|
|
#endif // AC_FENCE
|
|
|
|
|
#endif // AP_FENCE_ENABLED
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// calculate minimum distance between a path and proximity sensor obstacles
|
|
|
|
|