AC_Avoid: add support for complex fence types

AP_OABendyRuler: support exclusion polygons

AP_OADijkstra: support exclusion polygons

AC_Avoid: adjust_velocity supports exclusion polygons

AC_Avoidance: handle fence::get_boundary_points returning nullptr instead of setting num_points to zero

AC_Avoidance: Dijkstra's works with only exclusion polygons

AC_Avoidance: Dijkstra: check for fence counts instead of polyfence validity

We really only care whether fences can be returned - and they won't be
returned unless they are valid

AC_Avoidance: BendyRuler: just try to get inclusion fence rather than checking validity

AC_Avoidance: BendyRuler supports exclusion circles

AC_Avoid: Dijkstra support for exclusion circles

AC_Avoid: BendyRuler support for inclusion circles

AC_Avoid: stop an inclusion/exclusion circular fences

AC_Avoid: stop at inclusion/exclusion circular fences

AC_Avoid: fixes to Dijkstra's use of inclusion/exclusion circles and polygons

AP_Avoidance: take semaphores when interacting with AHRS and polyfence

AC_Avoid: Dijkstra's fix for some_fences_enabled inclusion circles

libraries/AC_Avoidance/AC_Avoid.cpp

@@ -19,6 +19,8 @@
 #include <AP_Proximity/AP_Proximity.h>
 #include <AP_Beacon/AP_Beacon.h>
 
+#include <stdio.h>
+
 #if APM_BUILD_TYPE(APM_BUILD_APMrover2)
  # define AP_AVOID_BEHAVE_DEFAULT AC_Avoid::BehaviourType::BEHAVIOR_STOP
 #else
@@ -89,7 +91,9 @@ void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel
 
     if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0) {
         adjust_velocity_circle_fence(kP, accel_cmss_limited, desired_vel_cms, dt);
-        adjust_velocity_polygon_fence(kP, accel_cmss_limited, desired_vel_cms, dt);
+        adjust_velocity_inclusion_and_exclusion_polygons(kP, accel_cmss_limited, desired_vel_cms, dt);
+        adjust_velocity_inclusion_circles(kP, accel_cmss_limited, desired_vel_cms, dt);
+        adjust_velocity_exclusion_circles(kP, accel_cmss_limited, desired_vel_cms, dt);
     }
 
     if ((_enabled & AC_AVOID_STOP_AT_BEACON_FENCE) > 0) {
@@ -347,15 +351,17 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f
         const Vector2f target_offset = stopping_point * ((fence_radius - margin_cm) / stopping_point_dist_from_home);
         const Vector2f target_direction = target_offset - position_xy;
         const float distance_to_target = target_direction.length();
-        const float max_speed = get_max_speed(kP, accel_cmss, distance_to_target, dt);
-        desired_vel_cms = target_direction * (MIN(desired_speed,max_speed) / distance_to_target);
+        if (is_positive(distance_to_target)) {
+            const float max_speed = get_max_speed(kP, accel_cmss, distance_to_target, dt);
+            desired_vel_cms = target_direction * (MIN(desired_speed,max_speed) / distance_to_target);
+        }
     } else {
         // implement stopping behaviour
         // calculate stopping point plus a margin so we look forward far enough to intersect with circular fence
         const Vector2f stopping_point_plus_margin = position_xy + desired_vel_cms*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, desired_speed))/desired_speed);
         const float stopping_point_plus_margin_dist_from_home = stopping_point_plus_margin.length();
         if (dist_from_home >= fence_radius - margin_cm) {
-            // if vehicle has already breached margin around fence
+            // vehicle has already breached margin around fence
             // if stopping point is even further from home (i.e. in wrong direction) then adjust speed to zero
             // otherwise user is backing away from fence so do not apply limits
             if (stopping_point_plus_margin_dist_from_home >= dist_from_home) {
@@ -376,33 +382,272 @@ void AC_Avoid::adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f
 }
 
 /*
- * Adjusts the desired velocity for the polygon fence.
+ * Adjusts the desired velocity for the exclusion polygons
  */
-void AC_Avoid::adjust_velocity_polygon_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt)
+void AC_Avoid::adjust_velocity_inclusion_and_exclusion_polygons(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt)
 {
-    AC_Fence *fence = AP::fence();
+    const AC_Fence *fence = AP::fence();
     if (fence == nullptr) {
         return;
     }
 
-    AC_Fence &_fence = *fence;
+    // exit if polygon fences are not enabled
+    if ((fence->get_enabled_fences() & AC_FENCE_TYPE_POLYGON) == 0) {
+        return;
+    }
+
+    // iterate through inclusion polygons
+    const uint8_t num_inclusion_polygons = fence->polyfence().get_inclusion_polygon_count();
+    for (uint8_t i = 0; i < num_inclusion_polygons; i++) {
+        uint16_t num_points;
+        const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points);
+        if (num_points < 3) {
+            // ignore exclusion polygons with less than 3 points
+            continue;
+        }
+        // adjust velocity
+        adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, boundary, num_points, true, fence->get_margin(), dt, true);
+    }
+
+    // iterate through exclusion polygons
+    const uint8_t num_exclusion_polygons = fence->polyfence().get_exclusion_polygon_count();
+    for (uint8_t i = 0; i < num_exclusion_polygons; i++) {
+        uint16_t num_points;
+        const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points);
+        if (num_points < 3) {
+            // ignore exclusion polygons with less than 3 points
+            continue;
+        }
+        // adjust velocity
+        adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, boundary, num_points, true, fence->get_margin(), dt, false);
+    }
+}
+
+/*
+ * Adjusts the desired velocity for the inclusion circles
+ */
+void AC_Avoid::adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt)
+{
+    const AC_Fence *fence = AP::fence();
+    if (fence == nullptr) {
+        return;
+    }
+
+    // return immediately if no inclusion circles
+    const uint8_t num_circles = fence->polyfence().get_inclusion_circle_count();
+    if (num_circles == 0) {
+        return;
+    }
+
+    // exit if polygon fences are not enabled
+    if ((fence->get_enabled_fences() & AC_FENCE_TYPE_POLYGON) == 0) {
+        return;
+    }
+
+    // get desired speed
+    const float desired_speed = desired_vel_cms.length();
+    if (is_zero(desired_speed)) {
+        // no avoidance necessary when desired speed is zero
+        return;
+    }
+
+    // get vehicle position
+    Vector2f position_NE;
+    if (!AP::ahrs().get_relative_position_NE_origin(position_NE)) {
+        // do not limit velocity if we don't have a position estimate
+        return;
+    }
+    position_NE = position_NE * 100.0f;  // m to cm
+
+    // get the margin to the fence in cm
+    const float margin_cm = fence->get_margin() * 100.0f;
+
+    // get stopping distance as an offset from the vehicle
+    Vector2f stopping_offset;
+    switch ((AC_Avoid::BehaviourType)_behavior.get()) {
+        case BEHAVIOR_SLIDE:
+            stopping_offset = desired_vel_cms*(get_stopping_distance(kP, accel_cmss, desired_speed)/desired_speed);
+            break;
+        case BEHAVIOR_STOP:
+            // calculate stopping point plus a margin so we look forward far enough to intersect with circular fence
+            stopping_offset = desired_vel_cms*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, desired_speed))/desired_speed);
+            break;
+    }
+
+    // iterate through inclusion circles
+    for (uint8_t i = 0; i < num_circles; i++) {
+        Vector2f center_pos_cm;
+        float radius;
+        if (fence->polyfence().get_inclusion_circle(i, center_pos_cm, radius)) {
+            // get position relative to circle's center
+            const Vector2f position_NE_rel = (position_NE - center_pos_cm);
+
+            // if we are outside this circle do not limit velocity for this circle
+            const float dist_sq_cm = position_NE_rel.length_squared();
+            const float radius_cm = (radius * 100.0f);
+            if (dist_sq_cm > sq(radius_cm)) {
+                continue;
+            }
+
+            switch ((AC_Avoid::BehaviourType)_behavior.get()) {
+                case BEHAVIOR_SLIDE: {
+                    // implement sliding behaviour
+                    const Vector2f stopping_point = position_NE_rel + stopping_offset;
+                    const float stopping_point_dist = stopping_point.length();
+                    if (is_zero(stopping_point_dist) || (stopping_point_dist <= (radius_cm - margin_cm))) {
+                        // stopping before before fence so no need to adjust for this circle
+                        continue;
+                    }
+                    // unsafe desired velocity - will not be able to stop before reaching margin from fence
+                    // project stopping point radially onto fence boundary
+                    // adjusted velocity will point towards this projected point at a safe speed
+                    const Vector2f target_offset = stopping_point * ((radius_cm - margin_cm) / stopping_point_dist);
+                    const Vector2f target_direction = target_offset - position_NE_rel;
+                    const float distance_to_target = target_direction.length();
+                    if (is_positive(distance_to_target)) {
+                        const float max_speed = get_max_speed(kP, accel_cmss, distance_to_target, dt);
+                        desired_vel_cms = target_direction * (MIN(desired_speed,max_speed) / distance_to_target);
+                    }
+                }
+                break;
+                case BEHAVIOR_STOP: {
+                    // implement stopping behaviour
+                    const Vector2f stopping_point_plus_margin = position_NE_rel + stopping_offset;
+                    const float dist_cm = safe_sqrt(dist_sq_cm);
+                    if (dist_cm >= radius_cm - margin_cm) {
+                        // vehicle has already breached margin around fence
+                        // if stopping point is even further from center (i.e. in wrong direction) then adjust speed to zero
+                        // otherwise user is backing away from fence so do not apply limits
+                        if (stopping_point_plus_margin.length() >= dist_cm) {
+                            desired_vel_cms.zero();
+                            return;
+                        }
+                    } else {
+                        // shorten vector without adjusting its direction
+                        Vector2f intersection;
+                        if (Vector2f::circle_segment_intersection(position_NE_rel, stopping_point_plus_margin, Vector2f(0.0f,0.0f), radius_cm - margin_cm, intersection)) {
+                            const float distance_to_target = MAX((intersection - position_NE_rel).length() - margin_cm, 0.0f);
+                            const float max_speed = get_max_speed(kP, accel_cmss, distance_to_target, dt);
+                            if (max_speed < desired_speed) {
+                                desired_vel_cms *= MAX(max_speed, 0.0f) / desired_speed;
+                            }
+                        }
+                    }
+                }
+                break;
+            }
+        }
+    }
+}
 
-    // exit if the polygon fence is not enabled
-    if ((_fence.get_enabled_fences() & AC_FENCE_TYPE_POLYGON) == 0) {
+/*
+ * Adjusts the desired velocity for the exclusion circles
+ */
+void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt)
+{
+    const AC_Fence *fence = AP::fence();
+    if (fence == nullptr) {
         return;
     }
 
-    // exit if the polygon fence has already been breached
-    if ((_fence.get_breaches() & AC_FENCE_TYPE_POLYGON) != 0) {
+    // return immediately if no inclusion circles
+    const uint8_t num_circles = fence->polyfence().get_exclusion_circle_count();
+    if (num_circles == 0) {
         return;
     }
 
-    // get polygon boundary
-    uint16_t num_points;
-    const Vector2f* boundary = _fence.polyfence().get_boundary_points(num_points);
+    // exit if polygon fences are not enabled
+    if ((fence->get_enabled_fences() & AC_FENCE_TYPE_POLYGON) == 0) {
+        return;
+    }
 
-    // adjust velocity using polygon
-    adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, boundary, num_points, true, _fence.get_margin(), dt);
+    // get desired speed
+    const float desired_speed = desired_vel_cms.length();
+    if (is_zero(desired_speed)) {
+        // no avoidance necessary when desired speed is zero
+        return;
+    }
+
+    // get vehicle position
+    Vector2f position_NE;
+    if (!AP::ahrs().get_relative_position_NE_origin(position_NE)) {
+        // do not limit velocity if we don't have a position estimate
+        return;
+    }
+    position_NE = position_NE * 100.0f;  // m to cm
+
+    // get the margin to the fence in cm
+    const float margin_cm = fence->get_margin() * 100.0f;
+
+    // calculate stopping distance as an offset from the vehicle (only used for BEHAVIOR_STOP)
+    // add a margin so we look forward far enough to intersect with circular fence
+    Vector2f stopping_offset;
+    if ((AC_Avoid::BehaviourType)_behavior.get() == BEHAVIOR_STOP) {
+        stopping_offset = desired_vel_cms*((2.0f + margin_cm + get_stopping_distance(kP, accel_cmss, desired_speed))/desired_speed);
+    }
+
+    // iterate through exclusion circles
+    for (uint8_t i = 0; i < num_circles; i++) {
+        Vector2f center_pos_cm;
+        float radius;
+        if (fence->polyfence().get_exclusion_circle(i, center_pos_cm, radius)) {
+            // get position relative to circle's center
+            const Vector2f position_NE_rel = (position_NE - center_pos_cm);
+
+            // if we are inside this circle do not limit velocity for this circle
+            const float dist_sq_cm = position_NE_rel.length_squared();
+            const float radius_cm = (radius * 100.0f);
+            if (dist_sq_cm < sq(radius_cm)) {
+                continue;
+            }
+
+            switch ((AC_Avoid::BehaviourType)_behavior.get()) {
+                case BEHAVIOR_SLIDE: {
+                    // vector from current position to circle's center
+                    Vector2f limit_direction = center_pos_cm - position_NE;
+                    if (limit_direction.is_zero()) {
+                        // vehicle is exactly on circle center so do not limit velocity
+                        continue;
+                    }
+                    // calculate distance to edge of circle
+                    const float limit_distance_cm = limit_direction.length() - radius_cm;
+                    if (!is_positive(limit_distance_cm)) {
+                        // vehicle is within circle so do not limit velocity
+                        continue;
+                    }
+                    // vehicle is outside the circle, adjust velocity to stay outside
+                    limit_direction.normalize();
+                    limit_velocity(kP, accel_cmss, desired_vel_cms, limit_direction, MAX(limit_distance_cm - margin_cm, 0.0f), dt);
+                }
+                break;
+                case BEHAVIOR_STOP: {
+                    // implement stopping behaviour
+                    const Vector2f stopping_point_plus_margin = position_NE_rel + stopping_offset;
+                    const float dist_cm = safe_sqrt(dist_sq_cm);
+                    if (dist_cm < radius_cm + margin_cm) {
+                        // vehicle has already breached margin around fence
+                        // if stopping point is closer to center (i.e. in wrong direction) then adjust speed to zero
+                        // otherwise user is backing away from fence so do not apply limits
+                        if (stopping_point_plus_margin.length() <= dist_cm) {
+                            desired_vel_cms.zero();
+                            return;
+                        }
+                    } else {
+                        // shorten vector without adjusting its direction
+                        Vector2f intersection;
+                        if (Vector2f::circle_segment_intersection(position_NE_rel, stopping_point_plus_margin, Vector2f(0.0f,0.0f), radius_cm + margin_cm, intersection)) {
+                            const float distance_to_target = MAX((intersection - position_NE_rel).length() - margin_cm, 0.0f);
+                            const float max_speed = get_max_speed(kP, accel_cmss, distance_to_target, dt);
+                            if (max_speed < desired_speed) {
+                                desired_vel_cms *= MAX(max_speed, 0.0f) / desired_speed;
+                            }
+                        }
+                    }
+                }
+                break;
+            }
+        }
+    }
 }
 
 /*
@@ -418,9 +663,9 @@ void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f
     }
 
     // get boundary from beacons
-    uint16_t num_points;
+    uint16_t num_points = 0;
     const Vector2f* boundary = _beacon->get_boundary_points(num_points);
-    if (boundary == nullptr || num_points == 0) {
+    if ((boundary == nullptr) || (num_points == 0)) {
         return;
     }
 
@@ -429,7 +674,7 @@ void AC_Avoid::adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f
     if (AP::fence()) {
         margin = AP::fence()->get_margin();
     }
-    adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, boundary, num_points, true, margin, dt);
+    adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, boundary, num_points, true, margin, dt, true);
 }
 
 /*
@@ -450,15 +695,15 @@ void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector2f &d
     }
 
     // get boundary from proximity sensor
-    uint16_t num_points;
+    uint16_t num_points = 0;
     const Vector2f *boundary = _proximity.get_boundary_points(num_points);
-    adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, boundary, num_points, false, _margin, dt);
+    adjust_velocity_polygon(kP, accel_cmss, desired_vel_cms, boundary, num_points, false, _margin, dt, true);
 }
 
 /*
  * Adjusts the desired velocity for the polygon fence.
  */
-void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel_cms, const Vector2f* boundary, uint16_t num_points, bool earth_frame, float margin, float dt)
+void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel_cms, const Vector2f* boundary, uint16_t num_points, bool earth_frame, float margin, float dt, bool stay_inside)
 {
     // exit if there are no points
     if (boundary == nullptr || num_points == 0) {
@@ -483,7 +728,9 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des
         position_xy = position_xy * 100.0f;  // m to cm
     }
 
-    if (Polygon_outside(position_xy, boundary, num_points)) {
+    // return if we have already breached polygon
+    const bool inside_polygon = !Polygon_outside(position_xy, boundary, num_points);
+    if (inside_polygon != stay_inside) {
         return;
     }
 

libraries/AC_Avoidance/AC_Avoid.h

@@ -91,9 +91,15 @@ private:
     void adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt);
 
     /*
-     * Adjusts the desired velocity for the polygon fence.
+     * Adjusts the desired velocity for inclusion and exclusion polygon fences
      */
-    void adjust_velocity_polygon_fence(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt);
+    void adjust_velocity_inclusion_and_exclusion_polygons(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt);
+
+    /*
+     * Adjusts the desired velocity for the inclusion and exclusion circles
+     */
+    void adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt);
+    void adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_cms, float dt);
 
     /*
      * Adjusts the desired velocity for the beacon fence.
@@ -109,8 +115,9 @@ private:
      * Adjusts the desired velocity given an array of boundary points
      *   earth_frame should be true if boundary is in earth-frame, false for body-frame
      *   margin is the distance (in meters) that the vehicle should stop short of the polygon
+     *   stay_inside should be true for fences, false for exclusion polygons
      */
-    void adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel_cms, const Vector2f* boundary, uint16_t num_points, bool earth_frame, float margin, float dt);
+    void adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel_cms, const Vector2f* boundary, uint16_t num_points, bool earth_frame, float margin, float dt, bool stay_inside);
 
     /*
      * Computes distance required to stop, given current speed.

libraries/AC_Avoidance/AP_OABendyRuler.cpp

@@ -152,23 +152,27 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
 // calculate minimum distance between a segment and any obstacle
 float AP_OABendyRuler::calc_avoidance_margin(const Location &start, const Location &end)
 {
-    float circular_fence_margin;
-    if (!calc_margin_from_circular_fence(start, end, circular_fence_margin)) {
-        circular_fence_margin = FLT_MAX;
+    float margin_min = FLT_MAX;
+
+    float latest_margin;
+    if (calc_margin_from_circular_fence(start, end, latest_margin)) {
+        margin_min = MIN(margin_min, latest_margin);
+    }
+
+    if (calc_margin_from_object_database(start, end, latest_margin)) {
+        margin_min = MIN(margin_min, latest_margin);
     }
 
-    float polygon_fence_margin;
-    if (!calc_margin_from_polygon_fence(start, end, polygon_fence_margin)) {
-        polygon_fence_margin = FLT_MAX;
+    if (calc_margin_from_inclusion_and_exclusion_polygons(start, end, latest_margin)) {
+        margin_min = MIN(margin_min, latest_margin);
     }
 
-    float proximity_margin;
-    if (!calc_margin_from_object_database(start, end, proximity_margin)) {
-        proximity_margin = FLT_MAX;
+    if (calc_margin_from_inclusion_and_exclusion_circles(start, end, latest_margin)) {
+        margin_min = MIN(margin_min, latest_margin);
     }
 
     // return smallest margin from any obstacle
-    return MIN(MIN(circular_fence_margin, polygon_fence_margin), proximity_margin);
+    return margin_min;
 }
 
 // calculate minimum distance between a path and the circular fence (centered on home)
@@ -189,32 +193,32 @@ bool AP_OABendyRuler::calc_margin_from_circular_fence(const Location &start, con
     const float start_dist_sq = ahrs_home.get_distance_NE(start).length_squared();
     const float end_dist_sq = ahrs_home.get_distance_NE(end).length_squared();
 
-    // get circular fence radius
-    const float fence_radius = fence->get_radius();
+    // get circular fence radius + margin
+    const float fence_radius_plus_margin = fence->get_radius() - fence->get_margin();
 
     // margin is fence radius minus the longer of start or end distance
-    margin = fence_radius - sqrtf(MAX(start_dist_sq, end_dist_sq));
+    margin = fence_radius_plus_margin - sqrtf(MAX(start_dist_sq, end_dist_sq));
     return true;
 }
 
-// calculate minimum distance between a path and the polygon 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_polygon_fence(const Location &start, const Location &end, float &margin)
+bool AP_OABendyRuler::calc_margin_from_inclusion_and_exclusion_polygons(const Location &start, const Location &end, float &margin)
 {
-    // exit immediately if polygon fence is not enabled
     const AC_Fence *fence = AC_Fence::get_singleton();
     if (fence == nullptr) {
         return false;
     }
-    if (((fence->get_enabled_fences() & AC_FENCE_TYPE_POLYGON) == 0) || !fence->polyfence_const().valid_const()) {
+
+    // exclusion polygons enabled along with polygon fences
+    if ((fence->get_enabled_fences() & AC_FENCE_TYPE_POLYGON) == 0) {
         return false;
     }
 
-    // get polygon boundary
-    uint16_t num_points;
-    const Vector2f* boundary = fence->polyfence_const().get_boundary_points(num_points);
-    if (num_points < 3) {
-        // this should have already been checked by is_polygon_valid() but just in case
+    // return immediately if no inclusion nor exclusion polygons
+    const uint8_t num_inclusion_polygons = fence->polyfence().get_inclusion_polygon_count();
+    const uint8_t num_exclusion_polygons = fence->polyfence().get_exclusion_polygon_count();
+    if ((num_inclusion_polygons == 0) && (num_exclusion_polygons == 0)) {
         return false;
     }
 
@@ -224,12 +228,127 @@ bool AP_OABendyRuler::calc_margin_from_polygon_fence(const Location &start, cons
         return false;
     }
 
-    // if outside the fence margin is the closest distance but with negative sign
-    const float sign = Polygon_outside(start_NE, boundary, num_points) ? -1.0f : 1.0f;
+    // get fence margin
+    const float fence_margin = fence->get_margin();
 
-    // calculate min distance (in meters) from line to polygon
-    margin = sign * Polygon_closest_distance_line(boundary, num_points, start_NE, end_NE) * 0.01f;
-    return true;
+    // iterate through inclusion polygons and calculate minimum margin
+    bool margin_updated = false;
+    for (uint8_t i = 0; i < num_inclusion_polygons; i++) {
+        uint16_t num_points;
+        const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points);
+        if (num_points < 3) {
+            // ignore exclusion polygons with less than 3 points
+            continue;
+        }
+
+        // if outside the fence margin is the closest distance but with negative sign
+        const float sign = Polygon_outside(start_NE, boundary, num_points) ? -1.0f : 1.0f;
+
+        // calculate min distance (in meters) from line to polygon
+        float margin_new = (sign * Polygon_closest_distance_line(boundary, num_points, start_NE, end_NE) * 0.01f) - fence_margin;
+        if (!margin_updated || (margin_new < margin)) {
+            margin_updated = true;
+            margin = margin_new;
+        }
+    }
+
+    // iterate through exclusion polygons and calculate minimum margin
+    for (uint8_t i = 0; i < num_exclusion_polygons; i++) {
+        uint16_t num_points;
+        const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points);
+        if (num_points < 3) {
+            // ignore exclusion polygons with less than 3 points
+            continue;
+        }
+
+        // if start is inside the polygon the margin's sign is reversed
+        const float sign = Polygon_outside(start_NE, boundary, num_points) ? 1.0f : -1.0f;
+
+        // calculate min distance (in meters) from line to polygon
+        float margin_new = (sign * Polygon_closest_distance_line(boundary, num_points, start_NE, end_NE) * 0.01f) - fence_margin;
+        if (!margin_updated || (margin_new < margin)) {
+            margin_updated = true;
+            margin = margin_new;
+        }
+    }
+
+    return margin_updated;
+}
+
+// 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)
+{
+    // exit immediately if fence is not enabled
+    const AC_Fence *fence = AC_Fence::get_singleton();
+    if (fence == nullptr) {
+        return false;
+    }
+
+    // inclusion/exclusion circles enabled along with polygon fences
+    if ((fence->get_enabled_fences() & AC_FENCE_TYPE_POLYGON) == 0) {
+        return false;
+    }
+
+    // return immediately if no inclusion nor exclusion circles
+    const uint8_t num_inclusion_circles = fence->polyfence().get_inclusion_circle_count();
+    const uint8_t num_exclusion_circles = fence->polyfence().get_exclusion_circle_count();
+    if ((num_inclusion_circles == 0) && (num_exclusion_circles == 0)) {
+        return false;
+    }
+
+    // convert start and end to offsets from EKF origin
+    Vector2f start_NE, end_NE;
+    if (!start.get_vector_xy_from_origin_NE(start_NE) || !end.get_vector_xy_from_origin_NE(end_NE)) {
+        return false;
+    }
+
+    // get fence margin
+    const float fence_margin = fence->get_margin();
+
+    // iterate through inclusion circles and calculate minimum margin
+    bool margin_updated = false;
+    for (uint8_t i = 0; i < num_inclusion_circles; i++) {
+        Vector2f center_pos_cm;
+        float radius;
+        if (fence->polyfence().get_inclusion_circle(i, center_pos_cm, radius)) {
+
+            // calculate start and ends distance from the center of the circle
+            const float start_dist_sq = (start_NE - center_pos_cm).length_squared();
+            const float end_dist_sq = (end_NE - center_pos_cm).length_squared();
+
+            // margin is fence radius minus the longer of start or end distance
+            const float margin_new = (radius + fence_margin) - (sqrtf(MAX(start_dist_sq, end_dist_sq)) * 0.01f);
+
+            // update margin with lowest value so far
+            if (!margin_updated || (margin_new < margin)) {
+                margin_updated = true;
+                margin = margin_new;
+            }
+        }
+    }
+
+    // iterate through exclusion circles and calculate minimum margin
+    for (uint8_t i = 0; i < num_exclusion_circles; i++) {
+        Vector2f center_pos_cm;
+        float radius;
+        if (fence->polyfence().get_exclusion_circle(i, center_pos_cm, radius)) {
+
+            // first calculate distance between circle's center and segment
+            const float dist_cm = Vector2f::closest_distance_between_line_and_point(start_NE, end_NE, center_pos_cm);
+
+            // margin is distance to the center minus the radius
+            const float margin_new = (dist_cm * 0.01f) - (radius + fence_margin);
+
+            // update margin with lowest value so far
+            if (!margin_updated || (margin_new < margin)) {
+                margin_updated = true;
+                margin = margin_new;
+            }
+        }
+    }
+
+    return margin_updated;
 }
 
 // calculate minimum distance between a path and proximity sensor obstacles

libraries/AC_Avoidance/AP_OABendyRuler.h

@@ -33,9 +33,13 @@ private:
     // on success returns true and updates margin
     bool calc_margin_from_circular_fence(const Location &start, const Location &end, float &margin);
 
-    // calculate minimum distance between a path and the polygon fence
+    // calculate minimum distance between a path and all inclusion and exclusion polygons
     // on success returns true and updates margin
-    bool calc_margin_from_polygon_fence(const Location &start, const Location &end, float &margin);
+    bool calc_margin_from_inclusion_and_exclusion_polygons(const Location &start, const Location &end, float &margin);
+
+    // calculate minimum distance between a path and all inclusion and exclusion circles
+    // on success returns true and updates margin
+    bool calc_margin_from_inclusion_and_exclusion_circles(const Location &start, const Location &end, float &margin);
 
     // calculate minimum distance between a path and proximity sensor obstacles
     // on success returns true and updates margin

libraries/AC_Avoidance/AP_OADijkstra.cpp

@@ -19,61 +19,107 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Logger/AP_Logger.h>
 
-#define OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK  32      // expanding arrays for inner polygon fence and paths to destination will grow in increments of 20 elements
+#define OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK  32      // expanding arrays for fence points and paths to destination will grow in increments of 20 elements
 #define OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX        255     // index use to indicate we do not have a tentative short path for a node
+#define OA_DIJKSTRA_ERROR_REPORTING_INTERVAL_MS         5000    // failure messages sent to GCS every 5 seconds
 
 /// Constructor
 AP_OADijkstra::AP_OADijkstra() :
-        _polyfence_pts(OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK),
+        _inclusion_polygon_pts(OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK),
+        _exclusion_polygon_pts(OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK),
+        _exclusion_circle_pts(OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK),
         _short_path_data(OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK),
         _path(OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK)
 {
 }
 
-// calculate a destination to avoid the polygon fence
+// calculate a destination to avoid fences
 // returns DIJKSTRA_STATE_SUCCESS and populates origin_new and destination_new if avoidance is required
 AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current_loc, const Location &destination, Location& origin_new, Location& destination_new)
 {
     // require ekf origin to have been set
     struct Location ekf_origin {};
-    if (!AP::ahrs().get_origin(ekf_origin)) {
-        AP::logger().Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, destination, destination);
-        return DIJKSTRA_STATE_NOT_REQUIRED;
+    {
+        WITH_SEMAPHORE(AP::ahrs().get_semaphore());
+        if (!AP::ahrs().get_origin(ekf_origin)) {
+            AP::logger().Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination);
+            return DIJKSTRA_STATE_NOT_REQUIRED;
+        }
     }
 
-    // no avoidance required if fence is disabled
-    if (!polygon_fence_enabled()) {
-        AP::logger().Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, destination, destination);
+    WITH_SEMAPHORE(AP::fence()->polyfence().get_loaded_fence_semaphore());
+
+    // avoidance is not required if no fences
+    if (!some_fences_enabled()) {
+        AP::logger().Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination);
         return DIJKSTRA_STATE_NOT_REQUIRED;
     }
 
     // no avoidance required if destination is same as current location
     if (current_loc.same_latlon_as(destination)) {
+        AP::logger().Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination);
         return DIJKSTRA_STATE_NOT_REQUIRED;
     }
 
-    // check for fence updates
-    if (check_polygon_fence_updated()) {
-        _polyfence_with_margin_ok = false;
+    // check for inclusion polygon updates
+    if (check_inclusion_polygon_updated()) {
+        _inclusion_polygon_with_margin_ok = false;
+        _polyfence_visgraph_ok = false;
+        _shortest_path_ok = false;
+    }
+
+    // check for exclusion polygon updates
+    if (check_exclusion_polygon_updated()) {
+        _exclusion_polygon_with_margin_ok = false;
+        _polyfence_visgraph_ok = false;
+        _shortest_path_ok = false;
+    }
+
+    // check for exclusion circle updates
+    if (check_exclusion_circle_updated()) {
+        _exclusion_circle_with_margin_ok = false;
         _polyfence_visgraph_ok = false;
         _shortest_path_ok = false;
     }
 
     // create inner polygon fence
-    if (!_polyfence_with_margin_ok) {
-        _polyfence_with_margin_ok = create_polygon_fence_with_margin(_polyfence_margin * 100.0f);
-        if (!_polyfence_with_margin_ok) {
-            AP::logger().Write_OADijkstra(DIJKSTRA_STATE_ERROR, 0, 0, destination, destination);
+    AP_OADijkstra_Error error_id;
+    if (!_inclusion_polygon_with_margin_ok) {
+        _inclusion_polygon_with_margin_ok = create_inclusion_polygon_with_margin(_polyfence_margin * 100.0f, error_id);
+        if (!_inclusion_polygon_with_margin_ok) {
+            report_error(error_id);
+            AP::logger().Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
             return DIJKSTRA_STATE_ERROR;
         }
     }
 
-    // create visgraph for inner polygon fence
+    // create exclusion polygon outer fence
+    if (!_exclusion_polygon_with_margin_ok) {
+        _exclusion_polygon_with_margin_ok = create_exclusion_polygon_with_margin(_polyfence_margin * 100.0f, error_id);
+        if (!_exclusion_polygon_with_margin_ok) {
+            report_error(error_id);
+            AP::logger().Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
+            return DIJKSTRA_STATE_ERROR;
+        }
+    }
+
+    // create exclusion circle points
+    if (!_exclusion_circle_with_margin_ok) {
+        _exclusion_circle_with_margin_ok = create_exclusion_circle_with_margin(_polyfence_margin * 100.0f, error_id);
+        if (!_exclusion_circle_with_margin_ok) {
+            report_error(error_id);
+            AP::logger().Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
+            return DIJKSTRA_STATE_ERROR;
+        }
+    }
+
+    // create visgraph for all fence (with margin) points
     if (!_polyfence_visgraph_ok) {
-        _polyfence_visgraph_ok = create_polygon_fence_visgraph();
+        _polyfence_visgraph_ok = create_fence_visgraph(error_id);
         if (!_polyfence_visgraph_ok) {
             _shortest_path_ok = false;
-            AP::logger().Write_OADijkstra(DIJKSTRA_STATE_ERROR, 0, 0, destination, destination);
+            report_error(error_id);
+            AP::logger().Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
             return DIJKSTRA_STATE_ERROR;
         }
     }
@@ -86,9 +132,10 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
 
     // calculate shortest path from current_loc to destination
     if (!_shortest_path_ok) {
-        _shortest_path_ok = calc_shortest_path(current_loc, destination);
+        _shortest_path_ok = calc_shortest_path(current_loc, destination, error_id);
         if (!_shortest_path_ok) {
-            AP::logger().Write_OADijkstra(DIJKSTRA_STATE_ERROR, 0, 0, destination, destination);
+            report_error(error_id);
+            AP::logger().Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination);
             return DIJKSTRA_STATE_ERROR;
         }
         // start from 2nd point on path (first is the original origin)
@@ -123,199 +170,512 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
             _path_idx_returned++;
         }
         // log success
-        AP::logger().Write_OADijkstra(DIJKSTRA_STATE_SUCCESS, _path_idx_returned, _path_numpoints, destination, destination_new);
+        AP::logger().Write_OADijkstra(DIJKSTRA_STATE_SUCCESS, 0, _path_idx_returned, _path_numpoints, destination, destination_new);
         return DIJKSTRA_STATE_SUCCESS;
     }
 
     // we have reached the destination so avoidance is no longer required
-    AP::logger().Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, destination, destination);
+    AP::logger().Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination);
     return DIJKSTRA_STATE_NOT_REQUIRED;
 }
 
-// returns true if polygon fence is enabled
-bool AP_OADijkstra::polygon_fence_enabled() const
+// returns true if at least one inclusion or exclusion zone is enabled
+bool AP_OADijkstra::some_fences_enabled() const
 {
     const AC_Fence *fence = AC_Fence::get_singleton();
     if (fence == nullptr) {
         return false;
     }
-    if (!fence->polyfence_const().valid_const()) {
+    if ((fence->polyfence().get_inclusion_polygon_count() == 0) &&
+        (fence->polyfence().get_exclusion_polygon_count() == 0) &&
+        (fence->polyfence().get_exclusion_circle_count() == 0)) {
         return false;
     }
     return ((fence->get_enabled_fences() & AC_FENCE_TYPE_POLYGON) > 0);
 }
 
+// return error message for a given error id
+const char* AP_OADijkstra::get_error_msg(AP_OADijkstra_Error error_id) const
+{
+    switch (error_id) {
+    case AP_OADijkstra_Error::DIJKSTRA_ERROR_NONE:
+        return "no error";
+        break;
+    case AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY:
+        return "out of memory";
+        break;
+    case AP_OADijkstra_Error::DIJKSTRA_ERROR_OVERLAPPING_POLYGON_POINTS:
+        return "overlapping polygon points";
+        break;
+    case AP_OADijkstra_Error::DIJKSTRA_ERROR_FAILED_TO_BUILD_INNER_POLYGON:
+        return "failed to build inner polygon";
+        break;
+    case AP_OADijkstra_Error::DIJKSTRA_ERROR_OVERLAPPING_POLYGON_LINES:
+        return "overlapping polygon lines";
+        break;
+    case AP_OADijkstra_Error::DIJKSTRA_ERROR_FENCE_DISABLED:
+        return "fence disabled";
+        break;
+    case AP_OADijkstra_Error::DIJKSTRA_ERROR_TOO_MANY_FENCE_POINTS:
+        return "too many fence points";
+        break;
+    case AP_OADijkstra_Error::DIJKSTRA_ERROR_NO_POSITION_ESTIMATE:
+        return "no position estimate";
+        break;
+    case AP_OADijkstra_Error::DIJKSTRA_ERROR_COULD_NOT_FIND_PATH:
+        return "could not find path";
+        break;
+    }
+
+    // we should never reach here but just in case
+    return "unknown error";
+}
+
+void AP_OADijkstra::report_error(AP_OADijkstra_Error error_id)
+{
+    // report errors to GCS every 5 seconds
+    uint32_t now_ms = AP_HAL::millis();
+    if ((error_id != AP_OADijkstra_Error::DIJKSTRA_ERROR_NONE) &&
+        ((error_id != _error_last_id) || ((now_ms - _error_last_report_ms) > OA_DIJKSTRA_ERROR_REPORTING_INTERVAL_MS))) {
+        const char* error_msg = get_error_msg(error_id);
+        gcs().send_text(MAV_SEVERITY_CRITICAL, "Dijkstra: %s", error_msg);
+        _error_last_id = error_id;
+        _error_last_report_ms = now_ms;
+    }
+}
+
 // check if polygon fence has been updated since we created the inner fence. returns true if changed
-bool AP_OADijkstra::check_polygon_fence_updated() const
+bool AP_OADijkstra::check_inclusion_polygon_updated() const
 {
     // exit immediately if polygon fence is not enabled
     const AC_Fence *fence = AC_Fence::get_singleton();
     if (fence == nullptr) {
         return false;
     }
-    return (_polyfence_update_ms != fence->polyfence_const().update_ms());
+    return (_inclusion_polygon_update_ms != fence->polyfence().get_inclusion_polygon_update_ms());
 }
 
-// create a smaller polygon fence within the existing polygon fence
-// returns true on success
-bool AP_OADijkstra::create_polygon_fence_with_margin(float margin_cm)
+// create polygons inside the existing inclusion polygons
+// returns true on success.  returns false on failure and err_id is updated
+bool AP_OADijkstra::create_inclusion_polygon_with_margin(float margin_cm, AP_OADijkstra_Error &err_id)
 {
-    // exit immediately if polygon fence is not enabled
     const AC_Fence *fence = AC_Fence::get_singleton();
     if (fence == nullptr) {
+        err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_FENCE_DISABLED;
         return false;
     }
 
-    // get polygon boundary
-    uint16_t num_points;
-    const Vector2f* boundary = fence->polyfence_const().get_boundary_points(num_points);
-    if ((boundary == nullptr) || (num_points < 3)) {
+    // clear all points
+    _inclusion_polygon_numpoints = 0;
+
+    // return immediately if no polygons
+    const uint8_t num_inclusion_polygons = fence->polyfence().get_inclusion_polygon_count();
+
+    // iterate through polygons and create inner points
+    for (uint8_t i = 0; i < num_inclusion_polygons; i++) {
+        uint16_t num_points;
+        const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points);
+        if (num_points < 3) {
+            // ignore exclusion polygons with less than 3 points
+            continue;
+        }
+
+        // expand array if required
+        if (!_inclusion_polygon_pts.expand_to_hold(_inclusion_polygon_numpoints + num_points)) {
+            err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
+            return false;
+        }
+
+        // for each point in inclusion polygon
+        // Note: boundary is "unclosed" meaning the last point is *not* the same as the first
+        for (uint16_t j = 0; j < num_points; j++) {
+
+            // find points before and after current point (relative to current point)
+            const uint16_t before_idx = (j == 0) ? num_points-1 : j-1;
+            const uint16_t after_idx = (j == num_points-1) ? 0 : j+1;
+            Vector2f before_pt = boundary[before_idx] - boundary[j];
+            Vector2f after_pt = boundary[after_idx] - boundary[j];
+
+            // if points are overlapping fail
+            if (before_pt.is_zero() || after_pt.is_zero() || (before_pt == after_pt)) {
+                err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OVERLAPPING_POLYGON_POINTS;
+                return false;
+            }
+
+            // scale points to be unit vectors
+            before_pt.normalize();
+            after_pt.normalize();
+
+            // calculate intermediate point and scale to margin
+            Vector2f intermediate_pt = (after_pt + before_pt) * 0.5f;
+            float intermediate_len = intermediate_pt.length();
+            intermediate_pt *= (margin_cm / intermediate_len);
+
+            // find final point which is outside the inside polygon
+            uint16_t next_index = _inclusion_polygon_numpoints + j;
+            _inclusion_polygon_pts[next_index] = boundary[j] + intermediate_pt;
+            if (Polygon_outside(_inclusion_polygon_pts[next_index], boundary, num_points)) {
+                _inclusion_polygon_pts[next_index] = boundary[j] - intermediate_pt;
+                if (Polygon_outside(_inclusion_polygon_pts[next_index], boundary, num_points)) {
+                    // could not find a point on either side that was outside the exclusion polygon so fail
+                    // this may happen if the exclusion polygon has overlapping lines
+                    err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OVERLAPPING_POLYGON_LINES;
+                    return false;
+                }
+            }
+        }
+
+        // update total number of points
+        _inclusion_polygon_numpoints += num_points;
+    }
+
+    // record fence update time so we don't process these inclusion polygons again
+    _inclusion_polygon_update_ms = fence->polyfence().get_inclusion_polygon_update_ms();
+
+    return true;
+}
+
+// check if exclusion polygons have been updated since create_exclusion_polygon_with_margin was run
+// returns true if changed
+bool AP_OADijkstra::check_exclusion_polygon_updated() const
+{
+    const AC_Fence *fence = AC_Fence::get_singleton();
+    if (fence == nullptr) {
         return false;
     }
+    return (_exclusion_polygon_update_ms != fence->polyfence().get_exclusion_polygon_update_ms());
+}
 
-    // expand fence point array if required
-    if (!_polyfence_pts.expand_to_hold(num_points)) {
+// create polygons around existing exclusion polygons
+// returns true on success.  returns false on failure and err_id is updated
+bool AP_OADijkstra::create_exclusion_polygon_with_margin(float margin_cm, AP_OADijkstra_Error &err_id)
+{
+    const AC_Fence *fence = AC_Fence::get_singleton();
+    if (fence == nullptr) {
+        err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_FENCE_DISABLED;
         return false;
     }
 
-    // for each point on polygon fence
-    // Note: boundary is "unclosed" meaning the last point is *not* the same as the first
-    for (uint8_t i=0; i<num_points; i++) {
+    // clear all points
+    _exclusion_polygon_numpoints = 0;
 
-        // find points before and after current point (relative to current point)
-        const uint8_t before_idx = (i == 0) ? num_points-1 : i-1;
-        const uint8_t after_idx = (i == num_points-1) ? 0 : i+1;
-        Vector2f before_pt = boundary[before_idx] - boundary[i];
-        Vector2f after_pt = boundary[after_idx] - boundary[i];
+    // return immediately if no exclusion polygons
+    const uint8_t num_exclusion_polygons = fence->polyfence().get_exclusion_polygon_count();
 
-        // if points are overlapping fail
-        if (before_pt.is_zero() || after_pt.is_zero() || (before_pt == after_pt)) {
+    // iterate through exclusion polygons and create outer points
+    for (uint8_t i = 0; i < num_exclusion_polygons; i++) {
+        uint16_t num_points;
+        const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points);
+        if (num_points < 3) {
+            // ignore exclusion polygons with less than 3 points
+            continue;
+        }
+
+        // expand array if required
+        if (!_exclusion_polygon_pts.expand_to_hold(_exclusion_polygon_numpoints + num_points)) {
+            err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
             return false;
         }
 
-        // scale points to be unit vectors
-        before_pt.normalize();
-        after_pt.normalize();
-
-        // calculate intermediate point and scale to margin
-        Vector2f intermediate_pt = (after_pt + before_pt) * 0.5f;
-        float intermediate_len = intermediate_pt.length();
-        intermediate_pt *= (margin_cm / intermediate_len);
-
-        // find final point which is inside the original polygon
-        _polyfence_pts[i] = boundary[i] + intermediate_pt;
-        if (Polygon_outside(_polyfence_pts[i], boundary, num_points)) {
-            _polyfence_pts[i] = boundary[i] - intermediate_pt;
-            if (Polygon_outside(_polyfence_pts[i], boundary, num_points)) {
-                // could not find a point on either side that was within the fence so fail
-                // this can happen if fence lines are closer than margin_cm
+        // for each point in exclusion polygon
+        // Note: boundary is "unclosed" meaning the last point is *not* the same as the first
+        for (uint16_t j = 0; j < num_points; j++) {
+
+            // find points before and after current point (relative to current point)
+            const uint16_t before_idx = (j == 0) ? num_points-1 : j-1;
+            const uint16_t after_idx = (j == num_points-1) ? 0 : j+1;
+            Vector2f before_pt = boundary[before_idx] - boundary[j];
+            Vector2f after_pt = boundary[after_idx] - boundary[j];
+
+            // if points are overlapping fail
+            if (before_pt.is_zero() || after_pt.is_zero() || (before_pt == after_pt)) {
+                err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OVERLAPPING_POLYGON_POINTS;
                 return false;
             }
+
+            // scale points to be unit vectors
+            before_pt.normalize();
+            after_pt.normalize();
+
+            // calculate intermediate point and scale to margin
+            Vector2f intermediate_pt = (after_pt + before_pt) * 0.5f;
+            float intermediate_len = intermediate_pt.length();
+            intermediate_pt *= (margin_cm / intermediate_len);
+
+            // find final point which is outside the original polygon
+            uint16_t next_index = _exclusion_polygon_numpoints + j;
+            _exclusion_polygon_pts[next_index] = boundary[j] + intermediate_pt;
+            if (!Polygon_outside(_exclusion_polygon_pts[next_index], boundary, num_points)) {
+                _exclusion_polygon_pts[next_index] = boundary[j] - intermediate_pt;
+                if (!Polygon_outside(_exclusion_polygon_pts[next_index], boundary, num_points)) {
+                    // could not find a point on either side that was outside the exclusion polygon so fail
+                    // this may happen if the exclusion polygon has overlapping lines
+                    err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OVERLAPPING_POLYGON_LINES;
+                    return false;
+                }
+            }
         }
-    }
 
-    // update number of fence points
-    _polyfence_numpoints = num_points;
+        // update total number of points
+        _exclusion_polygon_numpoints += num_points;
+    }
 
-    // record fence update time so we don't process this exact fence again
-    _polyfence_update_ms = fence->polyfence_const().update_ms();
+    // record fence update time so we don't process these exclusion polygons again
+    _exclusion_polygon_update_ms = fence->polyfence().get_exclusion_polygon_update_ms();
 
     return true;
 }
 
-// create a visibility graph of the polygon fence
-// returns true on success
-// requires create_polygon_fence_with_margin to have been run
-bool AP_OADijkstra::create_polygon_fence_visgraph()
+// check if exclusion circles have been updated since create_exclusion_circle_with_margin was run
+// returns true if changed
+bool AP_OADijkstra::check_exclusion_circle_updated() const
 {
-    // exit immediately if no polygon fence (with margin)
-    if (_polyfence_numpoints == 0) {
+    // exit immediately if fence is not enabled
+    const AC_Fence *fence = AC_Fence::get_singleton();
+    if (fence == nullptr) {
         return false;
     }
+    return (_exclusion_circle_update_ms != fence->polyfence().get_exclusion_circle_update_ms());
+}
 
-    // exit immediately if polygon fence is not enabled
+// create polygons around existing exclusion circles
+// returns true on success.  returns false on failure and err_id is updated
+bool AP_OADijkstra::create_exclusion_circle_with_margin(float margin_cm, AP_OADijkstra_Error &err_id)
+{
+    // exit immediately if fence is not enabled
     const AC_Fence *fence = AC_Fence::get_singleton();
     if (fence == nullptr) {
+        err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_FENCE_DISABLED;
         return false;
     }
 
-    // get polygon boundary
-    uint16_t num_points;
-    const Vector2f* boundary = fence->polyfence_const().get_boundary_points(num_points);
-    if ((boundary == nullptr) || (num_points < 3)) {
+    // clear all points
+    _exclusion_circle_numpoints = 0;
+
+    // unit length offsets for polygon points around circles
+    const Vector2f unit_offsets[] = {
+            {cosf(radians(30)), cosf(radians(30-90))},  // north-east
+            {cosf(radians(90)), cosf(radians(90-90))},  // east
+            {cosf(radians(150)), cosf(radians(150-90))},// south-east
+            {cosf(radians(210)), cosf(radians(210-90))},// south-west
+            {cosf(radians(270)), cosf(radians(270-90))},// west
+            {cosf(radians(330)), cosf(radians(330-90))},// north-west
+    };
+    const uint8_t num_points_per_circle = ARRAY_SIZE(unit_offsets);
+
+    // expand polygon point array if required
+    const uint8_t num_exclusion_circles = fence->polyfence().get_exclusion_circle_count();
+    if (!_exclusion_circle_pts.expand_to_hold(num_exclusion_circles * num_points_per_circle)) {
+        err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
         return false;
     }
 
-    // fail if more than number of polygon points algorithm can handle
-    if (num_points >= OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX) {
+    // iterate through exclusion circles and create outer polygon points
+    for (uint8_t i = 0; i < num_exclusion_circles; i++) {
+        Vector2f circle_pos_cm;
+        float radius;
+        if (fence->polyfence().get_exclusion_circle(i, circle_pos_cm, radius)) {
+            // scaler to ensure lines between points do not intersect circle
+            const float scaler = (1.0f / cosf(radians(180.0f / (float)num_points_per_circle))) * ((radius * 100.0f) + margin_cm);
+
+            // add points to array
+            for (uint8_t j = 0; j < num_points_per_circle; j++) {
+                _exclusion_circle_pts[_exclusion_circle_numpoints] = circle_pos_cm + (unit_offsets[j] * scaler);
+                _exclusion_circle_numpoints++;
+            }
+        }
+    }
+
+    // record fence update time so we don't process these exclusion circles again
+    _exclusion_circle_update_ms = fence->polyfence().get_exclusion_circle_update_ms();
+
+    return true;
+}
+
+// returns total number of points across all fence types
+uint16_t AP_OADijkstra::total_numpoints() const
+{
+    return _inclusion_polygon_numpoints + _exclusion_polygon_numpoints + _exclusion_circle_numpoints;
+}
+
+// get a single point across the total list of points from all fence types
+bool AP_OADijkstra::get_point(uint16_t index, Vector2f &point) const
+{
+    // sanity check index
+    if (index >= total_numpoints()) {
         return false;
     }
 
-    // clear polygon visibility graph
-    _polyfence_visgraph.clear();
+    // return an inclusion polygon point
+    if (index < _inclusion_polygon_numpoints) {
+        point = _inclusion_polygon_pts[index];
+        return true;
+    }
+    index -= _inclusion_polygon_numpoints;
 
-    // calculate distance from each polygon fence point to all other points
-    for (uint8_t i=0; i<_polyfence_numpoints-1; i++) {
-        const Vector2f &start1 = _polyfence_pts[i];
-        for (uint8_t j=i+1; j<_polyfence_numpoints; j++) {
-            const Vector2f &end1 = _polyfence_pts[j];
+    // return an exclusion polygon point
+    if (index < _exclusion_polygon_numpoints) {
+        point = _exclusion_polygon_pts[index];
+        return true;
+    }
+    index -= _exclusion_polygon_numpoints;
+
+    // return an exclusion circle point
+    if (index < _exclusion_circle_numpoints) {
+        point = _exclusion_circle_pts[index];
+        return true;
+    }
+
+    // we should never get here but just in case
+    return false;
+}
+
+// returns true if line segment intersects polygon or circular fence
+bool AP_OADijkstra::intersects_fence(const Vector2f &seg_start, const Vector2f &seg_end) const
+{
+    // return immediately if fence is not enabled
+    const AC_Fence *fence = AC_Fence::get_singleton();
+    if (fence == nullptr) {
+        return false;
+    }
+
+    // determine if segment crosses any of the inclusion polygons
+    uint16_t num_points = 0;
+    for (uint8_t i = 0; i < fence->polyfence().get_inclusion_polygon_count(); i++) {
+        const Vector2f* boundary = fence->polyfence().get_inclusion_polygon(i, num_points);
+        if ((boundary != nullptr) && (num_points >= 3)) {
             Vector2f intersection;
-            // ToDo: calculation below could be sped up by removing unused intersection and
-            //       skipping segments we know are parallel to our fence-with-margin segments
-            if (!Polygon_intersects(boundary, num_points, start1, end1, intersection)) {
-                // line segment does not intersect with original fence so add to visgraph
-                _polyfence_visgraph.add_item({AP_OAVisGraph::OATYPE_FENCE_POINT, i},
-                                             {AP_OAVisGraph::OATYPE_FENCE_POINT, j},
-                                             (_polyfence_pts[i] - _polyfence_pts[j]).length());
+            if (Polygon_intersects(boundary, num_points, seg_start, seg_end, intersection)) {
+                return true;
             }
         }
     }
 
-    return true;
+    // determine if segment crosses any of the exclusion polygons
+    for (uint8_t i = 0; i < fence->polyfence().get_exclusion_polygon_count(); i++) {
+        const Vector2f* boundary = fence->polyfence().get_exclusion_polygon(i, num_points);
+        if ((boundary != nullptr) && (num_points >= 3)) {
+            Vector2f intersection;
+            if (Polygon_intersects(boundary, num_points, seg_start, seg_end, intersection)) {
+                return true;
+            }
+        }
+    }
+
+    // determine if segment crosses any of the inclusion circles
+    for (uint8_t i = 0; i < fence->polyfence().get_inclusion_circle_count(); i++) {
+        Vector2f center_pos_cm;
+        float radius;
+        if (fence->polyfence().get_inclusion_circle(i, center_pos_cm, radius)) {
+            // intersects circle if either start or end is further from the center than the radius
+            const float radius_cm_sq = sq(radius * 100.0f) ;
+            if ((seg_start - center_pos_cm).length_squared() > radius_cm_sq) {
+                return true;
+            }
+            if ((seg_end - center_pos_cm).length_squared() > radius_cm_sq) {
+                return true;
+            }
+        }
+    }
+
+    // determine if segment crosses any of the exclusion circles
+    for (uint8_t i = 0; i < fence->polyfence().get_exclusion_circle_count(); i++) {
+        Vector2f center_pos_cm;
+        float radius;
+        if (fence->polyfence().get_exclusion_circle(i, center_pos_cm, radius)) {
+            // calculate distance between circle's center and segment
+            const float dist_cm = Vector2f::closest_distance_between_line_and_point(seg_start, seg_end, center_pos_cm);
+
+            // intersects if distance is less than radius
+            if (dist_cm <= (radius * 100.0f)) {
+                return true;
+            }
+        }
+    }
+
+    // if we got this far then no intersection
+    return false;
 }
 
-// updates visibility graph for a given position which is an offset (in cm) from the ekf origin
-// to add an additional position (i.e. the destination) set add_extra_position = true and provide the position in the extra_position argument
-// requires create_polygon_fence_with_margin to have been run
-// returns true on success
-bool AP_OADijkstra::update_visgraph(AP_OAVisGraph& visgraph, const AP_OAVisGraph::OAItemID& oaid, const Vector2f &position, bool add_extra_position, Vector2f extra_position)
+// create visibility graph for all fence (with margin) points
+// returns true on success.  returns false on failure and err_id is updated
+// requires these functions to have been run create_inclusion_polygon_with_margin, create_exclusion_polygon_with_margin, create_exclusion_circle_with_margin
+bool AP_OADijkstra::create_fence_visgraph(AP_OADijkstra_Error &err_id)
 {
-    // exit immediately if no polygon fence (with margin)
-    if (_polyfence_numpoints == 0) {
+    // exit immediately if fence is not enabled
+    const AC_Fence *fence = AC_Fence::get_singleton();
+    if (fence == nullptr) {
+        err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_FENCE_DISABLED;
         return false;
     }
 
-    // exit immediately if polygon fence is not enabled
-    const AC_Fence *fence = AC_Fence::get_singleton();
-    if (fence == nullptr) {
+    // fail if more fence points than algorithm can handle
+    if (total_numpoints() >= OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX) {
+        err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_TOO_MANY_FENCE_POINTS;
         return false;
     }
 
-    // get polygon boundary
-    uint16_t num_points;
-    const Vector2f* boundary = fence->polyfence_const().get_boundary_points(num_points);
-    if ((boundary == nullptr) || (num_points < 3)) {
+    // clear fence points visibility graph
+    _fence_visgraph.clear();
+
+    // calculate distance from each point to all other points
+    for (uint8_t i = 0; i < total_numpoints() - 1; i++) {
+        Vector2f start_seg;
+        if (get_point(i, start_seg)) {
+            for (uint8_t j = i + 1; j < total_numpoints(); j++) {
+                Vector2f end_seg;
+                if (get_point(j, end_seg)) {
+                    // if line segment does not intersect with any inclusion or exclusion zones add to visgraph
+                    if (!intersects_fence(start_seg, end_seg)) {
+                        if (!_fence_visgraph.add_item({AP_OAVisGraph::OATYPE_INTERMEDIATE_POINT, i},
+                                                      {AP_OAVisGraph::OATYPE_INTERMEDIATE_POINT, j},
+                                                      (start_seg - end_seg).length())) {
+                            // failure to add a point can only be caused by out-of-memory
+                            err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
+                            return false;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    return true;
+}
+
+// updates visibility graph for a given position which is an offset (in cm) from the ekf origin
+// to add an additional position (i.e. the destination) set add_extra_position = true and provide the position in the extra_position argument
+// requires create_inclusion_polygon_with_margin to have been run
+// returns true on success
+bool AP_OADijkstra::update_visgraph(AP_OAVisGraph& visgraph, const AP_OAVisGraph::OAItemID& oaid, const Vector2f &position, bool add_extra_position, Vector2f extra_position)
+{
+    // exit immediately if no fence (with margin) points
+    if (total_numpoints() == 0) {
         return false;
     }
 
     // clear visibility graph
     visgraph.clear();
 
-    // calculate distance from extra_position to all fence points
-    for (uint8_t i=0; i<_polyfence_numpoints; i++) {
-        Vector2f intersection;
-        if (!Polygon_intersects(boundary, num_points, position, _polyfence_pts[i], intersection)) {
-            // line segment does not intersect with original fence so add to visgraph
-            visgraph.add_item(oaid, {AP_OAVisGraph::OATYPE_FENCE_POINT, i}, (position - _polyfence_pts[i]).length());
+    // calculate distance from position to all inclusion/exclusion fence points
+    for (uint8_t i = 0; i < total_numpoints(); i++) {
+        Vector2f seg_end;
+        if (get_point(i, seg_end)) {
+            if (!intersects_fence(position, seg_end)) {
+                // line segment does not intersect with fences so add to visgraph
+                if (!visgraph.add_item(oaid, {AP_OAVisGraph::OATYPE_INTERMEDIATE_POINT, i}, (position - seg_end).length())) {
+                    return false;
+                }
+            }
         }
-        // ToDo: store infinity when there is no clear path between points to allow faster search later
     }
 
-    // add extra point to visibility graph if it doesn't intersect with polygon fence
+    // add extra point to visibility graph if it doesn't intersect with polygon fence or exclusion polygons
     if (add_extra_position) {
-        Vector2f intersection;
-        if (!Polygon_intersects(boundary, num_points, position, extra_position, intersection)) {
-            visgraph.add_item(oaid, {AP_OAVisGraph::OATYPE_DESTINATION, 0}, (position - extra_position).length());
+        if (!intersects_fence(position, extra_position)) {
+            if (!visgraph.add_item(oaid, {AP_OAVisGraph::OATYPE_DESTINATION, 0}, (position - extra_position).length())) {
+                return false;
+            }
         }
     }
 
@@ -327,7 +687,7 @@ bool AP_OADijkstra::update_visgraph(AP_OAVisGraph& visgraph, const AP_OAVisGraph
 void AP_OADijkstra::update_visible_node_distances(node_index curr_node_idx)
 {
     // sanity check
-    if (curr_node_idx > _short_path_data_numpoints) {
+    if (curr_node_idx >= _short_path_data_numpoints) {
         return;
     }
 
@@ -335,7 +695,7 @@ void AP_OADijkstra::update_visible_node_distances(node_index curr_node_idx)
     const ShortPathNode &curr_node = _short_path_data[curr_node_idx];
 
     // for each visibility graph
-    const AP_OAVisGraph* visgraphs[] = {&_polyfence_visgraph, &_destination_visgraph};
+    const AP_OAVisGraph* visgraphs[] = {&_fence_visgraph, &_destination_visgraph};
     for (uint8_t v=0; v<ARRAY_SIZE(visgraphs); v++) {
 
         // skip if empty
@@ -345,7 +705,7 @@ void AP_OADijkstra::update_visible_node_distances(node_index curr_node_idx)
         }
 
         // search visibility graph for items visible from current_node
-        for (uint8_t i=0; i<curr_visgraph.num_items(); i++) {
+        for (uint16_t i = 0; i < curr_visgraph.num_items(); i++) {
             const AP_OAVisGraph::VisGraphItem &item = curr_visgraph[i];
             // match if current node's id matches either of the id's in the graph (i.e. either end of the vector)
             if ((curr_node.id == item.id1) || (curr_node.id == item.id2)) {
@@ -385,8 +745,8 @@ bool AP_OADijkstra::find_node_from_id(const AP_OAVisGraph::OAItemID &id, node_in
             return true;
         }
         break;
-    case AP_OAVisGraph::OATYPE_FENCE_POINT:
-        // must be a fence node which start from 3rd node
+    case AP_OAVisGraph::OATYPE_INTERMEDIATE_POINT:
+        // intermediate nodes start from 3rd node
         if (_short_path_data_numpoints > id.id_num + 2) {
             node_idx = id.id_num + 2;
             return true;
@@ -422,23 +782,31 @@ bool AP_OADijkstra::find_closest_node_idx(node_index &node_idx) const
 }
 
 // calculate shortest path from origin to destination
-// returns true on success
-// requires create_polygon_fence_with_margin and create_polygon_fence_visgraph to have been run
+// returns true on success.  returns false on failure and err_id is updated
+// requires these functions to have been run: create_inclusion_polygon_with_margin, create_exclusion_polygon_with_margin, create_exclusion_circle_with_margin, create_polygon_fence_visgraph
 // resulting path is stored in _shortest_path array as vector offsets from EKF origin
-bool AP_OADijkstra::calc_shortest_path(const Location &origin, const Location &destination)
+bool AP_OADijkstra::calc_shortest_path(const Location &origin, const Location &destination, AP_OADijkstra_Error &err_id)
 {
     // convert origin and destination to offsets from EKF origin
     Vector2f origin_NE, destination_NE;
     if (!origin.get_vector_xy_from_origin_NE(origin_NE) || !destination.get_vector_xy_from_origin_NE(destination_NE)) {
+        err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_NO_POSITION_ESTIMATE;
         return false;
     }
 
-    // create origin and destination visgraphs of polygon points
-    update_visgraph(_source_visgraph, {AP_OAVisGraph::OATYPE_SOURCE, 0}, origin_NE, true, destination_NE);
-    update_visgraph(_destination_visgraph, {AP_OAVisGraph::OATYPE_DESTINATION, 0}, destination_NE);
+    // create visgraphs of origin and destination to fence points
+    if (!update_visgraph(_source_visgraph, {AP_OAVisGraph::OATYPE_SOURCE, 0}, origin_NE, true, destination_NE)) {
+        err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
+        return false;
+    }
+    if (!update_visgraph(_destination_visgraph, {AP_OAVisGraph::OATYPE_DESTINATION, 0}, destination_NE)) {
+        err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
+        return false;
+    }
 
     // expand _short_path_data if necessary
-    if (!_short_path_data.expand_to_hold(2 + _polyfence_numpoints)) {
+    if (!_short_path_data.expand_to_hold(2 + total_numpoints())) {
+        err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
         return false;
     }
 
@@ -447,21 +815,22 @@ bool AP_OADijkstra::calc_shortest_path(const Location &origin, const Location &d
     _short_path_data[1] = {{AP_OAVisGraph::OATYPE_DESTINATION, 0}, false, OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX, FLT_MAX};
     _short_path_data_numpoints = 2;
 
-    // add fence points to short_path_data array (node_type, id, visited, distance_from_idx, distance_cm)
-    for (uint8_t i=0; i<_polyfence_numpoints; i++) {
-        _short_path_data[_short_path_data_numpoints++] = {{AP_OAVisGraph::OATYPE_FENCE_POINT, i}, false, OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX, FLT_MAX};
+    // add all inclusion and exclusion fence points to short_path_data array (node_type, id, visited, distance_from_idx, distance_cm)
+    for (uint8_t i=0; i<total_numpoints(); i++) {
+        _short_path_data[_short_path_data_numpoints++] = {{AP_OAVisGraph::OATYPE_INTERMEDIATE_POINT, i}, false, OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX, FLT_MAX};
     }
 
     // start algorithm from source point
     node_index current_node_idx = 0;
 
     // update nodes visible from source point
-    for (uint8_t i=0; i<_source_visgraph.num_items(); i++) {
+    for (uint16_t i = 0; i < _source_visgraph.num_items(); i++) {
         node_index node_idx;
         if (find_node_from_id(_source_visgraph[i].id2, node_idx)) {
             _short_path_data[node_idx].distance_cm = _source_visgraph[i].distance_cm;
             _short_path_data[node_idx].distance_from_idx = current_node_idx;
         } else {
+            err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_COULD_NOT_FIND_PATH;
             return false;
         }
     }
@@ -481,15 +850,14 @@ bool AP_OADijkstra::calc_shortest_path(const Location &origin, const Location &d
     bool success = false;
     node_index nidx;
     if (!find_node_from_id({AP_OAVisGraph::OATYPE_DESTINATION,0}, nidx)) {
+        err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_COULD_NOT_FIND_PATH;
         return false;
     }
     _path_numpoints = 0;
     while (true) {
-        // fail if out of space
-        if (_path_numpoints >= _path.max_items()) {
-            if (!_path.expand()) {
-                break;
-            }
+        if (!_path.expand_to_hold(_path_numpoints + 1)) {
+            err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_OUT_OF_MEMORY;
+            return false;
         }
         // fail if newest node has invalid distance_from_index
         if ((_short_path_data[nidx].distance_from_idx == OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX) ||
@@ -514,6 +882,8 @@ bool AP_OADijkstra::calc_shortest_path(const Location &origin, const Location &d
     if (success) {
         _path_source = origin_NE;
         _path_destination = destination_NE;
+    } else {
+        err_id = AP_OADijkstra_Error::DIJKSTRA_ERROR_COULD_NOT_FIND_PATH;
     }
 
     return success;
@@ -537,15 +907,11 @@ bool AP_OADijkstra::get_shortest_path_point(uint8_t point_num, Vector2f& pos)
     case AP_OAVisGraph::OATYPE_DESTINATION:
         pos = _path_destination;
         return true;
-    case AP_OAVisGraph::OATYPE_FENCE_POINT:
-        // sanity check polygon fence has the point
-        if (id.id_num < _polyfence_numpoints) {
-            pos = _polyfence_pts[id.id_num];
-            return true;
-        }
-        return false;
+    case AP_OAVisGraph::OATYPE_INTERMEDIATE_POINT:
+        return get_point(id.id_num, pos);
     }
 
     // we should never reach here but just in case
     return false;
 }
+

libraries/AC_Avoidance/AP_OADijkstra.h

@@ -35,45 +35,117 @@ public:
 
 private:
 
-    // returns true if polygon fence is enabled
-    bool polygon_fence_enabled() const;
+    // returns true if at least one inclusion or exclusion zone is enabled
+    bool some_fences_enabled() const;
+
+    enum class AP_OADijkstra_Error : uint8_t {
+        DIJKSTRA_ERROR_NONE = 0,
+        DIJKSTRA_ERROR_OUT_OF_MEMORY,
+        DIJKSTRA_ERROR_OVERLAPPING_POLYGON_POINTS,
+        DIJKSTRA_ERROR_FAILED_TO_BUILD_INNER_POLYGON,
+        DIJKSTRA_ERROR_OVERLAPPING_POLYGON_LINES,
+        DIJKSTRA_ERROR_FENCE_DISABLED,
+        DIJKSTRA_ERROR_TOO_MANY_FENCE_POINTS,
+        DIJKSTRA_ERROR_NO_POSITION_ESTIMATE,
+        DIJKSTRA_ERROR_COULD_NOT_FIND_PATH
+    };
 
-    // check if polygon fence has been updated since we created the inner fence. returns true if changed
-    bool check_polygon_fence_updated() const;
+    // return error message for a given error id
+    const char* get_error_msg(AP_OADijkstra_Error error_id) const;
 
-    // create a smaller polygon fence within the existing polygon fence
-    // returns true on success
-    bool create_polygon_fence_with_margin(float margin_cm);
+    // report error to ground station
+    void report_error(AP_OADijkstra_Error error_id);
 
-    // create a visibility graph of the polygon fence
-    // returns true on success
-    // requires create_polygon_fence_with_margin to have been run
-    bool create_polygon_fence_visgraph();
+    //
+    // inclusion polygon methods
+    //
+
+    // check if inclusion polygons have been updated since create_inclusion_polygon_with_margin was run
+    // returns true if changed
+    bool check_inclusion_polygon_updated() const;
+
+    // create polygons inside the existing inclusion polygons
+    // returns true on success.  returns false on failure and err_id is updated
+    bool create_inclusion_polygon_with_margin(float margin_cm, AP_OADijkstra_Error &err_id);
+
+    //
+    // exclusion polygon methods
+    //
+
+    // check if exclusion polygons have been updated since create_exclusion_polygon_with_margin was run
+    // returns true if changed
+    bool check_exclusion_polygon_updated() const;
+
+    // create polygons around existing exclusion polygons
+    // returns true on success.  returns false on failure and err_id is updated
+    bool create_exclusion_polygon_with_margin(float margin_cm, AP_OADijkstra_Error &err_id);
+
+    //
+    // exclusion circle methods
+    //
+
+    // check if exclusion circles have been updated since create_exclusion_circle_with_margin was run
+    // returns true if changed
+    bool check_exclusion_circle_updated() const;
+
+    // create polygons around existing exclusion circles
+    // returns true on success.  returns false on failure and err_id is updated
+    bool create_exclusion_circle_with_margin(float margin_cm, AP_OADijkstra_Error &err_id);
+
+    //
+    // other methods
+    //
+
+    // returns total number of points across all fence types
+    uint16_t total_numpoints() const;
+
+    // get a single point across the total list of points from all fence types
+    // also returns the type of point
+    bool get_point(uint16_t index, Vector2f& point) const;
+
+    // returns true if line segment intersects polygon or circular fence
+    bool intersects_fence(const Vector2f &seg_start, const Vector2f &seg_end) const;
+
+    // create visibility graph for all fence (with margin) points
+    // returns true on success.  returns false on failure and err_id is updated
+    bool create_fence_visgraph(AP_OADijkstra_Error &err_id);
 
     // calculate shortest path from origin to destination
-    // returns true on success
+    // returns true on success.  returns false on failure and err_id is updated
     // requires create_polygon_fence_with_margin and create_polygon_fence_visgraph to have been run
     // resulting path is stored in _shortest_path array as vector offsets from EKF origin
-    bool calc_shortest_path(const Location &origin, const Location &destination);
+    bool calc_shortest_path(const Location &origin, const Location &destination, AP_OADijkstra_Error &err_id);
 
     // shortest path state variables
-    bool _polyfence_with_margin_ok;
+    bool _inclusion_polygon_with_margin_ok;
+    bool _exclusion_polygon_with_margin_ok;
+    bool _exclusion_circle_with_margin_ok;
     bool _polyfence_visgraph_ok;
     bool _shortest_path_ok;
 
     Location _destination_prev;     // destination of previous iterations (used to determine if path should be re-calculated)
     uint8_t _path_idx_returned;     // index into _path array which gives location vehicle should be currently moving towards
 
-    // polygon fence (with margin) related variables
-    float _polyfence_margin = 10;
-    AP_ExpandingArray<Vector2f> _polyfence_pts;
-    uint8_t _polyfence_numpoints;
-    uint32_t _polyfence_update_ms;  // system time of boundary update from AC_Fence (used to detect changes to polygon fence)
+    // inclusion polygon (with margin) related variables
+    float _polyfence_margin = 10;           // margin around polygon defaults to 10m but is overriden with set_fence_margin
+    AP_ExpandingArray<Vector2f> _inclusion_polygon_pts; // array of nodes corresponding to inclusion polygon points plus a margin
+    uint8_t _inclusion_polygon_numpoints;   // number of points held in above array
+    uint32_t _inclusion_polygon_update_ms;  // system time of boundary update from AC_Fence (used to detect changes to polygon fence)
+
+    // exclusion polygon related variables
+    AP_ExpandingArray<Vector2f> _exclusion_polygon_pts; // array of nodes corresponding to exclusion polygon points plus a margin
+    uint8_t _exclusion_polygon_numpoints;   // number of points held in above array
+    uint32_t _exclusion_polygon_update_ms;  // system time exclusion polygon was updated (used to detect changes)
+
+    // exclusion circle related variables
+    AP_ExpandingArray<Vector2f> _exclusion_circle_pts; // array of nodes surrounding exclusion circles plus a margin
+    uint8_t _exclusion_circle_numpoints;    // number of points held in above array
+    uint32_t _exclusion_circle_update_ms;   // system time exclusion circles were updated (used to detect changes)
 
     // visibility graphs
-    AP_OAVisGraph _polyfence_visgraph;
-    AP_OAVisGraph _source_visgraph;
-    AP_OAVisGraph _destination_visgraph;
+    AP_OAVisGraph _fence_visgraph;          // holds distances between all inclusion/exclusion fence points (with margin)
+    AP_OAVisGraph _source_visgraph;         // holds distances from source point to all other nodes
+    AP_OAVisGraph _destination_visgraph;    // holds distances from the destination to all other nodes
 
     // updates visibility graph for a given position which is an offset (in cm) from the ekf origin
     // to add an additional position (i.e. the destination) set add_extra_position = true and provide the position in the extra_position argument
@@ -111,4 +183,7 @@ private:
 
     // return point from final path as an offset (in cm) from the ekf origin
     bool get_shortest_path_point(uint8_t point_num, Vector2f& pos);
+
+    AP_OADijkstra_Error _error_last_id;                 // last error id sent to GCS
+    uint32_t _error_last_report_ms;                     // last time an error message was sent to GCS
 };

libraries/AC_Avoidance/AP_OAVisGraph.cpp

@@ -24,6 +24,11 @@ AP_OAVisGraph::AP_OAVisGraph() :
 // add item to visiblity graph, returns true on success, false if graph is full
 bool AP_OAVisGraph::add_item(const OAItemID &id1, const OAItemID &id2, float distance_cm)
 {
+    // no more than 65k items
+    if (_num_items == UINT16_MAX) {
+        return false;
+    }
+
     // ensure there is space in the array
     if (!_items.expand_to_hold(_num_items+1)) {
         return false;

libraries/AC_Avoidance/AP_OAVisGraph.h

@@ -19,7 +19,7 @@ public:
     enum OAType : uint8_t {
         OATYPE_SOURCE = 0,
         OATYPE_DESTINATION,
-        OATYPE_FENCE_POINT
+        OATYPE_INTERMEDIATE_POINT,
     };
 
     // support up to 255 items of each type
@@ -43,17 +43,17 @@ public:
     void clear() { _num_items = 0; }
 
     // get number of items in visibility graph table
-    uint8_t num_items() const { return _num_items; }
+    uint16_t num_items() const { return _num_items; }
 
     // add item to visiblity graph, returns true on success, false if graph is full
     bool add_item(const OAItemID &id1, const OAItemID &id2, float distance_cm);
 
     // allow accessing graph as an array, 0 indexed
     // Note: no protection against out-of-bounds accesses so use with num_items()
-    const VisGraphItem& operator[](uint8_t i) const { return _items[i]; }
+    const VisGraphItem& operator[](uint16_t i) const { return _items[i]; }
 
 private:
 
     AP_ExpandingArray<VisGraphItem> _items;
-    uint8_t _num_items;
+    uint16_t _num_items;
 };