common/config/eive/definitions.h

@@ -59,7 +59,7 @@ namespace spiSched {
 
 static constexpr uint32_t SCHED_BLOCK_1_SUS_READ_MS = 15;
 static constexpr uint32_t SCHED_BLOCK_2_SENSOR_READ_MS = 30;
-static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 42;
+static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 43;
 static constexpr uint32_t SCHED_BLOCK_4_ACS_CTRL_MS = 45;
 static constexpr uint32_t SCHED_BLOCK_5_ACTUATOR_MS = 55;
 static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 105;

linux/devices/ImtqPollingTask.cpp

@@ -28,6 +28,8 @@ ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) {
     // Stopwatch watch;
     switch (currentRequest) {
       case imtq::RequestType::MEASURE_NO_ACTUATION: {
+        // Measured to take 24 ms for debug and release builds.
+        // Stopwatch watch;
         handleMeasureStep();
         break;
       }
@@ -47,6 +49,9 @@ void ImtqPollingTask::handleMeasureStep() {
   size_t replyLen = 0;
   uint8_t* replyPtr;
   ImtqRepliesDefault replies(replyBuf.data());
+  // If some startup handling is added later, set configured after it was done once.
+  replies.setConfigured();
+
   // Can be used later to verify correct timing (e.g. all data has been read)
   clearReadFlagsDefault(replies);
   auto i2cCmdExecMeasure = [&](imtq::CC::CC cc) {
@@ -127,17 +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;
+    replyPtr[0] = false;
-    TaskFactory::delayTask(2);
+    mgmMeasurementTooOld = true;
-    if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
-      return;
-    }
-    if ((replyPtr[2] >> 7) == 0b1) {
-      replyPtr[0] = false;
-    }
   }
 
   cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
@@ -149,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;
 }
 
@@ -186,25 +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;
+    measurementWasTooOld = true;
-    TaskFactory::delayTask(2);
+    replyPtr[0] = false;
-    cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
-    if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
-      return;
-    }
-    if ((replyPtr[2] >> 7) == 0b1) {
-      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;
 }
 
@@ -223,7 +222,7 @@ ReturnValue_t ImtqPollingTask::initializeInterface(CookieIF* cookie) {
 
 ReturnValue_t ImtqPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData,
                                            size_t sendLen) {
-  const auto* imtqReq = reinterpret_cast<const ImtqRequest*>(sendData);
+  const auto* imtqReq = reinterpret_cast<const imtq::Request*>(sendData);
   {
     MutexGuard mg(ipcLock);
     if (imtqReq->request == imtq::RequestType::ACTUATE) {

linux/devices/ImtqPollingTask.h

@@ -33,13 +33,12 @@ 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 = 3;
+  static constexpr uint32_t MGM_READ_BUFFER_TIME_MS = 6;
   bool ignoreNextActuateRequest = false;
 
   imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE;
   int16_t dipoles[3] = {};
   uint16_t torqueDuration = 0;
-  // uint8_t startActuateRawBuf[3] = {};
 
   std::array<uint8_t, 32> cmdBuf;
   std::array<uint8_t, 524> replyBuf;

mission/devices/ImtqHandler.cpp

@@ -131,12 +131,21 @@ ReturnValue_t ImtqHandler::performOperation(uint8_t opCode) {
 ImtqHandler::~ImtqHandler() = default;
 
 void ImtqHandler::doStartUp() {
-  updatePeriodicReply(true, imtq::cmdIds::REPLY_NO_TORQUE);
+  if (internalState != InternalState::STARTUP) {
-  updatePeriodicReply(true, imtq::cmdIds::REPLY_WITH_TORQUE);
+    commandExecuted = false;
-  if (goToNormalMode) {
+    updatePeriodicReply(true, imtq::cmdIds::REPLY_NO_TORQUE);
-    setMode(MODE_NORMAL);
+    updatePeriodicReply(true, imtq::cmdIds::REPLY_WITH_TORQUE);
-  } else {
+    internalState = InternalState::STARTUP;
-    setMode(_MODE_TO_ON);
+  }
+  if (internalState == InternalState::STARTUP) {
+    if (commandExecuted) {
+      if (goToNormalMode) {
+        setMode(MODE_NORMAL);
+      } else {
+        setMode(_MODE_TO_ON);
+      }
+      commandExecuted = false;
+    }
   }
 }
 
@@ -145,6 +154,8 @@ void ImtqHandler::doShutDown() {
   updatePeriodicReply(false, imtq::cmdIds::REPLY_WITH_TORQUE);
   specialRequestActive = false;
   firstReplyCycle = true;
+  internalState = InternalState::NONE;
+  commandExecuted = false;
   setMode(_MODE_POWER_DOWN);
 }
 
@@ -162,8 +173,9 @@ 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(ImtqRequest);
+      rawPacketLen = sizeof(imtq::Request);
       return returnvalue::OK;
     }
   }
@@ -171,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;
 }
 
@@ -183,7 +199,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
     expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
     specialRequestActive = true;
     rawPacket = reinterpret_cast<uint8_t*>(&request);
-    rawPacketLen = sizeof(ImtqRequest);
+    rawPacketLen = sizeof(imtq::Request);
   };
   switch (deviceCommand) {
     case (imtq::cmdIds::POS_X_SELF_TEST): {
@@ -219,7 +235,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
       request.specialRequest = imtq::SpecialRequest::NONE;
       expectedReply = imtq::cmdIds::REPLY_NO_TORQUE;
       rawPacket = reinterpret_cast<uint8_t*>(&request);
-      rawPacketLen = sizeof(ImtqRequest);
+      rawPacketLen = sizeof(imtq::Request);
       return returnvalue::OK;
     }
     case (imtq::cmdIds::START_ACTUATION_DIPOLE): {
@@ -258,7 +274,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
       torquer::TORQUEING = true;
       torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
       rawPacket = reinterpret_cast<uint8_t*>(&request);
-      rawPacketLen = sizeof(ImtqRequest);
+      rawPacketLen = sizeof(imtq::Request);
       return returnvalue::OK;
     }
     default:
@@ -298,13 +314,20 @@ 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);
+    if (replies.devWasConfigured() and internalState == InternalState::STARTUP) {
+      commandExecuted = true;
+    }
     if (specialRequestActive) {
       if (replies.wasSpecialRequestRead()) {
         uint8_t* specialRequest = replies.getSpecialRequest();
@@ -347,7 +370,7 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
     }
 
     if (not replies.wasGetRawMgmMeasurementRead() and not firstReplyCycle) {
-      sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
+      sif::warning << "IMTQ: Possible timing issue, raw MGM measurement was not read" << std::endl;
     }
     uint8_t* rawMgmMeasurement = replies.getRawMgmMeasurement();
     result = parseStatusByte(imtq::CC::GET_RAW_MTM_MEASUREMENT, rawMgmMeasurement);
@@ -358,7 +381,8 @@ ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
     }
 
     if (not replies.wasCalibMgmMeasurementRead() and not firstReplyCycle) {
-      sif::warning << "IMTQ: Possible timing issue, system state was not read" << std::endl;
+      sif::warning << "IMTQ: Possible timing issue, calib MGM measurement was not read"
+                   << std::endl;
     }
     uint8_t* calibMgmMeasurement = replies.getCalibMgmMeasurement();
     result = parseStatusByte(imtq::CC::GET_CAL_MTM_MEASUREMENT, calibMgmMeasurement);

mission/devices/ImtqHandler.h

@@ -83,6 +83,11 @@ class ImtqHandler : public DeviceHandlerBase {
   //! link between IMTQ and OBC.
   static const Event INVALID_ERROR_BYTE = MAKE_EVENT(8, severity::LOW);
 
+  enum class InternalState { NONE, STARTUP, SHUTDOWN } internalState = InternalState::NONE;
+  bool commandExecuted = false;
+
+  imtq::Request request{};
+
   imtq::StatusDataset statusSet;
   imtq::DipoleActuationSet dipoleSet;
   imtq::RawMtmMeasurementNoTorque rawMtmNoTorque;
@@ -123,8 +128,7 @@ class ImtqHandler : public DeviceHandlerBase {
 
   power::Switch_t switcher = power::NO_SWITCH;
 
-  ImtqRequest request{};
+  DeviceCommandId_t expectedReply = DeviceHandlerIF::NO_COMMAND_ID;
-  DeviceCommandId_t expectedReply = imtq::cmdIds::REPLY_WITH_TORQUE;
   bool goToNormalMode = false;
   bool debugMode = false;
   bool specialRequestActive = false;

mission/devices/devicedefinitions/imtqHelpers.h

@@ -10,18 +10,6 @@ class ImtqHandler;
 
 namespace imtq {
 
-enum ComStep : uint8_t {
-  DHB_OP = 0,
-  START_MEASURE_SEND = 1,
-  START_MEASURE_GET = 2,
-  READ_MEASURE_SEND = 3,
-  READ_MEASURE_GET = 4,
-  START_ACTUATE_SEND = 5,
-  START_ACTUATE_GET = 6,
-  READ_ACTUATE_SEND = 7,
-  READ_ACTUATE_GET = 8,
-};
-
 enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE, DO_NOTHING };
 
 enum class SpecialRequest : uint8_t {
@@ -35,6 +23,26 @@ enum class SpecialRequest : uint8_t {
   GET_SELF_TEST_RESULT = 7
 };
 
+struct Request {
+  imtq::RequestType request = imtq::RequestType::MEASURE_NO_ACTUATION;
+  imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE;
+  uint8_t integrationTimeSel = 3;
+  int16_t dipoles[3]{};
+  uint16_t torqueDuration = 0;
+};
+
+enum ComStep : uint8_t {
+  DHB_OP = 0,
+  START_MEASURE_SEND = 1,
+  START_MEASURE_GET = 2,
+  READ_MEASURE_SEND = 3,
+  READ_MEASURE_GET = 4,
+  START_ACTUATE_SEND = 5,
+  START_ACTUATE_GET = 6,
+  READ_ACTUATE_SEND = 7,
+  READ_ACTUATE_GET = 8,
+};
+
 static const uint8_t INTERFACE_ID = CLASS_ID::IMTQ_HANDLER;
 
 static constexpr ReturnValue_t INVALID_COMMAND_CODE = MAKE_RETURN_CODE(0);
@@ -1124,25 +1132,21 @@ class NegZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 
 }  // namespace imtq
 
-struct ImtqRequest {
-  imtq::RequestType request = imtq::RequestType::MEASURE_NO_ACTUATION;
-  imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE;
-  int16_t dipoles[3]{};
-  uint16_t torqueDuration = 0;
-};
-
 struct ImtqRepliesDefault {
   friend class ImtqPollingTask;
 
  public:
   static constexpr size_t BASE_LEN =
-      imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::SYSTEM_STATE + 1 +
+      1 + imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::SYSTEM_STATE + 1 +
       +imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::RAW_MTM_MEASUREMENT + 1 +
       imtq::replySize::ENG_HK_DATA_REPLY + 1 + imtq::replySize::CAL_MTM_MEASUREMENT + 1;
   ImtqRepliesDefault(const uint8_t* rawData) : rawData(const_cast<uint8_t*>(rawData)) {
     initPointers();
   }
 
+  void setConfigured() { rawData[0] = true; }
+  bool devWasConfigured() const { return rawData[0]; }
+
   uint8_t* getCalibMgmMeasurement() const { return calibMgmMeasurement + 1; }
   bool wasCalibMgmMeasurementRead() const { return calibMgmMeasurement[0]; };
 
@@ -1164,7 +1168,7 @@ struct ImtqRepliesDefault {
 
  private:
   void initPointers() {
-    swReset = rawData;
+    swReset = rawData + 1;
     systemState = swReset + imtq::replySize::DEFAULT_MIN_LEN + 1;
     startMtmMeasurement = systemState + imtq::replySize::SYSTEM_STATE + 1;
     rawMgmMeasurement = startMtmMeasurement + imtq::replySize::DEFAULT_MIN_LEN + 1;
@@ -1172,6 +1176,7 @@ struct ImtqRepliesDefault {
     calibMgmMeasurement = engHk + imtq::replySize::ENG_HK_DATA_REPLY + 1;
     specialRequestReply = calibMgmMeasurement + imtq::replySize::CAL_MTM_MEASUREMENT + 1;
   }
+
   uint8_t* rawData;
   uint8_t* swReset;
   uint8_t* systemState;