From cd6ddf7d4fcc3a75a5ef687efab8bf1c119b7a2e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 1 Dec 2020 14:22:34 +1100
Subject: [PATCH] Plane: fixed disable of geofence on quadplane landing

fixes #15917
---
 ArduPlane/AP_Arming.cpp      |  2 +-
 ArduPlane/Plane.h            |  1 +
 ArduPlane/commands_logic.cpp | 16 +---------------
 ArduPlane/geofence.cpp       | 21 +++++++++++++++++++++
 ArduPlane/quadplane.cpp      |  2 ++
 5 files changed, 26 insertions(+), 16 deletions(-)

diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp
index f90d2c023c..e677e79386 100644
--- a/ArduPlane/AP_Arming.cpp
+++ b/ArduPlane/AP_Arming.cpp
@@ -239,7 +239,7 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method)
     gcs().send_text(MAV_SEVERITY_INFO, "Throttle disarmed");
 
 #if GEOFENCE_ENABLED == ENABLED
-    if (plane.g.fence_autoenable == FenceAutoEnable::AutoDisableFloorOnly) {
+    if (plane.g.fence_autoenable == FenceAutoEnable::WhenArmed) {
         plane.geofence_set_enabled(false);
     }
 #endif
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 86625c4bb4..33aa08d62e 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -949,6 +949,7 @@ private:
     void geofence_send_status(mavlink_channel_t chan);
     bool geofence_breached(void);
     void geofence_disable_and_send_error_msg(const char *errorMsg);
+    void disable_fence_for_landing(void);
 
     // ArduPlane.cpp
     void disarm_if_autoland_complete();
diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
index 58bd0f2729..e5df746af0 100644
--- a/ArduPlane/commands_logic.cpp
+++ b/ArduPlane/commands_logic.cpp
@@ -385,21 +385,7 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
         set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND);
     }
 
-#if GEOFENCE_ENABLED == ENABLED 
-    if (g.fence_autoenable == FenceAutoEnable::Auto) {
-        if (! geofence_set_enabled(false)) {
-            gcs().send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)");
-        } else {
-            gcs().send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)");
-        }
-    } else if (g.fence_autoenable == FenceAutoEnable::AutoDisableFloorOnly) {
-        if (! geofence_set_floor_enabled(false)) {
-            gcs().send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)");
-        } else {
-            gcs().send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
-        }
-    }
-#endif
+    disable_fence_for_landing();
 }
 
 void Plane::do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd)
diff --git a/ArduPlane/geofence.cpp b/ArduPlane/geofence.cpp
index e08239e0b2..404a6b94a1 100644
--- a/ArduPlane/geofence.cpp
+++ b/ArduPlane/geofence.cpp
@@ -513,6 +513,23 @@ bool Plane::geofence_breached(void)
     return geofence_state ? geofence_state->fence_triggered : false;
 }
 
+void Plane::disable_fence_for_landing(void)
+{
+    if (g.fence_autoenable == FenceAutoEnable::Auto) {
+        if (!geofence_set_enabled(false)) {
+            gcs().send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)");
+        } else {
+            gcs().send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)");
+        }
+    } else if (g.fence_autoenable == FenceAutoEnable::AutoDisableFloorOnly) {
+        if (!geofence_set_floor_enabled(false)) {
+            gcs().send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)");
+        } else {
+            gcs().send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
+        }
+    }
+}
+
 
 #else // GEOFENCE_ENABLED
 
@@ -542,4 +559,8 @@ bool Plane::geofence_breached(void)
     return false;
 }
 
+void Plane::disable_fence_for_landing(void)
+{
+}
+
 #endif // GEOFENCE_ENABLED
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 6d533a78d8..90477e6352 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -1201,6 +1201,7 @@ void QuadPlane::init_qland(void)
 #if LANDING_GEAR_ENABLED == ENABLED
     plane.g2.landing_gear.deploy_for_landing();
 #endif
+    plane.disable_fence_for_landing();
 }
 
 
@@ -2929,6 +2930,7 @@ bool QuadPlane::verify_vtol_land(void)
     if (poscontrol.state == QPOS_POSITION2 &&
         plane.auto_state.wp_distance < 2) {
         poscontrol.state = QPOS_LAND_DESCEND;
+        plane.disable_fence_for_landing();
 #if LANDING_GEAR_ENABLED == ENABLED
         plane.g2.landing_gear.deploy_for_landing();
 #endif