From 521b47d9fc5ca4a88391c64cc269e0aecf2667f4 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 27 Jan 2022 15:29:58 +0100
Subject: [PATCH] decoupled IMTQ handler from OBSWConfig.h

---
 bsp_q7s/core/ObjectFactory.cpp                    |  1 +
 .../pollingsequence/pollingSequenceFactory.cpp    |  1 -
 mission/devices/IMTQHandler.cpp                   | 15 +++++++--------
 mission/devices/IMTQHandler.h                     |  3 ++-
 4 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp
index a453cdf0..09a0a46a 100644
--- a/bsp_q7s/core/ObjectFactory.cpp
+++ b/bsp_q7s/core/ObjectFactory.cpp
@@ -147,6 +147,7 @@ void ObjectFactory::produce(void* args) {
       new I2cCookie(addresses::IMTQ, IMTQ::MAX_REPLY_SIZE, q7s::I2C_DEFAULT_DEV);
   auto imtqHandler = new IMTQHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie);
 #if OBSW_DEBUG_IMTQ == 1
+  imtqHandler->setToGoToNormal();
   imtqHandler->setStartUpImmediately();
 #endif
 #endif
diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
index d8e659e5..70119272 100644
--- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
+++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
@@ -517,7 +517,6 @@ ReturnValue_t pst::pstSpi(FixedTimeslotTaskIF *thisSequence) {
 }
 
 ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) {
-
   // Length of a communication cycle
   uint32_t length = thisSequence->getPeriodMs();
 #if OBSW_ADD_MGT == 1
diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/IMTQHandler.cpp
index 499b4d6e..a4323d1c 100644
--- a/mission/devices/IMTQHandler.cpp
+++ b/mission/devices/IMTQHandler.cpp
@@ -2,9 +2,8 @@
 
 #include <fsfw/datapool/PoolReadGuard.h>
 #include <fsfw/globalfunctions/CRC.h>
-#include <math.h>
 
-#include "OBSWConfig.h"
+#include <cmath>
 
 IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie)
     : DeviceHandlerBase(objectId, comIF, comCookie),
@@ -25,11 +24,11 @@ IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC
 IMTQHandler::~IMTQHandler() {}
 
 void IMTQHandler::doStartUp() {
-#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
-  setMode(MODE_NORMAL);
-#else
-  setMode(_MODE_TO_ON);
-#endif
+  if (goToNormalMode) {
+    setMode(MODE_NORMAL);
+  } else {
+    setMode(_MODE_TO_ON);
+  }
 }
 
 void IMTQHandler::doShutDown() { setMode(_MODE_POWER_DOWN); }
@@ -694,7 +693,7 @@ void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
 #endif
 }
 
-void IMTQHandler::setModeNormal() { mode = MODE_NORMAL; }
+void IMTQHandler::setToGoToNormal() { mode = MODE_NORMAL; }
 
 void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) {
   if (wiretappingMode == RAW) {
diff --git a/mission/devices/IMTQHandler.h b/mission/devices/IMTQHandler.h
index b12cdf97..662b555b 100644
--- a/mission/devices/IMTQHandler.h
+++ b/mission/devices/IMTQHandler.h
@@ -18,7 +18,7 @@ class IMTQHandler : public DeviceHandlerBase {
   /**
    * @brief   Sets mode to MODE_NORMAL. Can be used for debugging.
    */
-  void setModeNormal();
+  void setToGoToNormal();
 
  protected:
   void doStartUp() override;
@@ -96,6 +96,7 @@ class IMTQHandler : public DeviceHandlerBase {
   IMTQ::NegZSelfTestSet negZselfTestDataset;
 
   uint8_t commandBuffer[IMTQ::MAX_COMMAND_SIZE];
+  bool goToNormalMode = false;
 
   enum class CommunicationStep {
     GET_ENG_HK_DATA,