fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 00:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: use new failsafe state machine and add user intention class

Browse Source
This commit is contained in:
Beat Küng
2022-10-11 22:31:20 -04:00
committed by Daniel Agar
parent a04230faa1
commit 455b885f86
44 changed files with 1927 additions and 2280 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/failsafe/failsafe.cpp
+276 -12
View File
@@ -35,8 +35,12 @@
#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>
FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value) const
using namespace time_literals;
FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value)
{
ActionOptions options{};
@@ -77,7 +81,7 @@ FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value) c
return options;
}
FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value) const
FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value)
{
ActionOptions options{};
@@ -119,9 +123,211 @@ FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value) const
return options;
}
FailsafeBase::ActionOptions Failsafe::fromImbalancedPropActParam(int param_value)
{
ActionOptions options{};
switch (param_value) {
case -1:
default:
options.action = Action::None;
break;
case 0:
options.action = Action::Warn;
break;
case 1:
options.action = Action::RTL;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case 2:
options.action = Action::Land;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
}
return options;
}
FailsafeBase::ActionOptions Failsafe::fromActuatorFailureActParam(int param_value)
{
ActionOptions options{};
switch (param_value) {
case 0:
default:
options.action = Action::Warn;
break;
case 1:
options.action = Action::Hold;
break;
case 2:
options.action = Action::Land;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case 3:
options.action = Action::RTL;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case 4:
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::BATTERY_WARNING_NONE:
options.action = Action::None;
break;
case battery_status_s::BATTERY_WARNING_LOW:
options.action = Action::Warn;
options.cause = Cause::BatteryLow;
break;
case battery_status_s::BATTERY_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::BATTERY_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 (param_value) {
case -1:
default:
options.action = Action::Warn;
break;
case 0:
options.action = Action::RTL;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case 1:
options.action = Action::Land;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case 2:
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 (param_value) {
case 0:
default:
action = Action::FallbackPosCtrl;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
break;
case 1:
action = Action::FallbackAltCtrl;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
break;
case 2:
action = Action::FallbackStab;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB;
break;
case 3:
action = Action::RTL;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
break;
case 4:
action = Action::Land;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
break;
case 5:
action = Action::Hold;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
break;
case 6:
action = Action::Terminate;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
break;
case 7:
action = Action::Disarm;
break;
}
return action;
}
void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
const vehicle_status_flags_s &status_flags)
{
updateArmingState(time_us, state.armed, status_flags);
const bool in_forward_flight = state.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
|| state.vtol_in_transition_mode;
@@ -131,6 +337,11 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
&& in_forward_flight && !state.mission_finished;
// RC loss
if (!status_flags.rc_signal_lost) {
// If RC was lost and arming was allowed, consider it optional until we regain RC
_rc_lost_at_arming = false;
}
const bool rc_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
&& (_param_com_rcl_except.get() & (int)RCLossExceptionBits::Mission);
const bool rc_loss_ignored_loiter = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER
@@ -140,11 +351,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
const bool rc_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
&& (_param_com_rcl_except.get() & (int)RCLossExceptionBits::Hold);
const bool rc_loss_ignored = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL ||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND ||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND ||
rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard || rc_loss_ignored_takeoff ||
ignore_link_failsafe;
const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard ||
rc_loss_ignored_takeoff || ignore_link_failsafe || _rc_lost_at_arming;
if (_param_com_rc_in_mode.get() != 4 && !rc_loss_ignored) {
CHECK_FAILSAFE(status_flags, rc_signal_lost,
@@ -160,12 +368,12 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::DatalinkLoss));
}
// VTOL transition failure
// 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_transition_failure, Action::RTL);
CHECK_FAILSAFE(status_flags, vtol_transition_failure, fromQuadchuteActParam(_param_com_qc_act.get()));
}
// Mission
@@ -182,10 +390,53 @@ 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).allowUserTakeover(UserTakeoverAllowed::Never));
CHECK_FAILSAFE(status_flags, geofence_violated, fromGfActParam(_param_gf_action.get()));
// Battery
CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::BatteryLow));
CHECK_FAILSAFE(status_flags, battery_unhealthy, Action::Warn);
if (status_flags.battery_warning == battery_status_s::BATTERY_WARNING_LOW) {
_last_state_battery_warning_low = checkFailsafe(_caller_id_battery_warning_low, _last_state_battery_warning_low,
true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_LOW));
} else if (status_flags.battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
_last_state_battery_warning_critical = checkFailsafe(_caller_id_battery_warning_critical,
_last_state_battery_warning_critical,
true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_CRITICAL));
} else if (status_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
_last_state_battery_warning_emergency = checkFailsafe(_caller_id_battery_warning_emergency,
_last_state_battery_warning_emergency,
true, fromBatteryWarningActParam(_param_com_low_bat_act.get(), battery_status_s::BATTERY_WARNING_EMERGENCY));
}
// Failure detector
if (_armed_time != 0 && time_us - _armed_time < _param_com_spoolup_time.get() * 1_s) {
CHECK_FAILSAFE(status_flags, fd_esc_arming_failure, Action::Disarm);
}
if (_armed_time != 0 && time_us - _armed_time < (_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);
} else if (!circuit_breaker_enabled_by_val(_param_cbrk_flightterm.get(), CBRK_FLIGHTTERM_KEY)) {
CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Terminate);
} 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);
@@ -194,6 +445,20 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
ActionOptions(mode_fallback_action).allowUserTakeover(UserTakeoverAllowed::Always));
}
void Failsafe::updateArmingState(const hrt_abstime &time_us, bool armed, const vehicle_status_flags_s &status_flags)
{
if (!_was_armed && armed) {
_armed_time = time_us;
_rc_lost_at_arming = status_flags.rc_signal_lost;
} else if (!armed) {
_rc_lost_at_arming = status_flags.rc_signal_lost; // ensure action isn't added while disarmed
_armed_time = 0;
}
_was_armed = armed;
}
FailsafeBase::Action Failsafe::checkModeFallback(const vehicle_status_flags_s &status_flags,
uint8_t user_intended_mode) const
{
@@ -201,8 +466,7 @@ FailsafeBase::Action Failsafe::checkModeFallback(const vehicle_status_flags_s &s
// offboard signal
if (status_flags.offboard_control_signal_lost && (status_flags.mode_req_offboard_signal & (1u << user_intended_mode))) {
action = Action::FallbackPosCtrl; // TODO: use param
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
action = fromOffboardLossActParam(_param_com_obl_rc_act.get(), user_intended_mode);
}
// posctrl
src/modules/commander/failsafe/failsafe.h
+37 -3
View File
@@ -51,28 +51,62 @@ protected:
uint8_t user_intended_mode) const override;
private:
void updateArmingState(const hrt_abstime &time_us, bool armed, const vehicle_status_flags_s &status_flags);
enum class RCLossExceptionBits : int32_t {
Mission = (1 << 0),
Hold = (1 << 1),
Offboard = (1 << 2)
};
ActionOptions fromNavDllOrRclActParam(int param_value) const;
// COM_LOW_BAT_ACT parameter values
enum class LowBatteryAction : int32_t {
Warning = 0, // Warning
Return = 1, // Return mode (deprecated)
Land = 2, // Land mode
ReturnOrLand = 3 // Return mode at critically low level, Land mode at current position if reaching dangerously low levels
};
ActionOptions fromGfActParam(int param_value) const;
static ActionOptions fromNavDllOrRclActParam(int param_value);
static ActionOptions fromGfActParam(int param_value);
static ActionOptions fromImbalancedPropActParam(int param_value);
static ActionOptions fromActuatorFailureActParam(int param_value);
static ActionOptions fromBatteryWarningActParam(int param_value, uint8_t battery_warning);
static ActionOptions fromQuadchuteActParam(int param_value);
static Action fromOffboardLossActParam(int param_value, uint8_t &user_intended_mode);
const int _caller_id_mode_fallback{genCallerId()};
bool _last_state_mode_fallback{false};
const int _caller_id_mission_control_lost{genCallerId()};
bool _last_state_mission_control_lost{false};
const int _caller_id_battery_warning_low{genCallerId()}; ///< battery warning caller ID's: use different ID's as they have different actions
bool _last_state_battery_warning_low{false};
const int _caller_id_battery_warning_critical{genCallerId()};
bool _last_state_battery_warning_critical{false};
const int _caller_id_battery_warning_emergency{genCallerId()};
bool _last_state_battery_warning_emergency{false};
hrt_abstime _armed_time{0};
bool _was_armed{false};
bool _rc_lost_at_arming{false}; ///< true if RC was lost at arming time
DEFINE_PARAMETERS_CUSTOM_PARENT(FailsafeBase,
(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,
(ParamInt<px4::params::NAV_RCL_ACT>) _param_nav_rcl_act,
(ParamInt<px4::params::COM_RCL_EXCEPT>) _param_com_rcl_except,
(ParamInt<px4::params::COM_RC_IN_MODE>) _param_com_rc_in_mode,
(ParamInt<px4::params::COM_POSCTL_NAVL>) _param_com_posctl_navl,
(ParamInt<px4::params::GF_ACTION>) _param_gf_action
(ParamInt<px4::params::GF_ACTION>) _param_gf_action,
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
(ParamInt<px4::params::COM_IMB_PROP_ACT>) _param_com_imb_prop_act,
(ParamFloat<px4::params::COM_LKDOWN_TKO>) _param_com_lkdown_tko,
(ParamInt<px4::params::CBRK_FLIGHTTERM>) _param_cbrk_flightterm,
(ParamInt<px4::params::COM_ACT_FAIL_ACT>) _param_com_actuator_failure_act,
(ParamInt<px4::params::COM_LOW_BAT_ACT>) _param_com_low_bat_act,
(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act
);
};
src/modules/commander/failsafe/framework.cpp
+3
View File
@@ -458,6 +458,9 @@ void FailsafeBase::getSelectedAction(const State &state, const vehicle_status_fl
if (!_user_takeover_active) {
PX4_DEBUG("Activating user takeover");
#ifndef EMSCRIPTEN_BUILD
events::send(events::ID("failsafe_rc_override"), events::Log::Info, "Pilot took over using sticks");
#endif // EMSCRIPTEN_BUILD
}
// We must check for mode fallback again here