From 22bdc3e54c5ad636d8a997f0ca56ce5ae5178735 Mon Sep 17 00:00:00 2001
From: "Dr.-Ing. Amilcar Do Carmo Lucas" <amilcar.lucas@iav.de>
Date: Thu, 24 Aug 2017 11:48:17 +0200
Subject: [PATCH] AC_Fence: extend pre-arm checks

Fix comments
---
 libraries/AC_Fence/AC_Fence.cpp | 20 ++++++++++++++++++--
 libraries/AC_Fence/AC_Fence.h   |  2 +-
 2 files changed, 19 insertions(+), 3 deletions(-)

diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp
index a68cf00e8b..f4aab8bc9f 100644
--- a/libraries/AC_Fence/AC_Fence.cpp
+++ b/libraries/AC_Fence/AC_Fence.cpp
@@ -276,6 +276,22 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const
         return false;
     }
 
+    // validate FENCE_MARGIN parameter range
+    if (_margin < 0.0f) {
+        fail_msg = "Invalid FENCE_MARGIN value";
+        return false;
+    }
+
+    if (_alt_max < _alt_min) {
+        fail_msg =  "FENCE_ALT_MAX < FENCE_ALT_MIN";
+        return false;
+    }
+
+    if (_alt_max - _alt_min <= 2.0f * _margin) {
+        fail_msg =  "FENCE_MARGIN too big";
+        return false;
+    }
+
     // if we got this far everything must be ok
     return true;
 }
@@ -343,14 +359,14 @@ bool AC_Fence::check_fence_alt_min()
     _curr_alt = -_curr_alt; // translate Down to Up
 
     // check if we are under the altitude fence
-    if (_curr_alt < _alt_min) {
+    if (_curr_alt <= _alt_min) {
 
         // record distance below breach
         _alt_min_breach_distance = _alt_min - _curr_alt;
 
         // check for a new breach or a breach of the backup fence
         if (!(_breached_fences & AC_FENCE_TYPE_ALT_MIN) ||
-            (!is_zero(_alt_min_backup) && _curr_alt < _alt_min_backup)) {
+            (!is_zero(_alt_min_backup) && _curr_alt <= _alt_min_backup)) {
 
             // new breach
             record_breach(AC_FENCE_TYPE_ALT_MIN);
diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h
index b0ec032775..acc2bb503c 100644
--- a/libraries/AC_Fence/AC_Fence.h
+++ b/libraries/AC_Fence/AC_Fence.h
@@ -120,7 +120,7 @@ public:
     /// get_safe_alt - returns maximum safe altitude (i.e. alt_max - margin)
     float get_safe_alt_max() const { return _alt_max - _margin; }
 
-    /// get_safe_alt_min - returns the minimum safe altitude (i.e. alt_min - margin)
+    /// get_safe_alt_min - returns the minimum safe altitude (i.e. alt_min + margin)
     float get_safe_alt_min() const { return _alt_min + _margin; }
 
     /// get_radius - returns the fence radius in meters