From 518399c13a42301d73a314a642d719723f10e564 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Dec 2017 10:25:52 +1100 Subject: [PATCH] AC_Fence: stop looking at EKF filter status Fence has no business looking at what the filter status is. Fence should only care whether it can currently get a position. --- libraries/AC_Fence/AC_Fence.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index f0f66a99d1..65c7caa7e6 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -130,15 +130,15 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const return false; } - // if we have horizontal limits enabled, check inertial nav position is ok - nav_filter_status filt_status; - if (!_ahrs.get_filter_status(filt_status)) { - fail_msg = "AHRS status not available"; - return false; - } - if ((_enabled_fences & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON))>0 && !filt_status.flags.horiz_pos_abs && !filt_status.flags.pred_horiz_pos_abs) { - fail_msg = "fence requires position"; - return false; + // if we have horizontal limits enabled, check we can get a + // relative position from the EKF + if ((_enabled_fences & AC_FENCE_TYPE_CIRCLE) || + (_enabled_fences & AC_FENCE_TYPE_POLYGON)) { + Vector2f position; + if (!_ahrs.get_relative_position_NE_origin(position)) { + fail_msg = "fence requires position"; + return false; + } } // if we got this far everything must be ok