diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 3a2534b184..99009628cc 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -807,7 +807,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { #endif { update_notify, 8, 10 }, { one_hz_loop, 400, 42 }, - { ekf_check, 40, 2 }, + { ekf_dcm_check, 40, 2 }, { crash_check, 40, 2 }, { gcs_check_input, 8, 550 }, { gcs_send_heartbeat, 400, 150 }, @@ -875,7 +875,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { #endif { update_notify, 2, 100 }, { one_hz_loop, 100, 420 }, - { ekf_check, 10, 20 }, + { ekf_dcm_check, 10, 20 }, { crash_check, 10, 20 }, { gcs_check_input, 2, 550 }, { gcs_send_heartbeat, 100, 150 }, diff --git a/ArduCopter/ekf_check.pde b/ArduCopter/ekf_check.pde index 11c59ac486..7e2de943b8 100644 --- a/ArduCopter/ekf_check.pde +++ b/ArduCopter/ekf_check.pde @@ -15,42 +15,48 @@ # define EKF_CHECK_WARNING_TIME (30*1000) // warning text messages are sent to ground no more than every 30 seconds #endif +// Enumerator for types of check +enum EKFCheckType { + CHECK_NONE = 0, + CHECK_DCM = 1, + CHECK_EKF = 2 +}; + //////////////////////////////////////////////////////////////////////////////// // EKF_check strucutre //////////////////////////////////////////////////////////////////////////////// static struct { - uint8_t fail_count_compass; // number of iterations ekf's compass variance has been out of tolerances + uint8_t fail_count_compass; // number of iterations ekf or dcm have been out of tolerances - uint8_t bad_compass : 1; // true if compass variance is bad + uint8_t bad_compass : 1; // true if dcm or ekf should be considered untrusted (fail_count_compass has exceeded EKF_CHECK_ITERATIONS_MAX) uint32_t last_warn_time; // system time of last warning in milliseconds. Used to throttle text warnings sent to GCS } ekf_check_state; -// ekf_check - detects ekf variances that are out of tolerance +// ekf_dcm_check - detects if ekf variances or dcm yaw errors that are out of tolerance and triggers failsafe // should be called at 10hz -void ekf_check() +void ekf_dcm_check() { -#if AP_AHRS_NAVEKF_AVAILABLE + EKFCheckType check_type = CHECK_NONE; + + // decide if we should check ekf or dcm + if (ahrs.have_inertial_nav() && g.ekfcheck_thresh > 0.0f) { + check_type = CHECK_EKF; + } else if (g.dcmcheck_thresh > 0.0f) { + check_type = CHECK_DCM; + } + // return immediately if motors are not armed, ekf check is disabled, not using ekf or usb is connected - if (!motors.armed() || g.ekfcheck_thresh == 0.0f || !ahrs.have_inertial_nav() || ap.usb_connected) { + if (!motors.armed() || ap.usb_connected || check_type == CHECK_NONE) { ekf_check_state.fail_count_compass = 0; - ekf_check_state.bad_compass = 0; + ekf_check_state.bad_compass = false; AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass; failsafe_ekf_off_event(); // clear failsafe return; } - // use EKF to get variance - float posVar, hgtVar, tasVar; - Vector3f magVar; - Vector2f offset; - float compass_variance; - float vel_variance; - ahrs.get_NavEKF().getVariances(vel_variance, posVar, hgtVar, magVar, tasVar, offset); - compass_variance = magVar.length(); - // compare compass and velocity variance vs threshold - if (compass_variance >= g.ekfcheck_thresh && vel_variance > g.ekfcheck_thresh) { + if ((check_type == CHECK_EKF && ekf_over_threshold()) || (check_type == CHECK_DCM && dcm_over_threshold())) { // if compass is not yet flagged as bad if (!ekf_check_state.bad_compass) { // increase counter @@ -64,7 +70,11 @@ void ekf_check() 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 (check_type == CHECK_EKF) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance")); + } else { + gcs_send_text_P(SEVERITY_HIGH,PSTR("DCM bad heading")); + } ekf_check_state.last_warn_time = hal.scheduler->millis(); } failsafe_ekf_event(); @@ -90,19 +100,46 @@ void ekf_check() AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass; // To-Do: add ekf variances to extended status +} + +// dcm_over_threshold - returns true if the dcm yaw error is over the tolerance +static bool dcm_over_threshold() +{ + // return true if yaw error is over the threshold + return (g.dcmcheck_thresh > 0.0f && ahrs.get_error_yaw() > g.dcmcheck_thresh); +} + +// ekf_over_threshold - returns true if the ekf's variance are over the tolerance +static bool ekf_over_threshold() +{ +#if AP_AHRS_NAVEKF_AVAILABLE + // return false immediately if disabled + if (g.ekfcheck_thresh <= 0.0f) { + return false; + } + + // use EKF to get variance + float posVar, hgtVar, tasVar; + Vector3f magVar; + Vector2f offset; + float compass_variance; + float vel_variance; + ahrs.get_NavEKF().getVariances(vel_variance, posVar, hgtVar, magVar, tasVar, offset); + compass_variance = magVar.length(); + + // return true if compass and velocity variance over the threshold + return (compass_variance >= g.ekfcheck_thresh && vel_variance >= g.ekfcheck_thresh); #else - // if no EKF available then never trigger failure - ekf_check_state.bad_compass = 0; - failsafe.ekf = false; + return false; #endif } -#if AP_AHRS_NAVEKF_AVAILABLE + // failsafe_ekf_event - perform ekf failsafe static void failsafe_ekf_event() { - // return immediately if ekf failsafe already triggered or disabled - if (failsafe.ekf || g.ekfcheck_thresh <= 0.0f) { + // return immediately if ekf failsafe already triggered + if (failsafe.ekf) { return; } @@ -138,4 +175,3 @@ static void failsafe_ekf_off_event(void) failsafe.ekf = false; Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_RESOLVED); } -#endif