linux/devices/ImtqPollingTask.cpp

@@ -118,7 +118,7 @@ void ImtqPollingTask::handleMeasureStep() {
   }
   // Takes a bit of time to take measurements. Subtract a bit because of the delay of previous
   // commands.
-  TaskFactory::delayTask(currentIntegrationTimeMs - 1);
+  TaskFactory::delayTask(currentIntegrationTimeMs);
 
   cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
   if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {

mission/devices/ImtqHandler.cpp

@@ -64,6 +64,8 @@ void ImtqHandler::doStartUp() {
 
 void ImtqHandler::doShutDown() {
   updatePeriodicReply(false, imtq::cmdIds::REPLY);
+  specialRequestActive = false;
+  firstReplyCycle = true;
   setMode(_MODE_POWER_DOWN);
 }
 
@@ -91,6 +93,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
   auto genericMeasureRequest = [&](imtq::SpecialRequest specialRequest) {
     ImtqRequest request(commandBuffer, sizeof(commandBuffer));
     request.setMeasureRequest(specialRequest);
+    specialRequestActive = true;
     rawPacket = commandBuffer;
     rawPacketLen = ImtqRequest::REQUEST_LEN;
   };
@@ -200,22 +203,25 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
   if (requestStep == imtq::RequestType::MEASURE_WITH_ACTUATION) {
     requestStep = imtq::RequestType::ACTUATE;
     ImtqRepliesDefault replies(packet);
-    if (replies.wasSpecialRequestRead()) {
+    if (specialRequestActive) {
-      uint8_t* specialRequest = replies.getSpecialRequest();
+      if (replies.wasSpecialRequestRead()) {
-      imtq::CC::CC cc = specialRequest[0];
+        uint8_t* specialRequest = replies.getSpecialRequest();
-      result = parseStatusByte(cc, packet);
+        imtq::CC::CC cc = static_cast<imtq::CC::CC>(specialRequest[0]);
-      if (result != returnvalue::OK) {
+        result = parseStatusByte(cc, packet);
-        status = result;
+        if (result != returnvalue::OK) {
+          status = result;
+        }
+        if (cc == imtq::CC::CC::GET_SELF_TEST_RESULT) {
+          handleSelfTestReply(specialRequest);
+        }
+        // For a special request, the other stuff was not read, so return here.
+        return status;
+      } else {
+        sif::warning << "IMTQ: Possible timing issue, special request was not read" << std::endl;
       }
-      if (cc == imtq::CC::CC::GET_SELF_TEST_RESULT) {
+      specialRequestActive = false;
-        handleSelfTestReply(specialRequest);
-      }
-      // For a special request, the other stuff was not read, so return here.
-      return status;
-    } else {
-      sif::warning << "IMTQ: Possible timing issue, special request was not read" << std::endl;
     }
-    if (not replies.wasEngHkRead()) {
+    if (not replies.wasEngHkRead() and not firstReplyCycle) {
       sif::warning << "IMTQ: Possible timing issue, ENG HK was not read" << std::endl;
     }
     // Still read it, even if it is old. Better than nothing
@@ -227,7 +233,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
       status = result;
     }
 
-    if (not replies.wasGetSystemStateRead()) {
+    if (not replies.wasGetSystemStateRead() and not firstReplyCycle) {
       sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
     }
     uint8_t* sysStateReply = replies.getSystemState();
@@ -238,7 +244,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
       status = result;
     }
 
-    if (not replies.wasGetRawMgmMeasurementRead()) {
+    if (not replies.wasGetRawMgmMeasurementRead() and not firstReplyCycle) {
       sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
     }
     uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
@@ -249,7 +255,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
       status = result;
     }
 
-    if (not replies.wasCalibMgmMeasurementRead()) {
+    if (not replies.wasCalibMgmMeasurementRead() and not firstReplyCycle) {
       sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
     }
     uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement();
@@ -264,15 +270,15 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
     ImtqRepliesWithTorque replies(packet);
     if (replies.wasDipoleActuationRead()) {
       parseStatusByte(imtq::CC::START_ACTUATION_DIPOLE, replies.getDipoleActuation());
-    } else {
+    } else if (not firstReplyCycle) {
       sif::warning << "IMTQ: Possible timing issue, start actuation dipole status was not read"
                    << std::endl;
     }
-    if (not replies.wasGetRawMgmMeasurementRead()) {
+
+    if (not replies.wasGetRawMgmMeasurementRead() and not firstReplyCycle) {
       sif::warning << "IMTQ: Possible timing issue, was MGM measurement with torque was not read"
                    << std::endl;
     }
-
     uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
     result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
     if (result == returnvalue::OK) {
@@ -280,11 +286,11 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
     } else {
       status = result;
     }
-    if (not replies.wasEngHkRead()) {
+
+    if (not replies.wasEngHkRead() and not firstReplyCycle) {
       sif::warning << "IMTQ: Possible timing issue, engineering HK with torque was not read"
                    << std::endl;
     }
-
     uint8_t* engHkReply = replies.getEngHk();
     result = parseStatusByte(imtq::CC::GET_ENG_HK_DATA, engHkReply);
     if (result != returnvalue::OK) {
@@ -294,6 +300,9 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
     }
     fillEngHkDataset(hkDatasetNoTorque, engHkReply);
   }
+  if (firstReplyCycle) {
+    firstReplyCycle = false;
+  }
   return status;
 }
 

mission/devices/ImtqHandler.h

@@ -115,6 +115,8 @@ class ImtqHandler : public DeviceHandlerBase {
   uint8_t commandBuffer[imtq::MAX_COMMAND_SIZE];
   bool goToNormalMode = false;
   bool debugMode = false;
+  bool specialRequestActive = false;
+  bool firstReplyCycle = true;
 
   imtq::RequestType requestStep = imtq::RequestType::MEASURE_WITH_ACTUATION;