Copter: add DCM check of yaw error

Triggers an "ekf" failsafe if the DCM yaw error is > 60deg

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 },

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) {
+                    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
+    return false;
-    ekf_check_state.bad_compass = 0;
-    failsafe.ekf = 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
+    // return immediately if ekf failsafe already triggered
-    if (failsafe.ekf || g.ekfcheck_thresh <= 0.0f) {
+    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