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,
};
enum Failsafe_Action {
Failsafe_Action_None = 0,
Failsafe_Action_Land = 1,
Failsafe_Action_RTL = 2,
Failsafe_Action_SmartRTL = 3,
Failsafe_Action_SmartRTL_Land = 4,
Failsafe_Action_Terminate = 5,
Failsafe_Action_Auto_DO_LAND_START = 6,
enum class FailsafeAction : uint8_t {
NONE = 0,
LAND = 1,
RTL = 2,
SMARTRTL = 3,
SMARTRTL_LAND = 4,
TERMINATE = 5,
AUTO_DO_LAND_START = 6
};
enum class FailsafeOption {
@ -616,17 +616,17 @@ private:
};
static constexpr int8_t _failsafe_priorities[] = {
Failsafe_Action_Terminate,
Failsafe_Action_Land,
Failsafe_Action_RTL,
Failsafe_Action_SmartRTL_Land,
Failsafe_Action_SmartRTL,
Failsafe_Action_None,
(int8_t)FailsafeAction::TERMINATE,
(int8_t)FailsafeAction::LAND,
(int8_t)FailsafeAction::RTL,
(int8_t)FailsafeAction::SMARTRTL_LAND,
(int8_t)FailsafeAction::SMARTRTL,
(int8_t)FailsafeAction::NONE,
-1 // the priority list must end with a sentinel of -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");
static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
"_failsafe_priorities is missing the sentinel");
@ -743,7 +743,7 @@ private:
void set_mode_SmartRTL_or_land_with_pause(ModeReason reason);
void set_mode_auto_do_land_start_or_RTL(ModeReason reason);
bool should_disarm_on_failsafe();
void do_failsafe_action(Failsafe_Action action, ModeReason reason);
void do_failsafe_action(FailsafeAction action, ModeReason reason);
// failsafe.cpp
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);
// set desired action based on FS_THR_ENABLE parameter
Failsafe_Action desired_action;
FailsafeAction desired_action;
switch (g.failsafe_throttle) {
case FS_THR_DISABLED:
desired_action = Failsafe_Action_None;
desired_action = FailsafeAction::NONE;
break;
case FS_THR_ENABLED_ALWAYS_RTL:
case FS_THR_ENABLED_CONTINUE_MISSION:
desired_action = Failsafe_Action_RTL;
desired_action = FailsafeAction::RTL;
break;
case FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL:
desired_action = Failsafe_Action_SmartRTL;
desired_action = FailsafeAction::SMARTRTL;
break;
case FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND:
desired_action = Failsafe_Action_SmartRTL_Land;
desired_action = FailsafeAction::SMARTRTL_LAND;
break;
case FS_THR_ENABLED_ALWAYS_LAND:
desired_action = Failsafe_Action_Land;
desired_action = FailsafeAction::LAND;
break;
case FS_THR_ENABLED_AUTO_RTL_OR_RTL:
desired_action = Failsafe_Action_Auto_DO_LAND_START;
desired_action = FailsafeAction::AUTO_DO_LAND_START;
break;
default:
desired_action = Failsafe_Action_Land;
desired_action = FailsafeAction::LAND;
}
// 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
gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Disarming");
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))) {
// Allow landing to continue when battery failsafe requires it (not a user option)
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)) {
// Allow landing to continue when FS_OPTIONS is set to continue 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)) {
// Allow mission to continue when FS_OPTIONS is set to continue mission
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()) &&
(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.
gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Guided Mode");
desired_action = Failsafe_Action_None;
desired_action = FailsafeAction::NONE;
} else {
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);
Failsafe_Action desired_action = (Failsafe_Action)action;
FailsafeAction desired_action = (FailsafeAction)action;
// Conditions to deviate from BATT_FS_XXX_ACT parameter setting
if (should_disarm_on_failsafe()) {
// should immediately disarm when we're on the ground
arming.disarm(AP_Arming::Method::BATTERYFAILSAFE);
desired_action = Failsafe_Action_None;
desired_action = FailsafeAction::NONE;
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
desired_action = Failsafe_Action_Land;
desired_action = FailsafeAction::LAND;
gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Continuing Landing");
} else {
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);
RC_Channels::clear_overrides();
// convert the desired failsafe response to the Failsafe_Action enum
Failsafe_Action desired_action;
// convert the desired failsafe response to the FailsafeAction enum
FailsafeAction desired_action;
switch (g.failsafe_gcs) {
case FS_GCS_DISABLED:
desired_action = Failsafe_Action_None;
desired_action = FailsafeAction::NONE;
break;
case FS_GCS_ENABLED_ALWAYS_RTL:
case FS_GCS_ENABLED_CONTINUE_MISSION:
desired_action = Failsafe_Action_RTL;
desired_action = FailsafeAction::RTL;
break;
case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL:
desired_action = Failsafe_Action_SmartRTL;
desired_action = FailsafeAction::SMARTRTL;
break;
case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND:
desired_action = Failsafe_Action_SmartRTL_Land;
desired_action = FailsafeAction::SMARTRTL_LAND;
break;
case FS_GCS_ENABLED_ALWAYS_LAND:
desired_action = Failsafe_Action_Land;
desired_action = FailsafeAction::LAND;
break;
case FS_GCS_ENABLED_AUTO_RTL_OR_RTL:
desired_action = Failsafe_Action_Auto_DO_LAND_START;
desired_action = FailsafeAction::AUTO_DO_LAND_START;
break;
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
if (!motors->armed()) {
desired_action = Failsafe_Action_None;
desired_action = FailsafeAction::NONE;
gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe");
} else if (should_disarm_on_failsafe()) {
// should immediately disarm when we're on the ground
arming.disarm(AP_Arming::Method::GCSFAILSAFE);
desired_action = Failsafe_Action_None;
desired_action = FailsafeAction::NONE;
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))) {
// Allow landing to continue when battery failsafe requires it (not a user option)
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)) {
// Allow landing to continue when FS_OPTIONS is set to continue 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)) {
// Allow mission to continue when FS_OPTIONS is set to continue mission
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()) {
// 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");
desired_action = Failsafe_Action_None;
desired_action = FailsafeAction::NONE;
} else {
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
switch (action) {
case Failsafe_Action_None:
case FailsafeAction::NONE:
return;
case Failsafe_Action_Land:
case FailsafeAction::LAND:
set_mode_land_with_pause(reason);
break;
case Failsafe_Action_RTL:
case FailsafeAction::RTL:
set_mode_RTL_or_land_with_pause(reason);
break;
case Failsafe_Action_SmartRTL:
case FailsafeAction::SMARTRTL:
set_mode_SmartRTL_or_RTL(reason);
break;
case Failsafe_Action_SmartRTL_Land:
case FailsafeAction::SMARTRTL_LAND:
set_mode_SmartRTL_or_land_with_pause(reason);
break;
case Failsafe_Action_Terminate: {
case FailsafeAction::TERMINATE: {
#if ADVANCED_FAILSAFE == ENABLED
g2.afs.gcs_terminate(true, "Failsafe");
#else
@ -405,7 +405,7 @@ void Copter::do_failsafe_action(Failsafe_Action action, ModeReason reason){
#endif
break;
}
case Failsafe_Action_Auto_DO_LAND_START:
case FailsafeAction::AUTO_DO_LAND_START:
set_mode_auto_do_land_start_or_RTL(reason);
break;
}