From 1b2e8b5c3f34be72b3d522ce45c9fc1eae05cb02 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 | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Fence.cpp b/libraries/GCS_MAVLink/GCS_Fence.cpp index d6365f40b9bfc1..63e47c17cdc4ff 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->enable(false, fences); return MAV_RESULT_ACCEPTED; - case 1: // enable fence - if (!fence->present()) - { + case AC_Fence::MavlinkFenceActions::ENABLE: + 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_FLOOR: + 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 !APM_BUILD_TYPE(APM_BUILD_ArduPlane) const AC_Avoid* avoid = AC_Avoid::get_singleton();