From a8c17873fc34240e0caae66fb41f840259873cc4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 23 Jul 2023 08:03:08 +1000 Subject: [PATCH] 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 | 4 ++-- libraries/AC_Fence/AC_Fence.h | 11 ++++++++--- libraries/AC_Fence/AC_PolyFence_loader.cpp | 21 ++++++++++++++++++++- libraries/AC_Fence/AC_PolyFence_loader.h | 6 ++++-- 4 files changed, 34 insertions(+), 8 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index ce10f2f63a..eea61ea6cc 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/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(OPTIONS::DISABLE_MODE_CHANGE), AP_PARAM_FRAME_PLANE), diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index d4c042c082..1e8bd1d37c 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/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 { diff --git a/libraries/AC_Fence/AC_PolyFence_loader.cpp b/libraries/AC_Fence/AC_PolyFence_loader.cpp index 242ed7c40f..c8d17102ba 100644 --- a/libraries/AC_Fence/AC_PolyFence_loader.cpp +++ b/libraries/AC_Fence/AC_PolyFence_loader.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include @@ -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; } } diff --git a/libraries/AC_Fence/AC_PolyFence_loader.h b/libraries/AC_Fence/AC_PolyFence_loader.h index 4c74bd7e2c..90da5691ab 100644 --- a/libraries/AC_Fence/AC_PolyFence_loader.h +++ b/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;