From ccce16dd8f3d955058b6e3b6ba313e0b0ac53f1e Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 26 May 2021 13:41:26 +0200
Subject: [PATCH] adis handler fixes

---
 mission/devices/GyroADIS16507Handler.cpp | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/mission/devices/GyroADIS16507Handler.cpp b/mission/devices/GyroADIS16507Handler.cpp
index 97426e81..2c3ed5a4 100644
--- a/mission/devices/GyroADIS16507Handler.cpp
+++ b/mission/devices/GyroADIS16507Handler.cpp
@@ -29,6 +29,7 @@ GyroADIS16507Handler::GyroADIS16507Handler(object_id_t objectId,
 }
 
 void GyroADIS16507Handler::doStartUp() {
+    // Initial 310 ms start up time after power-up
     if(internalState == InternalState::STARTUP) {
         if(not commandExecuted) {
             breakCountdown.setTimeout(ADIS16507::START_UP_TIME);
@@ -36,9 +37,11 @@ void GyroADIS16507Handler::doStartUp() {
         }
         if(breakCountdown.hasTimedOut()) {
             internalState = InternalState::CONFIG;
+            commandExecuted = false;
         }
     }
 
+    // Read all configuration registers first
     if(internalState == InternalState::CONFIG) {
         if(commandExecuted) {
             commandExecuted = false;
@@ -48,6 +51,7 @@ void GyroADIS16507Handler::doStartUp() {
 
     if(internalState == InternalState::IDLE) {
         setMode(MODE_NORMAL);
+        // setMode(MODE_ON);
     }
 }
 
@@ -62,11 +66,14 @@ ReturnValue_t GyroADIS16507Handler::buildNormalDeviceCommand(DeviceCommandId_t *
 
 ReturnValue_t GyroADIS16507Handler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
     switch(internalState) {
-    case(InternalState::STARTUP): {
+    case(InternalState::CONFIG): {
         *id = ADIS16507::READ_OUT_CONFIG;
         buildCommandFromCommand(*id, nullptr, 0);
         break;
     }
+    case(InternalState::STARTUP): {
+        break;
+    }
     default: {
         /* Might be a configuration error. */
         sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: "
@@ -202,7 +209,7 @@ ReturnValue_t GyroADIS16507Handler::interpretDeviceReply(DeviceCommandId_t id,
         configDataset.mscCtrlReg.value = packet[6] << 8 | packet[7];
         configDataset.decRateReg.value = packet[8] << 8 | packet[9];
         configDataset.setValidity(true, true);
-        if(internalState == InternalState::STARTUP) {
+        if(internalState == InternalState::CONFIG) {
             commandExecuted = true;
         }
         break;