ArduPlane: Retrigger fence breach if mode

ArduPlane: Move auto enabling code to common fence library

ArduPlane/Plane.h

@@ -1012,7 +1012,6 @@ private:
     int8_t takeoff_tail_hold(void);
     int16_t get_takeoff_pitch_min_cd(void);
     void landing_gear_update(void);
-    void complete_auto_takeoff(void);
 
     // avoidance_adsb.cpp
     void avoidance_adsb_update(void);

ArduPlane/commands_logic.cpp

@@ -557,7 +557,9 @@ bool Plane::verify_takeoff()
         auto_state.takeoff_complete = true;
         next_WP_loc = prev_WP_loc = current_loc;
 
-        plane.complete_auto_takeoff();
+#if AC_FENCE == ENABLED
+        plane.fence.auto_enable_fence_after_takeoff();
+#endif
 
         // don't cross-track on completion of takeoff, as otherwise we
         // can end up doing too sharp a turn

ArduPlane/fence.cpp

@@ -42,7 +42,15 @@ void Plane::fence_check()
         return;
     }
 
-    if (new_breaches) {
+    if (orig_breaches &&
+        (control_mode->is_guided_mode()
+        || control_mode == &mode_rtl || fence.get_action() == AC_FENCE_ACTION_REPORT_ONLY)) {
+        // we have already triggered, don't trigger again until the
+        // user disables/re-enables using the fence channel switch
+        return;
+    }
+
+    if (new_breaches || orig_breaches) {
         // if the user wants some kind of response and motors are armed
         const uint8_t fence_act = fence.get_action();
         switch (fence_act) {

ArduPlane/mode_takeoff.cpp

@@ -112,7 +112,10 @@ void ModeTakeoff::update()
         plane.next_WP_loc.alt = start_loc.alt + target_alt*100.0;
 
         plane.set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
-        plane.complete_auto_takeoff();
+        
+#if AC_FENCE == ENABLED
+        plane.fence.auto_enable_fence_after_takeoff();
+#endif
     }
 
     if (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF) {

ArduPlane/quadplane.cpp

@@ -2940,7 +2940,9 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd)
     plane.TECS_controller.set_pitch_max_limit(transition_pitch_max);
     set_alt_target_current();
 
-    plane.complete_auto_takeoff();
+#if AC_FENCE == ENABLED
+    plane.fence.auto_enable_fence_after_takeoff();
+#endif
 
     if (plane.control_mode == &plane.mode_auto) {
         // we reset TECS so that the target height filter is not

ArduPlane/takeoff.cpp

@@ -269,26 +269,6 @@ return_zero:
     return 0;
 }
 
-
-/*
-  called when an auto-takeoff is complete
- */
-void Plane::complete_auto_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
-}
-
-
 #if LANDING_GEAR_ENABLED == ENABLED
 /*
   update landing gear