commander failsafe: add API to defer failsafes

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);
 }
 

src/modules/commander/failsafe/emscripten_template.html

@@ -206,6 +206,11 @@
                                 <button onclick="moveRCSticks()">Move RC Sticks (user takeover request)</button>
                             </td>
                         </tr>
+                        <tr style="display:none;"> <!-- hide this by default -->
+                            <td><label for="defer_failsafes">Defer Failsafes:</label></td>
+                            <td><input type="checkbox" id="defer_failsafes" name="defer_failsafes"></td>
+                            <td></td>
+                        </tr>
                     </table>
                 </div>
                 <div class="box"><h3>Parameters</h3>
@@ -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);

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<hrt_abstime>(_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<hrt_abstime>((_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)

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());
+}

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;
+}

src/modules/commander/failsafe/framework.h

@@ -39,12 +39,16 @@
 
 #include <cstddef>
 
+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