From ff815882269b5c89991bb9be0e0e990115ed6498 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 17 Jun 2015 23:19:43 -0700 Subject: [PATCH] Copter: add GPS glitch failsafe --- ArduCopter/ArduCopter.pde | 3 ++ ArduCopter/control_auto.pde | 4 +++ ArduCopter/control_circle.pde | 4 +++ ArduCopter/control_drift.pde | 4 +++ ArduCopter/control_guided.pde | 4 +++ ArduCopter/control_land.pde | 2 +- ArduCopter/control_loiter.pde | 4 +++ ArduCopter/control_poshold.pde | 4 +++ ArduCopter/control_stop.pde | 4 +++ ArduCopter/gps_glitch.pde | 54 ++++++++++++++++++++++++++++++++++ 10 files changed, 86 insertions(+), 1 deletion(-) create mode 100644 ArduCopter/gps_glitch.pde diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 84d9cccd3a..efe5f9c12f 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -411,6 +411,7 @@ static struct { uint8_t battery : 1; // 2 // A status flag for the battery failsafe uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe uint8_t ekf : 1; // 5 // true if ekf failsafe has occurred + uint8_t gps_glitch : 1; // 6 // true if gps glitch failsafe has occurred int8_t radio_counter; // number of iterations with throttle below throttle_fs_value @@ -920,6 +921,8 @@ static void fast_loop() update_land_detector(); update_motor_fail_detector(); + + gps_glitch_update(); } // rc_loops - reads user input from transmitter/receiver diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index 8c5f4277a4..a29f652bc3 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -20,6 +20,10 @@ // auto_init - initialise auto controller static bool auto_init(bool ignore_checks) { + if (!ignore_checks && failsafe.gps_glitch) { + return false; + } + if ((position_ok() && mission.num_commands() > 1) || ignore_checks) { auto_mode = Auto_Loiter; diff --git a/ArduCopter/control_circle.pde b/ArduCopter/control_circle.pde index 35b7282996..2c1203ca10 100644 --- a/ArduCopter/control_circle.pde +++ b/ArduCopter/control_circle.pde @@ -7,6 +7,10 @@ // circle_init - initialise circle controller flight mode static bool circle_init(bool ignore_checks) { + if (!ignore_checks && failsafe.gps_glitch) { + return false; + } + if (position_ok() || ignore_checks) { circle_pilot_yaw_override = false; diff --git a/ArduCopter/control_drift.pde b/ArduCopter/control_drift.pde index 9e127cf686..0f74dce5d6 100644 --- a/ArduCopter/control_drift.pde +++ b/ArduCopter/control_drift.pde @@ -29,6 +29,10 @@ // drift_init - initialise drift controller static bool drift_init(bool ignore_checks) { + if (!ignore_checks && failsafe.gps_glitch) { + return false; + } + if (position_ok() || ignore_checks) { return true; }else{ diff --git a/ArduCopter/control_guided.pde b/ArduCopter/control_guided.pde index 0804542b85..450e44caab 100644 --- a/ArduCopter/control_guided.pde +++ b/ArduCopter/control_guided.pde @@ -26,6 +26,10 @@ struct Guided_Limit { // guided_init - initialise guided controller static bool guided_init(bool ignore_checks) { + if (!ignore_checks && failsafe.gps_glitch) { + return false; + } + if (position_ok() || ignore_checks) { // initialise yaw set_auto_yaw_mode(get_default_auto_yaw_mode(false)); diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 467cfc313f..0570e6d5e3 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -9,7 +9,7 @@ static bool land_pause; static bool land_init(bool ignore_checks) { // check if we have GPS and decide which LAND we're going to do - land_with_gps = position_ok(); + land_with_gps = position_ok() && !failsafe.gps_glitch; if (land_with_gps) { // set target to stopping point Vector3f stopping_point; diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde index f6b7a7edb4..7371e4b77c 100644 --- a/ArduCopter/control_loiter.pde +++ b/ArduCopter/control_loiter.pde @@ -7,6 +7,10 @@ // loiter_init - initialise loiter controller static bool loiter_init(bool ignore_checks) { + if (!ignore_checks && failsafe.gps_glitch) { + return false; + } + if (position_ok() || optflow_position_ok() || ignore_checks) { // set target to current position diff --git a/ArduCopter/control_poshold.pde b/ArduCopter/control_poshold.pde index 493edf2f21..78f3bd9d98 100644 --- a/ArduCopter/control_poshold.pde +++ b/ArduCopter/control_poshold.pde @@ -96,6 +96,10 @@ static struct { // poshold_init - initialise PosHold controller static bool poshold_init(bool ignore_checks) { + if (!ignore_checks && failsafe.gps_glitch) { + return false; + } + // fail to initialise PosHold mode if no GPS lock if (!position_ok() && !ignore_checks) { return false; diff --git a/ArduCopter/control_stop.pde b/ArduCopter/control_stop.pde index d9688805ff..db2ccef1f9 100644 --- a/ArduCopter/control_stop.pde +++ b/ArduCopter/control_stop.pde @@ -7,6 +7,10 @@ // stop_init - initialise stop controller static bool stop_init(bool ignore_checks) { + if (!ignore_checks && failsafe.gps_glitch) { + return false; + } + if (position_ok() || optflow_position_ok() || ignore_checks) { // set desired acceleration to zero diff --git a/ArduCopter/gps_glitch.pde b/ArduCopter/gps_glitch.pde new file mode 100644 index 0000000000..b518b77e38 --- /dev/null +++ b/ArduCopter/gps_glitch.pde @@ -0,0 +1,54 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +bool gps_glitch_switch_mode_on_resolve = false; + +static void gps_glitch_update() { + bool glitch = ahrs.get_NavEKF().getGpsGlitchStatus(); + + if (glitch && !failsafe.gps_glitch) { + gps_glitch_on_event(); + } else if (!glitch && failsafe.gps_glitch) { + gps_glitch_off_event(); + } + + if (failsafe.gps_glitch && control_mode != ALT_HOLD) { + // if the mode has changed during gps glitch, don't return to LOITER on resolve + gps_glitch_switch_mode_on_resolve = false; + } +} + +static bool gps_glitch_action_mode(uint8_t mode) { + switch(control_mode) { + case LAND: + return landing_with_GPS(); + case RTL: + return rtl_state == Land; + case GUIDED: // GUIDED for solo because shots modes use GUIDED + case LOITER: + case DRIFT: + case STOP: + case POSHOLD: + return true; + default: + return false; + } + return false; +} + +static void gps_glitch_on_event() { + failsafe.gps_glitch = true; + + if (gps_glitch_action_mode(control_mode) && !failsafe.radio) { + gps_glitch_switch_mode_on_resolve = true; + + set_mode(ALT_HOLD); + } +} + +static void gps_glitch_off_event() { + failsafe.gps_glitch = false; + + if (gps_glitch_switch_mode_on_resolve) { + set_mode(LOITER); + } +}