From c8e0ae6890d230b2100999515dfeaf74d5b3ec8d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 27 Dec 2017 20:10:08 +1100 Subject: [PATCH] AC_Fence: adjust fence sys_status failed based on position and parameters --- libraries/AC_Fence/AC_Fence.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index f19035089d..66e47881e4 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -588,6 +588,24 @@ bool AC_Fence::sys_status_failed() const return true; } } + if (_enabled_fences & AC_FENCE_TYPE_CIRCLE) { + if (_circle_radius < 0) { + return true; + } + } + if (_enabled_fences & AC_FENCE_TYPE_ALT_MAX) { + if (_alt_max < 0.0f) { + return true; + } + } + if ((_enabled_fences & AC_FENCE_TYPE_CIRCLE) || + (_enabled_fences & AC_FENCE_TYPE_POLYGON)) { + Vector2f position; + if (!_ahrs.get_relative_position_NE_home(position)) { + // both these fence types require position + return true; + } + } return false; }