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

Copter: simplify GCS failsafe

This commit is contained in:
Jonathan Challinger 2016-01-25 15:18:38 -08:00 committed by Randy Mackay
parent a0ce8af633
commit 6b5ba86f21
3 changed files with 13 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/config.h
View File

@ -195,11 +195,6 @@
# define GNDEFFECT_COMPENSATION DISABLED
#endif
// possible values for FS_GCS parameter
#define FS_GCS_DISABLED 0
#define FS_GCS_ENABLED_ALWAYS_RTL 1
#define FS_GCS_ENABLED_CONTINUE_MISSION 2
// Radio failsafe while using RC_override
#ifndef FS_RADIO_RC_OVERRIDE_TIMEOUT_MS
# define FS_RADIO_RC_OVERRIDE_TIMEOUT_MS 1000 // RC Radio failsafe triggers after 1 second while using RC_override from ground station

5
ArduCopter/defines.h
View File

@ -417,6 +417,11 @@ enum ThrowModeState {
#define FS_BATT_LAND 1 // switch to LAND mode on battery failsafe
#define FS_BATT_RTL 2 // switch to RTL mode on battery failsafe
// 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
// EKF failsafe definitions (FS_EKF_ACTION parameter)
#define FS_EKF_ACTION_LAND 1 // switch to LAND mode on EKF failsafe
#define FS_EKF_ACTION_ALTHOLD 2 // switch to ALTHOLD mode on EKF failsafe

54
ArduCopter/events.cpp
View File

@ -112,52 +112,14 @@ void Copter::failsafe_gcs_check()
hal.rcin->clear_overrides();
failsafe.rc_override_active = false;
// This is how to handle a failsafe.
// use the throttle failsafe setting to decide what to do
switch(control_mode) {
case STABILIZE:
case ACRO:
case SPORT:
// if throttle is zero disarm motors
if (ap.throttle_zero || ap.land_complete) {
init_disarm_motors();
}else if(home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
}else{
// We have no GPS or are very close to home so we will land
set_mode_land_with_pause();
}
break;
case AUTO:
// if mission has not started AND vehicle is landed, disarm motors
if (!ap.auto_armed && ap.land_complete) {
init_disarm_motors();
// if g.failsafe_gcs is 1 do RTL, 2 means continue with the mission
} else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL) {
if (home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
}else{
// We are very close to home so we will land
set_mode_land_with_pause();
}
}
// if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
break;
default:
// used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold
// if landed disarm
if (ap.land_complete) {
init_disarm_motors();
} else if (home_distance > FS_CLOSE_TO_HOME_CM) {
// switch to RTL or if that fails, LAND
set_mode_RTL_or_land_with_pause();
}else{
// We have no GPS or are very close to home so we will land
set_mode_land_with_pause();
}
break;
if (should_disarm_on_failsafe()) {
init_disarm_motors();
} else {
if (control_mode == AUTO && g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) {
// continue mission
} else if (g.failsafe_gcs != FS_GCS_DISABLED) {
set_mode_RTL_or_land_with_pause();
}
}
}