From 1e521f0575872412cd8a4bdefc9476ff85739357 Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 14 Apr 2023 11:38:05 +0200
Subject: [PATCH] detumble mode changes

---
 mission/controller/AcsController.cpp        | 11 ++++++-----
 mission/controller/acs/AcsParameters.cpp    |  8 +++++++-
 mission/controller/acs/AcsParameters.h      |  8 +++++---
 mission/controller/acs/control/Detumble.cpp |  4 ++--
 mission/controller/acs/control/Detumble.h   |  2 +-
 5 files changed, 21 insertions(+), 12 deletions(-)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index cd9e7bdb..a78b6669 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -259,18 +259,19 @@ void AcsController::performDetumble() {
     triggerEvent(acs::MEKF_RECOVERY);
     mekfInvalidFlag = false;
   }
-  uint8_t safeCtrlStrat = detumble.detumbleStrategy(mgmDataProcessed.mgmVecTot.isValid(),
-                                                    gyrDataProcessed.gyrVecTot.isValid(),
-                                                    mgmDataProcessed.mgmVecTotDerivative.isValid());
+  uint8_t safeCtrlStrat = detumble.detumbleStrategy(
+      mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
+      mgmDataProcessed.mgmVecTotDerivative.isValid(),
+      acsParameters.detumbleParameter.useFullDetumbleLaw);
   double magMomMtq[3] = {0, 0, 0};
   switch (safeCtrlStrat) {
     case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL):
       detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value,
-                           magMomMtq, acsParameters.detumbleParameter.gainD);
+                           magMomMtq, acsParameters.detumbleParameter.gainFull);
       break;
     case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
       detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value,
-                       magMomMtq, acsParameters.detumbleParameter.gainD);
+                       magMomMtq, acsParameters.detumbleParameter.gainBdot);
       break;
     case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
       safeCtrlFailure(1, 0);
diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp
index fb2adf43..03500cc3 100644
--- a/mission/controller/acs/AcsParameters.cpp
+++ b/mission/controller/acs/AcsParameters.cpp
@@ -711,7 +711,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
           parameterWrapper->set(detumbleParameter.omegaDetumbleEnd);
           break;
         case 0x3:
-          parameterWrapper->set(detumbleParameter.gainD);
+          parameterWrapper->set(detumbleParameter.gainBdot);
+          break;
+        case 0x4:
+          parameterWrapper->set(detumbleParameter.gainFull);
+          break;
+        case 0x5:
+          parameterWrapper->set(detumbleParameter.useFullDetumbleLaw);
           break;
         default:
           return INVALID_IDENTIFIER_ID;
diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h
index 0b37cfe0..0c1ca9a7 100644
--- a/mission/controller/acs/AcsParameters.h
+++ b/mission/controller/acs/AcsParameters.h
@@ -821,7 +821,7 @@ class AcsParameters : public HasParametersIF {
     double sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)};
     double sunTargetDir[3] = {0, 0, 1};
 
-    uint8_t useMekf = true;
+    uint8_t useMekf = false;
     uint8_t dampingDuringEclipse = true;
   } safeModeControllerParameters;
 
@@ -931,8 +931,10 @@ class AcsParameters : public HasParametersIF {
   struct DetumbleParameter {
     uint8_t detumblecounter = 75;  // 30 s
     double omegaDetumbleStart = 2 * M_PI / 180;
-    double omegaDetumbleEnd = 0.4 * M_PI / 180;
-    double gainD = pow(10.0, -3.3);
+    double omegaDetumbleEnd = 1 * M_PI / 180;
+    double gainBdot = pow(10.0, -3.3);
+    double gainFull = pow(10.0, -2.3);
+    uint8_t useFullDetumbleLaw = false;
   } detumbleParameter;
 };
 
diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp
index 2318d735..9ca20862 100644
--- a/mission/controller/acs/control/Detumble.cpp
+++ b/mission/controller/acs/control/Detumble.cpp
@@ -8,10 +8,10 @@ Detumble::Detumble() {}
 Detumble::~Detumble() {}
 
 uint8_t Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
-                                   const bool magFieldRateValid) {
+                                   const bool magFieldRateValid, const bool useFullDetumbleLaw) {
   if (not magFieldValid) {
     return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
-  } else if (satRotRateValid) {
+  } else if (satRotRateValid and useFullDetumbleLaw) {
     return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL;
   } else if (magFieldRateValid) {
     return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED;
diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h
index 576f4bae..4424896e 100644
--- a/mission/controller/acs/control/Detumble.h
+++ b/mission/controller/acs/control/Detumble.h
@@ -12,7 +12,7 @@ class Detumble {
   virtual ~Detumble();
 
   uint8_t detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
-                           const bool magFieldRateValid);
+                           const bool magFieldRateValid, const bool useFullDetumbleLaw);
 
   void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB,
                    double gain);