diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index eac6f7082d..a5ca5b41d6 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -183,6 +183,7 @@ public: // common send functions void send_heartbeat(void) const; void send_meminfo(void); + void send_fence_status() const; void send_power_status(void); void send_battery_status(const AP_BattMonitor &battery, const uint8_t instance) const; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 0ac86e64a0..571468d5ed 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3829,6 +3829,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) send_meminfo(); break; + case MSG_FENCE_STATUS: + CHECK_PAYLOAD_SIZE(FENCE_STATUS); + send_fence_status(); + break; + case MSG_RANGEFINDER: CHECK_PAYLOAD_SIZE(RANGEFINDER); send_rangefinder(); diff --git a/libraries/GCS_MAVLink/GCS_Fence.cpp b/libraries/GCS_MAVLink/GCS_Fence.cpp new file mode 100644 index 0000000000..b7c14b969f --- /dev/null +++ b/libraries/GCS_MAVLink/GCS_Fence.cpp @@ -0,0 +1,33 @@ +#include "GCS.h" + +#include + +// fence_send_mavlink_status - send fence status to ground station +void GCS_MAVLINK::send_fence_status() const +{ + const AC_Fence *fence = AP::fence(); + if (fence == nullptr) { + return; + } + if (!fence->enabled()) { + return; + } + + // traslate fence library breach types to mavlink breach types + uint8_t mavlink_breach_type = FENCE_BREACH_NONE; + const uint8_t breaches = fence->get_breaches(); + if ((breaches & AC_FENCE_TYPE_ALT_MAX) != 0) { + mavlink_breach_type = FENCE_BREACH_MAXALT; + } + if ((breaches & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) != 0) { + mavlink_breach_type = FENCE_BREACH_BOUNDARY; + } + + // send status + mavlink_msg_fence_status_send(chan, + static_cast(fence->get_breaches() != 0), + fence->get_breach_count(), + mavlink_breach_type, + fence->get_breach_time()); +} +