Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: Classify failsafe action definitions

This commit is contained in:
Pierre Kancir 2021-08-04 10:58:08 +02:00 committed by Andrew Tridgell
parent a368549a9d
commit 7e1716eaeb
2 changed files with 56 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
ArduCopter/Copter.h
View File

@ -590,14 +590,14 @@ private:
ESCCAL_DISABLED = 9, ESCCAL_DISABLED = 9,
}; };
enum Failsafe_Action { enum class FailsafeAction : uint8_t {
Failsafe_Action_None = 0, NONE = 0,
Failsafe_Action_Land = 1, LAND = 1,
Failsafe_Action_RTL = 2, RTL = 2,
Failsafe_Action_SmartRTL = 3, SMARTRTL = 3,
Failsafe_Action_SmartRTL_Land = 4, SMARTRTL_LAND = 4,
Failsafe_Action_Terminate = 5, TERMINATE = 5,
Failsafe_Action_Auto_DO_LAND_START = 6, AUTO_DO_LAND_START = 6
}; };
enum class FailsafeOption { enum class FailsafeOption {
@ -616,17 +616,17 @@ private:
}; };
static constexpr int8_t _failsafe_priorities[] = { static constexpr int8_t _failsafe_priorities[] = {
Failsafe_Action_Terminate, (int8_t)FailsafeAction::TERMINATE,
Failsafe_Action_Land, (int8_t)FailsafeAction::LAND,
Failsafe_Action_RTL, (int8_t)FailsafeAction::RTL,
Failsafe_Action_SmartRTL_Land, (int8_t)FailsafeAction::SMARTRTL_LAND,
Failsafe_Action_SmartRTL, (int8_t)FailsafeAction::SMARTRTL,
Failsafe_Action_None, (int8_t)FailsafeAction::NONE,
-1 // the priority list must end with a sentinel of -1 -1 // the priority list must end with a sentinel of -1
}; };
#define FAILSAFE_LAND_PRIORITY 1 #define FAILSAFE_LAND_PRIORITY 1
static_assert(_failsafe_priorities[FAILSAFE_LAND_PRIORITY] == Failsafe_Action_Land, static_assert(_failsafe_priorities[FAILSAFE_LAND_PRIORITY] == (int8_t)FailsafeAction::LAND,
"FAILSAFE_LAND_PRIORITY must match the entry in _failsafe_priorities"); "FAILSAFE_LAND_PRIORITY must match the entry in _failsafe_priorities");
static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1, static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
"_failsafe_priorities is missing the sentinel"); "_failsafe_priorities is missing the sentinel");
@ -743,7 +743,7 @@ private:
void set_mode_SmartRTL_or_land_with_pause(ModeReason reason); void set_mode_SmartRTL_or_land_with_pause(ModeReason reason);
void set_mode_auto_do_land_start_or_RTL(ModeReason reason); void set_mode_auto_do_land_start_or_RTL(ModeReason reason);
bool should_disarm_on_failsafe(); bool should_disarm_on_failsafe();
void do_failsafe_action(Failsafe_Action action, ModeReason reason); void do_failsafe_action(FailsafeAction action, ModeReason reason);
// failsafe.cpp // failsafe.cpp
void failsafe_enable(); void failsafe_enable();

80
ArduCopter/events.cpp
View File

@ -15,29 +15,29 @@ void Copter::failsafe_radio_on_event()
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_OCCURRED); AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_OCCURRED);
// set desired action based on FS_THR_ENABLE parameter // set desired action based on FS_THR_ENABLE parameter
Failsafe_Action desired_action; FailsafeAction desired_action;
switch (g.failsafe_throttle) { switch (g.failsafe_throttle) {
case FS_THR_DISABLED: case FS_THR_DISABLED:
desired_action = Failsafe_Action_None; desired_action = FailsafeAction::NONE;
break; break;
case FS_THR_ENABLED_ALWAYS_RTL: case FS_THR_ENABLED_ALWAYS_RTL:
case FS_THR_ENABLED_CONTINUE_MISSION: case FS_THR_ENABLED_CONTINUE_MISSION:
desired_action = Failsafe_Action_RTL; desired_action = FailsafeAction::RTL;
break; break;
case FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL: case FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL:
desired_action = Failsafe_Action_SmartRTL; desired_action = FailsafeAction::SMARTRTL;
break; break;
case FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND: case FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND:
desired_action = Failsafe_Action_SmartRTL_Land; desired_action = FailsafeAction::SMARTRTL_LAND;
break; break;
case FS_THR_ENABLED_ALWAYS_LAND: case FS_THR_ENABLED_ALWAYS_LAND:
desired_action = Failsafe_Action_Land; desired_action = FailsafeAction::LAND;
break; break;
case FS_THR_ENABLED_AUTO_RTL_OR_RTL: case FS_THR_ENABLED_AUTO_RTL_OR_RTL:
desired_action = Failsafe_Action_Auto_DO_LAND_START; desired_action = FailsafeAction::AUTO_DO_LAND_START;
break; break;
default: default:
desired_action = Failsafe_Action_Land; desired_action = FailsafeAction::LAND;
} }
// Conditions to deviate from FS_THR_ENABLE selection and send specific GCS warning // Conditions to deviate from FS_THR_ENABLE selection and send specific GCS warning
@ -45,28 +45,28 @@ void Copter::failsafe_radio_on_event()
// should immediately disarm when we're on the ground // should immediately disarm when we're on the ground
gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Disarming"); gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Disarming");
arming.disarm(AP_Arming::Method::RADIOFAILSAFE); arming.disarm(AP_Arming::Method::RADIOFAILSAFE);
desired_action = Failsafe_Action_None; desired_action = FailsafeAction::NONE;
} else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) { } else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) {
// Allow landing to continue when battery failsafe requires it (not a user option) // Allow landing to continue when battery failsafe requires it (not a user option)
gcs().send_text(MAV_SEVERITY_WARNING, "Radio + Battery Failsafe - Continuing Landing"); gcs().send_text(MAV_SEVERITY_WARNING, "Radio + Battery Failsafe - Continuing Landing");
desired_action = Failsafe_Action_Land; desired_action = FailsafeAction::LAND;
} else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING)) { } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING)) {
// Allow landing to continue when FS_OPTIONS is set to continue landing // Allow landing to continue when FS_OPTIONS is set to continue landing
gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Landing"); gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Landing");
desired_action = Failsafe_Action_Land; desired_action = FailsafeAction::LAND;
} else if (flightmode->mode_number() == Mode::Number::AUTO && failsafe_option(FailsafeOption::RC_CONTINUE_IF_AUTO)) { } else if (flightmode->mode_number() == Mode::Number::AUTO && failsafe_option(FailsafeOption::RC_CONTINUE_IF_AUTO)) {
// Allow mission to continue when FS_OPTIONS is set to continue mission // Allow mission to continue when FS_OPTIONS is set to continue mission
gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Auto Mode"); gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Auto Mode");
desired_action = Failsafe_Action_None; desired_action = FailsafeAction::NONE;
} else if ((flightmode->in_guided_mode()) && } else if ((flightmode->in_guided_mode()) &&
(failsafe_option(FailsafeOption::RC_CONTINUE_IF_GUIDED)) && (g.failsafe_gcs != FS_GCS_DISABLED)) { (failsafe_option(FailsafeOption::RC_CONTINUE_IF_GUIDED)) && (g.failsafe_gcs != FS_GCS_DISABLED)) {
// Allow guided mode to continue when FS_OPTIONS is set to continue in guided mode. Only if the GCS failsafe is enabled. // Allow guided mode to continue when FS_OPTIONS is set to continue in guided mode. Only if the GCS failsafe is enabled.
gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Guided Mode"); gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Guided Mode");
desired_action = Failsafe_Action_None; desired_action = FailsafeAction::NONE;
} else { } else {
gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe"); gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe");
@ -89,18 +89,18 @@ void Copter::handle_battery_failsafe(const char *type_str, const int8_t action)
{ {
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED); AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED);
Failsafe_Action desired_action = (Failsafe_Action)action; FailsafeAction desired_action = (FailsafeAction)action;
// Conditions to deviate from BATT_FS_XXX_ACT parameter setting // Conditions to deviate from BATT_FS_XXX_ACT parameter setting
if (should_disarm_on_failsafe()) { if (should_disarm_on_failsafe()) {
// should immediately disarm when we're on the ground // should immediately disarm when we're on the ground
arming.disarm(AP_Arming::Method::BATTERYFAILSAFE); arming.disarm(AP_Arming::Method::BATTERYFAILSAFE);
desired_action = Failsafe_Action_None; desired_action = FailsafeAction::NONE;
gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Disarming"); gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Disarming");
} else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING) && desired_action != Failsafe_Action_None) { } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING) && desired_action != FailsafeAction::NONE) {
// Allow landing to continue when FS_OPTIONS is set to continue when landing // Allow landing to continue when FS_OPTIONS is set to continue when landing
desired_action = Failsafe_Action_Land; desired_action = FailsafeAction::LAND;
gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Continuing Landing"); gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Continuing Landing");
} else { } else {
gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe"); gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe");
@ -154,62 +154,62 @@ void Copter::failsafe_gcs_on_event(void)
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED); AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED);
RC_Channels::clear_overrides(); RC_Channels::clear_overrides();
// convert the desired failsafe response to the Failsafe_Action enum // convert the desired failsafe response to the FailsafeAction enum
Failsafe_Action desired_action; FailsafeAction desired_action;
switch (g.failsafe_gcs) { switch (g.failsafe_gcs) {
case FS_GCS_DISABLED: case FS_GCS_DISABLED:
desired_action = Failsafe_Action_None; desired_action = FailsafeAction::NONE;
break; break;
case FS_GCS_ENABLED_ALWAYS_RTL: case FS_GCS_ENABLED_ALWAYS_RTL:
case FS_GCS_ENABLED_CONTINUE_MISSION: case FS_GCS_ENABLED_CONTINUE_MISSION:
desired_action = Failsafe_Action_RTL; desired_action = FailsafeAction::RTL;
break; break;
case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL: case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL:
desired_action = Failsafe_Action_SmartRTL; desired_action = FailsafeAction::SMARTRTL;
break; break;
case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND: case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND:
desired_action = Failsafe_Action_SmartRTL_Land; desired_action = FailsafeAction::SMARTRTL_LAND;
break; break;
case FS_GCS_ENABLED_ALWAYS_LAND: case FS_GCS_ENABLED_ALWAYS_LAND:
desired_action = Failsafe_Action_Land; desired_action = FailsafeAction::LAND;
break; break;
case FS_GCS_ENABLED_AUTO_RTL_OR_RTL: case FS_GCS_ENABLED_AUTO_RTL_OR_RTL:
desired_action = Failsafe_Action_Auto_DO_LAND_START; desired_action = FailsafeAction::AUTO_DO_LAND_START;
break; break;
default: // if an invalid parameter value is set, the fallback is RTL default: // if an invalid parameter value is set, the fallback is RTL
desired_action = Failsafe_Action_RTL; desired_action = FailsafeAction::RTL;
} }
// Conditions to deviate from FS_GCS_ENABLE parameter setting // Conditions to deviate from FS_GCS_ENABLE parameter setting
if (!motors->armed()) { if (!motors->armed()) {
desired_action = Failsafe_Action_None; desired_action = FailsafeAction::NONE;
gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe"); gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe");
} else if (should_disarm_on_failsafe()) { } else if (should_disarm_on_failsafe()) {
// should immediately disarm when we're on the ground // should immediately disarm when we're on the ground
arming.disarm(AP_Arming::Method::GCSFAILSAFE); arming.disarm(AP_Arming::Method::GCSFAILSAFE);
desired_action = Failsafe_Action_None; desired_action = FailsafeAction::NONE;
gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Disarming"); gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Disarming");
} else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) { } else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) {
// Allow landing to continue when battery failsafe requires it (not a user option) // Allow landing to continue when battery failsafe requires it (not a user option)
gcs().send_text(MAV_SEVERITY_WARNING, "GCS + Battery Failsafe - Continuing Landing"); gcs().send_text(MAV_SEVERITY_WARNING, "GCS + Battery Failsafe - Continuing Landing");
desired_action = Failsafe_Action_Land; desired_action = FailsafeAction::LAND;
} else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING)) { } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING)) {
// Allow landing to continue when FS_OPTIONS is set to continue landing // Allow landing to continue when FS_OPTIONS is set to continue landing
gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Landing"); gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Landing");
desired_action = Failsafe_Action_Land; desired_action = FailsafeAction::LAND;
} else if (flightmode->mode_number() == Mode::Number::AUTO && failsafe_option(FailsafeOption::GCS_CONTINUE_IF_AUTO)) { } else if (flightmode->mode_number() == Mode::Number::AUTO && failsafe_option(FailsafeOption::GCS_CONTINUE_IF_AUTO)) {
// Allow mission to continue when FS_OPTIONS is set to continue mission // Allow mission to continue when FS_OPTIONS is set to continue mission
gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Auto Mode"); gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Auto Mode");
desired_action = Failsafe_Action_None; desired_action = FailsafeAction::NONE;
} else if (failsafe_option(FailsafeOption::GCS_CONTINUE_IF_PILOT_CONTROL) && !flightmode->is_autopilot()) { } else if (failsafe_option(FailsafeOption::GCS_CONTINUE_IF_PILOT_CONTROL) && !flightmode->is_autopilot()) {
// should continue when in a pilot controlled mode because FS_OPTIONS is set to continue in pilot controlled modes // should continue when in a pilot controlled mode because FS_OPTIONS is set to continue in pilot controlled modes
gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Pilot Control"); gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Pilot Control");
desired_action = Failsafe_Action_None; desired_action = FailsafeAction::NONE;
} else { } else {
gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe"); gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe");
} }
@ -379,25 +379,25 @@ bool Copter::should_disarm_on_failsafe() {
} }
void Copter::do_failsafe_action(Failsafe_Action action, ModeReason reason){ void Copter::do_failsafe_action(FailsafeAction action, ModeReason reason){
// Execute the specified desired_action // Execute the specified desired_action
switch (action) { switch (action) {
case Failsafe_Action_None: case FailsafeAction::NONE:
return; return;
case Failsafe_Action_Land: case FailsafeAction::LAND:
set_mode_land_with_pause(reason); set_mode_land_with_pause(reason);
break; break;
case Failsafe_Action_RTL: case FailsafeAction::RTL:
set_mode_RTL_or_land_with_pause(reason); set_mode_RTL_or_land_with_pause(reason);
break; break;
case Failsafe_Action_SmartRTL: case FailsafeAction::SMARTRTL:
set_mode_SmartRTL_or_RTL(reason); set_mode_SmartRTL_or_RTL(reason);
break; break;
case Failsafe_Action_SmartRTL_Land: case FailsafeAction::SMARTRTL_LAND:
set_mode_SmartRTL_or_land_with_pause(reason); set_mode_SmartRTL_or_land_with_pause(reason);
break; break;
case Failsafe_Action_Terminate: { case FailsafeAction::TERMINATE: {
#if ADVANCED_FAILSAFE == ENABLED #if ADVANCED_FAILSAFE == ENABLED
g2.afs.gcs_terminate(true, "Failsafe"); g2.afs.gcs_terminate(true, "Failsafe");
#else #else
@ -405,7 +405,7 @@ void Copter::do_failsafe_action(Failsafe_Action action, ModeReason reason){
#endif #endif
break; break;
} }
case Failsafe_Action_Auto_DO_LAND_START: case FailsafeAction::AUTO_DO_LAND_START:
set_mode_auto_do_land_start_or_RTL(reason); set_mode_auto_do_land_start_or_RTL(reason);
break; break;
} }