ArduCopter: Improve auto-enable/disable of fence

ArduCopter/Copter.h

@@ -746,6 +746,7 @@ private:
     // fence.cpp
 #if AC_FENCE == ENABLED
     void fence_check();
+    void autoenable_fence_after_takeoff(void);
     void disable_fence_for_landing(void);
 #endif
 

ArduCopter/fence.cpp

@@ -83,6 +83,24 @@ void Copter::fence_check()
     }
 }
 
+/*
+  called when an auto-takeoff is complete
+ */
+void Copter::autoenable_fence_after_takeoff(void)
+{
+#if AC_FENCE == ENABLED
+    switch(fence.auto_enabled()) {
+        case AC_Fence::AutoEnable::ALWAYS_ENABLED:
+        case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
+            fence.enable(true);
+            break;
+        default:
+            // fence does not auto-enable in other takeoff conditions
+            break;
+    }
+#endif
+}
+
 void Copter::disable_fence_for_landing(void)
 {
     switch (fence.auto_enabled()) {

ArduCopter/mode_auto.cpp

@@ -1517,9 +1517,13 @@ bool ModeAuto::verify_takeoff()
     // have we reached our target altitude?
     const bool reached_wp_dest = copter.wp_nav->reached_wp_destination();
 
-    // retract the landing gear
+    // if we have reached our destination
     if (reached_wp_dest) {
+        // retract the landing gear
         copter.landinggear.retract_after_takeoff();
+
+        // auto-enable the fence after takeoff
+        copter.autoenable_fence_after_takeoff();
     }
 
     return reached_wp_dest;

ArduCopter/mode_guided.cpp

@@ -398,6 +398,9 @@ void ModeGuided::takeoff_run()
         // optionally retract landing gear
         copter.landinggear.retract_after_takeoff();
 
+        // takeoff complete, enable fence
+        copter.autoenable_fence_after_takeoff();
+
         // switch to position control mode but maintain current target
         const Vector3f target = wp_nav->get_wp_destination();
         set_destination(target, false, 0, false, 0, false, wp_nav->origin_and_destination_are_terrain_alt());

ArduCopter/mode_land.cpp

@@ -40,6 +40,9 @@ bool ModeLand::init(bool ignore_checks)
     // optionally deploy landing gear
     copter.landinggear.deploy_for_landing();
 
+    // disable the fence on landing
+    copter.disable_fence_for_landing();
+
     return true;
 }
 

ArduCopter/takeoff.cpp

@@ -76,7 +76,6 @@ void Mode::_TakeOff::stop()
 {
     _running = false;
     start_ms = 0;
-    copter.flightmode->autoenable_floor_fence();
 }
 
 // returns pilot and takeoff climb rates