diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index b77d6e3934..54f05d9604 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/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); } diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.cpp b/libraries/AC_Avoidance/AP_OABendyRuler.cpp index ba38ddd6e2..fa423a4243 100644 --- a/libraries/AC_Avoidance/AP_OABendyRuler.cpp +++ b/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 diff --git a/libraries/AC_Avoidance/AP_OADijkstra.cpp b/libraries/AC_Avoidance/AP_OADijkstra.cpp index 11cc432536..90325cceba 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.cpp +++ b/libraries/AC_Avoidance/AP_OADijkstra.cpp @@ -17,6 +17,9 @@ #include "AP_OAPathPlanner.h" #include + +#if AC_FENCE + #include #include @@ -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 + diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp index 84a9d47d4d..4fd7ced35e 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp +++ b/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; }