From 753b9f8a0ed4b373c090c97ab92f996c9cf1e6b0 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Thu, 20 Jul 2023 14:43:43 +0200
Subject: [PATCH] better detumble exit

---
 mission/controller/AcsController.cpp | 23 ++++++++++++++++-------
 1 file changed, 16 insertions(+), 7 deletions(-)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index f750aac0..0474a886 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -318,17 +318,26 @@ void AcsController::performDetumble() {
   actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
                            acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
 
-  if (mekfData.satRotRateMekf.isValid() &&
-      VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
-          acsParameters.detumbleParameter.omegaDetumbleEnd) {
-    detumbleCounter++;
-  } else if (gyrDataProcessed.gyrVecTot.isValid() &&
-             VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
-                 acsParameters.detumbleParameter.omegaDetumbleEnd) {
+  if (acsParameters.safeModeControllerParameters.useMekf) {
+    if (mekfData.satRotRateMekf.isValid() and
+        VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
+            acsParameters.detumbleParameter.omegaDetumbleStart) {
+      detumbleCounter++;
+    }
+  } else if (acsParameters.safeModeControllerParameters.useGyr) {
+    if (gyrDataProcessed.gyrVecTot.isValid() and
+        VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
+            acsParameters.detumbleParameter.omegaDetumbleStart) {
+      detumbleCounter++;
+    }
+  } else if (fusedRotRateData.rotRateTotal.isValid() and
+             VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
+                 acsParameters.detumbleParameter.omegaDetumbleStart) {
     detumbleCounter++;
   } else if (detumbleCounter > 0) {
     detumbleCounter -= 1;
   }
+
   if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
     detumbleCounter = 0;
     // Triggers safe mode transition in subsystem