From a419a2ebf37ba0fafd0b95732a0992d3d7425689 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 12 Jun 2014 15:57:13 +0200
Subject: [PATCH] Report excessive covariances

---
 src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
index 352d9a0ce9..c2282fbb9d 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
@@ -2376,6 +2376,12 @@ int AttPosEKF::CheckAndBound()
         ret = 5;
     }
 
+    if (current_ekf_state.covariancesExcessive) {
+        FillErrorReport(&last_ekf_error);
+        current_ekf_state.covariancesExcessive = false;
+        ret = 6;
+    }
+
     if (ret) {
         ekfDiverged = true;
     }