From 155407956c1b224c6c8311ccb392febe2508a07c Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 14 Dec 2017 13:54:03 +1100
Subject: [PATCH] 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 | 27 +++++++++++++++++++++++++++
 libraries/AC_Fence/AC_Fence.h   |  3 +++
 2 files changed, 30 insertions(+)

diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp
index 98550dfa8f..87b41d7825 100644
--- a/libraries/AC_Fence/AC_Fence.cpp
+++ b/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;
 }
diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h
index f8133826c4..924068f69e 100644
--- a/libraries/AC_Fence/AC_Fence.h
+++ b/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);