AC_Fence: Add Pre-Arm check for margin < radius

libraries/AC_Fence/AC_Fence.cpp

@@ -129,6 +129,11 @@ bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const
         return false;
     }
 
+    if (!_poly_loader.check_inclusion_circle_margin(_margin)) {
+        fail_msg = "Margin is less than inclusion circle radius";
+        return false;
+    }
+
     return true;
 }
 
@@ -139,6 +144,11 @@ bool AC_Fence::pre_arm_check_circle(const char* &fail_msg) const
         fail_msg = "Invalid FENCE_RADIUS value";
         return false;
     }
+    if (_circle_radius < _margin) {
+        fail_msg = "FENCE_MARGIN is less than FENCE_RADIUS";
+        return false;
+    }
+
     return true;
 }
 

libraries/AC_Fence/AC_PolyFence_loader.cpp

@@ -865,6 +865,19 @@ bool AC_PolyFence_loader::get_inclusion_circle(uint8_t index, Vector2f &center_p
     return true;
 }
 
+bool AC_PolyFence_loader::check_inclusion_circle_margin(float margin) const
+{
+    // check circular includes
+    for (uint8_t i=0; i<_num_loaded_circle_inclusion_boundaries; i++) {
+        const InclusionCircle &circle = _loaded_circle_inclusion_boundary[i];
+        if (circle.radius < margin) {
+            // circle radius should never be less than margin
+            return false;
+        } 
+    }
+    return true;
+}
+
 bool AC_PolyFence_loader::validate_fence(const AC_PolyFenceItem *new_items, uint16_t count) const
 {
     // validate the fence items...

libraries/AC_Fence/AC_PolyFence_loader.h

@@ -110,6 +110,9 @@ public:
     /// center is offsets in cm from EKF origin in NE frame, radius is in meters
     bool get_inclusion_circle(uint8_t index, Vector2f &center_pos_cm, float &radius) const;
 
+    // false if margin < fence radius 
+    bool check_inclusion_circle_margin(float margin) const;
+
     ///
     /// mavlink
     ///