From f73154349bb7b73c8853ce25802f718643d8e539 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 13 Jan 2024 18:02:54 +0000 Subject: [PATCH] GCS_MAVLink: use bitmask based enablement for fences --- libraries/GCS_MAVLink/GCS_Fence.cpp | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Fence.cpp b/libraries/GCS_MAVLink/GCS_Fence.cpp index d6dad1a769..b3bfb78a37 100644 --- a/libraries/GCS_MAVLink/GCS_Fence.cpp +++ b/libraries/GCS_MAVLink/GCS_Fence.cpp @@ -16,20 +16,24 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_fence_enable(const mavlink_command_int return MAV_RESULT_UNSUPPORTED; } - switch ((uint16_t)packet.param1) { - case 0: // disable fence - fence->enable(false); + uint8_t fences = AC_FENCE_ALL_FENCES; + if (uint8_t(packet.param2)) { + fences = uint8_t(packet.param2); + } + + switch (AC_Fence::MavlinkFenceActions(packet.param1)) { + case AC_Fence::MavlinkFenceActions::DISABLE_FENCE: + fence->enable(false, fences); return MAV_RESULT_ACCEPTED; - case 1: // enable fence - if (!fence->present()) - { + case AC_Fence::MavlinkFenceActions::ENABLE_FENCE: + if (!(fence->present() & fences)) { return MAV_RESULT_FAILED; } - - fence->enable(true); + + fence->enable(true, fences); return MAV_RESULT_ACCEPTED; - case 2: // disable fence floor only - fence->disable_floor(); + case AC_Fence::MavlinkFenceActions::DISABLE_ALT_MIN_FENCE: + fence->enable(false, AC_FENCE_TYPE_ALT_MIN); return MAV_RESULT_ACCEPTED; default: return MAV_RESULT_FAILED; @@ -82,7 +86,7 @@ void GCS_MAVLINK::send_fence_status() const mavlink_breach_type = FENCE_BREACH_BOUNDARY; } - // report on Avoidance liminting + // report on Avoidance limiting uint8_t breach_mitigation = FENCE_MITIGATE_UNKNOWN; #if AP_AVOIDANCE_ENABLED && !APM_BUILD_TYPE(APM_BUILD_ArduPlane) const AC_Avoid* avoid = AC_Avoid::get_singleton();