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

Copter: add SmartRTL failsafe action 

Adds SmartRTL or RTL and SmartRTL or Land failsafe options for batt, throttle, and GCS failsafes
This commit is contained in:
Dylan Herman 2018-01-19 08:44:02 -05:00 committed by Randy Mackay
parent c14af79975
commit 20fe5bb98f
4 changed files with 63 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -698,6 +698,8 @@ private:
void failsafe_terrain_on_event();
void gpsglitch_check();
void set_mode_RTL_or_land_with_pause(mode_reason_t reason);
void set_mode_SmartRTL_or_RTL(mode_reason_t reason);
void set_mode_SmartRTL_or_land_with_pause(mode_reason_t reason);
bool should_disarm_on_failsafe();
void update_events();

6
ArduCopter/Parameters.cpp
View File

@ -151,7 +151,7 @@ const AP_Param::Info Copter::var_info[] = {
// @Param: FS_BATT_ENABLE
// @DisplayName: Battery Failsafe Enable
// @Description: Controls whether failsafe will be invoked when battery voltage or current runs low
// @Values: 0:Disabled,1:Land,2:RTL
// @Values: 0:Disabled,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land
// @User: Standard
GSCALAR(failsafe_battery_enabled, "FS_BATT_ENABLE", FS_BATT_DISABLED),
@ -174,7 +174,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
// @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
// @User: Standard
GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_ENABLED_ALWAYS_RTL),
@ -272,7 +272,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
// @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
// @User: Standard
GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL),

20
ArduCopter/defines.h
View File

@ -474,20 +474,26 @@ enum DevOptions {
#define ERROR_CODE_GPS_GLITCH 2
// 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_ALWAYS_LAND 3
#define FS_THR_DISABLED 0
#define FS_THR_ENABLED_ALWAYS_RTL 1
#define FS_THR_ENABLED_CONTINUE_MISSION 2
#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
// Battery failsafe definitions (FS_BATT_ENABLE parameter)
#define FS_BATT_DISABLED 0 // battery failsafe disabled
#define FS_BATT_LAND 1 // switch to LAND mode on battery failsafe
#define FS_BATT_RTL 2 // switch to RTL mode on battery failsafe
#define FS_BATT_SMARTRTL_OR_RTL 3 // switch to SmartRTL, if can't, switch to RTL
#define FS_BATT_SMARTRTL_OR_LAND 4 // switch to SmartRTL, if can't, swtich to LAND
// 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_DISABLED 0
#define FS_GCS_ENABLED_ALWAYS_RTL 1
#define FS_GCS_ENABLED_CONTINUE_MISSION 2
#define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL 3
#define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND 4
// EKF failsafe definitions (FS_EKF_ACTION parameter)
#define FS_EKF_ACTION_LAND 1 // switch to LAND mode on EKF failsafe

51
ArduCopter/events.cpp
View File

@ -19,10 +19,14 @@ void Copter::failsafe_radio_on_event()
} else if (control_mode == LAND && g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) {
// continue landing
} else {
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
set_mode_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
} else {
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
} else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL) {
set_mode_SmartRTL_or_RTL(MODE_REASON_RADIO_FAILSAFE);
} else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND) {
set_mode_SmartRTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
} else { // g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND
set_mode_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
}
}
}
@ -54,9 +58,13 @@ void Copter::failsafe_battery_event(void)
if (should_disarm_on_failsafe()) {
init_disarm_motors();
} else {
if (g.failsafe_battery_enabled == FS_BATT_RTL || control_mode == AUTO) {
if (g.failsafe_battery_enabled == FS_BATT_SMARTRTL_OR_RTL) {
set_mode_SmartRTL_or_RTL(MODE_REASON_BATTERY_FAILSAFE);
} else if (g.failsafe_battery_enabled == FS_BATT_SMARTRTL_OR_LAND) {
set_mode_SmartRTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
} else if (g.failsafe_battery_enabled == FS_BATT_RTL || control_mode == AUTO) {
set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
} else {
} else { // g.failsafe_battery_enabled == FS_BATT_LAND
set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
}
}
@ -115,7 +123,11 @@ void Copter::failsafe_gcs_check()
} else {
if (control_mode == AUTO && g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) {
// continue mission
} else if (g.failsafe_gcs != FS_GCS_DISABLED) {
} else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL) {
set_mode_SmartRTL_or_RTL(MODE_REASON_GCS_FAILSAFE);
} else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND) {
set_mode_SmartRTL_or_land_with_pause(MODE_REASON_GCS_FAILSAFE);
} else { // g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL
set_mode_RTL_or_land_with_pause(MODE_REASON_GCS_FAILSAFE);
}
}
@ -218,6 +230,33 @@ void Copter::set_mode_RTL_or_land_with_pause(mode_reason_t reason)
}
}
// set_mode_SmartRTL_or_land_with_pause - sets mode to SMART_RTL if possible or LAND with 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_SmartRTL_or_land_with_pause(mode_reason_t reason)
{
// attempt to switch to SMART_RTL, if this failed then switch to Land
if (!set_mode(SMART_RTL, reason)) {
gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Using Land Mode");
set_mode_land_with_pause(reason);
} else {
AP_Notify::events.failsafe_mode_change = 1;
}
}
// set_mode_SmartRTL_or_RTL - sets mode to SMART_RTL if possible or RTL if possible or LAND with 4 second delay before descent starts
// this is always called from a failsafe so we trigger notification to pilot
void Copter::set_mode_SmartRTL_or_RTL(mode_reason_t reason)
{
// attempt to switch to SmartRTL, if this failed then attempt to RTL
// if that fails, then land
if (!set_mode(SMART_RTL, reason)) {
gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL Unavailable, Trying RTL Mode");
set_mode_RTL_or_land_with_pause(reason);
} else {
AP_Notify::events.failsafe_mode_change = 1;
}
}
bool Copter::should_disarm_on_failsafe() {
if (ap.in_arming_delay) {
return true;