AC_Avoid: fix complilation with no fence

libraries/AC_Avoidance/AC_Avoid.cpp

@@ -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 
     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) {
         // Store velocity needed to back away from 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);
         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) {
         // 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();
 
+#if AC_FENCE
     // 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) {
@@ -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;
         limit_alt = true;
     }
+#endif
 
     // calculate distance to (e.g.) optical flow altitude limit
     // 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.
  */
@@ -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 
     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.
@@ -1102,9 +1109,11 @@ 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()) {
         margin = AP::fence()->get_margin();
     }
+#endif
     adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, backup_vel, boundary, num_points, margin, dt, true);
 }
 

libraries/AC_Avoidance/AP_OABendyRuler.cpp

@@ -456,6 +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
     // exit immediately if polygon fence is not enabled
     const AC_Fence *fence = AC_Fence::get_singleton();
     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 = fence_radius_plus_margin - sqrtf(MAX(start_dist_sq, end_dist_sq));
     return true;
+#else
+    return false;
+#endif // AC_FENCE
 }
 
 // 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
     // exit immediately if polygon fence is not enabled
     const AC_Fence *fence = AC_Fence::get_singleton();
     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);
 
     return true;
+#else
+    return false;
+#endif // AC_FENCE
 }
 
 // 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
     const AC_Fence *fence = AC_Fence::get_singleton();
     if (fence == nullptr) {
         return false;
@@ -574,12 +583,16 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Lo
     }
 
     return margin_updated;
+#else
+    return false;
+#endif // AC_FENCE
 }
 
 // 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
     // exit immediately if fence is not enabled
     const AC_Fence *fence = AC_Fence::get_singleton();
     if (fence == nullptr) {
@@ -650,6 +663,9 @@ bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_circles(const Loc
     }
 
     return margin_updated;
+#else
+    return false;
+#endif // AC_FENCE
 }
 
 // calculate minimum distance between a path and proximity sensor obstacles

libraries/AC_Avoidance/AP_OADijkstra.cpp

@@ -17,6 +17,9 @@
 #include "AP_OAPathPlanner.h"
 
 #include <AC_Fence/AC_Fence.h>
+
+#if AC_FENCE
+
 #include <AP_AHRS/AP_AHRS.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
     return false;
 }
+#endif // AC_FENCE
+

libraries/AC_Avoidance/AP_OAPathPlanner.cpp

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