Fence warn at the margins

Tools/autotest/arducopter.py

@@ -1908,6 +1908,93 @@ def FenceFloorAutoEnableOnArming(self):
         # Disable the fence using mavlink command to ensure cleaned up SITL state
         self.assert_fence_disabled()
 
+    def FenceMargin(self, timeout=180):
+        '''Test warning on crossing fence margin'''
+        # enable fence, disable avoidance
+        self.set_parameters({
+            "FENCE_ENABLE": 1,
+            "FENCE_TYPE": 6,    # polygon and circle fences
+            "FENCE_MARGIN" : 20,
+            "FENCE_RADIUS" : 150,
+            "AVOID_ENABLE": 0,
+            "FENCE_OPTIONS": 4
+        })
+
+        self.change_mode("LOITER")
+        self.wait_ready_to_arm()
+
+        # fence requires home to be set:
+        m = self.poll_home_position(quiet=False)
+
+        # 110m polyfence
+        home_loc = self.mav.location()
+        locs = [
+            self.offset_location_ne(home_loc, -110, -110),
+            self.offset_location_ne(home_loc, 110, -110),
+            self.offset_location_ne(home_loc, 110, 110),
+            self.offset_location_ne(home_loc, -110, 110),
+        ]
+        self.upload_fences_from_locations([
+            (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, locs),
+        ])
+
+        self.takeoff(10, mode="LOITER")
+
+        # first east
+        self.progress("turn east")
+        self.set_rc(4, 1580)
+        self.wait_heading(160, timeout=60)
+        self.set_rc(4, 1500)
+
+        fence_radius = self.get_parameter("FENCE_RADIUS")
+
+        self.progress("flying forward (east) until we hit fence")
+        pitching_forward = True
+        self.set_rc(2, 1100)
+
+        self.wait_statustext("Circle fence outside margin")
+        self.progress("Waiting for fence breach")
+        tstart = self.get_sim_time()
+        while not self.mode_is("RTL"):
+            if self.get_sim_time_cached() - tstart > 30:
+                raise NotAchievedException("Did not breach fence")
+
+            m = self.assert_receive_message('GLOBAL_POSITION_INT')
+            alt = m.relative_alt / 1000.0 # mm -> m
+            home_distance = self.distance_to_home(use_cached_home=True)
+            self.progress("Alt: %.02f  HomeDistance: %.02f (fence radius=%f)" %
+                          (alt, home_distance, fence_radius))
+
+        self.wait_statustext("Circle fence inside margin")
+        self.progress("Waiting until we get home and disarm")
+        tstart = self.get_sim_time()
+        while self.get_sim_time_cached() < tstart + timeout:
+            m = self.assert_receive_message('GLOBAL_POSITION_INT')
+            alt = m.relative_alt / 1000.0 # mm -> m
+            home_distance = self.distance_to_home(use_cached_home=True)
+            self.progress("Alt: %.02f  HomeDistance: %.02f" %
+                          (alt, home_distance))
+            # recenter pitch sticks once we're home so we don't fly off again
+            if pitching_forward and home_distance < 50:
+                pitching_forward = False
+                self.set_rc(2, 1475)
+                # disable fence
+                self.set_parameter("FENCE_ENABLE", 0)
+            if (alt <= 1 and home_distance < 10) or (not self.armed() and home_distance < 10):
+                # reduce throttle
+                self.zero_throttle()
+                self.change_mode("LAND")
+                self.wait_landed_and_disarmed()
+                self.progress("Reached home OK")
+                self.zero_throttle()
+                return
+
+        # give we're testing RTL, doing one here probably doesn't make sense
+        home_distance = self.distance_to_home(use_cached_home=True)
+        raise AutoTestTimeoutException(
+            "Fence test failed to reach home (%fm distance) - "
+            "timed out after %u seconds" % (home_distance, timeout,))
+
     def GPSGlitchLoiter(self, timeout=30, max_distance=20):
         """fly_gps_glitch_loiter_test. Fly south east in loiter and test
         reaction to gps glitch."""
@@ -10806,6 +10893,7 @@ def tests1d(self):
              self.FenceFloorEnabledLanding,
              self.FenceFloorAutoDisableLanding,
              self.FenceFloorAutoEnableOnArming,
+             self.FenceMargin,
              self.AutoTuneSwitch,
              self.GPSGlitchLoiter,
              self.GPSGlitchLoiter2,

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
@@ -659,13 +667,23 @@ bool AC_Fence::check_fence_polygon()
     }
 
     const bool was_breached = _breached_fences & AC_FENCE_TYPE_POLYGON;
-    if (_poly_loader.breached()) {
+
+    Location loc;
+    bool have_location = AP::ahrs().get_location(loc);
+    bool inside_margin = false;
+
+    if (have_location && _poly_loader.breached(loc, _margin, inside_margin)) {
         if (!was_breached) {
             record_breach(AC_FENCE_TYPE_POLYGON);
             return true;
         }
         return false;
+    } else if (option_enabled(OPTIONS::MARGIN_BREACH) && inside_margin) {
+        record_margin_breach(AC_FENCE_TYPE_POLYGON);
+    } else {
+        clear_margin_breach(AC_FENCE_TYPE_POLYGON);
     }
+
     if (was_breached) {
         clear_breach(AC_FENCE_TYPE_POLYGON);
     }
@@ -705,6 +723,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 +898,48 @@ 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 & fence_type)) {
+        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 | fence_type);
+        }
+    }
+
+    // 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)
+{
+    if (_breached_fence_margins & fence_type) {
+        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("inside margin", 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

@@ -130,6 +130,9 @@ class AC_Fence
     /// get_breach_count - returns number of times we have breached the fence
     uint16_t get_breach_count() const { return _breach_count; }
 
+    /// get_breaches - returns bitmask of the fence types that have had their margins breached
+    uint8_t get_margin_breaches() const { return _breached_fence_margins; }
+
     /// get_breach_distance - returns maximum distance in meters outside
     /// of the given fences.  fence_type is a bitmask here.
     float get_breach_distance(uint8_t fence_type) const;
@@ -177,6 +180,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 +215,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 +262,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
 

libraries/AC_Fence/AC_PolyFence_loader.cpp

@@ -225,7 +225,7 @@ bool AC_PolyFence_loader::breached() const
 
 // check if a position (expressed as lat/lng) is within the boundary
 //   returns true if location is outside the boundary
-bool AC_PolyFence_loader::breached(const Location& loc) const
+bool AC_PolyFence_loader::breached(const Location& loc, float margin, bool& inside_margin) const
 {
     if (!loaded() || total_fence_count() == 0) {
         return false;
@@ -234,15 +234,21 @@ bool AC_PolyFence_loader::breached(const Location& loc) const
     Vector2l pos;
     pos.x = loc.lat;
     pos.y = loc.lng;
+    margin = margin * 1.0e7 / 111132.954;  // convert margin to degrees
 
     const uint16_t num_inclusion = _num_loaded_circle_inclusion_boundaries + _num_loaded_inclusion_boundaries;
     uint16_t num_inclusion_outside = 0;
+    inside_margin = false;
+
 
     // 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)) {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%f %f", Polygon_closest_distance_point(boundary.points_lla, boundary.count, pos), margin);
             num_inclusion_outside++;
+        } else if (Polygon_closest_distance_point(boundary.points_lla, boundary.count, pos) < margin) {
+            inside_margin = true;
         }
     }
 
@@ -251,6 +257,8 @@ bool AC_PolyFence_loader::breached(const Location& loc) const
         const ExclusionBoundary &boundary = _loaded_exclusion_boundary[i];
         if (!Polygon_outside(pos, boundary.points_lla, boundary.count)) {
             return true;
+        } else if (Polygon_closest_distance_point(boundary.points_lla, boundary.count, pos) < margin) {
+            inside_margin = true;
         }
     }
 
@@ -262,6 +270,8 @@ bool AC_PolyFence_loader::breached(const Location& loc) const
         const float diff_cm = loc.get_distance(circle_center)*100.0f;
         if (diff_cm < circle.radius * 100.0f) {
             return true;
+        } else if (diff_cm < (circle.radius + margin) * 100.0f) {
+            inside_margin = true;
         }
     }
 
@@ -273,6 +283,8 @@ bool AC_PolyFence_loader::breached(const Location& loc) const
         const float diff_cm = loc.get_distance(circle_center)*100.0f;
         if (diff_cm > circle.radius * 100.0f) {
             num_inclusion_outside++;
+        } else if (diff_cm > (circle.radius - margin) * 100.0f) {
+            inside_margin = true;
         }
     }
 
@@ -1657,7 +1669,7 @@ bool AC_PolyFence_loader::get_inclusion_circle(uint8_t index, Vector2f &center_p
 void AC_PolyFence_loader::handle_msg(GCS_MAVLINK &link, const mavlink_message_t& msg) {};
 
 bool AC_PolyFence_loader::breached() const { return false; }
-bool AC_PolyFence_loader::breached(const Location& loc) const { return false; }
+bool AC_PolyFence_loader::breached(const Location& loc, float margin, bool& inside_margin) const { return false; }
 
 uint16_t AC_PolyFence_loader::max_items() const { return 0; }
 

libraries/AC_Fence/AC_PolyFence_loader.h

@@ -132,8 +132,15 @@ class AC_PolyFence_loader
 
     //  breached() - returns true if the vehicle has breached any fence
     bool breached() const WARN_IF_UNUSED;
+    //  breached(Location&, float margin, bool& inside_margin) - returns true if location is outside the boundary
+    //  sets if loc is inside the margin specified
+    bool breached(const Location& loc, float margin, bool& inside_margin) const WARN_IF_UNUSED;
     //  breached(Location&) - returns true if location is outside the boundary
-    bool breached(const Location& loc) const WARN_IF_UNUSED;
+    bool breached(const Location& loc) const WARN_IF_UNUSED
+    {
+        bool inside_margin;
+        return breached(loc, 0.0f, inside_margin);
+    }
 
     // returns true if a polygonal include fence could be returned
     bool inclusion_boundary_available() const WARN_IF_UNUSED {