From 4501a84a5fd241aa2e97b25ece0b4917b9676f6a Mon Sep 17 00:00:00 2001 From: Ep Pravitra Date: Mon, 4 Sep 2023 15:29:57 +0700 Subject: [PATCH] AP_AdvancedFailsafe: option to continue the mission even after data link 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 Update libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h Co-authored-by: Peter Barker enum convention changed --- libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp | 11 +++++++++++ libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h | 8 ++++++++ 2 files changed, 19 insertions(+) diff --git a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp index dce629b7a9755e..b382e34dca3868 100644 --- 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 index ecf50386c6ccd0..caf1fbe888e7fe 100644 --- 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 {