Copter Fence Fixes

ArduCopter/fence.cpp

@@ -10,8 +10,10 @@ void Copter::fence_check()
 {
     const uint8_t orig_breaches = fence.get_breaches();
 
+    bool is_landing_or_landed = flightmode->is_landing() || ap.land_complete  || !motors->armed();
+
     // check for new breaches; new_breaches is bitmask of fence types breached
-    const uint8_t new_breaches = fence.check();
+    const uint8_t new_breaches = fence.check(is_landing_or_landed);
 
     // we still don't do anything when disarmed, but we do check for fence breaches.
     // fence pre-arm check actually checks if any fence has been breached 
@@ -24,7 +26,7 @@ void Copter::fence_check()
     if (new_breaches) {
 
         if (!copter.ap.land_complete) {
-            GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence Breached");
+            fence.print_fence_message("breached", new_breaches);
         }
 
         // if the user wants some kind of response and motors are armed
@@ -81,7 +83,10 @@ void Copter::fence_check()
 
         LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches));
 
-    } else if (orig_breaches) {
+    } else if (orig_breaches && fence.get_breaches() == 0) {
+        if (!copter.ap.land_complete) {
+            GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared");
+        }
         // record clearing of breach
         LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED);
     }

ArduCopter/mode.cpp

@@ -347,6 +347,21 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
         return false;
     }
 
+#if AP_FENCE_ENABLED
+    // may not be allowed to change mode if recovering from fence breach
+    if (!ignore_checks &&
+        fence.enabled() &&
+        fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) &&
+        fence.get_breaches() &&
+        !flightmode->is_landing() &&
+        motors->armed() &&
+        get_control_mode_reason() == ModeReason::FENCE_BREACHED &&
+        !ap.land_complete) {
+        mode_change_failed(new_flightmode, "in fence recovery");
+        return false;
+    }
+#endif
+
     if (!new_flightmode->init(ignore_checks)) {
         mode_change_failed(new_flightmode, "init failed");
         return false;
@@ -371,10 +386,12 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
 #endif
 
 #if AP_FENCE_ENABLED
-    // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
-    // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
-    // but it should be harmless to disable the fence temporarily in these situations as well
-    fence.manual_recovery_start();
+    if (fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY) {
+        // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
+        // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
+        // but it should be harmless to disable the fence temporarily in these situations as well
+        fence.manual_recovery_start();
+    }
 #endif
 
 #if AP_CAMERA_ENABLED

ArduCopter/mode_auto.cpp

@@ -480,11 +480,6 @@ void ModeAuto::land_start()
     copter.landinggear.deploy_for_landing();
 #endif
 
-#if AP_FENCE_ENABLED
-    // disable the fence on landing
-    copter.fence.auto_disable_fence_for_landing();
-#endif
-
     // reset flag indicating if pilot has applied roll or pitch inputs during landing
     copter.ap.land_repo_active = false;
 
@@ -779,18 +774,6 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
         // point the camera to a specified angle
         do_mount_control(cmd);
         break;
-
-    case MAV_CMD_DO_FENCE_ENABLE:
-#if AP_FENCE_ENABLED
-        if (cmd.p1 == 0) { //disable
-            copter.fence.enable(false);
-            gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled");
-        } else { //enable fence
-            copter.fence.enable(true);
-            gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled");
-        }
-#endif //AP_FENCE_ENABLED
-        break;
 
 #if AC_NAV_GUIDED == ENABLED
     case MAV_CMD_DO_GUIDED_LIMITS:                      // 220  accept guided mode limits

ArduCopter/mode_guided.cpp

@@ -685,6 +685,9 @@ void ModeGuided::takeoff_run()
     auto_takeoff.run();
     if (auto_takeoff.complete && !takeoff_complete) {
         takeoff_complete = true;
+#if AP_FENCE_ENABLED
+        copter.fence.auto_enable_fence_after_takeoff();
+#endif
 #if AP_LANDINGGEAR_ENABLED
         // optionally retract landing gear
         copter.landinggear.retract_after_takeoff();

ArduCopter/mode_land.cpp

@@ -41,11 +41,6 @@ bool ModeLand::init(bool ignore_checks)
     copter.landinggear.deploy_for_landing();
 #endif
 
-#if AP_FENCE_ENABLED
-    // disable the fence on landing
-    copter.fence.auto_disable_fence_for_landing();
-#endif
-
 #if AC_PRECLAND_ENABLED
     // initialise precland state machine
     copter.precland_statemachine.init();

ArduCopter/mode_rtl.cpp

@@ -255,11 +255,6 @@ void ModeRTL::descent_start()
     // optionally deploy landing gear
     copter.landinggear.deploy_for_landing();
 #endif
-
-#if AP_FENCE_ENABLED
-    // disable the fence on landing
-    copter.fence.auto_disable_fence_for_landing();
-#endif
 }
 
 // rtl_descent_run - implements the final descent to the RTL_ALT
@@ -347,11 +342,6 @@ void ModeRTL::land_start()
     // optionally deploy landing gear
     copter.landinggear.deploy_for_landing();
 #endif
-
-#if AP_FENCE_ENABLED
-    // disable the fence on landing
-    copter.fence.auto_disable_fence_for_landing();
-#endif
 }
 
 bool ModeRTL::is_landing() const

ArduPlane/GCS_Mavlink.cpp

@@ -841,6 +841,14 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com
         return MAV_RESULT_DENIED;
     }
 
+#if AP_FENCE_ENABLED
+    // reject destination if outside the fence
+    if (!plane.fence.check_destination_within_fence(requested_position)) {
+        LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
+        return MAV_RESULT_DENIED;
+    }
+#endif
+
     // location is valid load and set
     if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) ||
         (plane.control_mode == &plane.mode_guided)) {

ArduPlane/Plane.h

@@ -250,6 +250,9 @@ class Plane : public AP_Vehicle {
 
     // are we currently in long failsafe but have postponed it in MODE TAKEOFF until min level alt is reached
     bool long_failsafe_pending;
+
+    //flag that we have already called autoenable fences once in MODE TAKEOFF
+    bool have_autoenabled_fences;
 
     // GCS selection
     GCS_Plane _gcs; // avoid using this; use gcs()

ArduPlane/commands_logic.cpp

@@ -144,21 +144,6 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_DO_LAND_START:
         break;
 
-    case MAV_CMD_DO_FENCE_ENABLE:
-#if AP_FENCE_ENABLED
-        if (cmd.p1 == 0) { // disable fence
-            plane.fence.enable(false);
-            gcs().send_text(MAV_SEVERITY_INFO, "Fence disabled");
-        } else if (cmd.p1 == 1) { // enable fence
-            plane.fence.enable(true);
-            gcs().send_text(MAV_SEVERITY_INFO, "Fence enabled");
-        } else if (cmd.p1 == 2) { // disable fence floor only
-            plane.fence.disable_floor();
-            gcs().send_text(MAV_SEVERITY_INFO, "Fence floor disabled");
-        }
-#endif
-        break;
-
     case MAV_CMD_DO_AUTOTUNE_ENABLE:
         autotune_enable(cmd.p1);
         break;
@@ -434,10 +419,6 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
         // if we were in an abort we need to explicitly move out of the abort state, as it's sticky
         set_flight_stage(AP_FixedWing::FlightStage::LAND);
     }
-
-#if AP_FENCE_ENABLED
-    plane.fence.auto_disable_fence_for_landing();
-#endif
 }
 
 #if HAL_QUADPLANE_ENABLED

ArduPlane/fence.cpp

@@ -8,9 +8,32 @@
 void Plane::fence_check()
 {
     const uint8_t orig_breaches = fence.get_breaches();
+    const bool armed = arming.is_armed();
+
+    uint16_t mission_id = plane.mission.get_current_nav_cmd().id;
+    bool landing_or_landed = plane.flight_stage == AP_FixedWing::FlightStage::LAND
+                         || !armed
+#if HAL_QUADPLANE_ENABLED
+                         || control_mode->mode_number() == Mode::Number::QLAND
+                         || quadplane.in_vtol_land_descent()
+#endif
+                         || (plane.is_land_command(mission_id) && plane.mission.state() == AP_Mission::MISSION_RUNNING);
 
     // check for new breaches; new_breaches is bitmask of fence types breached
-    const uint8_t new_breaches = fence.check();
+    const uint8_t new_breaches = fence.check(landing_or_landed);
+
+    /*
+      if we are either disarmed or we are currently not in breach and
+      we are not flying then clear the state associated with the
+      previous mode breach handling. This allows the fence state
+      machine to reset at the end of a fence breach action such as an
+      RTL and autoland
+     */
+    if (plane.previous_mode_reason == ModeReason::FENCE_BREACHED) {
+        if (!armed || ((new_breaches == 0 && orig_breaches == 0) && !plane.is_flying())) {
+            plane.previous_mode_reason = ModeReason::UNKNOWN;
+        }
+    }
 
     if (!fence.enabled()) {
         // Switch back to the chosen control mode if still in
@@ -34,7 +57,7 @@ void Plane::fence_check()
     // we still don't do anything when disarmed, but we do check for fence breaches.
     // fence pre-arm check actually checks if any fence has been breached
     // that's not ever going to be true if we don't call check on AP_Fence while disarmed
-    if (!arming.is_armed()) {
+    if (!armed) {
         return;
     }
 
@@ -50,7 +73,7 @@ void Plane::fence_check()
     }
 
     if (new_breaches) {
-        GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence Breached");
+        fence.print_fence_message("breached", new_breaches);
 
         // if the user wants some kind of response and motors are armed
         const uint8_t fence_act = fence.get_action();
@@ -115,7 +138,8 @@ void Plane::fence_check()
         }
 
         LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches));
-    } else if (orig_breaches) {
+    } else if (orig_breaches && fence.get_breaches() == 0) {
+        GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence breach cleared");
         // record clearing of breach
         LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED);
     }

ArduPlane/mode_qland.cpp

@@ -16,9 +16,6 @@ bool ModeQLand::_enter()
 #if AP_LANDINGGEAR_ENABLED
     plane.g2.landing_gear.deploy_for_landing();
 #endif
-#if AP_FENCE_ENABLED
-    plane.fence.auto_disable_fence_for_landing();
-#endif
 
     return true;
 }

ArduPlane/mode_takeoff.cpp

@@ -63,6 +63,7 @@ ModeTakeoff::ModeTakeoff() :
 bool ModeTakeoff::_enter()
 {
     takeoff_mode_setup = false;
+    plane.have_autoenabled_fences = false;
 
     return true;
 }
@@ -154,7 +155,10 @@ void ModeTakeoff::update()
     } else {
         if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT ,or above and loitering
 #if AP_FENCE_ENABLED
-            plane.fence.auto_enable_fence_after_takeoff();
+            if (!plane.have_autoenabled_fences) {
+                plane.fence.auto_enable_fence_after_takeoff();
+                plane.have_autoenabled_fences = true;
+            }
 #endif
             plane.calc_nav_roll();
             plane.calc_nav_pitch();

ArduPlane/quadplane.cpp

@@ -3554,9 +3554,6 @@ bool QuadPlane::verify_vtol_land(void)
             poscontrol.pilot_correction_done = false;
             pos_control->set_lean_angle_max_cd(0);
             poscontrol.xy_correction.zero();
-#if AP_FENCE_ENABLED
-            plane.fence.auto_disable_fence_for_landing();
-#endif
 #if AP_LANDINGGEAR_ENABLED
             plane.g2.landing_gear.deploy_for_landing();
 #endif

Rover/mode_auto.cpp

@@ -593,18 +593,6 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
         do_set_reverse(cmd);
         break;
 
-    case MAV_CMD_DO_FENCE_ENABLE:
-#if AP_FENCE_ENABLED
-        if (cmd.p1 == 0) {  //disable
-            rover.fence.enable(false);
-            gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled");
-        } else {  //enable fence
-            rover.fence.enable(true);
-            gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled");
-        }
-#endif
-        break;
-
     case MAV_CMD_DO_GUIDED_LIMITS:
         do_guided_limits(cmd);
         break;

Tools/autotest/Generic_Missions/CMAC-fence.txt

@@ -0,0 +1,6 @@
+-35.363720	149.163651
+-35.358738	149.165070
+-35.359295	149.154434
+-35.372292	149.157135
+-35.368290	149.166809
+-35.358738	149.165070