zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: add SmartRTL failsafe action 

Adds SmartRTL or RTL and SmartRTL or Land failsafe options for batt, throttle, and GCS failsafes
master
Dylan Herman 7 years ago committed by Randy Mackay
parent
c14af79975
commit
20fe5bb98f
4 changed files with 63 additions and 16 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      ArduCopter/Copter.h
  2. 6
      ArduCopter/Parameters.cpp
  3. 20
      ArduCopter/defines.h
  4. 51
      ArduCopter/events.cpp

2
ArduCopter/Copter.h
Unescape View File

@ -698,6 +698,8 @@ private: @@ -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
Unescape View File

@ -151,7 +151,7 @@ const AP_Param::Info Copter::var_info[] = { @@ -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[] = { @@ -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[] = { @@ -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
Unescape View File

@ -474,20 +474,26 @@ enum DevOptions { @@ -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
Unescape View File

@ -19,10 +19,14 @@ void Copter::failsafe_radio_on_event() @@ -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) @@ -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() @@ -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) @@ -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;

Write Preview
Loading…
Cancel
Save