diff --git a/src/modules/commander/failsafe/emscripten.cpp b/src/modules/commander/failsafe/emscripten.cpp
index f9d56e5479..ec32d2f071 100644
--- a/src/modules/commander/failsafe/emscripten.cpp
+++ b/src/modules/commander/failsafe/emscripten.cpp
@@ -167,7 +167,7 @@ void set_param_value_float(const std::string &name, float value)
int failsafe_update(bool armed, bool vtol_in_transition_mode, bool mission_finished,
bool user_override, uint8_t user_intended_mode, uint8_t vehicle_type,
- failsafe_flags_s status_flags)
+ failsafe_flags_s status_flags, bool defer_failsafes)
{
uint64_t time_ms = emscripten_date_now();
FailsafeBase::State state{};
@@ -177,6 +177,7 @@ int failsafe_update(bool armed, bool vtol_in_transition_mode, bool mission_finis
state.user_intended_mode = user_intended_mode;
state.vehicle_type = vehicle_type;
mode_util::getModeRequirements(vehicle_type, status_flags);
+ failsafe_instance.current().deferFailsafes(defer_failsafes, 0);
return failsafe_instance.current().update(time_ms * 1000, state, false, user_override, status_flags);
}
diff --git a/src/modules/commander/failsafe/emscripten_template.html b/src/modules/commander/failsafe/emscripten_template.html
index 4159484860..1b21012de6 100644
--- a/src/modules/commander/failsafe/emscripten_template.html
+++ b/src/modules/commander/failsafe/emscripten_template.html
@@ -206,6 +206,11 @@
+
+ |
+ |
+ |
+
Parameters
@@ -322,8 +327,9 @@
var mission_finished = document.querySelector('input[id="mission_finished"]').checked;
var intended_nav_state = document.querySelector('select[id="intended_nav_state"]').value;
var vehicle_type = document.querySelector('select[id="vehicle_type"]').value;
+ var defer_failsafes = document.querySelector('input[id="defer_failsafes"]').checked;
var updated_user_intended_mode = Module.failsafe_update(armed, vtol_in_transition_mode, mission_finished, user_override_requested, parseInt(intended_nav_state),
- parseInt(vehicle_type), state);
+ parseInt(vehicle_type), state, defer_failsafes);
user_override_requested = false;
var action = Module.selected_action();
action_str = Module.action_str(action);
diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp
index 4d26276ef2..2f79e92cff 100644
--- a/src/modules/commander/failsafe/failsafe.cpp
+++ b/src/modules/commander/failsafe/failsafe.cpp
@@ -392,8 +392,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
CHECK_FAILSAFE(status_flags, wind_limit_exceeded,
- ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm));
- CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded, ActionOptions(Action::RTL));
+ ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm).cannotBeDeferred());
+ CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded, ActionOptions(Action::RTL).cannotBeDeferred());
// trigger RTL if low position accurancy is detected
if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
@@ -401,7 +401,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
CHECK_FAILSAFE(status_flags, local_position_accuracy_low, ActionOptions(Action::RTL));
}
- CHECK_FAILSAFE(status_flags, primary_geofence_breached, fromGfActParam(_param_gf_action.get()));
+ CHECK_FAILSAFE(status_flags, primary_geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred());
// Battery
CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::BatteryLow));
@@ -434,7 +434,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
if ((_armed_time != 0)
&& (time_us < _armed_time + static_cast(_param_com_spoolup_time.get() * 1_s))
) {
- CHECK_FAILSAFE(status_flags, fd_esc_arming_failure, Action::Disarm);
+ CHECK_FAILSAFE(status_flags, fd_esc_arming_failure, ActionOptions(Action::Disarm).cannotBeDeferred());
}
if ((_armed_time != 0)
@@ -442,10 +442,10 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
+ static_cast((_param_com_lkdown_tko.get() + _param_com_spoolup_time.get()) * 1_s))
) {
// This handles the case where something fails during the early takeoff phase
- CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Disarm);
+ CHECK_FAILSAFE(status_flags, fd_critical_failure, ActionOptions(Action::Disarm).cannotBeDeferred());
} else if (!circuit_breaker_enabled_by_val(_param_cbrk_flightterm.get(), CBRK_FLIGHTTERM_KEY)) {
- CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Terminate);
+ CHECK_FAILSAFE(status_flags, fd_critical_failure, ActionOptions(Action::Terminate).cannotBeDeferred());
} else {
CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Warn);
@@ -460,7 +460,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
Action mode_fallback_action = checkModeFallback(status_flags, state.user_intended_mode);
_last_state_mode_fallback = checkFailsafe(_caller_id_mode_fallback, _last_state_mode_fallback,
mode_fallback_action != Action::None,
- ActionOptions(mode_fallback_action).allowUserTakeover(UserTakeoverAllowed::Always));
+ ActionOptions(mode_fallback_action).allowUserTakeover(UserTakeoverAllowed::Always).cannotBeDeferred());
}
void Failsafe::updateArmingState(const hrt_abstime &time_us, bool armed, const failsafe_flags_s &status_flags)
diff --git a/src/modules/commander/failsafe/failsafe_test.cpp b/src/modules/commander/failsafe/failsafe_test.cpp
index 3777a55c9e..ad0f2e0615 100644
--- a/src/modules/commander/failsafe/failsafe_test.cpp
+++ b/src/modules/commander/failsafe/failsafe_test.cpp
@@ -62,7 +62,7 @@ protected:
ActionOptions(Action::RTL).allowUserTakeover(UserTakeoverAllowed::Never));
_last_state_test = checkFailsafe(_caller_id_test, _last_state_test, status_flags.fd_motor_failure
- && status_flags.fd_critical_failure, Action::Terminate);
+ && status_flags.fd_critical_failure, ActionOptions(Action::Terminate).cannotBeDeferred());
}
Action checkModeFallback(const failsafe_flags_s &status_flags, uint8_t user_intended_mode) const override
@@ -258,3 +258,114 @@ TEST_F(FailsafeTest, takeover_denied)
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate);
}
+TEST_F(FailsafeTest, defer)
+{
+ FailsafeTester failsafe(nullptr);
+
+ failsafe_flags_s failsafe_flags{};
+ FailsafeBase::State state{};
+ state.armed = true;
+ state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+ state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+ hrt_abstime time = 3847124342;
+
+ uint8_t updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+
+ const int defer_timeout_s = 10;
+ failsafe.deferFailsafes(true, defer_timeout_s);
+ ASSERT_TRUE(failsafe.getDeferFailsafes());
+ ASSERT_FALSE(failsafe.failsafeDeferred());
+ // GCS connection lost -> deferred
+ time += 10_ms;
+ failsafe_flags.gcs_connection_lost = true;
+ updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+ ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+ ASSERT_TRUE(failsafe.getDeferFailsafes());
+ ASSERT_TRUE(failsafe.failsafeDeferred());
+
+ // Wait a bit, still deferred
+ time += 5_s;
+ state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB;
+ updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+ ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+ ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+ ASSERT_TRUE(failsafe.getDeferFailsafes());
+ ASSERT_TRUE(failsafe.failsafeDeferred());
+
+ // Wait a bit, don't defer anymore -> failsafe triggered (hold)
+ time += 1_s;
+ failsafe.deferFailsafes(false, 0);
+ ASSERT_FALSE(failsafe.getDeferFailsafes());
+ updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+ ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
+ ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold);
+ ASSERT_FALSE(failsafe.getDeferFailsafes());
+ ASSERT_FALSE(failsafe.failsafeDeferred());
+
+ // Cannot enable while in failsafe
+ ASSERT_FALSE(failsafe.deferFailsafes(true, 0));
+
+ // Still in hold
+ time += 4_s;
+ updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+ ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold);
+
+ // Now changed to descend
+ time += 4_s;
+ updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+ ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Descend);
+
+ // Clear failsafe
+ failsafe_flags.gcs_connection_lost = false;
+ time += 10_ms;
+ updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+ ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+
+
+ // Defer and trigger timeout
+ failsafe.deferFailsafes(true, defer_timeout_s);
+ time += 10_ms;
+ failsafe_flags.wind_limit_exceeded = true;
+ updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+ ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+ ASSERT_TRUE(failsafe.failsafeDeferred());
+ time += 1_s * defer_timeout_s + 10_ms;
+ updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+ ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
+ ASSERT_FALSE(failsafe.failsafeDeferred());
+ time += 10_ms;
+ failsafe_flags.wind_limit_exceeded = false;
+ updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+ ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+
+ // Defer and clear failsafe -> no action
+ failsafe.deferFailsafes(true, defer_timeout_s);
+ time += 10_ms;
+ failsafe_flags.wind_limit_exceeded = true;
+ updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+ ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+ ASSERT_TRUE(failsafe.failsafeDeferred());
+ time += 10_ms;
+ failsafe_flags.wind_limit_exceeded = false;
+ updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+ ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+ ASSERT_FALSE(failsafe.failsafeDeferred());
+ failsafe.deferFailsafes(false, defer_timeout_s);
+ time += 10_ms;
+ updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+ ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+
+ // Defer and trigger non-deferrable failsafe
+ failsafe.deferFailsafes(true, defer_timeout_s);
+ time += 10_ms;
+ failsafe_flags.wind_limit_exceeded = true;
+ updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+ ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
+ ASSERT_TRUE(failsafe.failsafeDeferred());
+ time += 10_ms;
+ failsafe_flags.fd_motor_failure = true;
+ failsafe_flags.fd_critical_failure = true;
+ updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
+ ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate);
+ ASSERT_FALSE(failsafe.failsafeDeferred());
+}
diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp
index e8e98a51cb..ac318e2602 100644
--- a/src/modules/commander/failsafe/framework.cpp
+++ b/src/modules/commander/failsafe/framework.cpp
@@ -70,7 +70,14 @@ uint8_t FailsafeBase::update(const hrt_abstime &time_us, const State &state, boo
removeActions(ClearCondition::OnModeChangeOrDisarm);
}
- updateDelay(time_us - _last_update);
+ if (_defer_failsafes && _failsafe_defer_started != 0 && _defer_timeout > 0
+ && time_us > _failsafe_defer_started + _defer_timeout) {
+ _defer_failsafes = false;
+ }
+
+ if (_failsafe_defer_started == 0) {
+ updateDelay(time_us - _last_update);
+ }
checkStateAndMode(time_us, state, status_flags);
removeNonActivatedActions();
@@ -79,7 +86,8 @@ uint8_t FailsafeBase::update(const hrt_abstime &time_us, const State &state, boo
SelectedActionState action_state{};
getSelectedAction(state, status_flags, user_intended_mode_updated, rc_sticks_takeover_request, action_state);
- updateDelay(time_us - _last_update, action_state.delayed_action != Action::None);
+ updateStartDelay(time_us - _last_update, action_state.delayed_action != Action::None);
+ updateFailsafeDeferState(time_us, action_state.failsafe_deferred);
// Notify user if the action is worse than before, or a new action got added
if (action_state.action > _selected_action || (action_state.action != Action::None && _notification_required)) {
@@ -98,7 +106,19 @@ uint8_t FailsafeBase::update(const hrt_abstime &time_us, const State &state, boo
return _last_user_intended_mode;
}
-void FailsafeBase::updateDelay(const hrt_abstime &dt, bool delay_active)
+void FailsafeBase::updateFailsafeDeferState(const hrt_abstime &time_us, bool defer)
+{
+ if (defer) {
+ if (_failsafe_defer_started == 0) {
+ _failsafe_defer_started = time_us;
+ }
+
+ } else {
+ _failsafe_defer_started = 0;
+ }
+}
+
+void FailsafeBase::updateStartDelay(const hrt_abstime &dt, bool delay_active)
{
// Ensure that even with a toggling state the delayed action is executed at some point.
// This is done by increasing the delay slower than reducing it.
@@ -411,6 +431,7 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
returned_state.action = Action::None;
Action &selected_action = returned_state.action;
UserTakeoverAllowed allow_user_takeover = UserTakeoverAllowed::Always;
+ bool allow_failsafe_to_be_deferred{true};
// Select the worst action based on the current active actions
for (int action_idx = 0; action_idx < max_num_actions; ++action_idx) {
@@ -426,9 +447,19 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
selected_action = cur_action.action;
returned_state.cause = cur_action.cause;
}
+
+ if (!cur_action.can_be_deferred) {
+ allow_failsafe_to_be_deferred = false;
+ }
}
}
+ if (_defer_failsafes && allow_failsafe_to_be_deferred && selected_action != Action::None) {
+ returned_state.failsafe_deferred = selected_action > Action::Warn;
+ returned_state.action = Action::None;
+ return;
+ }
+
// Check if we should enter delayed Hold
if (_current_delay > 0 && !_user_takeover_active && allow_user_takeover <= UserTakeoverAllowed::AlwaysModeSwitchOnly
&& selected_action != Action::Disarm && selected_action != Action::Terminate) {
@@ -627,3 +658,23 @@ bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode
(!status_flags.home_position_invalid || ((status_flags.mode_req_home_position & mode_mask) == 0)) &&
((status_flags.mode_req_other & mode_mask) == 0);
}
+
+bool FailsafeBase::deferFailsafes(bool enabled, int timeout_s)
+{
+ if (enabled && _selected_action > Action::Warn) {
+ return false;
+ }
+
+ if (timeout_s == 0) {
+ _defer_timeout = DEFAULT_DEFER_TIMEOUT;
+
+ } else if (timeout_s < 0) {
+ _defer_timeout = 0;
+
+ } else {
+ _defer_timeout = timeout_s * 1_s;
+ }
+
+ _defer_failsafes = enabled;
+ return true;
+}
diff --git a/src/modules/commander/failsafe/framework.h b/src/modules/commander/failsafe/framework.h
index e1e9dfb50d..5da7f73770 100644
--- a/src/modules/commander/failsafe/framework.h
+++ b/src/modules/commander/failsafe/framework.h
@@ -39,12 +39,16 @@
#include
+using namespace time_literals;
+
#define CHECK_FAILSAFE(status_flags, flag_name, options) \
checkFailsafe((int)offsetof(failsafe_flags_s, flag_name), lastStatusFlags().flag_name, status_flags.flag_name, options)
class FailsafeBase: public ModuleParams
{
public:
+ static constexpr hrt_abstime DEFAULT_DEFER_TIMEOUT = 30_s;
+
enum class Action : uint8_t {
// Actions further down take precedence
None,
@@ -149,6 +153,16 @@ public:
bool userTakeoverActive() const { return _user_takeover_active; }
+ /**
+ * Defer all failsafes that can be deferred. Can be used to avoid triggering failsafes in critical situations.
+ * @param enabled
+ * @param timeout_s timeout in seconds, if set to 0, DEFAULT_DEFER_TIMEOUT is used, -1=no timeout
+ * @return true on success, false if failsafe already active
+ */
+ bool deferFailsafes(bool enabled, int timeout_s);
+ bool getDeferFailsafes() const { return _defer_failsafes; }
+ bool failsafeDeferred() const { return _failsafe_defer_started != 0; }
+
protected:
enum class UserTakeoverAllowed {
Always, ///< allow takeover (immediately)
@@ -162,6 +176,7 @@ protected:
ActionOptions &allowUserTakeover(UserTakeoverAllowed allow = UserTakeoverAllowed::Auto) { allow_user_takeover = allow; return *this; }
ActionOptions &clearOn(ClearCondition clear_condition_) { clear_condition = clear_condition_; return *this; }
ActionOptions &causedBy(Cause cause_) { cause = cause_; return *this; }
+ ActionOptions &cannotBeDeferred() { can_be_deferred = false; return *this; }
bool valid() const { return id != -1; }
void setInvalid() { id = -1; }
@@ -170,6 +185,7 @@ protected:
ClearCondition clear_condition{ClearCondition::WhenConditionClears};
Cause cause{Cause::Generic};
UserTakeoverAllowed allow_user_takeover{UserTakeoverAllowed::Auto};
+ bool can_be_deferred{true};
bool state_failure{false}; ///< used when the clear_condition isn't set to clear immediately
bool activated{false}; ///< true if checkFailsafe was called during current update
@@ -182,6 +198,7 @@ protected:
Cause cause{Cause::Generic};
uint8_t updated_user_intended_mode{};
bool user_takeover{false};
+ bool failsafe_deferred{false};
};
virtual void checkStateAndMode(const hrt_abstime &time_us, const State &state,
@@ -232,7 +249,9 @@ private:
bool actionAllowsUserTakeover(Action action) const;
- void updateDelay(const hrt_abstime &dt, bool delay_active);
+ void updateStartDelay(const hrt_abstime &dt, bool delay_active);
+
+ void updateFailsafeDeferState(const hrt_abstime &time_us, bool defer);
static constexpr int max_num_actions{8};
ActionOptions _actions[max_num_actions]; ///< currently active actions
@@ -245,6 +264,10 @@ private:
bool _user_takeover_active{false};
bool _notification_required{false};
+ bool _defer_failsafes{false};
+ hrt_abstime _defer_timeout{0};
+ hrt_abstime _failsafe_defer_started{0};
+
hrt_abstime _current_start_delay{0}; ///< _current_delay is set to this value when starting the delay
hrt_abstime _current_delay{0}; ///< If > 0, stay in Hold, and take action once delay reaches 0