From fbfc94cf69d803b5c7e3ad5a7dc67adf03002217 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 13 Mar 2015 11:05:30 +0900 Subject: [PATCH] Copter: remove GPS glitch and failsafe The EKF failsafe now captures all failures that could lead to a bad position including GPS glitches and a bad compass meaning we do not need this protection in the main flight code. --- ArduCopter/AP_State.pde | 10 ------ ArduCopter/ArduCopter.pde | 26 ++-------------- ArduCopter/GCS_Mavlink.pde | 4 +-- ArduCopter/Parameters.h | 5 ++- ArduCopter/Parameters.pde | 11 ------- ArduCopter/compassmot.pde | 2 -- ArduCopter/config.h | 5 +-- ArduCopter/defines.h | 6 ++-- ArduCopter/events.pde | 63 -------------------------------------- ArduCopter/motor_test.pde | 2 -- ArduCopter/motors.pde | 14 --------- 11 files changed, 9 insertions(+), 139 deletions(-) diff --git a/ArduCopter/AP_State.pde b/ArduCopter/AP_State.pde index 1ad9031f7f..b78cb93553 100644 --- a/ArduCopter/AP_State.pde +++ b/ArduCopter/AP_State.pde @@ -86,16 +86,6 @@ void set_failsafe_battery(bool b) AP_Notify::flags.failsafe_battery = b; } - -// --------------------------------------------- -static void set_failsafe_gps(bool b) -{ - failsafe.gps = b; - - // update AP_Notify - AP_Notify::flags.failsafe_gps = b; -} - // --------------------------------------------- static void set_failsafe_gcs(bool b) { diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 009619f786..467a392a3a 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -103,7 +103,6 @@ #include // MAVLink GCS definitions #include // Serial manager library #include // ArduPilot GPS library -#include // GPS glitch protection library #include // ArduPilot Mega Flash Memory Library #include // ArduPilot Mega Analog to Digital Converter Library #include @@ -257,8 +256,6 @@ static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor: static AP_GPS gps; -static GPS_Glitch gps_glitch(gps); - // flight modes convenience array static AP_Int8 *flight_modes = &g.flight_mode1; @@ -408,7 +405,6 @@ static struct { uint8_t rc_override_active : 1; // 0 // true if rc control are overwritten by ground station uint8_t radio : 1; // 1 // A status flag for the radio failsafe uint8_t battery : 1; // 2 // A status flag for the battery failsafe - uint8_t gps : 1; // 3 // A status flag for the gps 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 @@ -602,7 +598,7 @@ static float G_Dt = 0.02; //////////////////////////////////////////////////////////////////////////////// // Inertial Navigation //////////////////////////////////////////////////////////////////////////////// -static AP_InertialNav_NavEKF inertial_nav(ahrs, barometer, gps_glitch); +static AP_InertialNav_NavEKF inertial_nav(ahrs); //////////////////////////////////////////////////////////////////////////////// // Attitude, Position and Waypoint navigation objects @@ -1175,12 +1171,11 @@ static void one_hz_loop() static void update_GPS(void) { static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; // time of last gps message - bool report_gps_glitch; bool gps_updated = false; gps.update(); - // logging and glitch protection run after every gps message + // log after every gps message for (uint8_t i=0; isafety_switch_state() != AP_HAL::Util::SAFETY_DISARMED); - if (AP_Notify::flags.gps_glitching != report_gps_glitch) { - if (gps_glitch.glitching()) { - Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_GPS_GLITCH); - }else{ - Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_ERROR_RESOLVED); - } - AP_Notify::flags.gps_glitching = report_gps_glitch; - } - } - // set system time if necessary set_system_time_from_GPS(); @@ -1228,9 +1209,6 @@ static void update_GPS(void) #endif } } - - // check for loss of gps - failsafe_gps_check(); } static void diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 7bc4346815..623bddcb4b 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -43,7 +43,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan) uint32_t custom_mode = control_mode; // set system as critical if any failsafe have triggered - if (failsafe.radio || failsafe.battery || failsafe.gps || failsafe.gcs || failsafe.ekf) { + if (failsafe.radio || failsafe.battery || failsafe.gcs || failsafe.ekf) { system_status = MAV_STATE_CRITICAL; } @@ -197,7 +197,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan) if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; } - if (gps.status() > AP_GPS::NO_GPS && (!gps_glitch.glitching()||ap.usb_connected)) { + if (gps.status() > AP_GPS::NO_GPS) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; } #if OPTFLOW == ENABLED diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index cbea8e7a37..2ec0f2fbdf 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -135,7 +135,7 @@ public: k_param_geofence_limit, // deprecated - remove k_param_altitude_limit, // deprecated - remove k_param_fence, - k_param_gps_glitch, + k_param_gps_glitch, // deprecated k_param_baro_glitch, // 71 - deprecated // @@ -258,7 +258,7 @@ public: k_param_rc_speed = 192, k_param_failsafe_battery_enabled, k_param_throttle_mid, - k_param_failsafe_gps_enabled, + k_param_failsafe_gps_enabled, // remove k_param_rc_9, k_param_rc_12, k_param_failsafe_gcs, // 198 @@ -342,7 +342,6 @@ public: AP_Float fs_batt_voltage; // battery voltage below which failsafe will be triggered AP_Float fs_batt_mah; // battery capacity (in mah) below which failsafe will be triggered - AP_Int8 failsafe_gps_enabled; // gps failsafe enabled AP_Int8 failsafe_gcs; // ground station failsafe behavior AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index e100fa33b0..1bd6598250 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -115,13 +115,6 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(fs_batt_mah, "FS_BATT_MAH", FS_BATT_MAH_DEFAULT), - // @Param: FS_GPS_ENABLE - // @DisplayName: GPS Failsafe Enable - // @Description: Controls what action will be taken if GPS signal is lost for at least 5 seconds - // @Values: 0:Disabled,1:Land,2:AltHold,3:Land even from Stabilize - // @User: Standard - GSCALAR(failsafe_gps_enabled, "FS_GPS_ENABLE", FS_GPS_LAND), - // @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. @@ -899,10 +892,6 @@ const AP_Param::Info var_info[] PROGMEM = { GOBJECT(rally, "RALLY_", AP_Rally), #endif - // @Group: GPSGLITCH_ - // @Path: ../libraries/AP_GPS/AP_GPS_Glitch.cpp - GOBJECT(gps_glitch, "GPSGLITCH_", GPS_Glitch), - #if FRAME_CONFIG == HELI_FRAME // @Group: H_ // @Path: ../libraries/AP_Motors/AP_MotorsHeli.cpp diff --git a/ArduCopter/compassmot.pde b/ArduCopter/compassmot.pde index f255a252ef..e682dce290 100644 --- a/ArduCopter/compassmot.pde +++ b/ArduCopter/compassmot.pde @@ -101,7 +101,6 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan) // disable throttle and battery failsafe g.failsafe_throttle = FS_THR_DISABLED; g.failsafe_battery_enabled = FS_BATT_DISABLED; - g.failsafe_gps_enabled = FS_GPS_DISABLED; // disable motor compensation compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED); @@ -259,7 +258,6 @@ static uint8_t mavlink_compassmot(mavlink_channel_t chan) // re-enable failsafes g.failsafe_throttle.load(); g.failsafe_battery_enabled.load(); - g.failsafe_gps_enabled.load(); // flag we have completed ap.compass_mot = false; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 70721f4056..be67568708 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -274,10 +274,7 @@ # define BOARD_VOLTAGE_MAX 5.8f // max board voltage in volts for pre-arm checks #endif -// GPS failsafe -#ifndef FAILSAFE_GPS_TIMEOUT_MS - # define FAILSAFE_GPS_TIMEOUT_MS 5000 // gps failsafe triggers after 5 seconds with no GPS -#endif +// prearm GPS hdop check #ifndef GPS_HDOP_GOOD_DEFAULT # define GPS_HDOP_GOOD_DEFAULT 230 // minimum hdop that represents a good position. used during pre-arm checks if fence is enabled #endif diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index b261db0bf7..7d229fbbe4 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -320,11 +320,11 @@ enum FlipState { #define ERROR_SUBSYSTEM_OPTFLOW 4 #define ERROR_SUBSYSTEM_FAILSAFE_RADIO 5 #define ERROR_SUBSYSTEM_FAILSAFE_BATT 6 -#define ERROR_SUBSYSTEM_FAILSAFE_GPS 7 +#define ERROR_SUBSYSTEM_FAILSAFE_GPS 7 // not used #define ERROR_SUBSYSTEM_FAILSAFE_GCS 8 #define ERROR_SUBSYSTEM_FAILSAFE_FENCE 9 #define ERROR_SUBSYSTEM_FLIGHT_MODE 10 -#define ERROR_SUBSYSTEM_GPS 11 +#define ERROR_SUBSYSTEM_GPS 11 // not used #define ERROR_SUBSYSTEM_CRASH_CHECK 12 #define ERROR_SUBSYSTEM_FLIP 13 #define ERROR_SUBSYSTEM_AUTOTUNE 14 @@ -343,8 +343,6 @@ enum FlipState { #define ERROR_CODE_FAILSAFE_OCCURRED 1 // subsystem specific error codes -- compass #define ERROR_CODE_COMPASS_FAILED_TO_READ 2 -// subsystem specific error codes -- gps -#define ERROR_CODE_GPS_GLITCH 2 // subsystem specific error codes -- main #define ERROR_CODE_MAIN_INS_DELAY 1 // subsystem specific error codes -- crash checker diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde index e936d765c6..853bcf7e09 100644 --- a/ArduCopter/events.pde +++ b/ArduCopter/events.pde @@ -162,69 +162,6 @@ static void failsafe_battery_event(void) } -// failsafe_gps_check - check for gps failsafe -static void failsafe_gps_check() -{ - uint32_t last_gps_update_ms; - - // return immediately if gps failsafe is disabled or we have never had GPS lock - if (g.failsafe_gps_enabled == FS_GPS_DISABLED || !home_is_set()) { - // if we have just disabled the gps failsafe, ensure the gps failsafe event is cleared - if (failsafe.gps) { - failsafe_gps_off_event(); - set_failsafe_gps(false); - } - return; - } - - // calc time since last gps update - last_gps_update_ms = millis() - gps_glitch.last_good_update(); - - // check if all is well - if( last_gps_update_ms < FAILSAFE_GPS_TIMEOUT_MS) { - // check for recovery from gps failsafe - if( failsafe.gps ) { - failsafe_gps_off_event(); - set_failsafe_gps(false); - } - return; - } - - // do nothing if gps failsafe already triggered or motors disarmed - if( failsafe.gps || !motors.armed()) { - return; - } - - // GPS failsafe event has occured - // update state, warn the ground station and log to dataflash - set_failsafe_gps(true); - gcs_send_text_P(SEVERITY_HIGH,PSTR("Lost GPS!")); - Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GPS, ERROR_CODE_FAILSAFE_OCCURRED); - - // take action based on flight mode and FS_GPS_ENABLED parameter - if (mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) { - if (g.failsafe_gps_enabled == FS_GPS_ALTHOLD && !failsafe.radio) { - set_mode(ALT_HOLD); - // alert pilot to mode change - AP_Notify::events.failsafe_mode_change = 1; - }else{ - set_mode_land_with_pause(); - } - } - - // if flight mode is LAND ensure it's not the GPS controlled LAND - if (control_mode == LAND) { - land_do_not_use_GPS(); - } -} - -// failsafe_gps_off_event - actions to take when GPS contact is restored -static void failsafe_gps_off_event(void) -{ - // log recovery of GPS in logs? - Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GPS, ERROR_CODE_FAILSAFE_RESOLVED); -} - // failsafe_gcs_check - check for ground station failsafe static void failsafe_gcs_check() { diff --git a/ArduCopter/motor_test.pde b/ArduCopter/motor_test.pde index 5aaab01b35..12590cec97 100644 --- a/ArduCopter/motor_test.pde +++ b/ArduCopter/motor_test.pde @@ -118,7 +118,6 @@ static uint8_t mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_se // disable throttle, battery and gps failsafe g.failsafe_throttle = FS_THR_DISABLED; g.failsafe_battery_enabled = FS_BATT_DISABLED; - g.failsafe_gps_enabled = FS_GPS_DISABLED; g.failsafe_gcs = FS_GCS_DISABLED; // turn on notify leds @@ -160,7 +159,6 @@ static void motor_test_stop() // re-enable failsafes g.failsafe_throttle.load(); g.failsafe_battery_enabled.load(); - g.failsafe_gps_enabled.load(); g.failsafe_gcs.load(); // turn off notify leds diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 044731c184..de82d4ded2 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -499,11 +499,6 @@ static bool pre_arm_gps_checks(bool display_failure) // check if flight mode requires GPS bool gps_required = mode_requires_GPS(control_mode); - // if GPS failsafe will triggers even in stabilize mode we need GPS before arming - if (g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) { - gps_required = true; - } - #if AC_FENCE == ENABLED // if circular fence is enabled we need GPS if ((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) { @@ -517,15 +512,6 @@ static bool pre_arm_gps_checks(bool display_failure) return true; } - // check GPS is not glitching - if (gps_glitch.glitching()) { - if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: GPS Glitch")); - } - AP_Notify::flags.pre_arm_gps_check = false; - return false; - } - // ensure GPS is ok if (!position_ok()) { if (display_failure) {