diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 094c54faac..c0fb303427 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -518,3 +518,32 @@ bool AC_Fence::load_polygon_from_eeprom(bool force_reload) return true; } + +// methods for mavlink SYS_STATUS message (send_extended_status1) +bool AC_Fence::geofence_present() const +{ + return _enabled; +} + +bool AC_Fence::geofence_enabled() const +{ + if (!geofence_present()) { + return false; + } + if (_action == AC_FENCE_ACTION_REPORT_ONLY) { + return false; + } + return true; +} + +bool AC_Fence::geofence_failed() const +{ + if (!geofence_present()) { + // not failed if not present; can fail if present but not enabled + return false; + } + if (get_breaches() != 0) { + return true; + } + return false; +} diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 1f631f1d52..bfeb9ac543 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -121,6 +121,11 @@ public: static const struct AP_Param::GroupInfo var_info[]; + // methods for mavlink SYS_STATUS message (send_extended_status1) + bool geofence_present() const; + bool geofence_enabled() const; + bool geofence_failed() const; + private: AC_Fence(const AP_AHRS_NavEKF &ahrs);