Fence warn at the margins

ArduCopter/Copter.h

@@ -840,6 +840,7 @@ class Copter : public AP_Vehicle {
     // fence.cpp
 #if AP_FENCE_ENABLED
     void fence_check();
+    void fence_run_checks() override;
 #endif
 
     // heli.cpp

ArduCopter/fence.cpp

@@ -4,29 +4,57 @@
 
 #if AP_FENCE_ENABLED
 
-// fence_check - ask fence library to check for breaches and initiate the response
-// called at 1hz
-void Copter::fence_check()
+// async fence checking io callback at 1Khz
+void Copter::fence_run_checks()
 {
-    const uint8_t orig_breaches = fence.get_breaches();
+    const uint32_t now = AP_HAL::millis();
+
+    if (!AP_HAL::timeout_expired(fence_breaches.last_check_ms, now, 40U)) { // 25Hz update rate
+        return;
+    }
+
+    if (fence_breaches.have_updates) {
+        return; // wait for the main loop to pick up the new breaches before checking again
+    }
 
+    fence_breaches.last_check_ms = now;
+    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(is_landing_or_landed);
+    fence_breaches.new_breaches = fence.check(is_landing_or_landed);
+
+    if (!fence_breaches.new_breaches && 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);
+    }
+    fence_breaches.have_updates = true; // new breache status latched so main loop will now pick it up
+}
+
+// fence_check - ask fence library to check for breaches and initiate the response
+// called at 25hz
+void Copter::fence_check()
+{
+    // only take action if there is a new breach
+    if (!fence_breaches.have_updates) {
+        return;
+    }
 
     // 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 (!motors->armed()) {
+        fence_breaches.have_updates = false; // fence checking can now be processed again
         return;
     }
 
-    // if there is a new breach take action
-    if (new_breaches) {
+    if (fence_breaches.new_breaches) {
 
         if (!copter.ap.land_complete) {
-            fence.print_fence_message("breached", new_breaches);
+            fence.print_fence_message("breached", fence_breaches.new_breaches);
         }
 
         // if the user wants some kind of response and motors are armed
@@ -41,7 +69,7 @@ void Copter::fence_check()
             } else {
 
                 // if more than 100m outside the fence just force a land
-                if (fence.get_breach_distance(new_breaches) > AC_FENCE_GIVE_UP_DISTANCE) {
+                if (fence.get_breach_distance(fence_breaches.new_breaches) > AC_FENCE_GIVE_UP_DISTANCE) {
                     set_mode(Mode::Number::LAND, ModeReason::FENCE_BREACHED);
                 } else {
                     switch (fence_act) {
@@ -81,15 +109,9 @@ void Copter::fence_check()
             }
         }
 
-        LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_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);
+        LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(fence_breaches.new_breaches));
     }
+    fence_breaches.have_updates = false; // fence checking can now be processed again
 }
 
 #endif

ArduPlane/Plane.h

@@ -1058,8 +1058,10 @@ class Plane : public AP_Vehicle {
 #if AP_FENCE_ENABLED
     // fence.cpp
     void fence_check();
+    void fence_run_checks() override;
     bool fence_stickmixing() const;
     bool in_fence_recovery() const;
+    uint8_t orig_breaches;
 #endif
 
     // Plane.cpp

ArduPlane/fence.cpp

@@ -4,10 +4,21 @@
 
 #if AP_FENCE_ENABLED
 
-// fence_check - ask fence library to check for breaches and initiate the response
-void Plane::fence_check()
+// async fence checking io callback at 1Khz
+void Plane::fence_run_checks()
 {
-    const uint8_t orig_breaches = fence.get_breaches();
+    const uint32_t now = AP_HAL::millis();
+
+    if (!AP_HAL::timeout_expired(fence_breaches.last_check_ms, now, 333U)) { // 3Hz update rate
+        return;
+    }
+
+    if (fence_breaches.have_updates) {
+        return; // wait for the main loop to pick up the new breaches before checking again
+    }
+
+    fence_breaches.last_check_ms = now;
+    orig_breaches = fence.get_breaches();
     const bool armed = arming.is_armed();
 
     uint16_t mission_id = plane.mission.get_current_nav_cmd().id;
@@ -20,17 +31,27 @@ void Plane::fence_check()
                          || (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(landing_or_landed);
+    fence_breaches.new_breaches = fence.check(landing_or_landed);
+    fence_breaches.have_updates = true; // new breach status latched so main loop will now pick it up
+}
 
+// fence_check - ask fence library to check for breaches and initiate the response
+void Plane::fence_check()
+{
+    if (!fence_breaches.have_updates) {
+        return;
+    }
     /*
       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
      */
+    const bool armed = arming.is_armed();
+
     if (plane.previous_mode_reason == ModeReason::FENCE_BREACHED) {
-        if (!armed || ((new_breaches == 0 && orig_breaches == 0) && !plane.is_flying())) {
+        if (!armed || ((fence_breaches.new_breaches == 0 && orig_breaches == 0) && !plane.is_flying())) {
             plane.previous_mode_reason = ModeReason::UNKNOWN;
         }
     }
@@ -52,29 +73,29 @@ void Plane::fence_check()
                 // No returning to a previous mode, unless our action allows it
                 break;
         }
-        return;
+        goto fence_check_complete;
     }
 
     // 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 (!armed) {
-        return;
+        goto fence_check_complete;
     }
 
     // Never trigger a fence breach in the final stage of landing
     if (landing.is_expecting_impact()) {
-        return;
+        goto fence_check_complete;
     }
 
     if (in_fence_recovery()) {
         // we have already triggered, don't trigger again until the
         // user disables/re-enables using the fence channel switch
-        return;
+        goto fence_check_complete;
     }
 
-    if (new_breaches) {
-        fence.print_fence_message("breached", new_breaches);
+    if (fence_breaches.new_breaches) {
+        fence.print_fence_message("breached", fence_breaches.new_breaches);
 
         // if the user wants some kind of response and motors are armed
         const uint8_t fence_act = fence.get_action();
@@ -89,7 +110,7 @@ void Plane::fence_check()
                 (g.rtl_autoland == RtlAutoland::RTL_THEN_DO_LAND_START ||
                     g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START)) {
                 // already landing
-                return;
+                goto fence_check_complete;
             }
 #if MODE_AUTOLAND_ENABLED
             if (control_mode == &mode_autoland) {
@@ -146,12 +167,15 @@ void Plane::fence_check()
             break;
         }
 
-        LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches));
+        LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(fence_breaches.new_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);
     }
+
+fence_check_complete:
+    fence_breaches.have_updates = false;
 }
 
 bool Plane::fence_stickmixing(void) const

ArduSub/ArduSub.cpp

@@ -263,11 +263,6 @@ void Sub::three_hz_loop()
     // check if we've lost terrain data
     failsafe_terrain_check();
 
-#if AP_FENCE_ENABLED
-    // check if we have breached a fence
-    fence_check();
-#endif // AP_FENCE_ENABLED
-
 #if AP_SERVORELAYEVENTS_ENABLED
     ServoRelayEvents.update_events();
 #endif

ArduSub/Sub.h

@@ -458,7 +458,10 @@ class Sub : public AP_Vehicle {
     void failsafe_terrain_on_event();
     void mainloop_failsafe_enable();
     void mainloop_failsafe_disable();
+#if AP_FENCE_ENABLED
     void fence_check();
+    void fence_run_checks() override;
+#endif
     bool set_mode(Mode::Number mode, ModeReason reason);
     bool set_mode(const uint8_t new_mode, const ModeReason reason) override;
     uint8_t get_mode() const override { return (uint8_t)control_mode; }

ArduSub/fence.cpp

@@ -5,23 +5,26 @@
 #if AP_FENCE_ENABLED
 
 // fence_check - ask fence library to check for breaches and initiate the response
-// called at 1hz
-void Sub::fence_check()
+// async fence checking io callback at 1Khz
+void Sub::fence_run_checks()
 {
+    const uint32_t now = AP_HAL::millis();
     // ignore any fence activity when not armed
     if (!motors.armed()) {
         return;
     }
 
-    const uint8_t orig_breaches = fence.get_breaches();
+    if (!AP_HAL::timeout_expired(fence_breaches.last_check_ms, now, 333U)) { // 3Hz update rate
+        return;
+    }
 
+    fence_breaches.last_check_ms = now;
+    const uint8_t orig_breaches = fence.get_breaches();
     // check for new breaches; new_breaches is bitmask of fence types breached
-    const uint8_t new_breaches = sub.fence.check();
+    const uint8_t new_breaches = fence.check();
 
-    // if there is a new breach take action
+    // if the user wants some kind of response and motors are armed
     if (new_breaches) {
-
-        // if the user wants some kind of response and motors are armed
         if (fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY) {
             //
             //            // disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle
@@ -41,7 +44,8 @@ void Sub::fence_check()
             //            }
         }
 
-        LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches));
+        LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(fence_breaches.new_breaches));
+
     } else if (orig_breaches) {
         // record clearing of breach
         LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED);

Rover/Rover.cpp

@@ -114,7 +114,9 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_Camera,           &rover.camera,           update,         50,  200,  78),
 #endif
     SCHED_TASK(gcs_failsafe_check,     10,    200,  81),
+#if AP_FENCE_ENABLED
     SCHED_TASK(fence_check,            10,    200,  84),
+#endif
     SCHED_TASK(ekf_check,              10,    100,  87),
     SCHED_TASK_CLASS(ModeSmartRTL,        &rover.mode_smartrtl,    save_position,   3,  200,  90),
     SCHED_TASK(one_second_loop,         1,   1500,  96),

Rover/Rover.h

@@ -325,10 +325,11 @@ class Rover : public AP_Vehicle {
 #if AP_ROVER_ADVANCED_FAILSAFE_ENABLED
     void afs_fs_check(void);
 #endif
-
+#if AP_FENCE_ENABLED
     // fence.cpp
+    void fence_run_checks() override;
     void fence_check();
-
+#endif
     // GCS_Mavlink.cpp
     void send_wheel_encoder_distance(mavlink_channel_t chan);
 

Rover/fence.cpp

@@ -1,26 +1,51 @@
 #include "Rover.h"
 
-// fence_check - ask fence library to check for breaches and initiate the response
-void Rover::fence_check()
-{
 #if AP_FENCE_ENABLED
-    uint8_t new_breaches;  // the type of fence that has been breached
+
+// async fence checking io callback at 1Khz
+void Rover::fence_run_checks()
+{
+    const uint32_t now = AP_HAL::millis();
+
+    if (!AP_HAL::timeout_expired(fence_breaches.last_check_ms, now, 100U)) { // 10Hz update rate
+        return;
+    }
+
+    if (fence_breaches.have_updates) {
+        return; // wait for the main loop to pick up the new breaches before checking again
+    }
+
+    fence_breaches.last_check_ms = now;
     const uint8_t orig_breaches = fence.get_breaches();
+    // check for new breaches; new_breaches is bitmask of fence types breached
+    fence_breaches.new_breaches = fence.check();
 
-    // check for a breach
-    new_breaches = fence.check();
+    if (!fence_breaches.new_breaches && orig_breaches && fence.get_breaches() == 0) {
+        // record clearing of breach
+        LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED);
+    }
+    fence_breaches.have_updates = true; // new breaches latched so main loop will now pick it up
+}
+
+// fence_check - ask fence library to check for breaches and initiate the response
+void Rover::fence_check()
+{
+    // only take action if there is a new breach
+    if (!fence_breaches.have_updates) {
+        return;
+    }
 
     // return immediately if motors are not armed
     if (!arming.is_armed()) {
+        fence_breaches.have_updates = false;
         return;
     }
 
-    // if there is a new breach take action
-    if (new_breaches) {
+    if (fence_breaches.new_breaches) {
         // if the user wants some kind of response and motors are armed
         if ((FailsafeAction)fence.get_action() != FailsafeAction::None) {
             // if within 100m of the fence, it will take the action specified by the FENCE_ACTION parameter
-            if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
+            if (fence.get_breach_distance(fence_breaches.new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
                 switch ((FailsafeAction)fence.get_action()) {
                 case FailsafeAction::None:
                     break;
@@ -51,12 +76,9 @@ void Rover::fence_check()
                 set_mode(mode_hold, ModeReason::FENCE_BREACHED);
             }
         }
-        LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches));
-
-    } else if (orig_breaches) {
-        // record clearing of breach
-        LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE,
-                                 LogErrorCode::ERROR_RESOLVED);
+        LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(fence_breaches.new_breaches));
     }
-#endif // AP_FENCE_ENABLED
+    fence_breaches.have_updates = false;
 }
+
+#endif // AP_FENCE_ENABLED