AC_Fence: add safety margin parameter

This will allow auto modes to proactively avoid breaching the fence

libraries/AC_Fence/AC_Fence.cpp

@@ -42,6 +42,14 @@ const AP_Param::GroupInfo AC_Fence::var_info[] PROGMEM = {
     // @Range: 30 10000
     // @User: Standard
     AP_GROUPINFO("RADIUS",      4,  AC_Fence,   _circle_radius, AC_FENCE_CIRCLE_RADIUS_DEFAULT),
+
+    // @Param: MARGIN
+    // @DisplayName: Fence Margin
+    // @Description: Distance that autopilot's should maintain from the fence to avoid a breach
+    // @Units: Meters
+    // @Range: 1 10
+    // @User: Standard
+    AP_GROUPINFO("MARGIN",      5,  AC_Fence,   _margin, AC_FENCE_MARGIN_DEFAULT),
     
     AP_GROUPEND
 };

libraries/AC_Fence/AC_Fence.h

@@ -22,6 +22,7 @@
 #define AC_FENCE_CIRCLE_RADIUS_DEFAULT              150.0f  // default circular fence radius is 150m
 #define AC_FENCE_ALT_MAX_BACKUP_DISTANCE            20.0f   // after fence is broken we recreate the fence 20m further up
 #define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE      20.0f   // after fence is broken we recreate the fence 20m further out
+#define AC_FENCE_MARGIN_DEFAULT                     2.0f    // default distance in meters that autopilot's should maintain from the fence to avoid a breach
 
 // give up distance
 #define AC_FENCE_GIVE_UP_DISTANCE                   100.0f  // distance outside the fence at which we should give up and just land.  Note: this is not used by library directly but is intended to be used by the main code
@@ -66,6 +67,9 @@ public:
 
     /// get_action - getter for user requested action on limit breach
     uint8_t get_action() const { return _action.get(); }
+
+    /// get_safe_alt - returns maximum safe altitude (i.e. alt_max - margin)
+    float get_safe_alt() const { return _alt_max - _margin; }
     
     ///
     /// time saving methods to piggy-back on main code's calculations
@@ -93,6 +97,7 @@ private:
     AP_Int8         _action;                // recovery action specified by user
     AP_Float        _alt_max;               // altitude upper limit in meters
     AP_Float        _circle_radius;         // circle fence radius in meters
+    AP_Float        _margin;                // distance in meters that autopilot's should maintain from the fence to avoid a breach
 
     // backup fences
     float           _alt_max_backup;        // backup altitude upper limit in meters used to refire the breach if the vehicle continues to move further away
@@ -100,7 +105,7 @@ private:
 
     // breach distances
     float           _alt_max_breach_distance;   // distance above the altitude max
-    float           _circle_breach_distance;    // distance above the altitude max
+    float           _circle_breach_distance;    // distance beyond the circular fence
 
     // other internal variables
     float           _home_distance;         // distance from home in meters (provided by main code)