From 92122e5133bf0589a69faeb967bacd33d2b7c991 Mon Sep 17 00:00:00 2001
From: "Dr.-Ing. Amilcar do Carmo Lucas" <amilcar.lucas@iav.de>
Date: Wed, 6 Jan 2021 16:19:23 +0100
Subject: [PATCH] Copter: automatically enable and disable floor fence on
 automated takeoff and landing

---
 ArduCopter/Copter.h      |  5 ++++-
 ArduCopter/fence.cpp     | 17 +++++++++++++++++
 ArduCopter/mode.h        |  4 +++-
 ArduCopter/mode_auto.cpp |  4 ++++
 ArduCopter/takeoff.cpp   | 17 +++++++++++++++++
 5 files changed, 45 insertions(+), 2 deletions(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index f330191db0..fa59b371dc 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -383,7 +383,7 @@ private:
 
     RCMapper rcmap;
 
-    // intertial nav alt when we armed
+    // inertial nav alt when we armed
     float arming_altitude_m;
 
     // Failsafe
@@ -744,7 +744,10 @@ private:
 #endif
 
     // fence.cpp
+#if AC_FENCE == ENABLED
     void fence_check();
+    void disable_fence_for_landing(void);
+#endif
 
     // heli.cpp
     void heli_init();
diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp
index 049d004fcc..2fc83e5879 100644
--- a/ArduCopter/fence.cpp
+++ b/ArduCopter/fence.cpp
@@ -83,4 +83,21 @@ void Copter::fence_check()
     }
 }
 
+void Copter::disable_fence_for_landing(void)
+{
+    switch (fence.auto_enabled()) {
+        case AC_Fence::AutoEnable::ALWAYS_ENABLED:
+            fence.enable(false);
+            gcs().send_text(MAV_SEVERITY_NOTICE, "Fence disabled (auto disable)");
+            break;
+        case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
+            fence.disable_floor();
+            gcs().send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
+            break;
+        default:
+            // fence does not auto-disable in other landing conditions
+            break;
+    }
+}
+
 #endif
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index 0bb5f73757..6dbd597ce9 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -21,7 +21,7 @@ public:
         RTL =           6,  // automatic return to launching point
         CIRCLE =        7,  // automatic circular flight with automatic throttle
         LAND =          9,  // automatic landing with horizontal position control
-        DRIFT =        11,  // semi-automous position, yaw and throttle control
+        DRIFT =        11,  // semi-autonomous position, yaw and throttle control
         SPORT =        13,  // manual earth-frame angular rate control with manual throttle
         FLIP =         14,  // automatically flip the vehicle on the roll axis
         AUTOTUNE =     15,  // automatically tune the vehicle's roll and pitch gains
@@ -115,6 +115,8 @@ protected:
     // return expected input throttle setting to hover:
     virtual float throttle_hover() const;
 
+    void autoenable_floor_fence(void);
+
     // Alt_Hold based flight mode states used in Alt_Hold, Loiter, and Sport
     enum AltHoldModeState {
         AltHold_MotorStopped,
diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
index a17c18d720..113553b46f 100644
--- a/ArduCopter/mode_auto.cpp
+++ b/ArduCopter/mode_auto.cpp
@@ -250,6 +250,10 @@ void ModeAuto::land_start(const Vector3f& destination)
 
     // optionally deploy landing gear
     copter.landinggear.deploy_for_landing();
+
+#if AC_FENCE == ENABLED
+    copter.disable_fence_for_landing();
+#endif
 }
 
 // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp
index 7175c14591..f0c0b1ca17 100644
--- a/ArduCopter/takeoff.cpp
+++ b/ArduCopter/takeoff.cpp
@@ -76,6 +76,7 @@ void Mode::_TakeOff::stop()
 {
     _running = false;
     start_ms = 0;
+    copter.flightmode->autoenable_floor_fence();
 }
 
 // returns pilot and takeoff climb rates
@@ -225,3 +226,19 @@ bool Mode::is_taking_off() const
     }
     return takeoff.running();
 }
+
+//  called when takeoff is complete
+void Mode::autoenable_floor_fence(void)
+{
+#if AC_FENCE == ENABLED
+    switch(copter.fence.auto_enabled()) {
+        case AC_Fence::AutoEnable::ALWAYS_ENABLED:
+        case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
+            copter.fence.enable(true);
+            break;
+        default:
+            // fence does not auto-enable in other takeoff conditions
+            break;
+    }
+#endif
+}