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 43da68ae59
commit f71ef4c7ab
5 changed files with 254 additions and 106 deletions

View File

@ -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();

View File

@ -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);
}

View File

@ -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[];

View File

@ -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

View File

@ -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
}
}