From c1a25c25f788d289bed45d4f79dda2ae86880c16 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 24 Mar 2017 13:38:19 +0900 Subject: [PATCH] AC_Fence: return failure message --- libraries/AC_Fence/AC_Fence.cpp | 6 +++++- libraries/AC_Fence/AC_Fence.h | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index d962a12d390462..bc916ce07a64dc 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -108,8 +108,10 @@ uint8_t AC_Fence::get_enabled_fences() const } /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully -bool AC_Fence::pre_arm_check() const +bool AC_Fence::pre_arm_check(const char* &fail_msg) const { + fail_msg = nullptr; + // if not enabled or not fence set-up always return true if (!_enabled || _enabled_fences == AC_FENCE_TYPE_NONE) { return true; @@ -117,11 +119,13 @@ bool AC_Fence::pre_arm_check() const // check no limits are currently breached if (_breached_fences != AC_FENCE_TYPE_NONE) { + fail_msg = "vehicle outside fence"; return false; } // if we have horizontal limits enabled, check inertial nav position is ok if ((_enabled_fences & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON))>0 && !_inav.get_filter_status().flags.horiz_pos_abs && !_inav.get_filter_status().flags.pred_horiz_pos_abs) { + fail_msg = "fence requires position"; return false; } diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 8581a12c6c7093..95930523969227 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -49,7 +49,7 @@ class AC_Fence uint8_t get_enabled_fences() const; /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully - bool pre_arm_check() const; + bool pre_arm_check(const char* &fail_msg) const; /// /// methods to check we are within the boundaries and recover