AC_Fence: add a pre-arm check for polygon fence validity

Also change SYS_STATUS bit to be unhealthy if boundary invalid

libraries/AC_Fence/AC_Fence.cpp

@@ -104,6 +104,23 @@ uint8_t AC_Fence::get_enabled_fences() const
     return _enabled_fences;
 }
 
+// additional checks for the polygon fence:
+bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const
+{
+    if (!(_enabled_fences & AC_FENCE_TYPE_POLYGON)) {
+        // not enabled; all good
+        return true;
+    }
+
+    if (!_boundary_valid) {
+        fail_msg = "Polygon boundary invalid";
+        return false;
+    }
+
+    return true;
+}
+
+
 /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully
 bool AC_Fence::pre_arm_check(const char* &fail_msg) const
 {
@@ -131,6 +148,10 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const
         }
     }
 
+    if (!pre_arm_check_polygon(fail_msg)) {
+        return false;
+    }
+
     // if we got this far everything must be ok
     return true;
 }
@@ -542,5 +563,11 @@ bool AC_Fence::geofence_failed() const
     if (get_breaches() != 0) {
         return true;
     }
+    if (_enabled_fences & AC_FENCE_TYPE_POLYGON) {
+        if (!_boundary_valid) {
+            return true;
+        }
+    }
+
     return false;
 }

libraries/AC_Fence/AC_Fence.h

@@ -130,6 +130,9 @@ private:
     /// clear_breach - update breach bitmask, time and count
     void clear_breach(uint8_t fence_type);
 
+    // additional checks for the polygon fence:
+    bool pre_arm_check_polygon(const char* &fail_msg) const;
+
     /// load polygon points stored in eeprom into boundary array and perform validation.  returns true if load successfully completed
     bool load_polygon_from_eeprom(bool force_reload = false);