AC_Fence: added option bit for union of inclusion areas

this allows for treating total inclusion area as union of all
inclusion areas. This is useful for:

- circles with corridors between them
- a fence for each flying site all loaded at once
- temporary addition of an extra area to a complex fence

libraries/AC_Fence/AC_Fence.cpp

@@ -137,8 +137,8 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = {
 
     // @Param{Plane}: OPTIONS
     // @DisplayName: Fence options
-    // @Description: 0:Disable mode change following fence action until fence breach is cleared
-    // @Bitmask: 0:Disable mode change following fence action until fence breach is cleared
+    // @Description: 0:Disable mode change following fence action until fence breach is cleared. When bit 1 is set the allowable flight areas is the union of all polygon and circle fence areas instead of the intersection, which means a fence breach occurs only if you are outside all of the fence areas.
+    // @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas
     // @User: Standard
     AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast<uint16_t>(OPTIONS::DISABLE_MODE_CHANGE), AP_PARAM_FRAME_PLANE),
 

libraries/AC_Fence/AC_Fence.h

@@ -32,6 +32,7 @@
 class AC_Fence
 {
 public:
+    friend class AC_PolyFence_loader;
 
     enum class AutoEnable
     {
@@ -146,10 +147,14 @@ public:
     const AC_PolyFence_loader &polyfence() const;
 
     enum class OPTIONS {
-        DISABLE_MODE_CHANGE = 1 << 0,
+        DISABLE_MODE_CHANGE = 1U << 0,
+        INCLUSION_UNION = 1U << 1,
     };
+    static bool option_enabled(OPTIONS opt, const AP_Int16 &options) {
+        return (options.get() & int16_t(opt)) != 0;
+    }
     bool option_enabled(OPTIONS opt) const {
-        return (_options.get() & int16_t(opt)) != 0;
+        return option_enabled(opt, _options);
     }
 
     static const struct AP_Param::GroupInfo var_info[];
@@ -219,7 +224,7 @@ private:
     uint32_t        _manual_recovery_start_ms;  // system time in milliseconds that pilot re-took manual control
 
 
-    AC_PolyFence_loader _poly_loader{_total}; // polygon fence
+    AC_PolyFence_loader _poly_loader{_total, _options}; // polygon fence
 };
 
 namespace AP {

libraries/AC_Fence/AC_PolyFence_loader.cpp

@@ -13,6 +13,7 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Logger/AP_Logger.h>
+#include <AC_Fence/AC_Fence.h>
 
 #include <stdio.h>
 
@@ -226,11 +227,14 @@ bool AC_PolyFence_loader::breached(const Location& loc) const
     pos.x = loc.lat;
     pos.y = loc.lng;
 
+    const uint16_t num_inclusion = _num_loaded_circle_inclusion_boundaries + _num_loaded_inclusion_boundaries;
+    uint16_t num_inclusion_outside = 0;
+
     // check we are inside each inclusion zone:
     for (uint8_t i=0; i<_num_loaded_inclusion_boundaries; i++) {
         const InclusionBoundary &boundary = _loaded_inclusion_boundary[i];
         if (Polygon_outside(pos, boundary.points_lla, boundary.count)) {
-            return true;
+            num_inclusion_outside++;
         }
     }
 
@@ -260,6 +264,21 @@ bool AC_PolyFence_loader::breached(const Location& loc) const
         circle_center.lng = circle.point.y;
         const float diff_cm = loc.get_distance(circle_center)*100.0f;
         if (diff_cm > circle.radius * 100.0f) {
+            num_inclusion_outside++;
+        }
+    }
+
+    if (AC_Fence::option_enabled(AC_Fence::OPTIONS::INCLUSION_UNION, _options)) {
+        // using union of inclusion areas, we are outside the fence if
+        // there is at least one inclusion areas and we are outside
+        // all of them
+        if (num_inclusion > 0 && num_inclusion == num_inclusion_outside) {
+            return true;
+        }
+    } else {
+        // using intersection of inclusion areas. We are outside if we
+        // are outside any of them
+        if (num_inclusion_outside > 0) {
             return true;
         }
     }

libraries/AC_Fence/AC_PolyFence_loader.h

@@ -46,8 +46,9 @@ class AC_PolyFence_loader
 
 public:
 
-    AC_PolyFence_loader(AP_Int8 &total) :
-        _total(total) {}
+    AC_PolyFence_loader(AP_Int8 &total, const AP_Int16 &options) :
+        _total(total),
+        _options(options) {}
 
     /* Do not allow copies */
     CLASS_NO_COPY(AC_PolyFence_loader);
@@ -397,6 +398,7 @@ private:
     // _total - reference to FENCE_TOTAL parameter.  This is used
     // solely for compatability with the FENCE_POINT protocol
     AP_Int8 &_total;
+    const AP_Int16 &_options;
     uint8_t _old_total;