From 13fe25ff031e9d96c5e5d0989c87662b75c3457e Mon Sep 17 00:00:00 2001
From: Marius Eggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 21 Nov 2022 10:30:48 +0100
Subject: [PATCH] added dataPool Output, in case MEKF inverion fails

---
 mission/controller/acs/MultiplicativeKalmanFilter.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
index cc136f61..5d66bd8d 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
@@ -893,6 +893,16 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
   int inversionFailed = CholeskyDecomposition<double>::invertCholesky(*residualCov, *invResidualCov,
                                                                       invResidualCov1, MDF);
   if (inversionFailed) {
+    {
+      PoolReadGuard pg(mekfData);
+      if (pg.getReadResult() == returnvalue::OK) {
+        double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
+        double zeroVec[3] = {0.0, 0.0, 0.0};
+        std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
+        std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
+        mekfData->setValidity(false, true);
+      }
+    }
     validMekf = false;
     return KALMAN_INVERSION_FAILED;  // RETURN VALUE ? -- Like: Kalman Inversion Failed
   }