diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index c745b19381..6d53cf0cfd 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -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 diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index 1032d17edc..9cbb7d5764 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -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);