AC_Fence: add support for warnings at fence margins

libraries/AC_Fence/AC_Fence.cpp

@@ -142,7 +142,7 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = {
     // @Param{Plane, Copter}: OPTIONS
     // @DisplayName: Fence options
     // @Description: When bit 0 is set sisable 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
+    // @Bitmask: 0:Disable mode change following fence action until fence breach is cleared, 1:Allow union of inclusion areas, 2:Notify on margin breaches
     // @User: Standard
     AP_GROUPINFO_FRAME("OPTIONS", 11, AC_Fence, _options, static_cast<uint16_t>(AC_FENCE_OPTIONS_DEFAULT), AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_HELI),
 
@@ -556,6 +556,10 @@ bool AC_Fence::check_fence_alt_max()
         }
         // old breach
         return false;
+    } else if (_curr_alt >= _alt_max - _margin) {
+        record_margin_breach(AC_FENCE_TYPE_ALT_MAX);
+    } else {
+        clear_margin_breach(AC_FENCE_TYPE_ALT_MAX);
     }
 
     // not breached
@@ -605,6 +609,10 @@ bool AC_Fence::check_fence_alt_min()
         }
         // old breach
         return false;
+    } else if (_curr_alt <= _alt_min + _margin) {
+        record_margin_breach(AC_FENCE_TYPE_ALT_MIN);
+    } else {
+        clear_margin_breach(AC_FENCE_TYPE_ALT_MIN);
     }
 
     // not breached
@@ -705,6 +713,10 @@ bool AC_Fence::check_fence_circle()
             return true;
         }
         return false;
+    } else if (_home_distance >= _circle_radius - _margin) {
+        record_margin_breach(AC_FENCE_TYPE_CIRCLE);
+    } else {
+        clear_margin_breach(AC_FENCE_TYPE_CIRCLE);
     }
 
     // not currently breached
@@ -876,12 +888,37 @@ void AC_Fence::record_breach(uint8_t fence_type)
     _breached_fences |= fence_type;
 }
 
-/// clear_breach - update breach bitmask, time and count
+/// record_margin_breach - update margin_breach bitmask, time and count
+void AC_Fence::record_margin_breach(uint8_t fence_type)
+{
+    // if we haven't already breached a margin limit, update the breach time
+    if (!_breached_fence_margins) {
+        const uint32_t now = AP_HAL::millis();
+
+        // emit a message indicated we're newly-breached, but not too often
+        if (option_enabled(OPTIONS::MARGIN_BREACH) 
+            && AP_HAL::timeout_expired(_last_margin_breach_notify_sent_ms, now, 1000U)) {
+            _last_margin_breach_notify_sent_ms = now;
+            print_fence_message("outside margin", _breached_fence_margins);
+        }
+    }
+
+    // update bitmask
+    _breached_fence_margins |= fence_type;
+}
+
+/// clear_breach - update breach bitmask
 void AC_Fence::clear_breach(uint8_t fence_type)
 {
     _breached_fences &= ~fence_type;
 }
 
+/// clear_margin_breach - update margin reach bitmask
+void AC_Fence::clear_margin_breach(uint8_t fence_type)
+{
+    _breached_fence_margins &= ~fence_type;
+}
+
 /// get_breach_distance - returns maximum distance in meters outside
 /// of the given fences.  fence_type is a bitmask here.
 float AC_Fence::get_breach_distance(uint8_t fence_type) const

libraries/AC_Fence/AC_Fence.h

@@ -177,6 +177,7 @@ class AC_Fence
     enum class OPTIONS {
         DISABLE_MODE_CHANGE = 1U << 0,
         INCLUSION_UNION = 1U << 1,
+        MARGIN_BREACH = 1U << 2,
     };
     static bool option_enabled(OPTIONS opt, const AP_Int16 &options) {
         return (options.get() & int16_t(opt)) != 0;
@@ -211,9 +212,15 @@ class AC_Fence
     /// record_breach - update breach bitmask, time and count
     void record_breach(uint8_t fence_type);
 
-    /// clear_breach - update breach bitmask, time and count
+    /// clear_breach - update breach bitmask
     void clear_breach(uint8_t fence_type);
 
+    /// record_margin_breach - update margin breach bitmask
+    void record_margin_breach(uint8_t fence_type);
+
+    /// clear_margin_breach - update margin breach bitmask
+    void clear_margin_breach(uint8_t fence_type);
+
     // additional checks for the different fence types:
     bool pre_arm_check_polygon(char *failure_msg, const uint8_t failure_msg_len) const;
     bool pre_arm_check_circle(char *failure_msg, const uint8_t failure_msg_len) const;
@@ -252,10 +259,12 @@ class AC_Fence
     float           _home_distance;         // distance from home in meters (provided by main code)
 
     // breach information
-    uint8_t         _breached_fences;       // bitmask holding the fence type that was breached (i.e. AC_FENCE_TYPE_ALT_MIN, AC_FENCE_TYPE_CIRCLE)
+    uint8_t         _breached_fences;       // bitmask holding the fence types that were breached (i.e. AC_FENCE_TYPE_ALT_MIN, AC_FENCE_TYPE_CIRCLE)
+    uint8_t         _breached_fence_margins; // bitmask holding the fence types that have margin breaches (i.e. AC_FENCE_TYPE_ALT_MIN, AC_FENCE_TYPE_CIRCLE)
     uint32_t        _breach_time;           // time of last breach in milliseconds
     uint16_t        _breach_count;          // number of times we have breached the fence
     uint32_t _last_breach_notify_sent_ms;  // last time we sent a message about newly-breaching the fences
+    uint32_t _last_margin_breach_notify_sent_ms;  // last time we sent a message about newly-breaching the fences
 
     uint32_t        _manual_recovery_start_ms;  // system time in milliseconds that pilot re-took manual control