diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index f2e92510e6..a7c2ed6555 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1390,7 +1390,14 @@ static void update_GPS(void) // run glitch protection and update AP_Notify gps_glitch.check_position(); - AP_Notify::flags.gps_glitching = gps_glitch.glitching(); + if (AP_Notify::flags.gps_glitching != gps_glitch.glitching()) { + 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 = gps_glitch.glitching(); + } // check if we can initialise home yet if (!ap.home_is_set) { diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 33f9456f49..00f9fa7030 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -436,6 +436,7 @@ enum ap_message { #define ERROR_SUBSYSTEM_FAILSAFE_GCS 8 #define ERROR_SUBSYSTEM_FAILSAFE_FENCE 9 #define ERROR_SUBSYSTEM_FLGHT_MODE 10 +#define ERROR_SUBSYSTEM_GPS 11 // general error codes #define ERROR_CODE_ERROR_RESOLVED 0 #define ERROR_CODE_FAILED_TO_INITIALISE 1 @@ -446,6 +447,8 @@ enum ap_message { #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