From 4fa5aa6b845db06609e3f6e44feab45390b37d6d Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 6 Mar 2023 11:28:25 +0100
Subject: [PATCH] remove printout and re-polling

---
 linux/devices/ImtqPollingTask.cpp | 37 +++++++++++++------------------
 1 file changed, 15 insertions(+), 22 deletions(-)

diff --git a/linux/devices/ImtqPollingTask.cpp b/linux/devices/ImtqPollingTask.cpp
index ea54b9c2..8a2f6b76 100644
--- a/linux/devices/ImtqPollingTask.cpp
+++ b/linux/devices/ImtqPollingTask.cpp
@@ -28,7 +28,8 @@ ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) {
     // Stopwatch watch;
     switch (currentRequest) {
       case imtq::RequestType::MEASURE_NO_ACTUATION: {
-        //Stopwatch watch;
+        // Measured to take 24 ms for debug and release builds.
+        // Stopwatch watch;
         handleMeasureStep();
         break;
       }
@@ -131,18 +132,12 @@ void ImtqPollingTask::handleMeasureStep() {
   if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
     return;
   }
+  bool mgmMeasurementTooOld = false;
   // See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably
   // have old data. Which can be really bad for ACS. And everything.
   if ((replyPtr[2] >> 7) == 0) {
-    sif::error << "IMTQ: MGM measurement too old" << std::endl;
-    TaskFactory::delayTask(2);
-    if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
-      return;
-    }
-    if ((replyPtr[2] >> 7) == 0) {
-      sif::error << "IMTQ: MGM measurement still too old" << std::endl;
-      replyPtr[0] = false;
-    }
+    replyPtr[0] = false;
+    mgmMeasurementTooOld = true;
   }
 
   cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
@@ -154,7 +149,9 @@ void ImtqPollingTask::handleMeasureStep() {
   if (i2cCmdExecMeasure(imtq::CC::GET_CAL_MTM_MEASUREMENT) != returnvalue::OK) {
     return;
   }
-  // sif::debug << "measure done" << std::endl;
+  if(mgmMeasurementTooOld) {
+    sif::error << "IMTQ: MGM measurement too old" << std::endl;
+  }
   return;
 }
 
@@ -191,26 +188,22 @@ void ImtqPollingTask::handleActuateStep() {
   if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
     return;
   }
+  bool measurementWasTooOld = false;
   // See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably
   // have old data. Which can be really bad for ACS. And everything.
   if ((replyPtr[2] >> 7) == 0) {
-    sif::error << "IMTQ: MGM measurement too old" << std::endl;
-    TaskFactory::delayTask(2);
-    cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
-    if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
-      return;
-    }
-    if ((replyPtr[2] >> 7) == 0) {
-      sif::error << "IMTQ: MGM measurement still too old" << std::endl;
-      replyPtr[0] = false;
-    }
+    measurementWasTooOld = true;
+    replyPtr[0] = false;
   }
 
   cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
   if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
     return;
   }
-  // sif::debug << "measure with torque done" << std::endl;
+
+  if(measurementWasTooOld) {
+    sif::error << "IMTQ: MGM measurement too old" << std::endl;
+  }
   return;
 }