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

Copter: Refactor failsafes, add fs_options bitmask parameter 

- Radio failsafe, battery failsafe, GCS failsafe refactoring
- Add new FS_OPTIONS parameter
- Enhance GCS Failsafe abilities
This commit is contained in:
Matt Lawrence 2019-09-21 20:09:18 -04:00 committed by Randy Mackay
parent c11a6030d3
commit 9ba4941aa7
5 changed files with 254 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopter/Copter.h
View File

@ -616,6 +616,14 @@ private:
Failsafe_Action_Terminate = 5 Failsafe_Action_Terminate = 5
}; };
enum class FailsafeOption {
RC_CONTINUE_IF_AUTO = (1<<0), // 1
GCS_CONTINUE_IF_AUTO = (1<<1), // 2
RC_CONTINUE_IF_GUIDED = (1<<2), // 4
CONTINUE_IF_LANDING = (1<<3), // 8
GCS_CONTINUE_IF_PILOT_CONTROL = (1<<4), // 16
};
static constexpr int8_t _failsafe_priorities[] = { static constexpr int8_t _failsafe_priorities[] = {
Failsafe_Action_Terminate, Failsafe_Action_Terminate,
Failsafe_Action_Land, Failsafe_Action_Land,
@ -710,10 +718,12 @@ private:
void esc_calibration_setup(); void esc_calibration_setup();
// events.cpp // events.cpp
bool failsafe_option(FailsafeOption opt) const;
void failsafe_radio_on_event(); void failsafe_radio_on_event();
void failsafe_radio_off_event(); void failsafe_radio_off_event();
void handle_battery_failsafe(const char* type_str, const int8_t action); void handle_battery_failsafe(const char* type_str, const int8_t action);
void failsafe_gcs_check(); void failsafe_gcs_check();
void failsafe_gcs_on_event(void);
void failsafe_gcs_off_event(void); void failsafe_gcs_off_event(void);
void failsafe_terrain_check(); void failsafe_terrain_check();
void failsafe_terrain_set_status(bool data_ok); void failsafe_terrain_set_status(bool data_ok);
@ -723,6 +733,7 @@ private:
void set_mode_SmartRTL_or_RTL(ModeReason reason); void set_mode_SmartRTL_or_RTL(ModeReason reason);
void set_mode_SmartRTL_or_land_with_pause(ModeReason reason); void set_mode_SmartRTL_or_land_with_pause(ModeReason reason);
bool should_disarm_on_failsafe(); bool should_disarm_on_failsafe();
void do_failsafe_action(Failsafe_Action action, ModeReason reason);
// failsafe.cpp // failsafe.cpp
void failsafe_enable(); void failsafe_enable();
@ -813,6 +824,7 @@ private:
void convert_pid_parameters(void); void convert_pid_parameters(void);
void convert_lgr_parameters(void); void convert_lgr_parameters(void);
void convert_tradheli_parameters(void); void convert_tradheli_parameters(void);
void convert_fs_options_params(void);
// precision_landing.cpp // precision_landing.cpp
void init_precland(); void init_precland();

45
ArduCopter/Parameters.cpp
View File

@ -173,7 +173,7 @@ const AP_Param::Info Copter::var_info[] = {
// @Param: FS_GCS_ENABLE // @Param: FS_GCS_ENABLE
// @DisplayName: Ground Station Failsafe Enable // @DisplayName: Ground Station Failsafe Enable
// @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. NB. The GCS Failsafe is only active when RC_OVERRIDE is being used to control the vehicle. // @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. NB. The GCS Failsafe is only active when RC_OVERRIDE is being used to control the vehicle.
// @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode,3:Enabled always SmartRTL or RTL,4:Enabled always SmartRTL or Land // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Deprecated in 4.0+),3:Enabled always SmartRTL or RTL,4:Enabled always SmartRTL or Land,5:Enabled always land (4.0+ Only)
// @User: Standard // @User: Standard
GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_ENABLED_ALWAYS_RTL), GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_ENABLED_ALWAYS_RTL),
@ -237,7 +237,7 @@ const AP_Param::Info Copter::var_info[] = {
// @Param: FS_THR_ENABLE // @Param: FS_THR_ENABLE
// @DisplayName: Throttle Failsafe Enable // @DisplayName: Throttle Failsafe Enable
// @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel
// @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode,3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Deprecated in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land
// @User: Standard // @User: Standard
GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL), GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL),
@ -947,6 +947,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Standard // @User: Standard
AP_GROUPINFO("FS_VIBE_ENABLE", 35, ParametersG2, fs_vibe_enabled, 1), AP_GROUPINFO("FS_VIBE_ENABLE", 35, ParametersG2, fs_vibe_enabled, 1),
// @Param: FS_OPTIONS
// @DisplayName: Failsafe options bitmask
// @Description: Bitmask of additional options for battery, radio, & GCS failsafes. 0 (default) disables all options.
// @Values: 0:Disabled, 1:Continue if in Auto on RC failsafe only, 2:Continue if in Auto on GCS failsafe only, 3:Continue if in Auto on RC and/or GCS failsafe, 4:Continue if in Guided on RC failsafe only, 8:Continue if landing on any failsafe, 16:Continue if in pilot controlled modes on GCS failsafe, 19:Continue if in Auto on RC and/or GCS failsafe and continue if in pilot controlled modes on GCS failsafe
// @Bitmask: 0:Continue if in Auto on RC failsafe, 1:Continue if in Auto on GCS failsafe, 2:Continue if in Guided on RC failsafe, 3:Continue if landing on any failsafe, 4:Continue if in pilot controlled modes on GCS failsafe
// @User: Advanced
AP_GROUPINFO("FS_OPTIONS", 36, ParametersG2, fs_options, 0),
AP_GROUPEND AP_GROUPEND
}; };
@ -1103,6 +1111,9 @@ void Copter::load_parameters(void)
// convert landing gear parameters // convert landing gear parameters
convert_lgr_parameters(); convert_lgr_parameters();
// convert fs_options parameters
convert_fs_options_params();
hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before)); hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before));
// setup AP_Param frame type flags // setup AP_Param frame type flags
@ -1528,3 +1539,33 @@ void Copter::convert_tradheli_parameters(void)
} }
#endif #endif
void Copter::convert_fs_options_params(void)
{
// If FS_OPTIONS has already been configured and we don't change it.
enum ap_var_type ptype;
AP_Int32 *fs_opt = (AP_Int32 *)AP_Param::find("FS_OPTIONS", &ptype);
if (fs_opt == nullptr || fs_opt->configured_in_storage() || ptype != AP_PARAM_INT32) {
return;
}
// Variable to capture the new FS_OPTIONS setting
int32_t fs_options_converted = 0;
// If FS_THR_ENABLED is 2 (continue mission), change to RTL and add continue mission to the new FS_OPTIONS parameter
if (g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
fs_options_converted |= int32_t(FailsafeOption::RC_CONTINUE_IF_AUTO);
AP_Param::set_and_save_by_name("FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL);
}
// If FS_GCS_ENABLED is 2 (continue mission), change to RTL and add continue mission to the new FS_OPTIONS parameter
if (g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) {
fs_options_converted |= int32_t(FailsafeOption::GCS_CONTINUE_IF_AUTO);
AP_Param::set_and_save_by_name("FS_GCS_ENABLE", FS_GCS_ENABLED_ALWAYS_RTL);
}
// Write the new value to FS_OPTIONS
// AP_Param::set_and_save_by_name("FS_OPTIONS", fs_options_converted);
fs_opt->set_and_save(fs_options_converted);
}

3
ArduCopter/Parameters.h
View File

@ -612,6 +612,9 @@ public:
// vibration failsafe enable/disable // vibration failsafe enable/disable
AP_Int8 fs_vibe_enabled; AP_Int8 fs_vibe_enabled;
// Failsafe options bitmask #36
AP_Int32 fs_options;
}; };
extern const AP_Param::Info var_info[]; extern const AP_Param::Info var_info[];

5
ArduCopter/defines.h
View File

@ -204,7 +204,7 @@ enum HarmonicNotchDynamicMode {
// Radio failsafe definitions (FS_THR parameter) // Radio failsafe definitions (FS_THR parameter)
#define FS_THR_DISABLED 0 #define FS_THR_DISABLED 0
#define FS_THR_ENABLED_ALWAYS_RTL 1 #define FS_THR_ENABLED_ALWAYS_RTL 1
#define FS_THR_ENABLED_CONTINUE_MISSION 2 #define FS_THR_ENABLED_CONTINUE_MISSION 2 // Deprecated in 4.0+, now use fs_options
#define FS_THR_ENABLED_ALWAYS_LAND 3 #define FS_THR_ENABLED_ALWAYS_LAND 3
#define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL 4 #define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL 4
#define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND 5 #define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND 5
@ -212,9 +212,10 @@ enum HarmonicNotchDynamicMode {
// GCS failsafe definitions (FS_GCS_ENABLE parameter) // GCS failsafe definitions (FS_GCS_ENABLE parameter)
#define FS_GCS_DISABLED 0 #define FS_GCS_DISABLED 0
#define FS_GCS_ENABLED_ALWAYS_RTL 1 #define FS_GCS_ENABLED_ALWAYS_RTL 1
#define FS_GCS_ENABLED_CONTINUE_MISSION 2 #define FS_GCS_ENABLED_CONTINUE_MISSION 2 // Deprecated in 4.0+, now use fs_options
#define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL 3 #define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL 3
#define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND 4 #define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND 4
#define FS_GCS_ENABLED_ALWAYS_LAND 5
// EKF failsafe definitions (FS_EKF_ACTION parameter) // EKF failsafe definitions (FS_EKF_ACTION parameter)
#define FS_EKF_ACTION_LAND 1 // switch to LAND mode on EKF failsafe #define FS_EKF_ACTION_LAND 1 // switch to LAND mode on EKF failsafe

293
ArduCopter/events.cpp
View File

@ -4,104 +4,115 @@
* This event will be called when the failsafe changes * This event will be called when the failsafe changes
* boolean failsafe reflects the current state * boolean failsafe reflects the current state
*/ */
bool Copter::failsafe_option(FailsafeOption opt) const
{
return (g2.fs_options & (uint32_t)opt);
}
void Copter::failsafe_radio_on_event() void Copter::failsafe_radio_on_event()
{ {
// if motors are not armed there is nothing to do
if( !motors->armed() ) {
return;
}
if (should_disarm_on_failsafe()) {
arming.disarm();
} else {
if (control_mode == Mode::Number::AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
// continue mission
} else if (flightmode->is_landing() &&
battery.has_failsafed() &&
battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY) {
// continue landing or other high priority failsafes
} else {
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
set_mode_RTL_or_land_with_pause(ModeReason::RADIO_FAILSAFE);
} else if (g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
set_mode_RTL_or_land_with_pause(ModeReason::RADIO_FAILSAFE);
} else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL) {
set_mode_SmartRTL_or_RTL(ModeReason::RADIO_FAILSAFE);
} else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND) {
set_mode_SmartRTL_or_land_with_pause(ModeReason::RADIO_FAILSAFE);
} else { // g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND
set_mode_land_with_pause(ModeReason::RADIO_FAILSAFE);
}
}
}
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
Failsafe_Action desired_action;
switch (g.failsafe_throttle) {
case FS_THR_DISABLED:
desired_action = Failsafe_Action_None;
break;
case FS_THR_ENABLED_ALWAYS_RTL:
case FS_THR_ENABLED_CONTINUE_MISSION:
desired_action = Failsafe_Action_RTL;
break;
case FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL:
desired_action = Failsafe_Action_SmartRTL;
break;
case FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND:
desired_action = Failsafe_Action_SmartRTL_Land;
break;
case FS_THR_ENABLED_ALWAYS_LAND:
desired_action = Failsafe_Action_Land;
break;
default:
desired_action = Failsafe_Action_Land;
}
// Conditions to deviate from FS_THR_ENABLE selection and send specific GCS warning
if (should_disarm_on_failsafe()) {
// should immediately disarm when we're on the ground
gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Disarming");
arming.disarm();
desired_action = Failsafe_Action_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;
} 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;
} else if (control_mode == 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;
} 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;
} else {
gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe");
}
// Call the failsafe action handler
do_failsafe_action(desired_action, ModeReason::RADIO_FAILSAFE);
} }
// failsafe_off_event - respond to radio contact being regained // failsafe_off_event - respond to radio contact being regained
// we must be in AUTO, LAND or RTL modes
// or Stabilize or ACRO mode but with motors disarmed
void Copter::failsafe_radio_off_event() void Copter::failsafe_radio_off_event()
{ {
// no need to do anything except log the error as resolved // no need to do anything except log the error as resolved
// user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode // user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_RESOLVED); AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_RESOLVED);
gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe Cleared");
} }
void Copter::handle_battery_failsafe(const char *type_str, const int8_t action) 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 check Failsafe_Action desired_action = (Failsafe_Action)action;
// 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
arming.disarm(); arming.disarm();
desired_action = Failsafe_Action_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) {
// Allow landing to continue when FS_OPTIONS is set to continue when landing
desired_action = Failsafe_Action_Land;
gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Continuing Landing");
} else { } else {
switch ((Failsafe_Action)action) { gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe");
case Failsafe_Action_None:
return;
case Failsafe_Action_Land:
set_mode_land_with_pause(ModeReason::BATTERY_FAILSAFE);
break;
case Failsafe_Action_RTL:
set_mode_RTL_or_land_with_pause(ModeReason::BATTERY_FAILSAFE);
break;
case Failsafe_Action_SmartRTL:
set_mode_SmartRTL_or_RTL(ModeReason::BATTERY_FAILSAFE);
break;
case Failsafe_Action_SmartRTL_Land:
set_mode_SmartRTL_or_land_with_pause(ModeReason::BATTERY_FAILSAFE);
break;
case Failsafe_Action_Terminate:
#if ADVANCED_FAILSAFE == ENABLED
char battery_type_str[17];
snprintf(battery_type_str, 17, "%s battery", type_str);
g2.afs.gcs_terminate(true, battery_type_str);
#else
arming.disarm();
#endif
}
} }
// Battery FS options already use the Failsafe_Options enum. So use them directly.
do_failsafe_action(desired_action, ModeReason::BATTERY_FAILSAFE);
} }
// failsafe_gcs_check - check for ground station failsafe // failsafe_gcs_check - check for ground station failsafe
void Copter::failsafe_gcs_check() void Copter::failsafe_gcs_check()
{ {
if (failsafe.gcs) { // Bypass GCS failsafe checks if disabled or GCS never connected
// we must run the failsafe checks if we are in failsafe - if (g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0) {
// otherwise we will never leave failsafe
} else if (g.failsafe_gcs == FS_GCS_DISABLED) {
// simply disabled
return;
} else if (failsafe.last_heartbeat_ms == 0) {
// GCS has never connected
return;
} else if (RC_Channels::has_active_overrides()) {
// GCS is currently telling us what to do!
} else if (control_mode == Mode::Number::GUIDED ||
control_mode == Mode::Number::GUIDED_NOGPS) {
// GCS is currently telling us what to do!
} else {
return; return;
} }
@ -109,47 +120,96 @@ void Copter::failsafe_gcs_check()
// note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs // note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs
const uint32_t last_gcs_update_ms = millis() - failsafe.last_heartbeat_ms; const uint32_t last_gcs_update_ms = millis() - failsafe.last_heartbeat_ms;
// check if all is well // Determine which event to trigger
if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS) { if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS && failsafe.gcs) {
// check for recovery from gcs failsafe // Recovery from a GCS failsafe
if (failsafe.gcs) { set_failsafe_gcs(false);
failsafe_gcs_off_event(); failsafe_gcs_off_event();
set_failsafe_gcs(false);
}
return;
}
// do nothing if gcs failsafe already triggered or motors disarmed } else if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS && !failsafe.gcs) {
if (failsafe.gcs || !motors->armed()) { // No problem, do nothing
return;
}
// GCS failsafe event has occurred } else if (last_gcs_update_ms > FS_GCS_TIMEOUT_MS && failsafe.gcs) {
set_failsafe_gcs(true); // Already in failsafe, do nothing
} else if (last_gcs_update_ms > FS_GCS_TIMEOUT_MS && !failsafe.gcs) {
// New GCS failsafe event, trigger events
set_failsafe_gcs(true);
failsafe_gcs_on_event();
}
}
// failsafe_gcs_on_event - actions to take when GCS contact is lost
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);
// clear overrides so that RC control can be regained with radio.
RC_Channels::clear_overrides(); RC_Channels::clear_overrides();
if (should_disarm_on_failsafe()) { // convert the desired failsafe response to the Failsafe_Action enum
arming.disarm(); Failsafe_Action desired_action;
} else { switch (g.failsafe_gcs) {
if (control_mode == Mode::Number::AUTO && case FS_GCS_DISABLED:
g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) { desired_action = Failsafe_Action_None;
// continue mission break;
} else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL) { case FS_GCS_ENABLED_ALWAYS_RTL:
set_mode_SmartRTL_or_RTL(ModeReason::GCS_FAILSAFE); case FS_GCS_ENABLED_CONTINUE_MISSION:
} else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND) { desired_action = Failsafe_Action_RTL;
set_mode_SmartRTL_or_land_with_pause(ModeReason::GCS_FAILSAFE); break;
} else { // g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL:
set_mode_RTL_or_land_with_pause(ModeReason::GCS_FAILSAFE); desired_action = Failsafe_Action_SmartRTL;
} break;
case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND:
desired_action = Failsafe_Action_SmartRTL_Land;
break;
case FS_GCS_ENABLED_ALWAYS_LAND:
desired_action = Failsafe_Action_Land;
break;
default: // if an invalid parameter value is set, the fallback is RTL
desired_action = Failsafe_Action_RTL;
} }
// Conditions to deviate from FS_GCS_ENABLE parameter setting
if (!motors->armed()) {
desired_action = Failsafe_Action_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();
desired_action = Failsafe_Action_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;
} 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;
} else if (control_mode == 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;
} 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;
} else {
gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe");
}
// Call the failsafe action handler
do_failsafe_action(desired_action, ModeReason::GCS_FAILSAFE);
} }
// failsafe_gcs_off_event - actions to take when GCS contact is restored // failsafe_gcs_off_event - actions to take when GCS contact is restored
void Copter::failsafe_gcs_off_event(void) void Copter::failsafe_gcs_off_event(void)
{ {
gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe Cleared");
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED); AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED);
} }
@ -294,3 +354,34 @@ bool Copter::should_disarm_on_failsafe() {
return ap.land_complete; return ap.land_complete;
} }
} }
void Copter::do_failsafe_action(Failsafe_Action action, ModeReason reason){
// Execute the specified desired_action
switch (action) {
case Failsafe_Action_None:
return;
case Failsafe_Action_Land:
set_mode_land_with_pause(reason);
break;
case Failsafe_Action_RTL:
set_mode_RTL_or_land_with_pause(reason);
break;
case Failsafe_Action_SmartRTL:
set_mode_SmartRTL_or_RTL(reason);
break;
case Failsafe_Action_SmartRTL_Land:
set_mode_SmartRTL_or_land_with_pause(reason);
break;
case Failsafe_Action_Terminate:
#if ADVANCED_FAILSAFE == ENABLED
char battery_type_str[17];
snprintf(battery_type_str, 17, "%s battery", type_str);
g2.afs.gcs_terminate(true, battery_type_str);
#else
arming.disarm();
#endif
}
}