dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: failsafe define enums for actions (#20880)

This commit is contained in:
murata,katsutoshi 2023-01-08 00:10:57 +09:00 committed by GitHub
parent 49f8bcfc69
commit d821404e4f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 112 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
src/modules/commander/failsafe/failsafe.cpp
View File

@ -44,31 +44,31 @@ FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value)
{
ActionOptions options{};
switch (param_value) {
case 0:
switch (gcs_connection_loss_failsafe_mode(param_value)) {
case gcs_connection_loss_failsafe_mode::Disabled:
options.action = Action::None;
break;
case 1:
case gcs_connection_loss_failsafe_mode::Hold_mode:
options.action = Action::Hold;
break;
case 2:
case gcs_connection_loss_failsafe_mode::Return_mode:
options.action = Action::RTL;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case 3:
case gcs_connection_loss_failsafe_mode::Land_mode:
options.action = Action::Land;
break;
case 5:
case gcs_connection_loss_failsafe_mode::Terminate:
options.allow_user_takeover = UserTakeoverAllowed::Never;
options.action = Action::Terminate;
options.clear_condition = ClearCondition::Never;
break;
case 6: // Lockdown
case gcs_connection_loss_failsafe_mode::Disarm: // Lockdown
options.allow_user_takeover = UserTakeoverAllowed::Never;
options.action = Action::Disarm;
break;
@ -85,33 +85,33 @@ FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value)
{
ActionOptions options{};
switch (param_value) {
case 0:
switch (geofence_violation_action(param_value)) {
case geofence_violation_action::None:
options.action = Action::None;
break;
case 1:
case geofence_violation_action::Warning:
options.action = Action::Warn;
break;
case 2:
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 3:
case geofence_violation_action::Return_mode:
options.action = Action::RTL;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case 4:
case geofence_violation_action::Terminate:
options.allow_user_takeover = UserTakeoverAllowed::Never;
options.action = Action::Terminate;
options.clear_condition = ClearCondition::Never;
break;
case 5:
case geofence_violation_action::Land_mode:
options.action = Action::Land;
break;
@ -127,22 +127,22 @@ FailsafeBase::ActionOptions Failsafe::fromImbalancedPropActParam(int param_value
{
ActionOptions options{};
switch (param_value) {
case -1:
switch (imbalanced_propeller_failsafe_mode(param_value)) {
case imbalanced_propeller_failsafe_mode::Disabled:
default:
options.action = Action::None;
break;
case 0:
case imbalanced_propeller_failsafe_mode::Warning:
options.action = Action::Warn;
break;
case 1:
case imbalanced_propeller_failsafe_mode::Return:
options.action = Action::RTL;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case 2:
case imbalanced_propeller_failsafe_mode::Land:
options.action = Action::Land;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
@ -155,27 +155,27 @@ FailsafeBase::ActionOptions Failsafe::fromActuatorFailureActParam(int param_valu
{
ActionOptions options{};
switch (param_value) {
case 0:
switch (actuator_failure_failsafe_mode(param_value)) {
case actuator_failure_failsafe_mode::Warning_only:
default:
options.action = Action::Warn;
break;
case 1:
case actuator_failure_failsafe_mode::Hold_mode:
options.action = Action::Hold;
break;
case 2:
case actuator_failure_failsafe_mode::Land_mode:
options.action = Action::Land;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case 3:
case actuator_failure_failsafe_mode::Return_mode:
options.action = Action::RTL;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case 4:
case actuator_failure_failsafe_mode::Terminate:
options.action = Action::Terminate;
options.clear_condition = ClearCondition::Never;
break;
@ -249,23 +249,23 @@ FailsafeBase::ActionOptions Failsafe::fromQuadchuteActParam(int param_value)
{
ActionOptions options{};
switch (param_value) {
case -1:
switch (command_after_quadchute(param_value)) {
case command_after_quadchute::Warning_only:
default:
options.action = Action::Warn;
break;
case 0:
case command_after_quadchute::Return_mode:
options.action = Action::RTL;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case 1:
case command_after_quadchute::Land_mode:
options.action = Action::Land;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
case 2:
case command_after_quadchute::Hold_mode:
options.action = Action::Hold;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
break;
@ -278,44 +278,44 @@ FailsafeBase::Action Failsafe::fromOffboardLossActParam(int param_value, uint8_t
{
Action action{Action::None};
switch (param_value) {
case 0:
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 1:
case offboard_loss_failsafe_mode::Altitude_mode:
action = Action::FallbackAltCtrl;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
break;
case 2:
case offboard_loss_failsafe_mode::Manual:
action = Action::FallbackStab;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB;
break;
case 3:
case offboard_loss_failsafe_mode::Return_mode:
action = Action::RTL;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
break;
case 4:
case offboard_loss_failsafe_mode::Land_mode:
action = Action::Land;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
break;
case 5:
case offboard_loss_failsafe_mode::Hold_mode:
action = Action::Hold;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
break;
case 6:
case offboard_loss_failsafe_mode::Terminate:
action = Action::Terminate;
user_intended_mode = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
break;
case 7:
case offboard_loss_failsafe_mode::Disarm:
action = Action::Disarm;
break;
}
@ -354,7 +354,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard ||
rc_loss_ignored_takeoff || ignore_link_failsafe || _manual_control_lost_at_arming;
if (_param_com_rc_in_mode.get() != 4 && !rc_loss_ignored) {
if (_param_com_rc_in_mode.get() != int32_t(offboard_loss_failsafe_mode::Land_mode) && !rc_loss_ignored) {
CHECK_FAILSAFE(status_flags, manual_control_signal_lost,
fromNavDllOrRclActParam(_param_nav_rcl_act.get()).causedBy(Cause::ManualControlLoss));
}
@ -363,7 +363,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
const bool gcs_connection_loss_ignored = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND ||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND || ignore_link_failsafe;
if (_param_nav_dll_act.get() != 0 && !gcs_connection_loss_ignored) {
if (_param_nav_dll_act.get() != int32_t(gcs_connection_loss_failsafe_mode::Disabled) && !gcs_connection_loss_ignored) {
CHECK_FAILSAFE(status_flags, gcs_connection_lost,
fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::GCSConnectionLoss));
}
@ -382,7 +382,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
// 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() == 4 || rc_loss_ignored_mission) && _param_nav_dll_act.get() == 0
if ((_param_com_rc_in_mode.get() == int32_t(offboard_loss_failsafe_mode::Land_mode) || rc_loss_ignored_mission)
&& _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);
@ -401,19 +402,26 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
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) {
switch (status_flags.battery_warning) {
case 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));
break;
} else if (status_flags.battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
case 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));
break;
} else if (status_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
case 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));
break;
default:
break;
}
@ -475,8 +483,8 @@ FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_
}
// posctrl
switch (_param_com_posctl_navl.get()) {
case 0: // AltCtrl/Manual
switch (position_control_navigation_loss_response(_param_com_posctl_navl.get())) {
case position_control_navigation_loss_response::Altitude_Manual: // AltCtrl/Manual
// PosCtrl -> AltCtrl
if (user_intended_mode == vehicle_status_s::NAVIGATION_STATE_POSCTL
@ -494,7 +502,7 @@ FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_
break;
case 1: // Land/Terminate
case position_control_navigation_loss_response::Land_Descend: // Land/Terminate
// PosCtrl -> Land
if (user_intended_mode == vehicle_status_s::NAVIGATION_STATE_POSCTL

56
src/modules/commander/failsafe/failsafe.h
View File

@ -67,6 +67,62 @@ private:
ReturnOrLand = 3 // Return mode at critically low level, Land mode at current position if reaching dangerously low levels
};
enum class offboard_loss_failsafe_mode : int32_t {
Position_mode = 0,
Altitude_mode = 1,
Manual = 2,
Return_mode = 3,
Land_mode = 4,
Hold_mode = 5,
Terminate = 6,
Disarm = 7,
};
enum class position_control_navigation_loss_response : int32_t {
Altitude_Manual = 0,
Land_Descend = 1,
};
enum class actuator_failure_failsafe_mode : int32_t {
Warning_only = 0,
Hold_mode = 1,
Land_mode = 2,
Return_mode = 3,
Terminate = 4,
};
enum class imbalanced_propeller_failsafe_mode : int32_t {
Disabled = -1,
Warning = 0,
Return = 1,
Land = 2,
};
enum class geofence_violation_action : int32_t {
None = 0,
Warning = 1,
Hold_mode = 2,
Return_mode = 3,
Terminate = 4,
Land_mode = 5,
};
enum class gcs_connection_loss_failsafe_mode : int32_t {
Disabled = 0,
Hold_mode = 1,
Return_mode = 2,
Land_mode = 3,
Terminate = 5,
Disarm = 6,
};
enum class command_after_quadchute : int32_t {
Warning_only = -1,
Return_mode = 0,
Land_mode = 1,
Hold_mode = 2,
};
static ActionOptions fromNavDllOrRclActParam(int param_value);
static ActionOptions fromGfActParam(int param_value);