AP_AdvancedFailsafe: option to continue the mission even after data l…

…ink is recovered This feature is useful when the landing sequence is part of the flight plan. New parameter AFS_OPTIONS was added. Update libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp Co-authored-by: Peter Barker <[email protected]> Update libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h Co-authored-by: Peter Barker <[email protected]> enum convention changed
ArduPilot · Sep 4, 2023 · 95e3c57 · 95e3c57
1 parent a2d5a32
commit 95e3c57
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 0 deletions.
diff --git a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp
@@ -162,6 +162,12 @@ const AP_Param::GroupInfo AP_AdvancedFailsafe::var_info[] = {
     // @User: Advanced
     // @Units: km
     AP_GROUPINFO("MAX_RANGE",   20, AP_AdvancedFailsafe, _max_range_km,    0),
+
+    // @Param: OPTIONS
+    // @DisplayName: AFS options
+    // @Description: See description for each bitmask bit description
+    // @Bitmask: 0: Continue the mission even after comms are recovered (does not go to the mission item at the time comms were lost)
+    AP_GROUPINFO("OPTIONS", 21, AP_AdvancedFailsafe, options, 0),
 
     AP_GROUPEND
 };
@@ -277,6 +283,11 @@ AP_AdvancedFailsafe::check(uint32_t last_valid_rc_ms)
         } else if (gcs_link_ok) {
             _state = STATE_AUTO;
             gcs().send_text(MAV_SEVERITY_DEBUG, "AFS State: AFS_AUTO, GCS now OK");
+
+            if (option_is_set(Option::CONTINUE_AFTER_RECOVERED)) {
+                break;
+            }
+
             // we only return to the mission if we have not exceeded AFS_MAX_COM_LOSS
             if (_saved_wp != 0 && 
                 (_max_comms_loss <= 0 || 

diff --git a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h
@@ -162,6 +162,14 @@ class AP_AdvancedFailsafe
 
     // update maximum range check
     void max_range_update();
+
+    AP_Int16 options;
+    enum class Option {
+        CONTINUE_AFTER_RECOVERED = (1U<<0),
+    };
+    bool option_is_set(Option option) const {
+        return (options.get() & int16_t(option)) != 0;
+    }
 };
 
 namespace AP {