diff --git a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp index dce629b7a9..b382e34dca 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 ecf50386c6..caf1fbe888 100644 --- a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h +++ b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h @@ -162,6 +162,14 @@ private: // 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 {