Copter: add gps glitch notification and pre-arm check

This commit is contained in:
Randy Mackay 2017-07-25 20:41:18 +09:00
parent b78bad2da9
commit 63c032b8cc
5 changed files with 37 additions and 2 deletions

View File

@ -435,6 +435,17 @@ bool AP_Arming_Copter::pre_arm_gps_checks(bool display_failure)
return false;
}
// check for GPS glitch (as reported by EKF)
nav_filter_status filt_status;
if (_ahrs_navekf.get_filter_status(filt_status)) {
if (filt_status.flags.gps_glitching) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: GPS glitching");
}
return false;
}
}
// check EKF compass variance is below failsafe threshold
float vel_variance, pos_variance, hgt_variance, tas_variance;
Vector3f mag_variance;

View File

@ -113,6 +113,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(update_notify, 50, 90),
SCHED_TASK(one_hz_loop, 1, 100),
SCHED_TASK(ekf_check, 10, 75),
SCHED_TASK(gpsglitch_check, 10, 50),
SCHED_TASK(landinggear_update, 10, 75),
SCHED_TASK(lost_vehicle_check, 10, 50),
SCHED_TASK(gcs_check_input, 400, 180),

View File

@ -267,7 +267,7 @@ private:
uint8_t land_complete_maybe : 1; // 14 // true if we may have landed (less strict version of land_complete)
uint8_t throttle_zero : 1; // 15 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock
uint8_t system_time_set : 1; // 16 // true if the system time has been set from the GPS
uint8_t gps_base_pos_set : 1; // 17 // true when the gps base position has been set (used for RTK gps only)
uint8_t gps_glitching : 1; // 17 // true if the gps is glitching
enum HomeState home_state : 2; // 18,19 // home status (unset, set, locked)
uint8_t using_interlock : 1; // 20 // aux switch motor interlock function is in use
uint8_t motor_emergency_stop : 1; // 21 // motor estop switch, shuts off motors when enabled
@ -955,6 +955,7 @@ private:
void failsafe_terrain_check();
void failsafe_terrain_set_status(bool data_ok);
void failsafe_terrain_on_event();
void gpsglitch_check();
void set_mode_RTL_or_land_with_pause(mode_reason_t reason);
void update_events();
void failsafe_enable();

View File

@ -408,7 +408,7 @@ enum DevOptions {
#define ERROR_SUBSYSTEM_FAILSAFE_GCS 8
#define ERROR_SUBSYSTEM_FAILSAFE_FENCE 9
#define ERROR_SUBSYSTEM_FLIGHT_MODE 10
#define ERROR_SUBSYSTEM_GPS 11 // not used
#define ERROR_SUBSYSTEM_GPS 11
#define ERROR_SUBSYSTEM_CRASH_CHECK 12
#define ERROR_SUBSYSTEM_FLIP 13
#define ERROR_SUBSYSTEM_AUTOTUNE 14
@ -456,6 +456,8 @@ enum DevOptions {
#define ERROR_CODE_EKFCHECK_VARIANCE_CLEARED 0
// Baro specific error codes
#define ERROR_CODE_BARO_GLITCH 2
// GPS specific error coces
#define ERROR_CODE_GPS_GLITCH 2
// Arming Check Enable/Disable bits
#define ARMING_CHECK_NONE 0x00

View File

@ -184,6 +184,26 @@ void Copter::failsafe_terrain_on_event()
}
}
// check for gps glitch failsafe
void Copter::gpsglitch_check()
{
// get filter status
nav_filter_status filt_status = inertial_nav.get_filter_status();
bool gps_glitching = filt_status.flags.gps_glitching;
// log start or stop of gps glitch. AP_Notify update is handled from within AP_AHRS
if (ap.gps_glitching != gps_glitching) {
ap.gps_glitching = gps_glitching;
if (gps_glitching) {
Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_GPS_GLITCH);
gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch");
} else {
Log_Write_Error(ERROR_SUBSYSTEM_GPS, ERROR_CODE_ERROR_RESOLVED);
gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch cleared");
}
}
}
// set_mode_RTL_or_land_with_pause - sets mode to 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_RTL_or_land_with_pause(mode_reason_t reason)