From f19729e11d2258013c974720395641b46f679c00 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Tue, 21 Feb 2023 17:10:59 +0100
Subject: [PATCH] throw a single event every time the mekf stops working

---
 mission/controller/AcsController.cpp | 52 ++++++++++++++++++++--------
 mission/controller/AcsController.h   |  3 ++
 2 files changed, 41 insertions(+), 14 deletions(-)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 65515b78..c6448a82 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -124,17 +124,25 @@ void AcsController::performSafe() {
 
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
-  ReturnValue_t validMekf;
-  navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
-                     &mekfData, &validMekf);
-
+  ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
+                                            &susDataProcessed, &mekfData);
+  if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
+      result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
+    if (not mekfInvalidFlag) {
+      triggerEvent(acs::MEKF_INVALID_SAFE_MODE);
+      mekfInvalidFlag = true;
+    }
+    return;
+  } else {
+    mekfInvalidFlag = false;
+  }
   // get desired satellite rate and sun direction to align
   double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
   guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
   // if MEKF is working
   double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
   bool magMomMtqValid = false;
-  if (validMekf == returnvalue::OK) {
+  if (result == MultiplicativeKalmanFilter::KALMAN_RUNNING) {
     safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
                       mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
                       susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
@@ -183,10 +191,18 @@ void AcsController::performDetumble() {
 
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
-  ReturnValue_t validMekf;
-  navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
-                     &mekfData, &validMekf);
-
+  ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
+                                            &susDataProcessed, &mekfData);
+  if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
+      result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
+    if (not mekfInvalidFlag) {
+      triggerEvent(acs::MEKF_INVALID_SAFE_MODE);
+      mekfInvalidFlag = true;
+    }
+    return;
+  } else {
+    mekfInvalidFlag = false;
+  }
   double magMomMtq[3] = {0, 0, 0};
   detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
                    mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
@@ -224,13 +240,21 @@ void AcsController::performPointingCtrl() {
 
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
-  ReturnValue_t validMekf;
-  navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
-                     &mekfData, &validMekf);
-
+  ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
+                                            &susDataProcessed, &mekfData);
+  if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
+      result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
+    if (not mekfInvalidFlag) {
+      triggerEvent(acs::MEKF_INVALID_PTG_MODE);
+      mekfInvalidFlag = true;
+    }
+    return;
+  } else {
+    mekfInvalidFlag = false;
+  }
   uint8_t enableAntiStiction = true;
   double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
-  ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
+  result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
   if (result == returnvalue::FAILED) {
     multipleRwUnavailableCounter++;
     if (multipleRwUnavailableCounter > 4) {
diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h
index 715baef4..9e6c2c4e 100644
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@@ -10,6 +10,7 @@
 
 #include "acs/ActuatorCmd.h"
 #include "acs/Guidance.h"
+#include "acs/MultiplicativeKalmanFilter.h"
 #include "acs/Navigation.h"
 #include "acs/SensorProcessing.h"
 #include "acs/control/Detumble.h"
@@ -54,6 +55,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
 
   ParameterHelper parameterHelper;
 
+  bool mekfInvalidFlag = true;
+
 #if OBSW_THREAD_TRACING == 1
   uint32_t opCounter = 0;
 #endif