failsafe.cpp

/****************************************************************************
 *
 *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#include "failsafe.h"

#include <px4_platform_common/log.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/battery_status.h>
#include <lib/circuit_breaker/circuit_breaker.h>

using namespace time_literals;

FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value)
{
	ActionOptions options{};

	switch (gcs_connection_loss_failsafe_mode(param_value)) {
	case gcs_connection_loss_failsafe_mode::Disabled:
		options.action = Action::None;
		break;

	case gcs_connection_loss_failsafe_mode::Hold_mode:
		options.action = Action::Hold;
		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
		break;

	case gcs_connection_loss_failsafe_mode::Return_mode:
		options.action = Action::RTL;
		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
		break;

	case gcs_connection_loss_failsafe_mode::Land_mode:
		options.action = Action::Land;
		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
		break;

	case gcs_connection_loss_failsafe_mode::Terminate:
		options.allow_user_takeover = UserTakeoverAllowed::Never;
		options.action = Action::Terminate;
		options.clear_condition = ClearCondition::Never;
		break;

	case gcs_connection_loss_failsafe_mode::Disarm: // Lockdown
		options.allow_user_takeover = UserTakeoverAllowed::Never;
		options.action = Action::Disarm;
		break;

	default:
		options.action = Action::None;
		break;
	}

	return options;
}

FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value)
{
	ActionOptions options{};

	switch (geofence_violation_action(param_value)) {
	case geofence_violation_action::None:
		options.action = Action::None;
		break;

	case geofence_violation_action::Warning:
		options.action = Action::Warn;
		break;

	case geofence_violation_action::Hold_mode:
		options.allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly; // ensure the user can escape again
		options.action = Action::Hold;
		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
		break;

	case geofence_violation_action::Return_mode:
		options.action = Action::RTL;
		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
		break;

	case geofence_violation_action::Terminate:
		options.allow_user_takeover = UserTakeoverAllowed::Never;
		options.action = Action::Terminate;
		options.clear_condition = ClearCondition::Never;
		break;

	case geofence_violation_action::Land_mode:
		options.action = Action::Land;
		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
		break;

	default:
		options.action = Action::Warn;
		break;
	}

	return options;
}

FailsafeBase::ActionOptions Failsafe::fromImbalancedPropActParam(int param_value)
{
	ActionOptions options{};

	switch (imbalanced_propeller_failsafe_mode(param_value)) {
	case imbalanced_propeller_failsafe_mode::Disabled:
	default:
		options.action = Action::None;
		break;

	case imbalanced_propeller_failsafe_mode::Warning:
		options.action = Action::Warn;
		break;

	case imbalanced_propeller_failsafe_mode::Return:
		options.action = Action::RTL;
		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
		break;

	case imbalanced_propeller_failsafe_mode::Land:
		options.action = Action::Land;
		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
		break;
	}

	return options;
}

FailsafeBase::ActionOptions Failsafe::fromActuatorFailureActParam(int param_value)
{
	ActionOptions options{};

	switch (actuator_failure_failsafe_mode(param_value)) {
	case actuator_failure_failsafe_mode::Warning_only:
	default:
		options.action = Action::Warn;
		break;

	case actuator_failure_failsafe_mode::Hold_mode:
		options.action = Action::Hold;
		break;

	case actuator_failure_failsafe_mode::Land_mode:
		options.action = Action::Land;
		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
		break;

	case actuator_failure_failsafe_mode::Return_mode:
		options.action = Action::RTL;
		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
		break;

	case actuator_failure_failsafe_mode::Terminate:
		options.action = Action::Terminate;
		options.clear_condition = ClearCondition::Never;
		break;
	}

	return options;
}

FailsafeBase::ActionOptions Failsafe::fromBatteryWarningActParam(int param_value, uint8_t battery_warning)
{
	ActionOptions options{};

	switch (battery_warning) {
	default:
	case battery_status_s::WARNING_NONE:
		options.action = Action::None;
		break;

	case battery_status_s::WARNING_LOW:
		options.action = Action::Warn;
		options.cause = Cause::BatteryLow;
		break;

	case battery_status_s::WARNING_CRITICAL:
		options.action = Action::Warn;
		options.cause = Cause::BatteryCritical;

		switch ((LowBatteryAction)param_value) {
		case LowBatteryAction::Return:
		case LowBatteryAction::ReturnOrLand:
			options.action = Action::RTL;
			break;

		case LowBatteryAction::Land:
			options.action = Action::Land;
			break;

		case LowBatteryAction::Warning:
			options.action = Action::Warn;
			break;
		}

		break;

	case battery_status_s::WARNING_EMERGENCY:
		options.action = Action::Warn;
		options.cause = Cause::BatteryEmergency;

		switch ((LowBatteryAction)param_value) {
		case LowBatteryAction::Return:
			options.action = Action::RTL;
			break;

		case LowBatteryAction::ReturnOrLand:
		case LowBatteryAction::Land:
			options.action = Action::Land;
			break;

		case LowBatteryAction::Warning:
			options.action = Action::Warn;
			break;
		}

		break;
	}

	return options;
}

FailsafeBase::ActionOptions Failsafe::fromQuadchuteActParam(int param_value)
{
	ActionOptions options{};

	switch (command_after_quadchute(param_value)) {
	case command_after_quadchute::Warning_only:
	default:
		options.action = Action::Warn;
		break;

	case command_after_quadchute::Return_mode:
		options.action = Action::RTL;
		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
		break;

	case command_after_quadchute::Land_mode:
		options.action = Action::Land;
		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
		break;

	case command_after_quadchute::Hold_mode:
		options.action = Action::Hold;
		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
		break;
	}

	return options;
}

FailsafeBase::Action Failsafe::fromOffboardLossActParam(int param_value, uint8_t &user_intended_mode)
{
	Action action{Action::None};

	switch (offboard_loss_failsafe_mode(param_value)) {
	case offboard_loss_failsafe_mode::Position_mode:
	default:
		action = Action::FallbackPosCtrl;
		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
		break;

	case offboard_loss_failsafe_mode::Altitude_mode:
		action = Action::FallbackAltCtrl;
		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
		break;

	case offboard_loss_failsafe_mode::Stabilized:
		action = Action::FallbackStab;
		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB;
		break;

	case offboard_loss_failsafe_mode::Return_mode:
		action = Action::RTL;
		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
		break;

	case offboard_loss_failsafe_mode::Land_mode:
		action = Action::Land;
		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
		break;

	case offboard_loss_failsafe_mode::Hold_mode:
		action = Action::Hold;
		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
		break;

	case offboard_loss_failsafe_mode::Terminate:
		action = Action::Terminate;
		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
		break;

	case offboard_loss_failsafe_mode::Disarm:
		action = Action::Disarm;
		break;
	}

	return action;
}

FailsafeBase::ActionOptions Failsafe::fromHighWindLimitActParam(int param_value)
{
	ActionOptions options{};

	switch (command_after_high_wind_failsafe(param_value)) {
	case command_after_high_wind_failsafe::None:
		options.action = Action::None;
		break;

	case command_after_high_wind_failsafe::Warning:
		options.action = Action::Warn;
		break;

	case command_after_high_wind_failsafe::Hold_mode:
		options.allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly; // ensure the user can escape again
		options.action = Action::Hold;
		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
		break;

	case command_after_high_wind_failsafe::Return_mode:
		options.action = Action::RTL;
		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
		break;

	case command_after_high_wind_failsafe::Terminate:
		options.allow_user_takeover = UserTakeoverAllowed::Never;
		options.action = Action::Terminate;
		options.clear_condition = ClearCondition::Never;
		break;

	case command_after_high_wind_failsafe::Land_mode:
		options.action = Action::Land;
		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
		break;

	default:
		options.action = Action::Warn;
		break;
	}

	return options;
}

FailsafeBase::ActionOptions Failsafe::fromPosLowActParam(int param_value)
{
	ActionOptions options{};
	options.allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly; // ensure the user can escape again

	switch (command_after_pos_low_failsafe(param_value)) {
	case command_after_pos_low_failsafe::None:
		options.action = Action::None;
		break;

	case command_after_pos_low_failsafe::Warning:
		options.action = Action::Warn;
		break;

	case command_after_pos_low_failsafe::Hold_mode:
		options.action = Action::Hold;
		options.clear_condition = ClearCondition::WhenConditionClears;
		break;

	case command_after_pos_low_failsafe::Return_mode:
		options.action = Action::RTL;
		options.clear_condition = ClearCondition::WhenConditionClears;
		break;

	case command_after_pos_low_failsafe::Terminate:
		options.allow_user_takeover = UserTakeoverAllowed::Never;
		options.action = Action::Terminate;
		options.clear_condition = ClearCondition::Never;
		break;

	case command_after_pos_low_failsafe::Land_mode:
		options.action = Action::Land;
		options.clear_condition = ClearCondition::WhenConditionClears;
		break;

	default:
		options.action = Action::Warn;
		break;
	}

	return options;
}

FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int param_value)
{
	ActionOptions options{};

	options.allow_user_takeover = UserTakeoverAllowed::Auto;
	options.cause = Cause::RemainingFlightTimeLow;

	switch (command_after_remaining_flight_time_low(param_value)) {
	case command_after_remaining_flight_time_low::None:
		options.action = Action::None;
		break;

	case command_after_remaining_flight_time_low::Warning:
		options.action = Action::Warn;
		break;

	case command_after_remaining_flight_time_low::Return_mode:
		options.action = Action::RTL;
		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
		break;

	default:
		options.action = Action::None;
		break;

	}

	return options;
}

bool Failsafe::isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter)
{
	switch (user_intended_mode) {
	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
		return exception_mask_parameter & (int)LinkLossExceptionBits::Mission;

	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
	case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
	case vehicle_status_s::NAVIGATION_STATE_DESCEND:
	case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
	case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
	case vehicle_status_s::NAVIGATION_STATE_ORBIT:
		return exception_mask_parameter & (int)LinkLossExceptionBits::AutoModes;

	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
		return exception_mask_parameter & (int)LinkLossExceptionBits::Offboard;

	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1:
	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2:
	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3:
	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4:
	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5:
	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6:
	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7:
	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8:
		return exception_mask_parameter & (int)LinkLossExceptionBits::ExternalMode;

	case vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE:
		return exception_mask_parameter & (int)LinkLossExceptionBits::AltitudeCruise;

	default:
		return false;
	}
}

void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
				 const failsafe_flags_s &status_flags)
{
	updateArmingState(time_us, state.armed, status_flags);

	// Do not enter failsafe while doing a vtol takeoff after the vehicle has started a transition and before it reaches the loiter
	// altitude. The vtol takeoff navigaton mode will set mission_finished to true as soon as the loiter is established
	const bool in_forward_flight = (state.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) || state.vtol_in_transition_mode;
	const bool ignore_any_link_loss_vtol_takeoff_fixedwing = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
			&& in_forward_flight && !state.mission_finished;

	// Manual control (RC or joystick) loss
	if (!status_flags.manual_control_signal_lost) {
		// If manual control was lost and arming was allowed, consider it optional until we regain manual control
		_manual_control_lost_at_arming = false;
	}

	const bool rc_loss_ignored = isFailsafeIgnored(state.user_intended_mode, _param_com_rcl_except.get())
				     || ignore_any_link_loss_vtol_takeoff_fixedwing || _manual_control_lost_at_arming;

	if (_param_com_rc_in_mode.get() != int32_t(RcInMode::DisableManualControl) && !rc_loss_ignored) {
		CHECK_FAILSAFE(status_flags, manual_control_signal_lost,
			       fromNavDllOrRclActParam(_param_nav_rcl_act.get()).causedBy(Cause::ManualControlLoss));
	}

	// Ground control station connection loss
	const bool dll_loss_ignored_land = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
					   || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;

	const bool dll_loss_ignored = isFailsafeIgnored(state.user_intended_mode, _param_com_dll_except.get())
				      || ignore_any_link_loss_vtol_takeoff_fixedwing || dll_loss_ignored_land;

	if (_param_nav_dll_act.get() != int32_t(gcs_connection_loss_failsafe_mode::Disabled) && !dll_loss_ignored) {
		CHECK_FAILSAFE(status_flags, gcs_connection_lost, fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::GCSConnectionLoss));
	}

	// VTOL transition failure (quadchute)
	if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
	    state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER ||
	    state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
	    state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) {
		CHECK_FAILSAFE(status_flags, vtol_fixed_wing_system_failure, fromQuadchuteActParam(_param_com_qc_act.get()));
	}

	// Mission
	if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
		CHECK_FAILSAFE(status_flags, mission_failure, Action::RTL);

		// If manual control loss and GCS connection loss are disabled and we lose both command links and the mission finished,
		// trigger RTL to avoid losing the vehicle
		if ((_param_com_rc_in_mode.get() == int32_t(RcInMode::DisableManualControl)
		     || isFailsafeIgnored(state.user_intended_mode, _param_com_rcl_except.get()))
		    && _param_nav_dll_act.get() == int32_t(gcs_connection_loss_failsafe_mode::Disabled)
		    && state.mission_finished) {
			_last_state_mission_control_lost = checkFailsafe(_caller_id_mission_control_lost, _last_state_mission_control_lost,
							   status_flags.gcs_connection_lost, Action::RTL);
		}
	}

	CHECK_FAILSAFE(status_flags, wind_limit_exceeded,
		       ActionOptions(fromHighWindLimitActParam(_param_com_wind_max_act.get()).cannotBeDeferred()));
	CHECK_FAILSAFE(status_flags, flight_time_limit_exceeded, ActionOptions(Action::RTL).cannotBeDeferred());

	// trigger Low Position Accuracy Failsafe (only in auto mission and auto loiter)
	if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
	    state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
		CHECK_FAILSAFE(status_flags, position_accuracy_low, fromPosLowActParam(_param_com_pos_low_act.get()));
	}

	// Navigator failure: when the vehicle is already executing RTL (or Takeoff) and the
	// navigator reports an internal failure, switching to Land keeps the vehicle
	// descending in place rather than attempting an infeasible RTL.  In all other modes
	// Hold is used so the operator can take over.
	//
	// NOTE – two distinct failure paths lead to a Land/Descend during RTL:
	//   1. navigator_failure (handled here): navigator.cpp signals an internal error
	//      while executing a guided mode; this flag triggers an explicit Land action.
	//   2. Sensor-based degradation (global_position_invalid, local_position_invalid_relaxed,
	//      etc.) during active RTL: handled automatically by the framework's modeCanRun()
	//      check and the A3 fallthrough chain in getSelectedAction() – no additional
	//      logic is required here.  clearDelayIfNeeded() prevents a spurious Hold
	//      interlude in both paths because _selected_action = RTL > Hold.
	if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
	    state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
		CHECK_FAILSAFE(status_flags, navigator_failure,
			       ActionOptions(Action::Land).clearOn(ClearCondition::OnModeChangeOrDisarm));

	} else {
		CHECK_FAILSAFE(status_flags, navigator_failure,
			       ActionOptions(Action::Hold).clearOn(ClearCondition::OnModeChangeOrDisarm));
	}

	// Geofence breach: treated as a hard constraint that cannot be deferred (cannotBeDeferred()).
	// Interaction with battery failsafe (priority axioms A1/A5): both subsystems independently
	// add their action to the pool and the framework picks the highest severity.
	//   Example 1 – battery-EMERGENCY (→Land, sev 7) + geofence-Return (→RTL, sev 6):
	//               Land wins; the vehicle lands in place.
	//   Example 2 – battery-CRITICAL (→RTL, sev 6) + geofence-Return (→RTL, sev 6):
	//               Both are RTL; if the vehicle is already executing RTL the framework
	//               downgrades the duplicate to Warn (axiom A4 / skip logic).
	//   Example 3 – geofence-Return (→RTL) but home_position_invalid:
	//               RTL cannot run; the framework falls through to Land (axiom A3).
	CHECK_FAILSAFE(status_flags, geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred());

	// Battery flight time remaining failsafe
	CHECK_FAILSAFE(status_flags, battery_low_remaining_time,
		       ActionOptions(fromRemainingFlightTimeLowActParam(_param_com_fltt_low_act.get())));

	if ((_armed_time != 0)
	    && (time_us < _armed_time + static_cast<hrt_abstime>(_param_com_spoolup_time.get() * 1_s))
	   ) {
		CHECK_FAILSAFE(status_flags, battery_unhealthy, ActionOptions(Action::Disarm).cannotBeDeferred());

	} else {
		CHECK_FAILSAFE(status_flags, battery_unhealthy, Action::Warn);
	}

	// Battery low failsafe.
	// The three severity levels map to increasing action severity:
	//   WARNING_LOW      → Warn (advisory only)
	//   WARNING_CRITICAL → RTL or Land depending on COM_LOW_BAT_ACT
	//   WARNING_EMERGENCY→ Land (COM_LOW_BAT_ACT=ReturnOrLand) or RTL (=Return)
	// When a battery warning escalates (e.g. CRITICAL → EMERGENCY) the new, higher-
	// severity action is added and the framework selects it automatically (axiom A1).
	// If RTL is selected but home_position_invalid is set, the framework falls through
	// to Land (axiom A3); no special handling is needed here.
	//
	// If battery was low and arming was allowed through COM_ARM_BAT_MIN, don't failsafe immediately for the current low battery warning state
	const bool warning_worse_than_at_arming = (status_flags.battery_warning > _battery_warning_at_arming);
	const int32_t low_battery_action = warning_worse_than_at_arming ?
					   _param_com_low_bat_act.get() : (int32_t)LowBatteryAction::Warning;

	switch (status_flags.battery_warning) {
	case battery_status_s::WARNING_LOW:
		_last_state_battery_warning_low = checkFailsafe(_caller_id_battery_warning_low, _last_state_battery_warning_low,
						  true, fromBatteryWarningActParam(low_battery_action, battery_status_s::WARNING_LOW));
		break;

	case battery_status_s::WARNING_CRITICAL:
		_last_state_battery_warning_critical = checkFailsafe(_caller_id_battery_warning_critical,
						       _last_state_battery_warning_critical,
						       true, fromBatteryWarningActParam(low_battery_action, battery_status_s::WARNING_CRITICAL));
		break;

	case battery_status_s::WARNING_EMERGENCY:
		_last_state_battery_warning_emergency = checkFailsafe(_caller_id_battery_warning_emergency,
							_last_state_battery_warning_emergency,
							true, fromBatteryWarningActParam(low_battery_action, battery_status_s::WARNING_EMERGENCY));
		break;

	default:
		break;
	}


	// Handle fails during spoolup just after arming
	if ((_armed_time != 0)
	    && (time_us < _armed_time + static_cast<hrt_abstime>(_param_com_spoolup_time.get() * 1_s))
	   ) {
		_last_state_fd_esc_arming = checkFailsafe(_caller_id_fd_esc_arming, _last_state_fd_esc_arming,
					    status_flags.fd_esc_arming_failure,
					    ActionOptions(Action::Disarm).cannotBeDeferred());
		_last_state_battery_unhealthy_spoolup = checkFailsafe(_caller_id_battery_unhealthy_spoolup,
							_last_state_battery_unhealthy_spoolup, status_flags.battery_unhealthy,
							ActionOptions(Action::Disarm).cannotBeDeferred());
	}

	// Handle fails during the early takeoff phase
	if ((_armed_time != 0)
	    && (time_us < _armed_time
		+ static_cast<hrt_abstime>((_param_com_lkdown_tko.get() + _param_com_spoolup_time.get()) * 1_s))
	   ) {
		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, ActionOptions(Action::Terminate).cannotBeDeferred());

	} else {
		CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Warn);
	}

	CHECK_FAILSAFE(status_flags, fd_imbalanced_prop, fromImbalancedPropActParam(_param_com_imb_prop_act.get()));
	CHECK_FAILSAFE(status_flags, fd_motor_failure, fromActuatorFailureActParam(_param_com_actuator_failure_act.get()));



	// Mode fallback (last)
	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).cannotBeDeferred());
}

void Failsafe::updateArmingState(const hrt_abstime &time_us, bool armed, const failsafe_flags_s &status_flags)
{
	if (!_was_armed && armed) {
		_armed_time = time_us;
		_manual_control_lost_at_arming = status_flags.manual_control_signal_lost;
		_battery_warning_at_arming = status_flags.battery_warning;

	} else if (!armed) {
		_manual_control_lost_at_arming = status_flags.manual_control_signal_lost; // ensure action isn't added while disarmed
		_armed_time = 0;
	}

	_was_armed = armed;
}

FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_flags,
		uint8_t user_intended_mode) const
{
	Action action = Action::None;

	// offboard signal
	if (status_flags.offboard_control_signal_lost && (status_flags.mode_req_offboard_signal & (1u << user_intended_mode))) {
		action = fromOffboardLossActParam(_param_com_obl_rc_act.get(), user_intended_mode);

		// for this specific case, user_intended_mode is not modified, we shouldn't check additional fallbacks
		if (action == Action::Disarm) {
			return action;
		}

		if (action == Action::FallbackPosCtrl || action == Action::FallbackAltCtrl || action == Action::FallbackStab) {
			// Check if RC is available, if not use the mode specified in NAV_RCL_ACT
			if (status_flags.manual_control_signal_lost) {
				ActionOptions rc_loss_action = fromNavDllOrRclActParam(_param_nav_rcl_act.get());
				action = rc_loss_action.action;
			}

		}
	}

	// PosCtrl/PositionSlow -> AltCtrl
	if ((user_intended_mode == vehicle_status_s::NAVIGATION_STATE_POSCTL ||
	     user_intended_mode == vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW)
	    && !modeCanRun(status_flags, user_intended_mode)) {
		action = Action::FallbackAltCtrl;
		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
	}

	// AltCtrl -> Stabilized
	if (user_intended_mode == vehicle_status_s::NAVIGATION_STATE_ALTCTL
	    && !modeCanRun(status_flags, user_intended_mode)) {
		action = Action::FallbackStab;
		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB;
	}

	// Last, check can_run for intended mode
	if (!modeCanRun(status_flags, user_intended_mode)) {
		action = Action::RTL;
		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
	}

	return action;
}

uint8_t Failsafe::modifyUserIntendedMode(Action previous_action, Action current_action,
		uint8_t user_intended_mode) const
{
	// If we switch from a failsafe back into orbit, switch to loiter instead
	if ((int)previous_action > (int)Action::Warn
	    && modeFromAction(current_action, user_intended_mode) == vehicle_status_s::NAVIGATION_STATE_ORBIT) {
		PX4_DEBUG("Failsafe cleared, switching from ORBIT to LOITER");
		return vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
	}

	return user_intended_mode;
}