Files
PX4-Autopilot/src/modules/commander/failsafe/failsafe.cpp
T

778 lines
27 KiB
C++

/****************************************************************************
*
* Copyright (c) 2022-2026 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;
static bool manualControlFallbackAction(FailsafeBase::Action action)
{
return action == FailsafeBase::Action::FallbackPosCtrl
|| action == FailsafeBase::Action::FallbackAltCtrl
|| action == FailsafeBase::Action::FallbackStab;
}
FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value)
{
ActionOptions options{};
switch (gcs_connection_loss_failsafe_mode(param_value)) {
case gcs_connection_loss_failsafe_mode::Disabled:
// No failsafe action: for NAV_RCL_ACT this is handled by Commander switching to Hold as a regular
// mode change (see Commander::manualControlLossModeSwitch()).
case gcs_connection_loss_failsafe_mode::Hold_mode_no_failsafe:
default:
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;
}
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:
default:
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;
}
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;
}
// "battery level" to handle the remaining flight time (Distance-aware return)
static constexpr uint8_t BATTERY_WARNING_DISTANCE_AWARE_RETURN = UINT8_MAX;
FailsafeBase::ActionOptions Failsafe::fromBatteryWarningActParam(int param_value, uint8_t battery_warning)
{
ActionOptions options{};
switch (battery_warning) {
case battery_status_s::WARNING_NONE:
default:
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:
case LowBatteryAction::ReturnOrLandWithoutDistanceAwareReturn:
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::Land:
case LowBatteryAction::ReturnOrLand:
case LowBatteryAction::ReturnOrLandWithoutDistanceAwareReturn:
options.action = Action::Land;
break;
case LowBatteryAction::Warning:
options.action = Action::Warn;
break;
}
break;
case BATTERY_WARNING_DISTANCE_AWARE_RETURN:
options.action = Action::Warn;
options.cause = Cause::RemainingFlightTimeLow;
switch ((LowBatteryAction)param_value) {
case LowBatteryAction::Return:
case LowBatteryAction::ReturnOrLand:
options.action = Action::RTL;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case LowBatteryAction::Warning:
case LowBatteryAction::Land:
case LowBatteryAction::ReturnOrLandWithoutDistanceAwareReturn:
options.action = Action::Warn;
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:
default:
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;
}
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:
default:
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;
}
return options;
}
FailsafeBase::ActionOptions Failsafe::fromGnssLossActParam(int param_value)
{
ActionOptions options{};
switch (gps_redundancy_failsafe_mode(param_value)) {
case gps_redundancy_failsafe_mode::Warning:
default:
options.action = Action::Warn;
break;
case gps_redundancy_failsafe_mode::Return_mode:
options.action = Action::RTL;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case gps_redundancy_failsafe_mode::Land_mode:
options.action = Action::Land;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case gps_redundancy_failsafe_mode::Terminate:
options.allow_user_takeover = UserTakeoverAllowed::Never;
options.action = Action::Terminate;
options.clear_condition = ClearCondition::Never;
break;
}
return options;
}
FailsafeBase::ActionOptions Failsafe::fromParachuteActParam(int param_value)
{
ActionOptions options{};
switch (parachute_unhealthy_failsafe_mode(param_value)) {
case parachute_unhealthy_failsafe_mode::Disabled:
default:
options.action = Action::None;
break;
case parachute_unhealthy_failsafe_mode::Warning:
case parachute_unhealthy_failsafe_mode::Error:
options.action = Action::Warn;
options.clear_condition = ClearCondition::WhenConditionClears;
break;
case parachute_unhealthy_failsafe_mode::Return:
options.action = Action::RTL;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case parachute_unhealthy_failsafe_mode::Land:
options.action = Action::Land;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
}
return options;
}
FailsafeBase::ActionOptions Failsafe::fromOdidFailActParam(int param_value)
{
ActionOptions options{};
switch (open_drone_id_failsafe_mode(param_value)) {
case open_drone_id_failsafe_mode::Return_mode:
options.action = Action::RTL;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case open_drone_id_failsafe_mode::Land_mode:
options.action = Action::Land;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case open_drone_id_failsafe_mode::Terminate:
options.allow_user_takeover = UserTakeoverAllowed::Never;
options.action = Action::Terminate;
options.clear_condition = ClearCondition::Never;
break;
case open_drone_id_failsafe_mode::None:
case open_drone_id_failsafe_mode::Warning:
case open_drone_id_failsafe_mode::Error:
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:
case vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE:
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 || !state.armed;
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()));
}
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));
}
CHECK_FAILSAFE(status_flags, geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred());
const hrt_abstime spoolup = static_cast<hrt_abstime>(_param_com_spoolup_time.get() * 1_s);
if ((_armed_time != 0) && (time_us < _armed_time + spoolup)) {
CHECK_FAILSAFE(status_flags, battery_unhealthy, ActionOptions(Action::Disarm).cannotBeDeferred());
} else {
CHECK_FAILSAFE(status_flags, battery_unhealthy, Action::Warn);
}
// Parachute system health failsafe
CHECK_FAILSAFE(status_flags, parachute_unhealthy, ActionOptions(fromParachuteActParam(_param_com_parachute.get())));
// Remote ID (Open Drone ID) loss failsafe
if (state.armed && _param_com_arm_odid.get() >= int32_t(open_drone_id_failsafe_mode::Return_mode)) {
CHECK_FAILSAFE(status_flags, remote_id_unhealthy, fromOdidFailActParam(_param_com_arm_odid.get()));
}
// Battery low failsafe
// 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;
}
// Battery flight time remaining failsafe (Distance-aware return)
CHECK_FAILSAFE(status_flags, battery_low_remaining_time,
ActionOptions(fromBatteryWarningActParam(_param_com_low_bat_act.get(), BATTERY_WARNING_DISTANCE_AWARE_RETURN)));
// Handle fails during spoolup just after arming
if ((_armed_time != 0) && (time_us < _armed_time + spoolup)) {
_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 + spoolup + 3_s)) {
CHECK_FAILSAFE(status_flags, fd_critical_failure, ActionOptions(Action::Disarm).cannotBeDeferred());
CHECK_FAILSAFE(status_flags, fd_alt_loss, 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());
CHECK_FAILSAFE(status_flags, fd_alt_loss, ActionOptions(Action::Terminate).cannotBeDeferred());
} else {
CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Warn);
CHECK_FAILSAFE(status_flags, fd_alt_loss, Action::Warn);
}
CHECK_FAILSAFE(status_flags, fd_imbalanced_prop, Action::Warn);
CHECK_FAILSAFE(status_flags, fd_motor_failure, fromActuatorFailureActParam(_param_com_actuator_failure_act.get()));
CHECK_FAILSAFE(status_flags, gnss_lost, fromGnssLossActParam(_param_com_gnssloss_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;
}
}
// 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;
}
if (status_flags.manual_control_signal_lost && manualControlFallbackAction(action)) {
action = manualControlLossFallbackAction();
}
// 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
{
// When a failsafe engages, immediately downgrade Orbit to Loiter so that
// if the failsafe later clears without a new explicit mode command the vehicle holds position.
if ((int)previous_action <= (int)Action::Warn
&& (int)current_action > (int)Action::Warn
&& user_intended_mode == vehicle_status_s::NAVIGATION_STATE_ORBIT) {
PX4_DEBUG("Failsafe engaged, downgrading ORBIT to LOITER");
return vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
}
return user_intended_mode;
}