From 6b5ba86f2184b03135ba38df7f68646bb1467f60 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 25 Jan 2016 15:18:38 -0800 Subject: [PATCH] Copter: simplify GCS failsafe --- ArduCopter/config.h | 5 ---- ArduCopter/defines.h | 5 ++++ ArduCopter/events.cpp | 54 +++++++------------------------------------ 3 files changed, 13 insertions(+), 51 deletions(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index e42b4e353a..01cff43a3c 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -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 diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index d090e3d115..f98dbbc626 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -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 diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index bf06d14ac7..260af92df1 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -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(); + } } }