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

AC_Avoidance: change AC_FENCE to AP_FENCE_ENABLED

This commit is contained in:
Iampete1 2022-07-19 12:33:12 +01:00 committed by Andrew Tridgell
parent 9dc318f0cc
commit 5f31818658
4 changed files with 20 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -132,7 +132,7 @@ void AC_Avoid::adjust_velocity_fence(float kP, float accel_cmss, Vector3f &desir
// maximum component of desired backup velocity in each quadrant // maximum component of desired backup velocity in each quadrant
Vector2f quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel; Vector2f quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel;
#if AC_FENCE #if AP_FENCE_ENABLED
if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0) { if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0) {
// Store velocity needed to back away from fence // Store velocity needed to back away from fence
Vector2f backup_vel_fence; Vector2f backup_vel_fence;
@ -365,7 +365,7 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c
const AP_AHRS &_ahrs = AP::ahrs(); const AP_AHRS &_ahrs = AP::ahrs();
#if AC_FENCE #if AP_FENCE_ENABLED
// calculate distance below fence // calculate distance below fence
AC_Fence *fence = AP::fence(); AC_Fence *fence = AP::fence();
if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && fence && (fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) { if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0 && fence && (fence->get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) > 0) {
@ -661,7 +661,7 @@ float AC_Avoid::get_max_speed(float kP, float accel_cmss, float distance_cm, flo
} }
} }
#if AC_FENCE #if AP_FENCE_ENABLED
/* /*
* Adjusts the desired velocity for the circular fence. * Adjusts the desired velocity for the circular fence.
@ -1086,7 +1086,7 @@ void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vec
// desired backup velocity is sum of maximum velocity component in each quadrant // desired backup velocity is sum of maximum velocity component in each quadrant
backup_vel = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel; backup_vel = quad_1_back_vel + quad_2_back_vel + quad_3_back_vel + quad_4_back_vel;
} }
#endif // AC_FENCE #endif // AP_FENCE_ENABLED
/* /*
* Adjusts the desired velocity for the beacon fence. * Adjusts the desired velocity for the beacon fence.
@ -1109,7 +1109,7 @@ void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f
// adjust velocity using beacon // adjust velocity using beacon
float margin = 0; float margin = 0;
#if AC_FENCE #if AP_FENCE_ENABLED
if (AP::fence()) { if (AP::fence()) {
margin = AP::fence()->get_margin(); margin = AP::fence()->get_margin();
} }

16
libraries/AC_Avoidance/AP_OABendyRuler.cpp
View File

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

4
libraries/AC_Avoidance/AP_OADijkstra.cpp
View File

@ -18,7 +18,7 @@
#include <AC_Fence/AC_Fence.h> #include <AC_Fence/AC_Fence.h>
#if AC_FENCE #if AP_FENCE_ENABLED
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <AP_Logger/AP_Logger.h> #include <AP_Logger/AP_Logger.h>
@ -962,5 +962,5 @@ bool AP_OADijkstra::convert_node_to_point(const AP_OAVisGraph::OAItemID& id, Vec
// we should never reach here but just in case // we should never reach here but just in case
return false; return false;
} }
#endif // AC_FENCE #endif // AP_FENCE_ENABLED

10
libraries/AC_Avoidance/AP_OAPathPlanner.cpp
View File

@ -93,14 +93,14 @@ void AP_OAPathPlanner::init()
} }
break; break;
case OA_PATHPLAN_DIJKSTRA: case OA_PATHPLAN_DIJKSTRA:
#if AC_FENCE #if AP_FENCE_ENABLED
if (_oadijkstra == nullptr) { if (_oadijkstra == nullptr) {
_oadijkstra = new AP_OADijkstra(_options); _oadijkstra = new AP_OADijkstra(_options);
} }
#endif #endif
break; break;
case OA_PATHPLAN_DJIKSTRA_BENDYRULER: case OA_PATHPLAN_DJIKSTRA_BENDYRULER:
#if AC_FENCE #if AP_FENCE_ENABLED
if (_oadijkstra == nullptr) { if (_oadijkstra == nullptr) {
_oadijkstra = new AP_OADijkstra(_options); _oadijkstra = new AP_OADijkstra(_options);
} }
@ -302,7 +302,7 @@ void AP_OAPathPlanner::avoidance_thread()
} }
case OA_PATHPLAN_DIJKSTRA: { case OA_PATHPLAN_DIJKSTRA: {
#if AC_FENCE #if AP_FENCE_ENABLED
if (_oadijkstra == nullptr) { if (_oadijkstra == nullptr) {
continue; continue;
} }
@ -338,7 +338,7 @@ void AP_OAPathPlanner::avoidance_thread()
break; break;
} else { } else {
// cleared all obstacles, trigger Dijkstra's to calculate path based on current deviated position // cleared all obstacles, trigger Dijkstra's to calculate path based on current deviated position
#if AC_FENCE #if AP_FENCE_ENABLED
if (proximity_only == false) { if (proximity_only == false) {
_oadijkstra->recalculate_path(); _oadijkstra->recalculate_path();
} }
@ -346,7 +346,7 @@ void AP_OAPathPlanner::avoidance_thread()
// only use proximity avoidance now for BendyRuler // only use proximity avoidance now for BendyRuler
proximity_only = true; proximity_only = true;
} }
#if AC_FENCE #if AP_FENCE_ENABLED
_oadijkstra->set_fence_margin(_margin_max); _oadijkstra->set_fence_margin(_margin_max);
const AP_OADijkstra::AP_OADijkstra_State dijkstra_state = _oadijkstra->update(avoidance_request2.current_loc, avoidance_request2.destination, origin_new, destination_new); const AP_OADijkstra::AP_OADijkstra_State dijkstra_state = _oadijkstra->update(avoidance_request2.current_loc, avoidance_request2.destination, origin_new, destination_new);
switch (dijkstra_state) { switch (dijkstra_state) {