From d18a0e98a557159536a9886e345b129a8baeda6d Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 6 Mar 2023 11:02:56 +0100
Subject: [PATCH] why do we need so much buffer time?

---
 linux/devices/ImtqPollingTask.cpp |  6 ++++--
 linux/devices/ImtqPollingTask.h   |  2 +-
 mission/devices/ImtqHandler.cpp   | 15 +++++++++++++--
 mission/devices/ImtqHandler.h     |  2 +-
 4 files changed, 19 insertions(+), 6 deletions(-)

diff --git a/linux/devices/ImtqPollingTask.cpp b/linux/devices/ImtqPollingTask.cpp
index 82093aca..0148bfe8 100644
--- a/linux/devices/ImtqPollingTask.cpp
+++ b/linux/devices/ImtqPollingTask.cpp
@@ -138,7 +138,8 @@ void ImtqPollingTask::handleMeasureStep() {
     if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
       return;
     }
-    if ((replyPtr[2] >> 7) == 0b1) {
+    if ((replyPtr[2] >> 7) == 0) {
+      sif::error << "IMTQ: MGM measurement still too old" << std::endl;
       replyPtr[0] = false;
     }
   }
@@ -198,7 +199,8 @@ void ImtqPollingTask::handleActuateStep() {
     if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
       return;
     }
-    if ((replyPtr[2] >> 7) == 0b1) {
+    if ((replyPtr[2] >> 7) == 0) {
+      sif::error << "IMTQ: MGM measurement still too old" << std::endl;
       replyPtr[0] = false;
     }
   }
diff --git a/linux/devices/ImtqPollingTask.h b/linux/devices/ImtqPollingTask.h
index 3b219c57..cb2d3882 100644
--- a/linux/devices/ImtqPollingTask.h
+++ b/linux/devices/ImtqPollingTask.h
@@ -33,7 +33,7 @@ class ImtqPollingTask : public SystemObject,
   address_t i2cAddr = 0;
   uint32_t currentIntegrationTimeMs = 10;
   // Required in addition to integration time, otherwise old data might be read.
-  static constexpr uint32_t MGM_READ_BUFFER_TIME_MS = 5;
+  static constexpr uint32_t MGM_READ_BUFFER_TIME_MS = 6;
   bool ignoreNextActuateRequest = false;
 
   imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE;
diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp
index a673a32e..38b38e71 100644
--- a/mission/devices/ImtqHandler.cpp
+++ b/mission/devices/ImtqHandler.cpp
@@ -144,6 +144,7 @@ void ImtqHandler::doStartUp() {
       } else {
         setMode(_MODE_TO_ON);
       }
+      commandExecuted = false;
     }
   }
 }
@@ -154,6 +155,7 @@ void ImtqHandler::doShutDown() {
   specialRequestActive = false;
   firstReplyCycle = true;
   internalState = InternalState::NONE;
+  commandExecuted = false;
   setMode(_MODE_POWER_DOWN);
 }
 
@@ -171,6 +173,7 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
       *id = imtq::cmdIds::REQUEST;
       request.request = imtq::RequestType::DO_NOTHING;
       request.specialRequest = imtq::SpecialRequest::NONE;
+      expectedReply = DeviceHandlerIF::NO_COMMAND_ID;
       rawPacket = reinterpret_cast<uint8_t*>(&request);
       rawPacketLen = sizeof(imtq::Request);
       return returnvalue::OK;
@@ -180,6 +183,10 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
 }
 
 ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
+  if (internalState == InternalState::STARTUP) {
+    *id = imtq::cmdIds::REQUEST;
+    return buildCommandFromCommand(*id, nullptr, 0);
+  }
   return NOTHING_TO_SEND;
 }
 
@@ -307,10 +314,14 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
   ReturnValue_t result;
   ReturnValue_t status = returnvalue::OK;
   if (getMode() != MODE_NORMAL) {
-    // Ignore replies during transitions.
+    if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) {
+      ImtqRepliesDefault replies(packet);
+      if (replies.devWasConfigured() and internalState == InternalState::STARTUP) {
+        commandExecuted = true;
+      }
+    }
     return returnvalue::OK;
   }
-  // arrayprinter::print(packet, ImtqReplies::BASE_LEN);
   if (expectedReply == imtq::cmdIds::REPLY_NO_TORQUE) {
     // sif::debug << "handle measure" << std::endl;
     ImtqRepliesDefault replies(packet);
diff --git a/mission/devices/ImtqHandler.h b/mission/devices/ImtqHandler.h
index dac5d2b0..e337f40c 100644
--- a/mission/devices/ImtqHandler.h
+++ b/mission/devices/ImtqHandler.h
@@ -128,7 +128,7 @@ class ImtqHandler : public DeviceHandlerBase {
 
   power::Switch_t switcher = power::NO_SWITCH;
 
-  DeviceCommandId_t expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE;
+  DeviceCommandId_t expectedReply = DeviceHandlerIF::NO_COMMAND_ID;
   bool goToNormalMode = false;
   bool debugMode = false;
   bool specialRequestActive = false;