From 7af4a777ae02179701aa7e1748b4bdba5072f193 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 20 Feb 2023 19:54:04 +0100
Subject: [PATCH] ignoe replies when not in nml mode

disable traces
---
 linux/devices/ImtqPollingTask.cpp | 4 ++--
 mission/devices/ImtqHandler.cpp   | 8 ++++++--
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/linux/devices/ImtqPollingTask.cpp b/linux/devices/ImtqPollingTask.cpp
index ee89febc..e96d0144 100644
--- a/linux/devices/ImtqPollingTask.cpp
+++ b/linux/devices/ImtqPollingTask.cpp
@@ -134,7 +134,7 @@ void ImtqPollingTask::handleMeasureStep() {
   if (i2cCmdExecMeasure(imtq::CC::GET_CAL_MTM_MEASUREMENT) != returnvalue::OK) {
     return;
   }
-  sif::debug << "measure done" << std::endl;
+  // sif::debug << "measure done" << std::endl;
   return;
 }
 
@@ -173,7 +173,7 @@ void ImtqPollingTask::handleActuateStep() {
   if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
     return;
   }
-  sif::debug << "measure with torque done" << std::endl;
+  // sif::debug << "measure with torque done" << std::endl;
   return;
 }
 
diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp
index 5e73b7b3..18c002c4 100644
--- a/mission/devices/ImtqHandler.cpp
+++ b/mission/devices/ImtqHandler.cpp
@@ -256,10 +256,14 @@ ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSi
 ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
   ReturnValue_t result;
   ReturnValue_t status = returnvalue::OK;
+  if(getMode() != MODE_NORMAL) {
+    // Ignore replies during transitions.
+    return returnvalue::OK;
+  }
   // arrayprinter::print(packet, ImtqReplies::BASE_LEN);
   if (requestStep == imtq::RequestType::MEASURE_NO_ACTUATION) {
     requestStep = imtq::RequestType::ACTUATE;
-    sif::debug << "handle measure" << std::endl;
+    // sif::debug << "handle measure" << std::endl;
     ImtqRepliesDefault replies(packet);
     if (specialRequestActive) {
       if (replies.wasSpecialRequestRead()) {
@@ -324,7 +328,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
       status = result;
     }
   } else {
-    sif::debug << "handle measure with torque" << std::endl;
+    // sif::debug << "handle measure with torque" << std::endl;
     requestStep = imtq::RequestType::MEASURE_NO_ACTUATION;
     ImtqRepliesWithTorque replies(packet);
     if (replies.wasDipoleActuationRead()) {