GCS_MAVLink: use bitmask based enablement for fences

ArduPilot · Jan 13, 2024 · 1b2e8b5 · 1b2e8b5
1 parent 7012a2c
commit 1b2e8b5
Showing 1 changed file with 14 additions and 10 deletions.
diff --git 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();