From 33432aa4a816b5be77718b6f7f44366fb01f1895 Mon Sep 17 00:00:00 2001 From: priseborough Date: Thu, 7 Aug 2014 01:59:19 +1000 Subject: [PATCH] Copter : Distinguish between EKF and INAV errors --- ArduCopter/defines.h | 8 ++++---- ArduCopter/ekf_check.pde | 18 +++++++++++++----- 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 8a19fa343b..51269ab16b 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -329,8 +329,8 @@ enum FlipState { #define ERROR_SUBSYSTEM_FLIP 13 #define ERROR_SUBSYSTEM_AUTOTUNE 14 #define ERROR_SUBSYSTEM_PARACHUTE 15 -#define ERROR_SUBSYSTEM_EKF_CHECK 16 -#define ERROR_SUBSYSTEM_FAILSAFE_EKF 17 +#define ERROR_SUBSYSTEM_EKFINAV_CHECK 16 +#define ERROR_SUBSYSTEM_FAILSAFE_EKFINAV 17 #define ERROR_SUBSYSTEM_BARO 18 // general error codes #define ERROR_CODE_ERROR_RESOLVED 0 @@ -356,8 +356,8 @@ enum FlipState { // parachute failed to deploy because of low altitude #define ERROR_CODE_PARACHUTE_TOO_LOW 2 // EKF check definitions -#define ERROR_CODE_EKF_CHECK_BAD_VARIANCE 2 -#define ERROR_CODE_EKF_CHECK_BAD_VARIANCE_CLEARED 0 +#define ERROR_CODE_EKFINAV_CHECK_BAD_VARIANCE 2 +#define ERROR_CODE_EKFINAV_CHECK_VARIANCE_CLEARED 0 // Baro specific error codes #define ERROR_CODE_BARO_GLITCH 2 diff --git a/ArduCopter/ekf_check.pde b/ArduCopter/ekf_check.pde index a69fa73ff4..a4d24be91a 100644 --- a/ArduCopter/ekf_check.pde +++ b/ArduCopter/ekf_check.pde @@ -76,10 +76,18 @@ void ekf_check() ekf_check_state.fail_count_compass = EKF_CHECK_ITERATIONS_MAX; ekf_check_state.bad_compass = true; // log an error in the dataflash - Log_Write_Error(ERROR_SUBSYSTEM_EKF_CHECK, ERROR_CODE_EKF_CHECK_BAD_VARIANCE); + Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_CHECK, ERROR_CODE_EKFINAV_CHECK_BAD_VARIANCE); // send message to gcs if ((hal.scheduler->millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance")); +#if AP_AHRS_NAVEKF_AVAILABLE + if (ahrs.have_inertial_nav()) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance")); + } else { + gcs_send_text_P(SEVERITY_HIGH,PSTR("INAV variance")); + } +#else + gcs_send_text_P(SEVERITY_HIGH,PSTR("INAV variance")); +#endif ekf_check_state.last_warn_time = hal.scheduler->millis(); } failsafe_ekf_event(); @@ -94,7 +102,7 @@ void ekf_check() if (ekf_check_state.fail_count_compass == 0) { ekf_check_state.bad_compass = false; // log recovery in the dataflash - Log_Write_Error(ERROR_SUBSYSTEM_EKF_CHECK, ERROR_CODE_EKF_CHECK_BAD_VARIANCE_CLEARED); + Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_CHECK, ERROR_CODE_EKFINAV_CHECK_VARIANCE_CLEARED); // clear failsafe failsafe_ekf_off_event(); } @@ -126,7 +134,7 @@ static void failsafe_ekf_event() // EKF failsafe event has occurred failsafe.ekf = true; - Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKF, ERROR_CODE_FAILSAFE_OCCURRED); + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_OCCURRED); // take action based on flight mode if (mode_requires_GPS(control_mode)) { @@ -149,5 +157,5 @@ static void failsafe_ekf_off_event(void) // clear flag and log recovery failsafe.ekf = false; - Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKF, ERROR_CODE_FAILSAFE_RESOLVED); + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_RESOLVED); }