From 06c5344d8a44b0cd73209c1fc649ca6713a5622a Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Wed, 7 Jun 2023 15:14:03 +0200
Subject: [PATCH] this might actually work

---
 linux/acs/AcsBoardPolling.cpp       | 1 -
 mission/acs/GyrAdis1650XHandler.cpp | 9 ++++++---
 2 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/linux/acs/AcsBoardPolling.cpp b/linux/acs/AcsBoardPolling.cpp
index 727e8405..b8539a8f 100644
--- a/linux/acs/AcsBoardPolling.cpp
+++ b/linux/acs/AcsBoardPolling.cpp
@@ -591,7 +591,6 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
     gyro.performStartup = false;
     gyro.replyResult = returnvalue::OK;
   }
-  sif::debug << "hello world 2" << std::endl;
   // Read regular registers
   std::memcpy(cmdBuf.data(), adis1650x::BURST_READ_ENABLE.data(),
               adis1650x::BURST_READ_ENABLE.size());
diff --git a/mission/acs/GyrAdis1650XHandler.cpp b/mission/acs/GyrAdis1650XHandler.cpp
index 6f447178..fe10c214 100644
--- a/mission/acs/GyrAdis1650XHandler.cpp
+++ b/mission/acs/GyrAdis1650XHandler.cpp
@@ -15,7 +15,7 @@ GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t devic
 }
 
 void GyrAdis1650XHandler::doStartUp() {
-  if (internalState != InternalState::STARTUP) {
+  if (internalState == InternalState::NONE) {
     internalState = InternalState::STARTUP;
     commandExecuted = false;
   }
@@ -24,12 +24,13 @@ void GyrAdis1650XHandler::doStartUp() {
     if (not commandExecuted) {
       warningSwitch = true;
       breakCountdown.setTimeout(adis1650x::START_UP_TIME);
+      updatePeriodicReply(true, adis1650x::REPLY);
       commandExecuted = true;
     }
-    updatePeriodicReply(true, adis1650x::REPLY);
   }
   if (internalState == InternalState::STARTUP_DONE) {
     setMode(MODE_ON);
+    commandExecuted = false;
     internalState = InternalState::NONE;
   }
 }
@@ -62,7 +63,6 @@ ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_
       }
       *id = adis1650x::REQUEST;
       adisRequest.cfg.decRateReg = adis1650x::DEC_RATE;
-      internalState = InternalState::STARTUP_DONE;
       return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL);
     }
     case (InternalState::SHUTDOWN): {
@@ -93,6 +93,9 @@ ReturnValue_t GyrAdis1650XHandler::scanForReply(const uint8_t *start, size_t rem
       getMode() == _MODE_POWER_DOWN) {
     return IGNORE_FULL_PACKET;
   }
+  if (internalState == InternalState::STARTUP) {
+    internalState = InternalState::STARTUP_DONE;
+  }
   *foundLen = remainingSize;
   if (remainingSize != sizeof(acs::Adis1650XReply)) {
     return returnvalue::FAILED;