From abe5f43ae95d7eadf9e958f9e7ff4b3bf7682a18 Mon Sep 17 00:00:00 2001
From: Marius Eggert <eggertm@irs.uni-stuttgart.de>
Date: Fri, 30 Sep 2022 11:06:17 +0200
Subject: [PATCH] added performDetumble and implemented ckeckModeCommand

---
 mission/controller/AcsController.cpp | 61 ++++++++++++++++++++++++++--
 mission/controller/AcsController.h   | 21 ++++++++++
 2 files changed, 78 insertions(+), 4 deletions(-)

diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp
index 896af482..26f6a1ee 100644
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@@ -1,11 +1,14 @@
 #include "AcsController.h"
 
 #include <fsfw/datapool/PoolReadGuard.h>
-
-#include <iostream>
+#include <fsfw/timemanager/Clock.h>
 
 AcsController::AcsController(object_id_t objectId)
-    : ExtendedControllerBase(objectId, objects::NO_OBJECT), mgmData(this), susData(this) {}
+    : ExtendedControllerBase(objectId, objects::NO_OBJECT),
+      mgmData(this),
+      susData(this),
+      sensorProcessing(&acsParameters),
+      detumbleCounter{0} {}
 
 ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
   return returnvalue::OK;
@@ -45,6 +48,43 @@ void AcsController::performControlOperation() {
   }*/
 }
 
+void AcsController::performDetumble() {
+  ACS::SensorValues sensorValues;
+  ACS::OutputValues outputValues;
+
+  // sensorValues.read();
+  // outputValues.read();
+
+  timeval now;  // = {0,0};
+  Clock::getClock_timeval(&now);
+
+  sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
+  ReturnValue_t validMekf;
+  navigation.useMekf(&sensorValues, &outputValues, &validMekf);
+  double magMomMtq[3] = {0, 0, 0};
+  detumble.bDotLaw(outputValues.magneticFieldVectorDerivative,
+                   &outputValues.magneticFieldVectorDerivativeValid, outputValues.magFieldEst,
+                   &outputValues.magFieldEstValid, magMomMtq);
+  double dipolCmdUnits[3] = {0, 0, 0};
+  actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);
+
+  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) {
+    detumbleCounter++;
+  }
+
+  if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
+    submode = SUBMODE_SAFE;
+    detumbleCounter = 0;
+  }
+}
+
 ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                      LocalDataPoolManager &poolManager) {
   localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0PoolVec);
@@ -68,7 +108,20 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
 
 ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
                                               uint32_t *msToReachTheMode) {
-  return returnvalue::OK;
+  if (mode == MODE_OFF) {
+    if (submode == SUBMODE_NONE) {
+      return returnvalue::OK;
+    } else {
+      return INVALID_SUBMODE;
+    }
+  } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) {
+    if ((submode > 5) || (submode < 2)) {
+      return INVALID_SUBMODE;
+    } else {
+      return returnvalue::OK;
+    }
+  }
+  return INVALID_MODE;
 }
 
 void AcsController::copyMgmData() {
diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h
index 210b1c04..21c77683 100644
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@@ -3,7 +3,12 @@
 
 #include <commonObjects.h>
 #include <fsfw/controller/ExtendedControllerBase.h>
+#include <fsfw/globalfunctions/math/VectorOperations.h>
 
+#include "acs/ActuatorCmd.h"
+#include "acs/Navigation.h"
+#include "acs/SensorProcessing.h"
+#include "acs/control/Detumble.h"
 #include "controllerdefinitions/AcsCtrlDefinitions.h"
 #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
 #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
@@ -16,7 +21,23 @@ class AcsController : public ExtendedControllerBase {
 
   AcsController(object_id_t objectId);
 
+  static const Submode_t SUBMODE_SAFE = 2;
+  static const Submode_t SUBMODE_DETUMBLE = 3;
+  static const Submode_t SUBMODE_PTG_GS = 4;
+  static const Submode_t SUBMODE_PTG_NADIR = 5;
+
+ protected:
+  void performDetumble();
+
  private:
+  AcsParameters acsParameters;
+  SensorProcessing sensorProcessing;
+  Navigation navigation;
+  ActuatorCmd actuatorCmd;
+
+  Detumble detumble;
+  uint8_t detumbleCounter;
+
   enum class InternalState { STARTUP, INITIAL_DELAY, READY };
 
   InternalState internalState = InternalState::STARTUP;