mirror of https://github.com/ArduPilot/ardupilot
AC_Avoidance: change AC_FENCE to AP_FENCE_ENABLED
This commit is contained in:
Iampete1
Andrew Tridgell
parent
9dc318f0cc
commit
5f31818658
|
|
@ -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
|
||||
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) {
|
||||
// Store velocity needed to back away from 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();
|
||||
|
||||
#if AC_FENCE
|
||||
#if AP_FENCE_ENABLED
|
||||
// calculate distance below 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) {
|
||||
|
|
@ -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.
|
||||
|
|
@ -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
|
||||
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.
|
||||
|
|
@ -1109,7 +1109,7 @@ void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f
|
|||
|
||||
// adjust velocity using beacon
|
||||
float margin = 0;
|
||||
#if AC_FENCE
|
||||
#if AP_FENCE_ENABLED
|
||||
if (AP::fence()) {
|
||||
margin = AP::fence()->get_margin();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
|||
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
|
|||
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
|
|||
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
|
|||
return margin_updated;
|
||||
#else
|
||||
return false;
|
||||
#endif // AC_FENCE
|
||||
#endif // AP_FENCE_ENABLED
|
||||
}
|
||||
|
||||
// calculate minimum distance between a path and proximity sensor obstacles
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@
|
|||
|
||||
#include <AC_Fence/AC_Fence.h>
|
||||
|
||||
#if AC_FENCE
|
||||
#if AP_FENCE_ENABLED
|
||||
|
||||
#include <AP_AHRS/AP_AHRS.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
|
||||
return false;
|
||||
}
|
||||
#endif // AC_FENCE
|
||||
#endif // AP_FENCE_ENABLED
|
||||
|
||||
|
|
|
|||
|
|
@ -93,14 +93,14 @@ void AP_OAPathPlanner::init()
|
|||
}
|
||||
break;
|
||||
case OA_PATHPLAN_DIJKSTRA:
|
||||
#if AC_FENCE
|
||||
#if AP_FENCE_ENABLED
|
||||
if (_oadijkstra == nullptr) {
|
||||
_oadijkstra = new AP_OADijkstra(_options);
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
case OA_PATHPLAN_DJIKSTRA_BENDYRULER:
|
||||
#if AC_FENCE
|
||||
#if AP_FENCE_ENABLED
|
||||
if (_oadijkstra == nullptr) {
|
||||
_oadijkstra = new AP_OADijkstra(_options);
|
||||
}
|
||||
|
|
@ -302,7 +302,7 @@ void AP_OAPathPlanner::avoidance_thread()
|
|||
}
|
||||
|
||||
case OA_PATHPLAN_DIJKSTRA: {
|
||||
#if AC_FENCE
|
||||
#if AP_FENCE_ENABLED
|
||||
if (_oadijkstra == nullptr) {
|
||||
continue;
|
||||
}
|
||||
|
|
@ -338,7 +338,7 @@ void AP_OAPathPlanner::avoidance_thread()
|
|||
break;
|
||||
} else {
|
||||
// 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) {
|
||||
_oadijkstra->recalculate_path();
|
||||
}
|
||||
|
|
@ -346,7 +346,7 @@ void AP_OAPathPlanner::avoidance_thread()
|
|||
// only use proximity avoidance now for BendyRuler
|
||||
proximity_only = true;
|
||||
}
|
||||
#if AC_FENCE
|
||||
#if AP_FENCE_ENABLED
|
||||
_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);
|
||||
switch (dijkstra_state) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue