diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 08cb8c6da0..e16fe909f6 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -38,7 +38,9 @@ void GCS::get_sensor_status_flags(uint32_t &present, MissionItemProtocol_Waypoints *GCS::_missionitemprotocol_waypoints; MissionItemProtocol_Rally *GCS::_missionitemprotocol_rally; +#if AC_FENCE MissionItemProtocol_Fence *GCS::_missionitemprotocol_fence; +#endif const MAV_MISSION_TYPE GCS_MAVLINK::supported_mission_types[] = { MAV_MISSION_TYPE_MISSION, @@ -267,7 +269,7 @@ void GCS::update_sensor_status_flags() } #endif -#if !defined(HAL_BUILD_AP_PERIPH) +#if !defined(HAL_BUILD_AP_PERIPH) && AC_FENCE const AC_Fence *fence = AP::fence(); if (fence != nullptr) { if (fence->sys_status_enabled()) { diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index b587c11b19..2be5bdd701 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -25,6 +25,7 @@ #include #include #include +#include #include "ap_message.h" @@ -1063,7 +1064,9 @@ public: static class MissionItemProtocol_Waypoints *_missionitemprotocol_waypoints; static class MissionItemProtocol_Rally *_missionitemprotocol_rally; +#if AC_FENCE static class MissionItemProtocol_Fence *_missionitemprotocol_fence; +#endif class MissionItemProtocol *get_prot_for_mission_type(const MAV_MISSION_TYPE mission_type) const; void try_send_queued_message_for_type(MAV_MISSION_TYPE type) const; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index a4e4e2987e..27ee062584 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -561,8 +561,10 @@ MissionItemProtocol *GCS::get_prot_for_mission_type(const MAV_MISSION_TYPE missi return _missionitemprotocol_waypoints; case MAV_MISSION_TYPE_RALLY: return _missionitemprotocol_rally; +#if AC_FENCE case MAV_MISSION_TYPE_FENCE: return _missionitemprotocol_fence; +#endif default: return nullptr; } @@ -2194,7 +2196,7 @@ void GCS::send_message(enum ap_message id) void GCS::update_send() { update_send_has_been_called = true; -#ifndef HAL_BUILD_AP_PERIPH +#if !defined(HAL_BUILD_AP_PERIPH) && AC_FENCE if (!initialised_missionitemprotocol_objects) { initialised_missionitemprotocol_objects = true; // once-only initialisation of MissionItemProtocol objects: @@ -6162,9 +6164,11 @@ uint64_t GCS_MAVLINK::capabilities() const } #endif +#if AC_FENCE if (AP::fence()) { ret |= MAV_PROTOCOL_CAPABILITY_MISSION_FENCE; } +#endif if (!AP_BoardConfig::ftp_disabled()){ //if ftp disable board option is not set ret |= MAV_PROTOCOL_CAPABILITY_FTP; diff --git a/libraries/GCS_MAVLink/GCS_Fence.cpp b/libraries/GCS_MAVLink/GCS_Fence.cpp index b6bffdad07..71da22bb6a 100644 --- a/libraries/GCS_MAVLink/GCS_Fence.cpp +++ b/libraries/GCS_MAVLink/GCS_Fence.cpp @@ -6,6 +6,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_fence_enable(const mavlink_command_long_t &packet) { +#if AC_FENCE AC_Fence *fence = AP::fence(); if (fence == nullptr) { return MAV_RESULT_UNSUPPORTED; @@ -29,10 +30,14 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_fence_enable(const mavlink_command_lon default: return MAV_RESULT_FAILED; } +#else + return MAV_RESULT_UNSUPPORTED; +#endif // AC_FENCE } void GCS_MAVLINK::handle_fence_message(const mavlink_message_t &msg) { +#if AC_FENCE AC_Fence *fence = AP::fence(); if (fence == nullptr) { return; @@ -49,11 +54,13 @@ void GCS_MAVLINK::handle_fence_message(const mavlink_message_t &msg) #endif break; } +#endif // AC_FENCE } // fence_send_mavlink_status - send fence status to ground station void GCS_MAVLINK::send_fence_status() const { +#if AC_FENCE const AC_Fence *fence = AP::fence(); if (fence == nullptr) { return; @@ -95,4 +102,5 @@ void GCS_MAVLINK::send_fence_status() const mavlink_breach_type, fence->get_breach_time(), breach_mitigation); +#endif // AC_FENCE } diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp index 294c28e387..611f03c8ab 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp @@ -4,6 +4,8 @@ #include #include +#if AC_FENCE + /* public function to format mission item as mavlink_mission_item_int_t */ @@ -241,3 +243,5 @@ MAV_MISSION_RESULT MissionItemProtocol_Fence::allocate_update_resources() _new_items_count = _item_count; return MAV_MISSION_ACCEPTED; } + +#endif // AC_FENCE