mirror of https://github.com/ArduPilot/ardupilot
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
Randy Mackay
parent
c11a6030d3
commit
9ba4941aa7
|
|
@ -616,6 +616,14 @@ private:
|
|||
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[] = {
|
||||
Failsafe_Action_Terminate,
|
||||
Failsafe_Action_Land,
|
||||
|
|
@ -710,10 +718,12 @@ private:
|
|||
void esc_calibration_setup();
|
||||
|
||||
// events.cpp
|
||||
bool failsafe_option(FailsafeOption opt) const;
|
||||
void failsafe_radio_on_event();
|
||||
void failsafe_radio_off_event();
|
||||
void handle_battery_failsafe(const char* type_str, const int8_t action);
|
||||
void failsafe_gcs_check();
|
||||
void failsafe_gcs_on_event(void);
|
||||
void failsafe_gcs_off_event(void);
|
||||
void failsafe_terrain_check();
|
||||
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_land_with_pause(ModeReason reason);
|
||||
bool should_disarm_on_failsafe();
|
||||
void do_failsafe_action(Failsafe_Action action, ModeReason reason);
|
||||
|
||||
// failsafe.cpp
|
||||
void failsafe_enable();
|
||||
|
|
@ -813,6 +824,7 @@ private:
|
|||
void convert_pid_parameters(void);
|
||||
void convert_lgr_parameters(void);
|
||||
void convert_tradheli_parameters(void);
|
||||
void convert_fs_options_params(void);
|
||||
|
||||
// precision_landing.cpp
|
||||
void init_precland();
|
||||
|
|
|
|||
|
|
@ -173,7 +173,7 @@ const AP_Param::Info Copter::var_info[] = {
|
|||
// @Param: FS_GCS_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.
|
||||
// @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
|
||||
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
|
||||
// @DisplayName: Throttle Failsafe Enable
|
||||
// @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
|
||||
GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL),
|
||||
|
||||
|
|
@ -939,7 +939,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
// @Path: mode_systemid.cpp
|
||||
AP_SUBGROUPPTR(mode_systemid_ptr, "SID", 34, ParametersG2, ModeSystemId),
|
||||
#endif
|
||||
|
||||
|
||||
// @Param: FS_VIBE_ENABLE
|
||||
// @DisplayName: Vibration Failsafe enable
|
||||
// @Description: This enables the vibration failsafe which will use modified altitude estimation and control during high vibrations
|
||||
|
|
@ -947,6 +947,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
// @User: Standard
|
||||
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
|
||||
};
|
||||
|
||||
|
|
@ -1103,6 +1111,9 @@ void Copter::load_parameters(void)
|
|||
// convert landing gear parameters
|
||||
convert_lgr_parameters();
|
||||
|
||||
// convert fs_options parameters
|
||||
convert_fs_options_params();
|
||||
|
||||
hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before));
|
||||
|
||||
// setup AP_Param frame type flags
|
||||
|
|
@ -1528,3 +1539,33 @@ void Copter::convert_tradheli_parameters(void)
|
|||
|
||||
}
|
||||
#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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -612,6 +612,9 @@ public:
|
|||
|
||||
// vibration failsafe enable/disable
|
||||
AP_Int8 fs_vibe_enabled;
|
||||
|
||||
// Failsafe options bitmask #36
|
||||
AP_Int32 fs_options;
|
||||
};
|
||||
|
||||
extern const AP_Param::Info var_info[];
|
||||
|
|
|
|||
|
|
@ -204,7 +204,7 @@ enum HarmonicNotchDynamicMode {
|
|||
// Radio failsafe definitions (FS_THR parameter)
|
||||
#define FS_THR_DISABLED 0
|
||||
#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_SMARTRTL_OR_RTL 4
|
||||
#define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND 5
|
||||
|
|
@ -212,9 +212,10 @@ enum HarmonicNotchDynamicMode {
|
|||
// GCS failsafe definitions (FS_GCS_ENABLE parameter)
|
||||
#define FS_GCS_DISABLED 0
|
||||
#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_LAND 4
|
||||
#define FS_GCS_ENABLED_ALWAYS_LAND 5
|
||||
|
||||
// EKF failsafe definitions (FS_EKF_ACTION parameter)
|
||||
#define FS_EKF_ACTION_LAND 1 // switch to LAND mode on EKF failsafe
|
||||
|
|
|
|||
|
|
@ -4,104 +4,115 @@
|
|||
* This event will be called when the failsafe changes
|
||||
* 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()
|
||||
{
|
||||
// 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);
|
||||
|
||||
// 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
|
||||
// we must be in AUTO, LAND or RTL modes
|
||||
// or Stabilize or ACRO mode but with motors disarmed
|
||||
void Copter::failsafe_radio_off_event()
|
||||
{
|
||||
// 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
|
||||
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)
|
||||
{
|
||||
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()) {
|
||||
// should immediately disarm when we're on the ground
|
||||
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 {
|
||||
switch ((Failsafe_Action)action) {
|
||||
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
|
||||
}
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe");
|
||||
}
|
||||
|
||||
// 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
|
||||
void Copter::failsafe_gcs_check()
|
||||
{
|
||||
if (failsafe.gcs) {
|
||||
// we must run the failsafe checks if we are in failsafe -
|
||||
// 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 {
|
||||
// Bypass GCS failsafe checks if disabled or GCS never connected
|
||||
if (g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0) {
|
||||
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
|
||||
const uint32_t last_gcs_update_ms = millis() - failsafe.last_heartbeat_ms;
|
||||
|
||||
// check if all is well
|
||||
if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS) {
|
||||
// check for recovery from gcs failsafe
|
||||
if (failsafe.gcs) {
|
||||
failsafe_gcs_off_event();
|
||||
set_failsafe_gcs(false);
|
||||
}
|
||||
return;
|
||||
}
|
||||
// Determine which event to trigger
|
||||
if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS && failsafe.gcs) {
|
||||
// Recovery from a GCS failsafe
|
||||
set_failsafe_gcs(false);
|
||||
failsafe_gcs_off_event();
|
||||
|
||||
// do nothing if gcs failsafe already triggered or motors disarmed
|
||||
if (failsafe.gcs || !motors->armed()) {
|
||||
return;
|
||||
}
|
||||
} else if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS && !failsafe.gcs) {
|
||||
// No problem, do nothing
|
||||
|
||||
// GCS failsafe event has occurred
|
||||
set_failsafe_gcs(true);
|
||||
} else if (last_gcs_update_ms > FS_GCS_TIMEOUT_MS && failsafe.gcs) {
|
||||
// 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);
|
||||
|
||||
// clear overrides so that RC control can be regained with radio.
|
||||
RC_Channels::clear_overrides();
|
||||
|
||||
if (should_disarm_on_failsafe()) {
|
||||
arming.disarm();
|
||||
} else {
|
||||
if (control_mode == Mode::Number::AUTO &&
|
||||
g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) {
|
||||
// continue mission
|
||||
} else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL) {
|
||||
set_mode_SmartRTL_or_RTL(ModeReason::GCS_FAILSAFE);
|
||||
} else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND) {
|
||||
set_mode_SmartRTL_or_land_with_pause(ModeReason::GCS_FAILSAFE);
|
||||
} else { // g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL
|
||||
set_mode_RTL_or_land_with_pause(ModeReason::GCS_FAILSAFE);
|
||||
}
|
||||
// convert the desired failsafe response to the Failsafe_Action enum
|
||||
Failsafe_Action desired_action;
|
||||
switch (g.failsafe_gcs) {
|
||||
case FS_GCS_DISABLED:
|
||||
desired_action = Failsafe_Action_None;
|
||||
break;
|
||||
case FS_GCS_ENABLED_ALWAYS_RTL:
|
||||
case FS_GCS_ENABLED_CONTINUE_MISSION:
|
||||
desired_action = Failsafe_Action_RTL;
|
||||
break;
|
||||
case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL:
|
||||
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
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -294,3 +354,34 @@ bool Copter::should_disarm_on_failsafe() {
|
|||
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
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue