From dc6a2ffe56ad448922ae61194329d334cca795ee Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 19 Jan 2022 14:08:30 +0100
Subject: [PATCH] minor ADIS updates

---
 bsp_q7s/core/ObjectFactory.cpp           |  7 +++++++
 fsfw                                     |  2 +-
 mission/devices/GyroADIS1650XHandler.cpp | 21 +++++++++++++++------
 mission/devices/GyroADIS1650XHandler.h   |  4 ++++
 tmtc                                     |  2 +-
 5 files changed, 28 insertions(+), 8 deletions(-)

diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp
index 69f6dfaf..baafa215 100644
--- a/bsp_q7s/core/ObjectFactory.cpp
+++ b/bsp_q7s/core/ObjectFactory.cpp
@@ -521,6 +521,10 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI
   auto adisHandler = new GyroADIS1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_COM_IF,
                                               spiCookie, ADIS1650X::Type::ADIS16505);
   adisHandler->setStartUpImmediately();
+#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1
+  adisHandler->setToGoToNormalModeImmediately();
+#endif
+
   // Gyro 1 Side A
   spiCookie =
       new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, spiDev, L3GD20H::MAX_BUFFER_SIZE,
@@ -538,6 +542,9 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI
   adisHandler = new GyroADIS1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_COM_IF,
                                          spiCookie, ADIS1650X::Type::ADIS16505);
   adisHandler->setStartUpImmediately();
+#if FSFW_HAL_ADIS1650X_GYRO_DEBUG == 1
+  adisHandler->setToGoToNormalModeImmediately();
+#endif
   // Gyro 3 Side B
   spiCookie =
       new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, spiDev, L3GD20H::MAX_BUFFER_SIZE,
diff --git a/fsfw b/fsfw
index 933da2f6..9b770602 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit 933da2f655717b66c1056e764c47a2eee473a137
+Subproject commit 9b77060295c9c32ebfc2e7cf6517eb2e66216191
diff --git a/mission/devices/GyroADIS1650XHandler.cpp b/mission/devices/GyroADIS1650XHandler.cpp
index 7ea76758..717a1bfd 100644
--- a/mission/devices/GyroADIS1650XHandler.cpp
+++ b/mission/devices/GyroADIS1650XHandler.cpp
@@ -36,6 +36,7 @@ void GyroADIS1650XHandler::doStartUp() {
   // Initial 310 ms start up time after power-up
   if (internalState == InternalState::STARTUP) {
     if (not commandExecuted) {
+      warningSwitch = true;
       breakCountdown.setTimeout(ADIS1650X::START_UP_TIME);
       commandExecuted = true;
     }
@@ -54,8 +55,11 @@ void GyroADIS1650XHandler::doStartUp() {
   }
 
   if (internalState == InternalState::IDLE) {
-    setMode(MODE_NORMAL);
-    // setMode(MODE_ON);
+    if(goToNormalMode) {
+      setMode(MODE_NORMAL);
+    } else {
+      setMode(MODE_ON);
+    }
   }
 }
 
@@ -81,7 +85,7 @@ ReturnValue_t GyroADIS1650XHandler::buildTransitionDeviceCommand(DeviceCommandId
       break;
     }
     default: {
-      /* Might be a configuration error. */
+      // Might be a configuration error
       sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: "
                     "Unknown internal state!"
                  << std::endl;
@@ -207,8 +211,11 @@ ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
       if (((adisType == ADIS1650X::Type::ADIS16507) and (readProdId != ADIS1650X::PROD_ID_16507)) or
           ((adisType == ADIS1650X::Type::ADIS16505) and (readProdId != ADIS1650X::PROD_ID_16505))) {
 #if OBSW_VERBOSE_LEVEL >= 1
-        sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
-                     << readProdId << std::endl;
+        if(warningSwitch) {
+          sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
+                       << readProdId << std::endl;
+        }
+        warningSwitch = false;
 #endif
         return HasReturnvaluesIF::RETURN_FAILED;
       }
@@ -321,7 +328,7 @@ ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) {
 }
 
 uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
-  return 10000;
+  return 6000;
 }
 
 void GyroADIS1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
@@ -479,4 +486,6 @@ ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *
   return HasReturnvaluesIF::RETURN_OK;
 }
 
+void GyroADIS1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; }
+
 #endif /* OBSW_ADIS1650X_LINUX_COM_IF == 1 */
diff --git a/mission/devices/GyroADIS1650XHandler.h b/mission/devices/GyroADIS1650XHandler.h
index c1f9ae98..1db0f3a8 100644
--- a/mission/devices/GyroADIS1650XHandler.h
+++ b/mission/devices/GyroADIS1650XHandler.h
@@ -23,6 +23,8 @@ class GyroADIS1650XHandler : public DeviceHandlerBase {
   GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
                        ADIS1650X::Type type);
 
+  void setToGoToNormalModeImmediately();
+
   // DeviceHandlerBase abstract function implementation
   void doStartUp() override;
   void doShutDown() override;
@@ -43,6 +45,8 @@ class GyroADIS1650XHandler : public DeviceHandlerBase {
   ADIS1650X::Type adisType;
   AdisGyroPrimaryDataset primaryDataset;
   AdisGyroConfigDataset configDataset;
+  bool goToNormalMode = false;
+  bool warningSwitch = true;
 
   enum class InternalState { STARTUP, CONFIG, IDLE };
 
diff --git a/tmtc b/tmtc
index 8417d342..6f24d6a8 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit 8417d34265555dbcfb92cb2f73e20dddbd8fb924
+Subproject commit 6f24d6a83995ca7a895c17a77a00bceac4d7f141