added performDetumble and implemented ckeckModeCommand

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() {