From 69099881bd112c4938726d21227bc9743c56273b Mon Sep 17 00:00:00 2001
From: Marius Eggert <eggertm@irs.uni-stuttgart.de>
Date: Tue, 25 Oct 2022 11:31:54 +0200
Subject: [PATCH] small stuff

---
 mission/controller/AcsController.cpp  | 33 +++++++++++----------------
 mission/controller/AcsController.h    |  2 +-
 mission/controller/acs/OutputValues.h |  2 +-
 3 files changed, 15 insertions(+), 22 deletions(-)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index b42d9c83..e82cbb7d 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -97,7 +97,7 @@ void AcsController::performSafe() {
 
   sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
   ReturnValue_t validMekf;
-  navigation.useMekf(&sensorValues, &outputValues, &validMekf);  // DOES THIS WORK WITH VALID?
+  navigation.useMekf(&sensorValues, &outputValues, &validMekf);
 
   // Give desired satellite rate and sun direction to align
   double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
@@ -127,25 +127,18 @@ void AcsController::performSafe() {
   if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) >
                                            acsParameters.detumbleParameter.omegaDetumbleStart) {
     detumbleCounter++;
-  }
-
-  else if (outputValues.satRateEstValid &&
-           VectorOperations<double>::norm(outputValues.satRateEst, 3) >
-               acsParameters.detumbleParameter.omegaDetumbleStart) {
+  } else if (outputValues.satRateEstValid &&
+             VectorOperations<double>::norm(outputValues.satRateEst, 3) >
+                 acsParameters.detumbleParameter.omegaDetumbleStart) {
     detumbleCounter++;
-
-  }
-
-  else {
+  } else {
     detumbleCounter = 0;
   }
-
   if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
     submode = SUBMODE_DETUMBLE;
     detumbleCounter = 0;
   }
-
-  //	commanding.magnetorquesDipol();
+  //  	commanding.magnetorquesDipol();
 }
 
 void AcsController::performDetumble() {
@@ -154,7 +147,6 @@ void AcsController::performDetumble() {
 
   timeval now;
   Clock::getClock_timeval(&now);
-  // Clock::getUptime(&now);
 
   sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
   ReturnValue_t validMekf;
@@ -170,14 +162,13 @@ void AcsController::performDetumble() {
   if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) <
                                            acsParameters.detumbleParameter.omegaDetumbleEnd) {
     detumbleCounter++;
-  }
-
-  else if (outputValues.satRateEstValid &&
-           VectorOperations<double>::norm(outputValues.satRateEst, 3) <
-               acsParameters.detumbleParameter.omegaDetumbleEnd) {
+  } else if (outputValues.satRateEstValid &&
+             VectorOperations<double>::norm(outputValues.satRateEst, 3) <
+                 acsParameters.detumbleParameter.omegaDetumbleEnd) {
     detumbleCounter++;
+  } else {
+    detumbleCounter = 0;
   }
-
   if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
     submode = SUBMODE_SAFE;
     detumbleCounter = 0;
@@ -235,6 +226,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc);
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_VEC, &mgm2VecProc);
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_VEC, &mgm3VecProc);
+  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_4_VEC, &mgm4VecProc);
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot);
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer);
   localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf);
@@ -539,3 +531,4 @@ void AcsController::copyGyrData() {
     }
   }
 }
+
diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h
index 87aaf37c..dcd78621 100644
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@@ -82,7 +82,7 @@ class AcsController : public ExtendedControllerBase {
   PoolEntry<float> mgm2VecProc = PoolEntry<float>(3);
   PoolEntry<float> mgm3VecProc = PoolEntry<float>(3);
   PoolEntry<float> mgm4VecProc = PoolEntry<float>(3);
-  PoolEntry<float> mgmVecTot = PoolEntry<float>(3);
+  PoolEntry<double> mgmVecTot = PoolEntry<double>(3);
   PoolEntry<float> mgmVecTotDer = PoolEntry<float>(3);
   PoolEntry<double> magIgrf = PoolEntry<double>(3);
 
diff --git a/mission/controller/acs/OutputValues.h b/mission/controller/acs/OutputValues.h
index bd182f42..bb1ed007 100644
--- a/mission/controller/acs/OutputValues.h
+++ b/mission/controller/acs/OutputValues.h
@@ -19,7 +19,7 @@ class OutputValues {
   double magneticFieldVectorDerivative[3];
   bool magneticFieldVectorDerivativeValid;
 
-  bool mgmUpdated;  // ToDo: ????
+  bool mgmUpdated;  // ToDo: relic from FLP. most likely not used
 
   double sunDirEst[3];  // sensor fusion (G)
   bool sunDirEstValid;