From 9cdc932ad805978eda3173258af5fcd936fcac33 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 13 Mar 2023 15:36:46 +0100
Subject: [PATCH 01/12] also reset propagatedQuaternion

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

diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
index 77a3ef00..2ada8ab1 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
@@ -1089,6 +1089,7 @@ ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
   double resetCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                         {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
   std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double));
+  std::memcpy(propagatedQuaternion, resetQuaternion, 4 * sizeof(double));
   std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double));
   updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
   return MEKF_UNINITIALIZED;

From 63f45ec27e64387d6fa40f071ee2d4c97c86e6e2 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 13 Mar 2023 15:39:43 +0100
Subject: [PATCH 02/12] added finite checks for vectors and matrices

---
 mission/controller/acs/util/MathOperations.h | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h
index f8537740..b344451a 100644
--- a/mission/controller/acs/util/MathOperations.h
+++ b/mission/controller/acs/util/MathOperations.h
@@ -404,6 +404,26 @@ class MathOperations {
     std::memcpy(inverse, identity, sizeof(identity));
     return 0;  // successful inversion
   }
+
+  static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
+    for (uint8_t i = 0; i < size; i++) {
+      if (not isfinite(inputVector[i])) {
+        return false;
+      }
+    }
+    return true;
+  }
+
+  static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
+    for (uint8_t col = 0; col < cols; col++) {
+      for (uint8_t row = 0; row < rows; row++) {
+        if (not isfinite(inputMatrix[row * cols + cols])) {
+          return false;
+        }
+      }
+    }
+    return true;
+  }
 };
 
 #endif /* ACS_MATH_MATHOPERATIONS_H_ */

From 79bd349ac76a08caaab1e7c87c9d0ca2760f72c5 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 13 Mar 2023 15:58:48 +0100
Subject: [PATCH 03/12] added finite check for MEKF

---
 mission/controller/acs/MultiplicativeKalmanFilter.cpp | 6 ++++++
 mission/controller/acs/MultiplicativeKalmanFilter.h   | 6 ++++--
 2 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
index 2ada8ab1..e752da39 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
@@ -1080,6 +1080,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
 
   MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
 
+  if (not(MathOperations<double>::checkVectorIsFinite(propagatedQuaternion, 4)) ||
+      not(MathOperations<double>::checkMatrixIsFinite(initialQuaternion, 6, 6))) {
+    updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE);
+    return MEKF_NOT_FINITE;
+  }
+
   updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst);
   return MEKF_RUNNING;
 }
diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h
index ceb98339..fe749890 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.h
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.h
@@ -62,6 +62,7 @@ class MultiplicativeKalmanFilter {
     NO_MODEL_VECTORS = 2,
     NO_SUS_MGM_STR_DATA = 3,
     COVARIANCE_INVERSION_FAILED = 4,
+    NOT_FINITE = 5,
     INITIALIZED = 10,
     RUNNING = 11,
   };
@@ -74,8 +75,9 @@ class MultiplicativeKalmanFilter {
   static constexpr ReturnValue_t MEKF_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_MEKF_ID, 5);
   static constexpr ReturnValue_t MEKF_COVARIANCE_INVERSION_FAILED =
       returnvalue::makeCode(IF_MEKF_ID, 6);
-  static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7);
-  static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8);
+  static constexpr ReturnValue_t MEKF_NOT_FINITE = returnvalue::makeCode(IF_MEKF_ID, 7);
+  static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 8);
+  static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 9);
 
  private:
   /*Parameters*/

From e77766ae593ba8efc905226854671b4d5ec1263f Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 13 Mar 2023 16:14:32 +0100
Subject: [PATCH 04/12] reset the MEKF in case its results are not finite

---
 mission/controller/AcsController.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index b4e7c5d3..d513d114 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -271,6 +271,9 @@ void AcsController::performPointingCtrl() {
       triggerEvent(acs::MEKF_INVALID_INFO);
       mekfInvalidFlag = true;
     }
+    if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE) {
+      navigation.resetMekf(&mekfData);
+    }
     if (mekfInvalidCounter == 5) {
       // Trigger this so STR FDIR can set the device faulty.
       EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, 0);

From 4d74c8ec4381cfa3a5018803d2dd7d5c84c664d5 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 13 Mar 2023 16:24:13 +0100
Subject: [PATCH 05/12] changelog

---
 CHANGELOG.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 7f9681d3..af4cef0a 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -19,6 +19,8 @@ will consitute of a breaking change warranting a new major release:
 ## Added
 
 - Added `EXECUTE_SHELL_CMD` action command for `CoreController` to execute arbitrary Linux commands.
+- Added NaN and Inf check for the `MEKF`. If these are detected, the `AcsController` will reset
+  the `MEKF` on its own.
 
 ## Fixed
 

From f358719778fff29646ac91959d030c17f2a5f6f7 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 13 Mar 2023 16:33:48 +0100
Subject: [PATCH 06/12] added event in case of safe mode controller failure

---
 mission/acsDefs.h                    | 2 ++
 mission/controller/AcsController.cpp | 9 ++++++++-
 mission/controller/AcsController.h   | 3 +++
 3 files changed, 13 insertions(+), 1 deletion(-)

diff --git a/mission/acsDefs.h b/mission/acsDefs.h
index a724bbd7..7ba8dc13 100644
--- a/mission/acsDefs.h
+++ b/mission/acsDefs.h
@@ -32,6 +32,8 @@ static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH);
 static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO);
 //!< MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time.
 static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(4, severity::HIGH);
+//!< The ACS safe mode controller was not able to compute a solution and has failed.
+static constexpr Event SAFE_MODE_CONTROLLER_FAILURE = MAKE_EVENT(5, severity::HIGH);
 
 extern const char* getModeStr(AcsMode mode);
 
diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index d513d114..5eb4abac 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -173,7 +173,14 @@ void AcsController::performSafe() {
         sunTargetDir, satRateSafe, &errAng, magMomMtq);
   }
   if (result == returnvalue::FAILED) {
-    // ToDo: this should never ever happen or we are dead. prob add an event at least
+    if (not doomFlag) {
+      triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE);
+      doomFlag = true;
+    }
+    doomCounter++;
+    if (doomCounter > 5) {
+      doomFlag = false;
+    }
   }
 
   actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h
index c0376127..829ab099 100644
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@@ -62,6 +62,9 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   uint8_t multipleRwUnavailableCounter = 0;
   bool mekfInvalidFlag = false;
   uint8_t mekfInvalidCounter = 0;
+  bool doomFlag = false;
+  uint8_t doomCounter = 0;
+
   int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
   int16_t cmdDipolMtqs[3] = {0, 0, 0};
 

From 1f9935cf22ea369c639632e948d0fbfb46063716 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 14 Mar 2023 13:12:42 +0100
Subject: [PATCH 07/12] tweak scheduling frequencies

---
 CHANGELOG.md                | 7 +++++++
 bsp_q7s/core/scheduling.cpp | 4 ++--
 2 files changed, 9 insertions(+), 2 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index be95f310..828f87a8 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -19,6 +19,8 @@ will consitute of a breaking change warranting a new major release:
 ## Added
 
 - Added `EXECUTE_SHELL_CMD` action command for `CoreController` to execute arbitrary Linux commands.
+- Added some missing PLOC commands.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/462
 
 ## Fixed
 
@@ -26,6 +28,11 @@ will consitute of a breaking change warranting a new major release:
 - Limitation of RW speeds was done before converting them to the correct unit scale.
 - The Syrlinks task now has a proper name instead of `MAIN_SPI`.
 
+## Changed
+
+- I2C PST now has a polling frequency of 0.4 seconds instead of 0.2 seconds.
+- GS PST now has a polling frequency of 0.5 seconds instead of 1 second.
+
 # [v1.37.0] 2023-03-11
 
 eive-tmtc: v2.18.1
diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp
index 33869afd..4faa643a 100644
--- a/bsp_q7s/core/scheduling.cpp
+++ b/bsp_q7s/core/scheduling.cpp
@@ -488,7 +488,7 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction
 
 #if OBSW_ADD_I2C_TEST_CODE == 0
   FixedTimeslotTaskIF* i2cPst = factory.createFixedTimeslotTask(
-      "I2C_PST", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.2, missedDeadlineFunc);
+      "I2C_PST", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.4, missedDeadlineFunc);
   result = pst::pstI2c(i2cPst);
   if (result != returnvalue::OK) {
     if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) {
@@ -503,7 +503,7 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction
 
 #if OBSW_ADD_GOMSPACE_PCDU == 1
   FixedTimeslotTaskIF* gomSpacePstTask = factory.createFixedTimeslotTask(
-      "GS_PST_TASK", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 1.0, missedDeadlineFunc);
+      "GS_PST_TASK", 65, PeriodicTaskIF::MINIMUM_STACK_SIZE * 4, 0.5, missedDeadlineFunc);
   result = pst::pstGompaceCan(gomSpacePstTask);
   if (result != returnvalue::OK) {
     if (result != FixedTimeslotTaskIF::SLOT_LIST_EMPTY) {

From ccf1fcaa7910126165564a1df57dd5e3fc4c811a Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Wed, 15 Mar 2023 10:06:24 +0100
Subject: [PATCH 08/12] stuff

---
 mission/controller/AcsController.cpp | 12 +++++++-----
 mission/controller/AcsController.h   |  4 ++--
 2 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 8e7b1b9a..220ba178 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -173,14 +173,16 @@ void AcsController::performSafe() {
         sunTargetDir, satRateSafe, &errAng, magMomMtq);
   }
   if (result == returnvalue::FAILED) {
-    if (not doomFlag) {
+    if (not safeCtrlFailureFlag) {
       triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE);
-      doomFlag = true;
+      safeCtrlFailureFlag = true;
     }
-    doomCounter++;
-    if (doomCounter > 5) {
-      doomFlag = false;
+    safeCtrlFailureCounter++;
+    if (safeCtrlFailureCounter > 50) {
+      safeCtrlFailureFlag = false;
     }
+  } else {
+    safeCtrlFailureFlag = false;
   }
 
   actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h
index 2b0cda17..3c309bd4 100644
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@@ -62,8 +62,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   uint8_t multipleRwUnavailableCounter = 0;
   bool mekfInvalidFlag = false;
   uint16_t mekfInvalidCounter = 0;
-  bool doomFlag = false;
-  uint8_t doomCounter = 0;
+  bool safeCtrlFailureFlag = false;
+  uint8_t safeCtrlFailureCounter = 0;
 
   int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
   int16_t cmdDipolMtqs[3] = {0, 0, 0};

From c4fe772fe7e76a2d07c6af8d7de2d794682a78ec Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Wed, 15 Mar 2023 10:08:58 +0100
Subject: [PATCH 09/12] changelog

---
 CHANGELOG.md | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 3c1891d6..dfa43418 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -16,6 +16,12 @@ will consitute of a breaking change warranting a new major release:
 
 # [unreleased]
 
+## Added
+
+- Failure of Safe Mode Ctrl will now trigger an event. As this can only be caused by sensors not
+  being in the correct mode, the assemblies should take care that this will never happen and no
+  additional FDIR is needed.
+
 # [v1.37.2] 2023-03-14
 
 - Changed `PoolManager` bugfix implementation in the FSFW.

From 102228100e61f3f0808e3af56ee84a8dd19ed3d3 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Wed, 15 Mar 2023 10:10:47 +0100
Subject: [PATCH 10/12] reran event gens

---
 bsp_hosted/fsfwconfig/events/translateEvents.cpp | 7 +++++--
 generators/bsp_hosted_events.csv                 | 1 +
 generators/bsp_q7s_events.csv                    | 1 +
 generators/events/translateEvents.cpp            | 7 +++++--
 linux/fsfwconfig/events/translateEvents.cpp      | 7 +++++--
 5 files changed, 17 insertions(+), 6 deletions(-)

diff --git a/bsp_hosted/fsfwconfig/events/translateEvents.cpp b/bsp_hosted/fsfwconfig/events/translateEvents.cpp
index e0f9c58f..32b61bca 100644
--- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp
+++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp
@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 277 translations.
+ * @brief    Auto-generated event translation file. Contains 278 translations.
  * @details
- * Generated on: 2023-03-14 17:08:41
+ * Generated on: 2023-03-15 10:10:04
  */
 #include "translateEvents.h"
 
@@ -97,6 +97,7 @@ const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY";
 const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID";
 const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO";
 const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
+const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE";
 const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
 const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
 const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
@@ -464,6 +465,8 @@ const char *translateEvents(Event event) {
       return MEKF_INVALID_INFO_STRING;
     case (11204):
       return MEKF_INVALID_MODE_VIOLATION_STRING;
+    case (11205):
+      return SAFE_MODE_CONTROLLER_FAILURE_STRING;
     case (11300):
       return SWITCH_CMD_SENT_STRING;
     case (11301):
diff --git a/generators/bsp_hosted_events.csv b/generators/bsp_hosted_events.csv
index ec5050cc..2f10693c 100644
--- a/generators/bsp_hosted_events.csv
+++ b/generators/bsp_hosted_events.csv
@@ -91,6 +91,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;No description;mission/acsDefs.h
 11203;0x2bc3;MEKF_INVALID_INFO;INFO;No description;mission/acsDefs.h
 11204;0x2bc4;MEKF_INVALID_MODE_VIOLATION;HIGH;No description;mission/acsDefs.h
+11205;0x2bc5;SAFE_MODE_CONTROLLER_FAILURE;HIGH;No description;mission/acsDefs.h
 11300;0x2c24;SWITCH_CMD_SENT;INFO;Indicates that a FSFW object requested setting a switch P1: 1 if on was requested, 0 for off | P2: Switch Index;mission/devices/devicedefinitions/powerDefinitions.h
 11301;0x2c25;SWITCH_HAS_CHANGED;INFO;Indicated that a switch state has changed P1: New switch state, 1 for on, 0 for off | P2: Switch Index;mission/devices/devicedefinitions/powerDefinitions.h
 11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/devices/devicedefinitions/powerDefinitions.h
diff --git a/generators/bsp_q7s_events.csv b/generators/bsp_q7s_events.csv
index ec5050cc..2f10693c 100644
--- a/generators/bsp_q7s_events.csv
+++ b/generators/bsp_q7s_events.csv
@@ -91,6 +91,7 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
 11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;No description;mission/acsDefs.h
 11203;0x2bc3;MEKF_INVALID_INFO;INFO;No description;mission/acsDefs.h
 11204;0x2bc4;MEKF_INVALID_MODE_VIOLATION;HIGH;No description;mission/acsDefs.h
+11205;0x2bc5;SAFE_MODE_CONTROLLER_FAILURE;HIGH;No description;mission/acsDefs.h
 11300;0x2c24;SWITCH_CMD_SENT;INFO;Indicates that a FSFW object requested setting a switch P1: 1 if on was requested, 0 for off | P2: Switch Index;mission/devices/devicedefinitions/powerDefinitions.h
 11301;0x2c25;SWITCH_HAS_CHANGED;INFO;Indicated that a switch state has changed P1: New switch state, 1 for on, 0 for off | P2: Switch Index;mission/devices/devicedefinitions/powerDefinitions.h
 11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/devices/devicedefinitions/powerDefinitions.h
diff --git a/generators/events/translateEvents.cpp b/generators/events/translateEvents.cpp
index e0f9c58f..32b61bca 100644
--- a/generators/events/translateEvents.cpp
+++ b/generators/events/translateEvents.cpp
@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 277 translations.
+ * @brief    Auto-generated event translation file. Contains 278 translations.
  * @details
- * Generated on: 2023-03-14 17:08:41
+ * Generated on: 2023-03-15 10:10:04
  */
 #include "translateEvents.h"
 
@@ -97,6 +97,7 @@ const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY";
 const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID";
 const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO";
 const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
+const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE";
 const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
 const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
 const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
@@ -464,6 +465,8 @@ const char *translateEvents(Event event) {
       return MEKF_INVALID_INFO_STRING;
     case (11204):
       return MEKF_INVALID_MODE_VIOLATION_STRING;
+    case (11205):
+      return SAFE_MODE_CONTROLLER_FAILURE_STRING;
     case (11300):
       return SWITCH_CMD_SENT_STRING;
     case (11301):
diff --git a/linux/fsfwconfig/events/translateEvents.cpp b/linux/fsfwconfig/events/translateEvents.cpp
index e0f9c58f..32b61bca 100644
--- a/linux/fsfwconfig/events/translateEvents.cpp
+++ b/linux/fsfwconfig/events/translateEvents.cpp
@@ -1,7 +1,7 @@
 /**
- * @brief    Auto-generated event translation file. Contains 277 translations.
+ * @brief    Auto-generated event translation file. Contains 278 translations.
  * @details
- * Generated on: 2023-03-14 17:08:41
+ * Generated on: 2023-03-15 10:10:04
  */
 #include "translateEvents.h"
 
@@ -97,6 +97,7 @@ const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY";
 const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID";
 const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO";
 const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
+const char *SAFE_MODE_CONTROLLER_FAILURE_STRING = "SAFE_MODE_CONTROLLER_FAILURE";
 const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
 const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
 const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
@@ -464,6 +465,8 @@ const char *translateEvents(Event event) {
       return MEKF_INVALID_INFO_STRING;
     case (11204):
       return MEKF_INVALID_MODE_VIOLATION_STRING;
+    case (11205):
+      return SAFE_MODE_CONTROLLER_FAILURE_STRING;
     case (11300):
       return SWITCH_CMD_SENT_STRING;
     case (11301):

From 95a4d88ed465489ca9b9a26e5570134d2c50e19f Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Wed, 15 Mar 2023 10:17:24 +0100
Subject: [PATCH 11/12] cleanup

---
 .../acs/MultiplicativeKalmanFilter.cpp        |  7 -------
 .../acs/MultiplicativeKalmanFilter.h          |  6 ++----
 mission/controller/acs/util/MathOperations.h  | 20 -------------------
 3 files changed, 2 insertions(+), 31 deletions(-)

diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
index e752da39..77a3ef00 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
@@ -1080,12 +1080,6 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
 
   MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
 
-  if (not(MathOperations<double>::checkVectorIsFinite(propagatedQuaternion, 4)) ||
-      not(MathOperations<double>::checkMatrixIsFinite(initialQuaternion, 6, 6))) {
-    updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE);
-    return MEKF_NOT_FINITE;
-  }
-
   updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst);
   return MEKF_RUNNING;
 }
@@ -1095,7 +1089,6 @@ ReturnValue_t MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
   double resetCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                         {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
   std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double));
-  std::memcpy(propagatedQuaternion, resetQuaternion, 4 * sizeof(double));
   std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double));
   updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
   return MEKF_UNINITIALIZED;
diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h
index fe749890..ceb98339 100644
--- a/mission/controller/acs/MultiplicativeKalmanFilter.h
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.h
@@ -62,7 +62,6 @@ class MultiplicativeKalmanFilter {
     NO_MODEL_VECTORS = 2,
     NO_SUS_MGM_STR_DATA = 3,
     COVARIANCE_INVERSION_FAILED = 4,
-    NOT_FINITE = 5,
     INITIALIZED = 10,
     RUNNING = 11,
   };
@@ -75,9 +74,8 @@ class MultiplicativeKalmanFilter {
   static constexpr ReturnValue_t MEKF_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_MEKF_ID, 5);
   static constexpr ReturnValue_t MEKF_COVARIANCE_INVERSION_FAILED =
       returnvalue::makeCode(IF_MEKF_ID, 6);
-  static constexpr ReturnValue_t MEKF_NOT_FINITE = returnvalue::makeCode(IF_MEKF_ID, 7);
-  static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 8);
-  static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 9);
+  static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7);
+  static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8);
 
  private:
   /*Parameters*/
diff --git a/mission/controller/acs/util/MathOperations.h b/mission/controller/acs/util/MathOperations.h
index b344451a..f8537740 100644
--- a/mission/controller/acs/util/MathOperations.h
+++ b/mission/controller/acs/util/MathOperations.h
@@ -404,26 +404,6 @@ class MathOperations {
     std::memcpy(inverse, identity, sizeof(identity));
     return 0;  // successful inversion
   }
-
-  static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
-    for (uint8_t i = 0; i < size; i++) {
-      if (not isfinite(inputVector[i])) {
-        return false;
-      }
-    }
-    return true;
-  }
-
-  static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
-    for (uint8_t col = 0; col < cols; col++) {
-      for (uint8_t row = 0; row < rows; row++) {
-        if (not isfinite(inputMatrix[row * cols + cols])) {
-          return false;
-        }
-      }
-    }
-    return true;
-  }
 };
 
 #endif /* ACS_MATH_MATHOPERATIONS_H_ */

From be848ea5e4392e86ce0b9fb43104d15348defeb1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Wed, 15 Mar 2023 11:32:58 +0100
Subject: [PATCH 12/12] changelog update

---
 CHANGELOG.md | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 9b2a096f..66bb6c93 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -19,6 +19,8 @@ will consitute of a breaking change warranting a new major release:
 ## Changed
 
 - Telemetry relevant datasets for the RWs are now set invalid and partially reset on shotdown.
+- I2C PST now has a polling frequency of 0.4 seconds instead of 0.2 seconds.
+- GS PST now has a polling frequency of 0.5 seconds instead of 1 second.
 
 # [v1.37.2] 2023-03-14
 
@@ -57,8 +59,6 @@ eive-tmtc: v2.19.1
 ## Changed
 
 - Set `OBSW_ADD_TCS_CTRL` to 1. Always add TCS controller now for both EM and FM.
-- I2C PST now has a polling frequency of 0.4 seconds instead of 0.2 seconds.
-- GS PST now has a polling frequency of 0.5 seconds instead of 1 second.
 - generators module: Bump `fsfwgen` dependency to v0.3.1. The returnvalue CSV files are now sorted.
 - generators module: Always generate subsystem ID CSV files now.