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] 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};