Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Avoid: fix complilation with no fence

Browse Source
This commit is contained in:
Iampete1 2022-03-05 00:45:18 +00:00 committed by Andrew Tridgell
parent fa440d532a
commit ed356d94cd
4 changed files with 41 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_Avoidance/AC_Avoid.cpp
View File

@ -132,6 +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 ((_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;
@ -152,6 +153,7 @@ void AC_Avoid::adjust_velocity_fence(float kP, float accel_cmss, Vector3f &desir
adjust_velocity_exclusion_circles(kP, accel_cmss_limited, desired_velocity_xy_cms, backup_vel_fence, dt); adjust_velocity_exclusion_circles(kP, accel_cmss_limited, desired_velocity_xy_cms, backup_vel_fence, dt);
find_max_quadrant_velocity(backup_vel_fence, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel); find_max_quadrant_velocity(backup_vel_fence, quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel);
} }
#endif // AP_FENCE_ENABLED
if ((_enabled & AC_AVOID_STOP_AT_BEACON_FENCE) > 0) { if ((_enabled & AC_AVOID_STOP_AT_BEACON_FENCE) > 0) {
// Store velocity needed to back away from beacon fence // Store velocity needed to back away from beacon fence
@ -363,6 +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
// 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) {
@ -373,6 +376,7 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c
alt_diff = fence->get_safe_alt_max() + veh_alt; alt_diff = fence->get_safe_alt_max() + veh_alt;
limit_alt = true; limit_alt = true;
} }
#endif
// calculate distance to (e.g.) optical flow altitude limit // calculate distance to (e.g.) optical flow altitude limit
// AHRS values are always in metres // AHRS values are always in metres
@ -657,6 +661,8 @@ float AC_Avoid::get_max_speed(float kP, float accel_cmss, float distance_cm, flo
} }
} }
#if AC_FENCE
/* /*
* Adjusts the desired velocity for the circular fence. * Adjusts the desired velocity for the circular fence.
*/ */
@ -1080,6 +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
/* /*
* Adjusts the desired velocity for the beacon fence. * Adjusts the desired velocity for the beacon fence.
@ -1102,9 +1109,11 @@ 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()) { if (AP::fence()) {
margin = AP::fence()->get_margin(); margin = AP::fence()->get_margin();
} }
#endif
adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, backup_vel, boundary, num_points, margin, dt, true); adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, backup_vel, boundary, num_points, margin, dt, true);
} }

16
libraries/AC_Avoidance/AP_OABendyRuler.cpp
View File

@ -456,6 +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
// 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) {
@ -476,12 +477,16 @@ bool AP_OABendyRuler::calc_margin_from_circular_fence(const Location &start, con
// margin is fence radius minus the longer of start or end distance // margin is fence radius minus the longer of start or end distance
margin = fence_radius_plus_margin - sqrtf(MAX(start_dist_sq, end_dist_sq)); margin = fence_radius_plus_margin - sqrtf(MAX(start_dist_sq, end_dist_sq));
return true; return true;
#else
return false;
#endif // AC_FENCE
} }
// 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
// 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) {
@ -508,12 +513,16 @@ bool AP_OABendyRuler::calc_margin_from_alt_fence(const Location &start, const Lo
margin = MIN(margin_start,margin_end); margin = MIN(margin_start,margin_end);
return true; return true;
#else
return false;
#endif // AC_FENCE
} }
// 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
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;
@ -574,12 +583,16 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Lo
} }
return margin_updated; return margin_updated;
#else
return false;
#endif // AC_FENCE
} }
// 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
// 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) {
@ -650,6 +663,9 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_circles(const Loc
} }
return margin_updated; return margin_updated;
#else
return false;
#endif // AC_FENCE
} }
// calculate minimum distance between a path and proximity sensor obstacles // calculate minimum distance between a path and proximity sensor obstacles

5
libraries/AC_Avoidance/AP_OADijkstra.cpp
View File

@ -17,6 +17,9 @@
#include "AP_OAPathPlanner.h" #include "AP_OAPathPlanner.h"
#include <AC_Fence/AC_Fence.h> #include <AC_Fence/AC_Fence.h>
#if AC_FENCE
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <AP_Logger/AP_Logger.h> #include <AP_Logger/AP_Logger.h>
@ -959,3 +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

10
libraries/AC_Avoidance/AP_OAPathPlanner.cpp
View File

@ -93,14 +93,18 @@ void AP_OAPathPlanner::init()
} }
break; break;
case OA_PATHPLAN_DIJKSTRA: case OA_PATHPLAN_DIJKSTRA:
#if AC_FENCE
if (_oadijkstra == nullptr) { if (_oadijkstra == nullptr) {
_oadijkstra = new AP_OADijkstra(_options); _oadijkstra = new AP_OADijkstra(_options);
} }
#endif
break; break;
case OA_PATHPLAN_DJIKSTRA_BENDYRULER: case OA_PATHPLAN_DJIKSTRA_BENDYRULER:
#if AC_FENCE
if (_oadijkstra == nullptr) { if (_oadijkstra == nullptr) {
_oadijkstra = new AP_OADijkstra(_options); _oadijkstra = new AP_OADijkstra(_options);
} }
#endif
if (_oabendyruler == nullptr) { if (_oabendyruler == nullptr) {
_oabendyruler = new AP_OABendyRuler(); _oabendyruler = new AP_OABendyRuler();
AP_Param::load_object_from_eeprom(_oabendyruler, AP_OABendyRuler::var_info); AP_Param::load_object_from_eeprom(_oabendyruler, AP_OABendyRuler::var_info);
@ -298,6 +302,7 @@ void AP_OAPathPlanner::avoidance_thread()
} }
case OA_PATHPLAN_DIJKSTRA: { case OA_PATHPLAN_DIJKSTRA: {
#if AC_FENCE
if (_oadijkstra == nullptr) { if (_oadijkstra == nullptr) {
continue; continue;
} }
@ -315,6 +320,7 @@ void AP_OAPathPlanner::avoidance_thread()
break; break;
} }
path_planner_used = OAPathPlannerUsed::Dijkstras; path_planner_used = OAPathPlannerUsed::Dijkstras;
#endif
break; break;
} }
@ -332,12 +338,15 @@ 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 (proximity_only == false) { if (proximity_only == false) {
_oadijkstra->recalculate_path(); _oadijkstra->recalculate_path();
} }
#endif
// only use proximity avoidance now for BendyRuler // only use proximity avoidance now for BendyRuler
proximity_only = true; proximity_only = true;
} }
#if AC_FENCE
_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) {
@ -352,6 +361,7 @@ void AP_OAPathPlanner::avoidance_thread()
break; break;
} }
path_planner_used = OAPathPlannerUsed::Dijkstras; path_planner_used = OAPathPlannerUsed::Dijkstras;
#endif
break; break;
} }