bsp_q7s/core/ObjectFactory.cpp

@@ -1,6 +1,7 @@
 #include "ObjectFactory.h"
 
 #include <fsfw/subsystem/Subsystem.h>
+#include <linux/devices/ImtqPollingTask.h>
 #include <linux/devices/RwPollingTask.h>
 #include <mission/system/objects/CamSwitcher.h>
 
@@ -906,8 +907,9 @@ void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher) {
 }
 
 void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher) {
-  I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, IMTQ::MAX_REPLY_SIZE, q7s::I2C_PL_EIVE);
+  new ImtqPollingTask(objects::IMTQ_POLLING);
-  auto imtqHandler = new ImtqHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie,
+  I2cCookie* imtqI2cCookie = new I2cCookie(addresses::IMTQ, imtq::MAX_REPLY_SIZE, q7s::I2C_PL_EIVE);
+  auto imtqHandler = new ImtqHandler(objects::IMTQ_HANDLER, objects::IMTQ_POLLING, imtqI2cCookie,
                                      pcdu::Switches::PDU1_CH3_MGT_5V);
   imtqHandler->enableThermalModule(ThermalStateCfg());
   imtqHandler->setPowerSwitcher(pwrSwitcher);

bsp_q7s/core/scheduling.cpp

@@ -204,6 +204,14 @@ void scheduling::initTasks() {
     scheduling::printAddObjectError("RW_POLLING_TASK", objects::RW_POLLING_TASK);
   }
 #endif
+#if OBSW_ADD_MGT == 1
+  PeriodicTaskIF* imtqPolling = factory->createPeriodicTask(
+      "IMTQ_POLLING_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
+  result = imtqPolling->addComponent(objects::IMTQ_POLLING);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("IMTQ_POLLING_TASK", objects::IMTQ_POLLING);
+  }
+#endif
 
   PeriodicTaskIF* acsSysTask = factory->createPeriodicTask(
       "ACS_SYS_TASK", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
@@ -345,6 +353,9 @@ void scheduling::initTasks() {
 #if OBSW_ADD_SA_DEPL == 1
   solarArrayDeplTask->startTask();
 #endif
+#if OBSW_ADD_MGT == 1
+  imtqPolling->startTask();
+#endif
 
   taskStarter(pstTasks, "PST task vector");
   taskStarter(pusTasks, "PUS task vector");

common/config/eive/definitions.h

@@ -59,17 +59,23 @@ namespace acs {
 
 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_ACS_CTRL_MS = 45;
+static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 42;
-static constexpr uint32_t SCHED_BLOCK_4_ACTUATOR_MS = 50;
+static constexpr uint32_t SCHED_BLOCK_4_ACS_CTRL_MS = 45;
-static constexpr uint32_t SCHED_BLOCK_5_RW_READ_MS = 300;
+static constexpr uint32_t SCHED_BLOCK_5_ACTUATOR_MS = 50;
+static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 65;
+static constexpr uint32_t SCHED_BLOCK_7_RW_READ_MS = 300;
 
 // 15 ms for FM
 static constexpr float SCHED_BLOCK_1_PERIOD = static_cast<float>(SCHED_BLOCK_1_SUS_READ_MS) / 400.0;
 static constexpr float SCHED_BLOCK_2_PERIOD =
     static_cast<float>(SCHED_BLOCK_2_SENSOR_READ_MS) / 400.0;
-static constexpr float SCHED_BLOCK_3_PERIOD = static_cast<float>(SCHED_BLOCK_3_ACS_CTRL_MS) / 400.0;
+static constexpr float SCHED_BLOCK_3_PERIOD =
-static constexpr float SCHED_BLOCK_4_PERIOD = static_cast<float>(SCHED_BLOCK_4_ACTUATOR_MS) / 400.0;
+    static_cast<float>(SCHED_BLOCK_3_READ_IMTQ_MGM_MS) / 400.0;
-static constexpr float SCHED_BLOCK_5_PERIOD = static_cast<float>(SCHED_BLOCK_5_RW_READ_MS) / 400.0;
+static constexpr float SCHED_BLOCK_4_PERIOD = static_cast<float>(SCHED_BLOCK_4_ACS_CTRL_MS) / 400.0;
+static constexpr float SCHED_BLOCK_5_PERIOD = static_cast<float>(SCHED_BLOCK_5_ACTUATOR_MS) / 400.0;
+static constexpr float SCHED_BLOCK_6_PERIOD =
+    static_cast<float>(SCHED_BLOCK_6_IMTQ_BLOCK_2_MS) / 400.0;
+static constexpr float SCHED_BLOCK_7_PERIOD = static_cast<float>(SCHED_BLOCK_7_RW_READ_MS) / 400.0;
 
 }  // namespace acs
 

common/config/eive/objects.h

@@ -44,6 +44,7 @@ enum commonObjects : uint32_t {
   STAR_TRACKER = 0x44130001,
   GPS_CONTROLLER = 0x44130045,
 
+  IMTQ_POLLING = 0x44140013,
   IMTQ_HANDLER = 0x44140014,
   TMP1075_HANDLER_TCS_0 = 0x44420004,
   TMP1075_HANDLER_TCS_1 = 0x44420005,

dummies/ImtqDummy.cpp

@@ -1,6 +1,6 @@
 #include "ImtqDummy.h"
 
-#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
+#include <mission/devices/devicedefinitions/imtqHelpers.h>
 
 ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
     : DeviceHandlerBase(objectId, comif, comCookie) {}
@@ -38,10 +38,10 @@ uint32_t ImtqDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur
 
 ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                  LocalDataPoolManager &poolManager) {
-  localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(imtq::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, new PoolEntry<float>({0.0, 0.0, 0.0}));
+  localDataPoolMap.emplace(imtq::MGM_CAL_NT, new PoolEntry<float>({0.0, 0.0, 0.0}));
-  localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
+  localDataPoolMap.emplace(imtq::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
-  localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry<float>({0.12, 0.76, -0.45}, true));
+  localDataPoolMap.emplace(imtq::MTM_RAW, new PoolEntry<float>({0.12, 0.76, -0.45}, true));
-  localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>({0}));
+  localDataPoolMap.emplace(imtq::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>({0}));
   return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager);
 }

linux/devices/CMakeLists.txt

@@ -3,8 +3,9 @@ if(EIVE_BUILD_GPSD_GPS_HANDLER)
 endif()
 
 target_sources(
-  ${OBSW_NAME} PRIVATE Max31865RtdLowlevelHandler.cpp ScexUartReader.cpp
+  ${OBSW_NAME}
-                       ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp)
+  PRIVATE Max31865RtdLowlevelHandler.cpp ScexUartReader.cpp ScexDleParser.cpp
+          ScexHelper.cpp RwPollingTask.cpp ImtqPollingTask.cpp)
 
 add_subdirectory(ploc)
 

linux/devices/ImtqPollingTask.cpp

@@ -0,0 +1,346 @@
+#include "ImtqPollingTask.h"
+
+#include <fcntl.h>
+#include <fsfw/tasks/SemaphoreFactory.h>
+#include <fsfw/tasks/TaskFactory.h>
+#include <fsfw/timemanager/Stopwatch.h>
+#include <fsfw_hal/linux/UnixFileGuard.h>
+#include <linux/i2c-dev.h>
+#include <sys/ioctl.h>
+
+#include "fsfw/FSFW.h"
+
+ImtqPollingTask::ImtqPollingTask(object_id_t imtqPollingTask) : SystemObject(imtqPollingTask) {
+  semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
+  semaphore->acquire();
+  ipcLock = MutexFactory::instance()->createMutex();
+  bufLock = MutexFactory::instance()->createMutex();
+}
+
+ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) {
+  size_t replyLen = 0;
+  uint8_t* replyPtr;
+  while (true) {
+    ipcLock->lockMutex();
+    state = InternalState::IDLE;
+    ipcLock->unlockMutex();
+    semaphore->acquire();
+
+    comStatus = returnvalue::OK;
+    // Stopwatch watch;
+    auto i2cCmdExecDefault = [&](imtq::CC::CC cc, ReturnValue_t comErrIfFails =
+                                                      imtq::MGM_MEASUREMENT_LOW_LEVEL_ERROR) {
+      ReturnValue_t res = performI2cFullRequest(replyPtr + 1, replyLen);
+      if (res != returnvalue::OK) {
+        sif::error << "IMTQ: I2C transaction for command 0x" << std::hex << std::setw(2) << cc
+                   << " failed" << std::dec << std::endl;
+        comStatus = comErrIfFails;
+        return returnvalue::FAILED;
+      }
+      if (replyPtr[1] != cc) {
+        sif::warning << "IMTQ: Unexpected CC 0x" << std::hex << std::setw(2)
+                     << static_cast<int>(replyPtr[1]) << " for command 0x" << cc << std::dec
+                     << std::endl;
+        comStatus = comErrIfFails;
+        return returnvalue::FAILED;
+      }
+      replyPtr[0] = true;
+      return returnvalue::OK;
+    };
+    switch (currentRequest) {
+      case imtq::RequestType::MEASURE: {
+        ImtqRepliesDefault replies(replyBuf.data());
+        auto i2cCmdExecMeasure = [&](imtq::CC::CC cc) {
+          ccToReplyPtrMeasure(replies, cc, &replyPtr, replyLen);
+          return i2cCmdExecDefault(cc);
+        };
+        cmdLen = 1;
+        cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
+        if (i2cCmdExecMeasure(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
+          break;
+        }
+
+        cmdBuf[0] = imtq::CC::GET_SYSTEM_STATE;
+        if (i2cCmdExecMeasure(imtq::CC::GET_SYSTEM_STATE) != returnvalue::OK) {
+          break;
+        }
+
+        // Takes a bit of time to take measurements. Subtract a bit because of the delay of previous
+        // commands.
+        TaskFactory::delayTask(currentIntegrationTimeMs - 1);
+
+        cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
+        if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
+          break;
+        }
+
+        cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
+        if (i2cCmdExecMeasure(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
+          break;
+        }
+
+        cmdBuf[0] = imtq::CC::GET_CAL_MTM_MEASUREMENT;
+        if (i2cCmdExecMeasure(imtq::CC::GET_CAL_MTM_MEASUREMENT) != returnvalue::OK) {
+          break;
+        }
+
+        break;
+      }
+      case imtq::RequestType::ACTUATE: {
+        ImtqRepliesWithTorque replies(replyBufActuation.data());
+        auto i2cCmdExecActuate = [&](imtq::CC::CC cc) {
+          ccToReplyPtrActuate(replies, cc, &replyPtr, replyLen);
+          return i2cCmdExecDefault(cc, imtq::ACTUATE_CMD_LOW_LEVEL_ERROR);
+        };
+        buildDipoleCommand();
+        if (i2cCmdExecActuate(imtq::CC::START_ACTUATION_DIPOLE) != returnvalue::OK) {
+          break;
+        }
+
+        cmdLen = 1;
+        cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
+        if (i2cCmdExecActuate(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
+          break;
+        }
+
+        TaskFactory::delayTask(currentIntegrationTimeMs);
+
+        cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
+        if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
+          break;
+        }
+        cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
+        if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
+          break;
+        }
+        break;
+      }
+    };
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t ImtqPollingTask::initialize() { return returnvalue::OK; }
+
+ReturnValue_t ImtqPollingTask::initializeInterface(CookieIF* cookie) {
+  i2cCookie = dynamic_cast<I2cCookie*>(cookie);
+  if (i2cCookie == nullptr) {
+    sif::error << "ImtqPollingTask::initializeInterface: Invalid I2C cookie" << std::endl;
+    return returnvalue::FAILED;
+  }
+  i2cDev = i2cCookie->getDeviceFile().c_str();
+  i2cAddr = i2cCookie->getAddress();
+  return returnvalue::OK;
+}
+
+ReturnValue_t ImtqPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData,
+                                           size_t sendLen) {
+  ImtqRequest request(sendData, sendLen);
+  {
+    MutexGuard mg(ipcLock);
+    currentRequest = request.getRequestType();
+    if (currentRequest == imtq::RequestType::ACTUATE) {
+      std::memcpy(dipoles, request.getDipoles(), 6);
+      torqueDuration = request.getTorqueDuration();
+    }
+    specialRequest = request.getSpecialRequest();
+    if (state != InternalState::IDLE) {
+      return returnvalue::FAILED;
+    }
+    state = InternalState::BUSY;
+  }
+  semaphore->release();
+
+  return returnvalue::OK;
+}
+
+ReturnValue_t ImtqPollingTask::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }
+
+ReturnValue_t ImtqPollingTask::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
+  return returnvalue::OK;
+}
+
+void ImtqPollingTask::ccToReplyPtrMeasure(ImtqRepliesDefault& replies, imtq::CC::CC cc,
+                                          uint8_t** replyBuf, size_t& replyLen) {
+  replyLen = imtq::getReplySize(cc);
+  switch (cc) {
+    case (imtq::CC::CC::GET_ENG_HK_DATA): {
+      *replyBuf = replies.engHk;
+      break;
+    }
+    case (imtq::CC::CC::SOFTWARE_RESET): {
+      *replyBuf = replies.swReset;
+      break;
+    }
+    case (imtq::CC::CC::GET_SYSTEM_STATE): {
+      *replyBuf = replies.systemState;
+      break;
+    }
+    case (imtq::CC::CC::START_MTM_MEASUREMENT): {
+      *replyBuf = replies.startMtmMeasurement;
+      break;
+    }
+    case (imtq::CC::CC::GET_RAW_MTM_MEASUREMENT): {
+      *replyBuf = replies.rawMgmMeasurement;
+      break;
+    }
+    case (imtq::CC::CC::GET_CAL_MTM_MEASUREMENT): {
+      *replyBuf = replies.calibMgmMeasurement;
+      break;
+    }
+    default: {
+      *replyBuf = replies.specialRequestReply;
+      break;
+    }
+  }
+}
+
+void ImtqPollingTask::ccToReplyPtrActuate(ImtqRepliesWithTorque& replies, imtq::CC::CC cc,
+                                          uint8_t** replyBuf, size_t& replyLen) {
+  replyLen = imtq::getReplySize(cc);
+  switch (cc) {
+    case (imtq::CC::CC::START_ACTUATION_DIPOLE): {
+      *replyBuf = replies.dipoleActuation;
+      break;
+    }
+    case (imtq::CC::CC::GET_ENG_HK_DATA): {
+      *replyBuf = replies.engHk;
+      break;
+    }
+    case (imtq::CC::CC::START_MTM_MEASUREMENT): {
+      *replyBuf = replies.startMtmMeasurement;
+      break;
+    }
+    case (imtq::CC::CC::GET_RAW_MTM_MEASUREMENT): {
+      *replyBuf = replies.rawMgmMeasurement;
+      break;
+    }
+    default: {
+      *replyBuf = nullptr;
+      replyLen = 0;
+      break;
+    }
+  }
+}
+size_t ImtqPollingTask::getExchangeBufLen(imtq::SpecialRequest specialRequest) {
+  size_t baseLen = ImtqRepliesDefault::BASE_LEN;
+  switch (specialRequest) {
+    case (imtq::SpecialRequest::NONE):
+    case (imtq::SpecialRequest::DO_SELF_TEST_X):
+    case (imtq::SpecialRequest::DO_SELF_TEST_Y):
+    case (imtq::SpecialRequest::DO_SELF_TEST_Z): {
+      break;
+    }
+    case (imtq::SpecialRequest::GET_SELF_TEST_RESULT): {
+      baseLen += imtq::replySize::SELF_TEST_RESULTS;
+      break;
+    }
+  }
+  return baseLen;
+}
+
+void ImtqPollingTask::buildDipoleCommand() {
+  cmdBuf[0] = imtq::CC::CC::START_ACTUATION_DIPOLE;
+  uint8_t* serPtr = cmdBuf.data() + 1;
+  size_t serLen = 0;
+  for (uint8_t idx = 0; idx < 3; idx++) {
+    SerializeAdapter::serialize(&dipoles[idx], &serPtr, &serLen, cmdBuf.size(),
+                                SerializeIF::Endianness::LITTLE);
+  }
+  SerializeAdapter::serialize(&torqueDuration, &serPtr, &serLen, cmdBuf.size(),
+                              SerializeIF::Endianness::LITTLE);
+  cmdLen = 1 + serLen;
+}
+
+ReturnValue_t ImtqPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
+                                                   size_t* size) {
+  imtq::RequestType currentRequest;
+  {
+    MutexGuard mg(ipcLock);
+    currentRequest = this->currentRequest;
+  }
+
+  size_t replyLen = 0;
+  MutexGuard mg(bufLock);
+  if (currentRequest == imtq::RequestType::MEASURE) {
+    replyLen = getExchangeBufLen(specialRequest);
+    memcpy(exchangeBuf.data(), replyBuf.data(), replyLen);
+  } else {
+    replyLen = ImtqRepliesWithTorque::BASE_LEN;
+    memcpy(exchangeBuf.data(), replyBufActuation.data(), replyLen);
+  }
+  *buffer = exchangeBuf.data();
+  *size = replyLen;
+  return comStatus;
+}
+
+void ImtqPollingTask::clearReadFlags(ImtqRepliesDefault& replies) {
+  replies.calibMgmMeasurement[0] = false;
+  replies.rawMgmMeasurement[0] = false;
+  replies.systemState[0] = false;
+  replies.specialRequestReply[0] = false;
+  replies.engHk[0] = false;
+}
+
+ReturnValue_t ImtqPollingTask::performI2cFullRequest(uint8_t* reply, size_t replyLen) {
+  int fd = 0;
+  if (cmdLen == 0 or reply == nullptr) {
+    return returnvalue::FAILED;
+  }
+
+  {
+    UnixFileGuard fileHelper(i2cDev, fd, O_RDWR, "ImtqPollingTask::performI2cFullRequest");
+    if (fileHelper.getOpenResult() != returnvalue::OK) {
+      return fileHelper.getOpenResult();
+    }
+    if (ioctl(fd, I2C_SLAVE, i2cAddr) < 0) {
+      sif::warning << "Opening IMTQ slave device failed with code " << errno << ": "
+                   << strerror(errno) << std::endl;
+    }
+
+    int written = write(fd, cmdBuf.data(), cmdLen);
+    if (written < 0) {
+      sif::error << "IMTQ: Failed to send with error code " << errno
+                 << ". Error description: " << strerror(errno) << std::endl;
+      return returnvalue::FAILED;
+    } else if (static_cast<size_t>(written) != cmdLen) {
+      sif::error << "IMTQ: Could not write all bytes" << std::endl;
+      return returnvalue::FAILED;
+    }
+  }
+#if FSFW_HAL_I2C_WIRETAPPING == 1
+  sif::info << "Sent I2C data to bus " << deviceFile << ":" << std::endl;
+  arrayprinter::print(sendData, sendLen);
+#endif
+
+  // wait 1 ms like specified in the datasheet. This is the time the IMTQ needs
+  // to prepare a reply.
+  usleep(1000);
+
+  {
+    UnixFileGuard fileHelper(i2cDev, fd, O_RDWR, "ImtqPollingTask::performI2cFullRequest");
+    if (fileHelper.getOpenResult() != returnvalue::OK) {
+      return fileHelper.getOpenResult();
+    }
+    if (ioctl(fd, I2C_SLAVE, i2cAddr) < 0) {
+      sif::warning << "Opening IMTQ slave device failed with code " << errno << ": "
+                   << strerror(errno) << std::endl;
+    }
+    MutexGuard mg(bufLock);
+    int readLen = read(fd, reply, replyLen);
+    if (readLen != static_cast<int>(replyLen)) {
+      if (readLen < 0) {
+        sif::warning << "IMTQ: Reading failed with error code " << errno << " | " << strerror(errno)
+                     << std::endl;
+      } else {
+        sif::warning << "IMTQ: Read only" << readLen << " from " << replyLen << " bytes"
+                     << std::endl;
+      }
+    }
+  }
+  if (reply[0] == 0xff or reply[1] == 0xff) {
+    sif::warning << "IMTQ: No reply available after 1 millisecond";
+    return NO_REPLY_AVAILABLE;
+  }
+  return returnvalue::OK;
+}

linux/devices/ImtqPollingTask.h

@@ -0,0 +1,64 @@
+#ifndef LINUX_DEVICES_IMTQPOLLINGTASK_H_
+#define LINUX_DEVICES_IMTQPOLLINGTASK_H_
+
+#include <fsfw/tasks/SemaphoreIF.h>
+#include <fsfw_hal/linux/i2c/I2cCookie.h>
+
+#include "fsfw/devicehandlers/DeviceCommunicationIF.h"
+#include "fsfw/objectmanager/SystemObject.h"
+#include "fsfw/tasks/ExecutableObjectIF.h"
+#include "mission/devices/devicedefinitions/imtqHelpers.h"
+
+class ImtqPollingTask : public SystemObject,
+                        public ExecutableObjectIF,
+                        public DeviceCommunicationIF {
+ public:
+  ImtqPollingTask(object_id_t imtqPollingTask);
+
+  ReturnValue_t performOperation(uint8_t operationCode) override;
+  ReturnValue_t initialize() override;
+
+ private:
+  static constexpr ReturnValue_t NO_REPLY_AVAILABLE = returnvalue::makeCode(2, 0);
+
+  enum class InternalState { IDLE, BUSY } state = InternalState::IDLE;
+  imtq::RequestType currentRequest = imtq::RequestType::MEASURE;
+
+  SemaphoreIF* semaphore;
+  ReturnValue_t comStatus = returnvalue::OK;
+  MutexIF* ipcLock;
+  MutexIF* bufLock;
+  I2cCookie* i2cCookie = nullptr;
+  const char* i2cDev = nullptr;
+  address_t i2cAddr = 0;
+  uint32_t currentIntegrationTimeMs = 10;
+
+  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;
+  std::array<uint8_t, 524> replyBufActuation;
+  std::array<uint8_t, 524> exchangeBuf;
+  size_t cmdLen = 0;
+
+  // DeviceCommunicationIF overrides
+  ReturnValue_t initializeInterface(CookieIF* cookie) override;
+  ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override;
+  ReturnValue_t getSendSuccess(CookieIF* cookie) override;
+  ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override;
+  ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override;
+
+  void ccToReplyPtrMeasure(ImtqRepliesDefault& replies, imtq::CC::CC cc, uint8_t** replyBuf,
+                           size_t& replyLen);
+  void ccToReplyPtrActuate(ImtqRepliesWithTorque& replies, imtq::CC::CC cc, uint8_t** replyBuf,
+                           size_t& replyLen);
+  void clearReadFlags(ImtqRepliesDefault& replies);
+  size_t getExchangeBufLen(imtq::SpecialRequest specialRequest);
+  void buildDipoleCommand();
+  ReturnValue_t performI2cFullRequest(uint8_t* reply, size_t replyLen);
+};
+
+#endif /* LINUX_DEVICES_IMTQPOLLINGTASK_H_ */

linux/devices/RwPollingTask.cpp

@@ -163,10 +163,11 @@ ReturnValue_t RwPollingTask::requestReceiveMessage(CookieIF* cookie, size_t requ
 ReturnValue_t RwPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) {
   RwCookie* rwCookie = dynamic_cast<RwCookie*>(cookie);
   {
-    MutexGuard mg(ipcLock);
+    MutexGuard mg(rwCookie->bufLock);
-    *buffer = rwCookie->replyBuf.data();
+    memcpy(rwCookie->exchangeBuf.data(), rwCookie->replyBuf.data(), rwCookie->replyBuf.size());
-    *size = rwCookie->replyBuf.size();
   }
+  *buffer = rwCookie->exchangeBuf.data();
+  *size = rwCookie->exchangeBuf.size();
   return returnvalue::OK;
 }
 
@@ -248,6 +249,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf
 #endif
 
   size_t decodedFrameLen = 0;
+  MutexGuard mg(rwCookie.bufLock);
 
   while (decodedFrameLen < maxReplyLen) {
     // First byte already read in
@@ -428,7 +430,12 @@ void RwPollingTask::handleSpecialRequests() {
     }
     uint8_t* replyBuf;
     size_t maxReadLen = idAndIdxToReadBuffer(specialRequestIds[idx], idx, &replyBuf);
-    readNextReply(*rwCookies[idx], replyBuf, maxReadLen);
+    result = readNextReply(*rwCookies[idx], replyBuf, maxReadLen);
+    if (result == returnvalue::OK) {
+      // The first byte is always a flag which shows whether the value was read
+      // properly at least once.
+      replyBuf[0] = true;
+    }
   }
 }
 

linux/devices/RwPollingTask.h

@@ -18,10 +18,14 @@ class RwCookie : public SpiCookie {
   static constexpr size_t REPLY_BUF_LEN = 524;
   RwCookie(uint8_t rwIdx, address_t spiAddress, gpioId_t chipSelect, const size_t maxSize,
            spi::SpiModes spiMode, uint32_t spiSpeed)
-      : SpiCookie(spiAddress, chipSelect, maxSize, spiMode, spiSpeed), rwIdx(rwIdx) {}
+      : SpiCookie(spiAddress, chipSelect, maxSize, spiMode, spiSpeed), rwIdx(rwIdx) {
+    bufLock = MutexFactory::instance()->createMutex();
+  }
 
  private:
   std::array<uint8_t, REPLY_BUF_LEN> replyBuf{};
+  std::array<uint8_t, REPLY_BUF_LEN> exchangeBuf{};
+  MutexIF* bufLock;
   bool setSpeed = true;
   int32_t currentRwSpeed = 0;
   uint16_t currentRampTime = 0;
@@ -56,6 +60,13 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev
   static constexpr uint32_t TIMEOUT_MS = 20;
   static constexpr uint8_t MAX_RETRIES_REPLY = 5;
 
+  // DeviceCommunicationIF overrides
+  ReturnValue_t initializeInterface(CookieIF* cookie) override;
+  ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override;
+  ReturnValue_t getSendSuccess(CookieIF* cookie) override;
+  ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override;
+  ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override;
+
   ReturnValue_t writeAndReadAllRws(DeviceCommandId_t id);
   ReturnValue_t writeOneRwCmd(uint8_t rwIdx, int fd);
   ReturnValue_t readAllRws(DeviceCommandId_t id);
@@ -64,15 +75,6 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev
   ReturnValue_t readNextReply(RwCookie& rwCookie, uint8_t* replyBuf, size_t maxReplyLen);
   void handleSpecialRequests();
 
-  ReturnValue_t initializeInterface(CookieIF* cookie) override;
-
-  ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override;
-
-  ReturnValue_t getSendSuccess(CookieIF* cookie) override;
-
-  ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override;
-
-  ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override;
   ReturnValue_t openSpi(int flags, int& fd);
   ReturnValue_t pullCsLow(gpioId_t gpioId, GpioIF& gpioIF);
   void prepareSimpleCommand(DeviceCommandId_t id);

linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp

@@ -60,8 +60,8 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) {
                         DeviceHandlerIF::PERFORM_OPERATION);
   thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
   thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.25, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.3, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.25, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.3, DeviceHandlerIF::GET_READ);
 #endif
   // These are actually part of another bus, but this works, so keep it like this for now
 #if OBSW_ADD_TMP_DEVICES == 1
@@ -590,93 +590,73 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                           DeviceHandlerIF::SEND_WRITE);
     thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
                           DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
                           DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
-                          DeviceHandlerIF::GET_READ);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::GET_READ);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
                           DeviceHandlerIF::GET_READ);
   }
 
-  thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::acs::SCHED_BLOCK_3_PERIOD, 0);
+  thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::acs::SCHED_BLOCK_4_PERIOD, 0);
 
   if (cfg.scheduleImtq) {
     // This is the torquing cycle.
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_6_PERIOD,
                           DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_6_PERIOD,
                           DeviceHandlerIF::GET_READ);
   }
 
   if (cfg.scheduleRws) {
     // this is the torquing cycle
-    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD,
+    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD,
+    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD,
+    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD,
+    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD,
                           DeviceHandlerIF::PERFORM_OPERATION);
 
-    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::SEND_WRITE);
-
-    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::GET_WRITE);
-
     thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD,
-                          DeviceHandlerIF::SEND_READ);
+                          DeviceHandlerIF::SEND_WRITE);
     thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD,
-                          DeviceHandlerIF::SEND_READ);
+                          DeviceHandlerIF::SEND_WRITE);
     thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD,
-                          DeviceHandlerIF::SEND_READ);
+                          DeviceHandlerIF::SEND_WRITE);
     thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD,
-                          DeviceHandlerIF::SEND_READ);
+                          DeviceHandlerIF::SEND_WRITE);
 
     thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_5_PERIOD,
-                          DeviceHandlerIF::GET_READ);
+                          DeviceHandlerIF::GET_WRITE);
     thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_5_PERIOD,
-                          DeviceHandlerIF::GET_READ);
+                          DeviceHandlerIF::GET_WRITE);
     thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_5_PERIOD,
-                          DeviceHandlerIF::GET_READ);
+                          DeviceHandlerIF::GET_WRITE);
     thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_5_PERIOD,
+                          DeviceHandlerIF::GET_WRITE);
+
+    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_7_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_7_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_7_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_7_PERIOD,
+                          DeviceHandlerIF::SEND_READ);
+
+    thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_7_PERIOD,
+                          DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_7_PERIOD,
+                          DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_7_PERIOD,
+                          DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_7_PERIOD,
                           DeviceHandlerIF::GET_READ);
   }
 

mission/controller/AcsController.h

@@ -5,6 +5,7 @@
 #include <fsfw/globalfunctions/math/VectorOperations.h>
 #include <fsfw/parameters/ParameterHelper.h>
 #include <fsfw/parameters/ReceivesParameterMessagesIF.h>
+#include <mission/devices/devicedefinitions/imtqHelpers.h>
 #include <mission/devices/devicedefinitions/rwHelpers.h>
 
 #include "acs/ActuatorCmd.h"
@@ -19,7 +20,6 @@
 #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
 #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
 #include "mission/devices/devicedefinitions/SusDefinitions.h"
-#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
 #include "mission/trace.h"
 
 class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
@@ -84,7 +84,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   ACS::SensorValues sensorValues;
 
   /* ACS Actuation Datasets */
-  IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
+  imtq::DipoleActuationSet dipoleSet = imtq::DipoleActuationSet(objects::IMTQ_HANDLER);
   rws::RwSpeedActuationSet rw1SpeedSet = rws::RwSpeedActuationSet(objects::RW1);
   rws::RwSpeedActuationSet rw2SpeedSet = rws::RwSpeedActuationSet(objects::RW2);
   rws::RwSpeedActuationSet rw3SpeedSet = rws::RwSpeedActuationSet(objects::RW3);

mission/controller/ThermalController.cpp

@@ -10,7 +10,7 @@
 #include <mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h>
 #include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
 #include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
-#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
+#include <mission/devices/devicedefinitions/imtqHelpers.h>
 #include <mission/devices/devicedefinitions/payloadPcduDefinitions.h>
 #include <mission/devices/devicedefinitions/rwHelpers.h>
 #include <objects/systemObjectList.h>
@@ -785,7 +785,7 @@ void ThermalController::copyDevices() {
   }
 
   {
-    lp_var_t<int16_t> tempMgt = lp_var_t<int16_t>(objects::IMTQ_HANDLER, IMTQ::MCU_TEMPERATURE);
+    lp_var_t<int16_t> tempMgt = lp_var_t<int16_t>(objects::IMTQ_HANDLER, imtq::MCU_TEMPERATURE);
     PoolReadGuard pg(&tempMgt, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg.getReadResult() != returnvalue::OK) {
       sif::warning << "ThermalController: Failed to read MGT temperature" << std::endl;

mission/controller/acs/SensorValues.h

@@ -1,6 +1,7 @@
 #ifndef SENSORVALUES_H_
 #define SENSORVALUES_H_
 
+#include <mission/devices/devicedefinitions/imtqHelpers.h>
 #include <mission/devices/devicedefinitions/rwHelpers.h>
 
 #include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h"
@@ -10,7 +11,6 @@
 #include "mission/devices/devicedefinitions/GPSDefinitions.h"
 #include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
 #include "mission/devices/devicedefinitions/SusDefinitions.h"
-#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
 
 namespace ACS {
 
@@ -35,7 +35,8 @@ class SensorValues {
       MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER);
   RM3100::Rm3100PrimaryDataset mgm3Rm3100Set =
       RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER);
-  IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_HANDLER);
+  imtq::RawMtmMeasurementNoTorque imtqMgmSet =
+      imtq::RawMtmMeasurementNoTorque(objects::IMTQ_HANDLER);
 
   std::array<SUS::SusDataset, 12> susSets{
       SUS::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF),

mission/devices/ImtqHandler.h

@@ -2,7 +2,7 @@
 #define MISSION_DEVICES_IMTQHANDLER_H_
 
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
-#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
+#include <mission/devices/devicedefinitions/imtqHelpers.h>
 #include <string.h>
 
 #include "events/subsystemIdRanges.h"
@@ -42,7 +42,6 @@ class ImtqHandler : public DeviceHandlerBase {
   ReturnValue_t scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId,
                              size_t* foundLen) override;
   ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) override;
-  void setNormalDatapoolEntriesInvalid() override;
   virtual LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
   uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
   ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
@@ -50,19 +49,6 @@ class ImtqHandler : public DeviceHandlerBase {
   ReturnValue_t getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) override;
 
  private:
-  static const uint8_t INTERFACE_ID = CLASS_ID::IMTQ_HANDLER;
-
-  static const ReturnValue_t INVALID_COMMAND_CODE = MAKE_RETURN_CODE(0xA0);
-  static const ReturnValue_t PARAMETER_MISSING = MAKE_RETURN_CODE(0xA1);
-  static const ReturnValue_t PARAMETER_INVALID = MAKE_RETURN_CODE(0xA2);
-  static const ReturnValue_t CC_UNAVAILABLE = MAKE_RETURN_CODE(0xA3);
-  static const ReturnValue_t INTERNAL_PROCESSING_ERROR = MAKE_RETURN_CODE(0xA4);
-  static const ReturnValue_t REJECTED_WITHOUT_REASON = MAKE_RETURN_CODE(0xA5);
-  static const ReturnValue_t CMD_ERR_UNKNOWN = MAKE_RETURN_CODE(0xA6);
-  //! [EXPORT] : [COMMENT] The status reply to a self test command was received but no self test
-  //! command has been sent. This should normally never happen.
-  static const ReturnValue_t UNEXPECTED_SELF_TEST_REPLY = MAKE_RETURN_CODE(0xA7);
-
   static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::IMTQ_HANDLER;
 
   //! [EXPORT] : [COMMENT] Get self test result returns I2C failure
@@ -97,49 +83,45 @@ class ImtqHandler : public DeviceHandlerBase {
   //! link between IMTQ and OBC.
   static const Event INVALID_ERROR_BYTE = MAKE_EVENT(8, severity::LOW);
 
-  IMTQ::EngHkDataset engHkDataset;
+  imtq::StatusDataset statusSet;
-  IMTQ::CalibratedMtmMeasurementSet calMtmMeasurementSet;
+  imtq::DipoleActuationSet dipoleSet;
-  IMTQ::RawMtmMeasurementSet rawMtmMeasurementSet;
+  imtq::RawMtmMeasurementNoTorque rawMtmNoTorque;
-  IMTQ::DipoleActuationSet dipoleSet;
+  imtq::HkDatasetNoTorque hkDatasetNoTorque;
-  IMTQ::PosXSelfTestSet posXselfTestDataset;
+
-  IMTQ::NegXSelfTestSet negXselfTestDataset;
+  imtq::RawMtmMeasurementWithTorque rawMtmWithTorque;
-  IMTQ::PosYSelfTestSet posYselfTestDataset;
+  imtq::HkDatasetWithTorque hkDatasetWithTorque;
-  IMTQ::NegYSelfTestSet negYselfTestDataset;
+
-  IMTQ::PosZSelfTestSet posZselfTestDataset;
+  imtq::CalibratedMtmMeasurementSet calMtmMeasurementSet;
-  IMTQ::NegZSelfTestSet negZselfTestDataset;
+  imtq::PosXSelfTestSet posXselfTestDataset;
+  imtq::NegXSelfTestSet negXselfTestDataset;
+  imtq::PosYSelfTestSet posYselfTestDataset;
+  imtq::NegYSelfTestSet negYselfTestDataset;
+  imtq::PosZSelfTestSet posZselfTestDataset;
+  imtq::NegZSelfTestSet negZselfTestDataset;
 
   NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED;
 
+  PoolEntry<uint8_t> statusMode = PoolEntry<uint8_t>({0});
+  PoolEntry<uint8_t> statusError = PoolEntry<uint8_t>({0});
+  PoolEntry<uint8_t> statusConfig = PoolEntry<uint8_t>({0});
+  PoolEntry<uint32_t> statusUptime = PoolEntry<uint32_t>({0});
+
   PoolEntry<int32_t> mgmCalEntry = PoolEntry<int32_t>(3);
   PoolEntry<int16_t> dipoleXEntry = PoolEntry<int16_t>(0, false);
   PoolEntry<int16_t> dipoleYEntry = PoolEntry<int16_t>(0, false);
   PoolEntry<int16_t> dipoleZEntry = PoolEntry<int16_t>(0, false);
   PoolEntry<uint16_t> torqueDurationEntry = PoolEntry<uint16_t>(0, false);
-  // Hardcoded to default integration time of 10 ms.
-  // SHOULDDO: Support for other integration times
-  Countdown integrationTimeCd = Countdown(10);
-
   power::Switch_t switcher = power::NO_SWITCH;
 
-  uint8_t commandBuffer[IMTQ::MAX_COMMAND_SIZE];
+  uint8_t commandBuffer[imtq::MAX_COMMAND_SIZE];
   bool goToNormalMode = false;
   bool debugMode = false;
 
-  enum class CommunicationStep {
+  imtq::RequestType requestStep = imtq::RequestType::MEASURE;
-    GET_ENG_HK_DATA,
-    START_MTM_MEASUREMENT,
-    GET_CAL_MTM_MEASUREMENT,
-    GET_RAW_MTM_MEASUREMENT,
-    DIPOLE_ACTUATION
-  };
 
-  CommunicationStep communicationStep = CommunicationStep::GET_ENG_HK_DATA;
+  // enum class StartupStep { NONE, COMMAND_SELF_TEST, GET_SELF_TEST_RESULT };
 
-  enum class StartupStep { NONE, COMMAND_SELF_TEST, GET_SELF_TEST_RESULT };
+  // StartupStep startupStep = StartupStep::COMMAND_SELF_TEST;
-
-  StartupStep startupStep = StartupStep::COMMAND_SELF_TEST;
-
-  bool selfTestPerformed = false;
 
   /**
    * @brief   In case of a status reply to a single axis self test command, this function
@@ -155,7 +137,7 @@ class ImtqHandler : public DeviceHandlerBase {
    *
    * @return  The return code derived from the received status byte.
    */
-  ReturnValue_t parseStatusByte(const uint8_t* packet);
+  ReturnValue_t parseStatusByte(imtq::CC::CC command, const uint8_t* packet);
 
   /**
    * @brief   This function fills the engineering housekeeping dataset with the received data.
@@ -163,7 +145,9 @@ class ImtqHandler : public DeviceHandlerBase {
    * @param packet    Pointer to the received data.
    *
    */
-  void fillEngHkDataset(const uint8_t* packet);
+  void fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* packet);
+
+  void fillSystemStateIntoDataset(const uint8_t* packet);
 
   /**
    * @brief   This function sends a command reply to the requesting queue.
@@ -212,7 +196,7 @@ class ImtqHandler : public DeviceHandlerBase {
   void handlePositiveZSelfTestReply(const uint8_t* packet);
   void handleNegativeZSelfTestReply(const uint8_t* packet);
 
-  ReturnValue_t buildDipoleActuationCommand();
+  // ReturnValue_t buildDipoleActuationCommand();
   /**
    * @brief   This function checks the error byte of a self test measurement.
    *

mission/devices/devicedefinitions/CMakeLists.txt

@@ -1 +1,2 @@
-target_sources(${LIB_EIVE_MISSION} PRIVATE ScexDefinitions.cpp rwHelpers.cpp)
+target_sources(${LIB_EIVE_MISSION} PRIVATE ScexDefinitions.cpp rwHelpers.cpp
+                                           imtqHelpers.cpp)

mission/devices/devicedefinitions/imtqHelpers.cpp

@@ -0,0 +1,49 @@
+#include "imtqHelpers.h"
+
+size_t imtq::getReplySize(CC::CC cc, size_t* optSecondSize) {
+  switch (cc) {
+    // Software reset is a bit special and can also cause a I2C NAK because
+    // the device might be reset at that moment. Otherwise, 2 bytes should be returned
+    case (CC::CC::SOFTWARE_RESET): {
+      if (optSecondSize != nullptr) {
+        *optSecondSize = 0;
+      }
+      return 2;
+    }
+    case (CC::CC::START_ACTUATION_DIPOLE):
+    case (CC::CC::SELF_TEST_CMD):
+    case (CC::CC::START_MTM_MEASUREMENT): {
+      return 2;
+    }
+    case (CC::CC::GET_SYSTEM_STATE): {
+      return 9;
+    }
+    case (CC::CC::GET_RAW_MTM_MEASUREMENT):
+    case (CC::CC::GET_CAL_MTM_MEASUREMENT): {
+      return 15;
+    }
+    case (CC::CC::GET_COIL_CURRENT):
+    case (CC::CC::GET_COMMANDED_DIPOLE):
+    case (CC::CC::GET_COIL_TEMPERATURES): {
+      return 8;
+    }
+    case (CC::CC::GET_SELF_TEST_RESULT): {
+      // Can also be 360 for the all axes self-test!
+      if (optSecondSize != nullptr) {
+        *optSecondSize = 360;
+      }
+      return 120;
+    }
+    case (CC::CC::GET_RAW_HK_DATA):
+    case (CC::CC::GET_ENG_HK_DATA): {
+      return 24;
+    }
+    case (CC::CC::GET_PARAM): {
+      return imtq::replySize::MAX_SET_GET_PARAM_LEN;
+    }
+    default: {
+      return 0;
+    }
+  }
+  return 0;
+}

mission/devices/devicedefinitions/imtqHelpers.h

@@ -3,21 +3,42 @@
 
 #include <fsfw/datapool/PoolReadGuard.h>
 #include <fsfw/datapoollocal/StaticLocalDataSet.h>
+#include <fsfw/devicehandlers/DeviceHandlerIF.h>
 
 class ImtqHandler;
 
-namespace IMTQ {
+namespace imtq {
 
-static const DeviceCommandId_t NONE = 0x0;
+enum class RequestType : uint8_t { MEASURE, ACTUATE };
-static const DeviceCommandId_t GET_ENG_HK_DATA = 0x1;
+
+enum class SpecialRequest : uint8_t {
+  NONE = 0,
+  DO_SELF_TEST_X = 1,
+  DO_SELF_TEST_Y = 2,
+  DO_SELF_TEST_Z = 3,
+  GET_SELF_TEST_RESULT = 4
+};
+
+static const uint8_t INTERFACE_ID = CLASS_ID::IMTQ_HANDLER;
+
+static constexpr ReturnValue_t INVALID_COMMAND_CODE = MAKE_RETURN_CODE(0);
+static constexpr ReturnValue_t MGM_MEASUREMENT_LOW_LEVEL_ERROR = MAKE_RETURN_CODE(1);
+static constexpr ReturnValue_t ACTUATE_CMD_LOW_LEVEL_ERROR = MAKE_RETURN_CODE(2);
+static const ReturnValue_t PARAMETER_MISSING = MAKE_RETURN_CODE(3);
+static const ReturnValue_t PARAMETER_INVALID = MAKE_RETURN_CODE(4);
+static const ReturnValue_t CC_UNAVAILABLE = MAKE_RETURN_CODE(5);
+static const ReturnValue_t INTERNAL_PROCESSING_ERROR = MAKE_RETURN_CODE(6);
+static const ReturnValue_t REJECTED_WITHOUT_REASON = MAKE_RETURN_CODE(7);
+static const ReturnValue_t CMD_ERR_UNKNOWN = MAKE_RETURN_CODE(8);
+//! [EXPORT] : [COMMENT] The status reply to a self test command was received but no self test
+//! command has been sent. This should normally never happen.
+static const ReturnValue_t UNEXPECTED_SELF_TEST_REPLY = MAKE_RETURN_CODE(0xA7);
+
+namespace cmdIds {
+
+static constexpr DeviceCommandId_t REQUEST = 0x70;
+static constexpr DeviceCommandId_t REPLY = 0x71;
 static const DeviceCommandId_t START_ACTUATION_DIPOLE = 0x2;
-static const DeviceCommandId_t GET_COMMANDED_DIPOLE = 0x3;
-/** Generates new measurement of the magnetic field */
-static const DeviceCommandId_t START_MTM_MEASUREMENT = 0x4;
-/** Requests the calibrated magnetometer measurement */
-static const DeviceCommandId_t GET_CAL_MTM_MEASUREMENT = 0x5;
-/** Requests the raw values measured by the built-in MTM XEN1210 */
-static const DeviceCommandId_t GET_RAW_MTM_MEASUREMENT = 0x6;
 static const DeviceCommandId_t POS_X_SELF_TEST = 0x7;
 static const DeviceCommandId_t NEG_X_SELF_TEST = 0x8;
 static const DeviceCommandId_t POS_Y_SELF_TEST = 0x9;
@@ -26,36 +47,59 @@ static const DeviceCommandId_t POS_Z_SELF_TEST = 0xB;
 static const DeviceCommandId_t NEG_Z_SELF_TEST = 0xC;
 static const DeviceCommandId_t GET_SELF_TEST_RESULT = 0xD;
 
-static const uint8_t GET_TEMP_REPLY_SIZE = 2;
+}  // namespace cmdIds
-static const uint8_t CFGR_CMD_SIZE = 3;
+
+// static const DeviceCommandId_t NONE = 0x0;
+// static const DeviceCommandId_t GET_ENG_HK_DATA = 0x1;
+// static const DeviceCommandId_t GET_COMMANDED_DIPOLE = 0x3;
+///** Generates new measurement of the magnetic field */
+// static const DeviceCommandId_t START_MTM_MEASUREMENT = 0x4;
+///** Requests the calibrated magnetometer measurement */
+// static const DeviceCommandId_t GET_CAL_MTM_MEASUREMENT = 0x5;
+///** Requests the raw values measured by the built-in MTM XEN1210 */
+// static const DeviceCommandId_t GET_RAW_MTM_MEASUREMENT = 0x6;
+
 static const uint8_t POINTER_REG_SIZE = 1;
 
 enum SetIds : uint32_t {
-  ENG_HK = 1,
+  ENG_HK_NO_TORQUE = 1,
-  CAL_MGM = 2,
+  RAW_MTM_NO_TORQUE = 2,
-  RAW_MGM = 3,
+  ENG_HK_SET_WITH_TORQUE = 3,
-  POS_X_TEST = 4,
+  RAW_MTM_WITH_TORQUE = 4,
-  NEG_X_TEST = 5,
+  STATUS_SET = 5,
-  POS_Y_TEST = 6,
+  DIPOLES = 6,
-  NEG_Y_TEST = 7,
+
-  POS_Z_TEST = 8,
+  CAL_MTM_SET = 9,
-  NEG_Z_TEST = 9,
+  POSITIVE_X_TEST = 10,
-  DIPOLES = 10
+  NEGATIVE_X_TEST = 11,
+  POSITIVE_Y_TEST = 12,
+  NEGATIVE_Y_TEST = 13,
+  POSITIVE_Z_TEST = 14,
+  NEGATIVE_Z_TEST = 15,
 };
 
-static const uint8_t SIZE_ENG_HK_COMMAND = 1;
+namespace replySize {
-static const uint8_t SIZE_STATUS_REPLY = 2;
-static const uint8_t SIZE_ENG_HK_DATA_REPLY = 24;
-static const uint8_t SIZE_GET_COMMANDED_DIPOLE_REPLY = 8;
-static const uint8_t SIZE_GET_CAL_MTM_MEASUREMENT = 15;
-static const uint8_t SIZE_GET_RAW_MTM_MEASUREMENT = 15;
-static const uint16_t SIZE_SELF_TEST_RESULTS = 120;
 
-static const uint16_t MAX_REPLY_SIZE = SIZE_SELF_TEST_RESULTS;
+static constexpr uint8_t GET_TEMP_REPLY_SIZE = 2;
-static const uint8_t MAX_COMMAND_SIZE = 9;
+static constexpr uint8_t CFGR_CMD_SIZE = 3;
+static constexpr size_t DEFAULT_MIN_LEN = 2;
+static constexpr size_t STATUS_REPLY = DEFAULT_MIN_LEN;
+static constexpr size_t ENG_HK_DATA_REPLY = 24;
+static constexpr size_t GET_COMMANDED_DIPOLE_REPLY = 8;
+static constexpr size_t MAX_SET_GET_PARAM_LEN = 12;
+static constexpr size_t SYSTEM_STATE = 9;
+static constexpr size_t CAL_MTM_MEASUREMENT = 15;
+static constexpr size_t RAW_MTM_MEASUREMENT = 15;
+static constexpr size_t SELF_TEST_RESULTS = 120;
+static constexpr size_t SELF_TEST_RESULTS_ALL_AXES = 360;
+
+}  // namespace replySize
+
+static const uint16_t MAX_REPLY_SIZE = replySize::SELF_TEST_RESULTS_ALL_AXES;
+static const uint8_t MAX_COMMAND_SIZE = 16;
 
 /** Define entries in IMTQ specific dataset */
-static const uint8_t ENG_HK_SET_POOL_ENTRIES = 11;
+static const uint8_t HK_SET_POOL_ENTRIES = 20;
 static const uint8_t CAL_MTM_POOL_ENTRIES = 4;
 static const uint8_t SELF_TEST_DATASET_ENTRIES = 104;
 
@@ -72,45 +116,75 @@ static const uint8_t INVALID_ERROR_BYTE =
 
 static const uint8_t MAIN_STEP_OFFSET = 43;
 
+// Command Code
+namespace CC {
+
 /**
  * Command code definitions. Each command or reply of an IMTQ request will begin with one of
  * the following command codes.
  */
-namespace CC {
+enum CC : uint8_t {
-static const uint8_t START_MTM_MEASUREMENT = 0x4;
+  START_MTM_MEASUREMENT = 0x4,
-static const uint8_t START_ACTUATION_DIPOLE = 0x6;
+  START_ACTUATION_DIPOLE = 0x6,
-static const uint8_t SELF_TEST_CMD = 0x8;
+  SELF_TEST_CMD = 0x8,
-static const uint8_t SOFTWARE_RESET = 0xAA;
+  GET_SYSTEM_STATE = 0x41,
-static const uint8_t GET_ENG_HK_DATA = 0x4A;
+  GET_RAW_MTM_MEASUREMENT = 0x42,
-static const uint8_t GET_COMMANDED_DIPOLE = 0x46;
+  GET_CAL_MTM_MEASUREMENT = 0x43,
-static const uint8_t GET_RAW_MTM_MEASUREMENT = 0x42;
+  GET_COIL_CURRENT = 0x44,
-static const uint8_t GET_CAL_MTM_MEASUREMENT = 0x43;
+  GET_COIL_TEMPERATURES = 0x45,
-static const uint8_t GET_SELF_TEST_RESULT = 0x47;
+  GET_COMMANDED_DIPOLE = 0x46,
-static const uint8_t PAST_AVAILABLE_RESPONSE_BYTES = 0xff;
+  GET_SELF_TEST_RESULT = 0x47,
-};  // namespace CC
+  GET_RAW_HK_DATA = 0x49,
+  GET_ENG_HK_DATA = 0x4A,
+  GET_PARAM = 0x81,
+  SET_PARAM = 0x82,
+  SOFTWARE_RESET = 0xAA,
+  PAST_AVAILABLE_RESPONSE_BYTES = 0xff
+};
 
-namespace SELF_TEST_AXIS {
+}  // namespace CC
-static const uint8_t ALL = 0x0;
-static const uint8_t X_POSITIVE = 0x1;
-static const uint8_t X_NEGATIVE = 0x2;
-static const uint8_t Y_POSITIVE = 0x3;
-static const uint8_t Y_NEGATIVE = 0x4;
-static const uint8_t Z_POSITIVE = 0x5;
-static const uint8_t Z_NEGATIVE = 0x6;
-}  // namespace SELF_TEST_AXIS
 
-namespace SELF_TEST_STEPS {
+size_t getReplySize(CC::CC cc, size_t* optSecondSize = nullptr);
-static const uint8_t INIT = 0x0;
+
-static const uint8_t X_POSITIVE = 0x1;
+namespace mode {
-static const uint8_t X_NEGATIVE = 0x2;
+enum Mode : uint8_t { IDLE = 0, SELF_TEST = 1, DETUMBLE = 2 };
-static const uint8_t Y_POSITIVE = 0x3;
+}
-static const uint8_t Y_NEGATIVE = 0x4;
+
-static const uint8_t Z_POSITIVE = 0x5;
+namespace selfTest {
-static const uint8_t Z_NEGATIVE = 0x6;
+
-static const uint8_t FINA = 0x7;
+enum Axis : uint8_t {
-}  // namespace SELF_TEST_STEPS
+
+  ALL = 0x0,
+  X_POSITIVE = 0x1,
+  X_NEGATIVE = 0x2,
+  Y_POSITIVE = 0x3,
+  Y_NEGATIVE = 0x4,
+  Z_POSITIVE = 0x5,
+  Z_NEGATIVE = 0x6,
+};
+
+namespace step {
+
+enum Step : uint8_t {
+
+  INIT = 0x0,
+  X_POSITIVE = 0x1,
+  X_NEGATIVE = 0x2,
+  Y_POSITIVE = 0x3,
+  Y_NEGATIVE = 0x4,
+  Z_POSITIVE = 0x5,
+  Z_NEGATIVE = 0x6,
+  FINA = 0x7
+};
+}
+}  // namespace selfTest
 
 enum PoolIds : lp_id_t {
+  STATUS_BYTE_MODE,
+  STATUS_BYTE_ERROR,
+  STATUS_BYTE_CONF,
+  STATUS_BYTE_UPTIME,
+
   DIGITAL_VOLTAGE_MV,
   ANALOG_VOLTAGE_MV,
   DIGITAL_CURRENT,
@@ -384,12 +458,23 @@ enum PoolIds : lp_id_t {
   FINA_NEG_Z_COIL_Z_TEMPERATURE,
 };
 
-class EngHkDataset : public StaticLocalDataSet<ENG_HK_SET_POOL_ENTRIES> {
+class StatusDataset : public StaticLocalDataSet<4> {
  public:
-  EngHkDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, IMTQ::SetIds::ENG_HK) {}
+  StatusDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, imtq::SetIds::STATUS_SET) {}
+  // Status byte variables
+  lp_var_t<uint8_t> statusByteMode = lp_var_t<uint8_t>(sid.objectId, STATUS_BYTE_MODE, this);
+  lp_var_t<uint8_t> statusByteError = lp_var_t<uint8_t>(sid.objectId, STATUS_BYTE_ERROR, this);
+  lp_var_t<uint8_t> statusByteConfig = lp_var_t<uint8_t>(sid.objectId, STATUS_BYTE_CONF, this);
+  lp_var_t<uint32_t> statusByteUptime = lp_var_t<uint32_t>(sid.objectId, STATUS_BYTE_UPTIME, this);
+};
 
-  EngHkDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::ENG_HK)) {}
+class HkDataset : public StaticLocalDataSet<HK_SET_POOL_ENTRIES> {
+ public:
+  HkDataset(HasLocalDataPoolIF* owner, uint32_t setId) : StaticLocalDataSet(owner, setId) {}
 
+  HkDataset(object_id_t objectId, uint32_t setId) : StaticLocalDataSet(sid_t(objectId, setId)) {}
+
+  // Engineering HK variables
   lp_var_t<uint16_t> digitalVoltageMv = lp_var_t<uint16_t>(sid.objectId, DIGITAL_VOLTAGE_MV, this);
   lp_var_t<uint16_t> analogVoltageMv = lp_var_t<uint16_t>(sid.objectId, ANALOG_VOLTAGE_MV, this);
   lp_var_t<float> digitalCurrentmA = lp_var_t<float>(sid.objectId, DIGITAL_CURRENT, this);
@@ -404,16 +489,27 @@ class EngHkDataset : public StaticLocalDataSet<ENG_HK_SET_POOL_ENTRIES> {
   lp_var_t<int16_t> mcuTemperature = lp_var_t<int16_t>(sid.objectId, MCU_TEMPERATURE, this);
 };
 
+class HkDatasetNoTorque : public HkDataset {
+ public:
+  HkDatasetNoTorque(HasLocalDataPoolIF* owner) : HkDataset(owner, imtq::SetIds::ENG_HK_NO_TORQUE) {}
+};
+
+class HkDatasetWithTorque : public HkDataset {
+ public:
+  HkDatasetWithTorque(HasLocalDataPoolIF* owner)
+      : HkDataset(owner, imtq::SetIds::ENG_HK_SET_WITH_TORQUE) {}
+};
 /**
+ *
  * @brief   This dataset holds the last calibrated MTM measurement.
  */
 class CalibratedMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
  public:
   CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner)
-      : StaticLocalDataSet(owner, IMTQ::SetIds::CAL_MGM) {}
+      : StaticLocalDataSet(owner, imtq::SetIds::CAL_MTM_SET) {}
 
   CalibratedMtmMeasurementSet(object_id_t objectId)
-      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::CAL_MGM)) {}
+      : StaticLocalDataSet(sid_t(objectId, imtq::SetIds::CAL_MTM_SET)) {}
 
   /** The unit of all measurements is nT */
   lp_vec_t<int32_t, 3> mgmXyz = lp_vec_t<int32_t, 3>(sid.objectId, MGM_CAL_NT, this);
@@ -427,11 +523,11 @@ class CalibratedMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRI
  */
 class RawMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
  public:
-  RawMtmMeasurementSet(HasLocalDataPoolIF* owner)
+  RawMtmMeasurementSet(HasLocalDataPoolIF* owner, uint32_t setId)
-      : StaticLocalDataSet(owner, IMTQ::SetIds::RAW_MGM) {}
+      : StaticLocalDataSet(owner, setId) {}
 
-  RawMtmMeasurementSet(object_id_t objectId)
+  RawMtmMeasurementSet(object_id_t objectId, uint32_t setId)
-      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::RAW_MGM)) {}
+      : StaticLocalDataSet(sid_t(objectId, setId)) {}
 
   /** The unit of all measurements is nT */
   lp_vec_t<float, 3> mtmRawNt = lp_vec_t<float, 3>(sid.objectId, MTM_RAW, this);
@@ -440,6 +536,21 @@ class RawMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
       lp_var_t<uint8_t>(sid.objectId, ACTUATION_RAW_STATUS, this);
 };
 
+class RawMtmMeasurementNoTorque : public RawMtmMeasurementSet {
+ public:
+  RawMtmMeasurementNoTorque(HasLocalDataPoolIF* owner)
+      : RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_NO_TORQUE) {}
+  RawMtmMeasurementNoTorque(object_id_t objectId)
+      : RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_NO_TORQUE) {}
+};
+class RawMtmMeasurementWithTorque : public RawMtmMeasurementSet {
+ public:
+  RawMtmMeasurementWithTorque(HasLocalDataPoolIF* owner)
+      : RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_WITH_TORQUE) {}
+  RawMtmMeasurementWithTorque(object_id_t objectId)
+      : RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_WITH_TORQUE) {}
+};
+
 /**
  * @brief   This class can be used to ease the generation of an action message commanding the
  *          IMTQHandler to configure the magnettorquer with the desired dipoles.
@@ -482,9 +593,9 @@ class DipoleActuationSet : public StaticLocalDataSet<4> {
 
  public:
   DipoleActuationSet(HasLocalDataPoolIF& owner)
-      : StaticLocalDataSet(&owner, IMTQ::SetIds::DIPOLES) {}
+      : StaticLocalDataSet(&owner, imtq::SetIds::DIPOLES) {}
   DipoleActuationSet(object_id_t objectId)
-      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::DIPOLES)) {}
+      : StaticLocalDataSet(sid_t(objectId, imtq::SetIds::DIPOLES)) {}
 
   // Refresh torque command without changing any of the set dipoles.
   void refreshTorqueing(uint16_t durationMs_) { currentTorqueDurationMs = durationMs_; }
@@ -534,10 +645,10 @@ class DipoleActuationSet : public StaticLocalDataSet<4> {
 class PosXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
  public:
   PosXSelfTestSet(HasLocalDataPoolIF* owner)
-      : StaticLocalDataSet(owner, IMTQ::SetIds::POS_X_TEST) {}
+      : StaticLocalDataSet(owner, imtq::SetIds::POSITIVE_X_TEST) {}
 
   PosXSelfTestSet(object_id_t objectId)
-      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_X_TEST)) {}
+      : StaticLocalDataSet(sid_t(objectId, imtq::SetIds::POSITIVE_X_TEST)) {}
 
   /** INIT block */
   lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_POS_X_ERR, this);
@@ -611,10 +722,10 @@ class PosXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 class NegXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
  public:
   NegXSelfTestSet(HasLocalDataPoolIF* owner)
-      : StaticLocalDataSet(owner, IMTQ::SetIds::NEG_X_TEST) {}
+      : StaticLocalDataSet(owner, imtq::SetIds::NEGATIVE_X_TEST) {}
 
   NegXSelfTestSet(object_id_t objectId)
-      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_X_TEST)) {}
+      : StaticLocalDataSet(sid_t(objectId, imtq::SetIds::NEGATIVE_X_TEST)) {}
 
   /** INIT block */
   lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_NEG_X_ERR, this);
@@ -688,10 +799,10 @@ class NegXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 class PosYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
  public:
   PosYSelfTestSet(HasLocalDataPoolIF* owner)
-      : StaticLocalDataSet(owner, IMTQ::SetIds::POS_Y_TEST) {}
+      : StaticLocalDataSet(owner, imtq::SetIds::POSITIVE_Y_TEST) {}
 
   PosYSelfTestSet(object_id_t objectId)
-      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_Y_TEST)) {}
+      : StaticLocalDataSet(sid_t(objectId, imtq::SetIds::POSITIVE_Y_TEST)) {}
 
   /** INIT block */
   lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_POS_Y_ERR, this);
@@ -765,10 +876,10 @@ class PosYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 class NegYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
  public:
   NegYSelfTestSet(HasLocalDataPoolIF* owner)
-      : StaticLocalDataSet(owner, IMTQ::SetIds::NEG_Y_TEST) {}
+      : StaticLocalDataSet(owner, imtq::SetIds::NEGATIVE_Y_TEST) {}
 
   NegYSelfTestSet(object_id_t objectId)
-      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_Y_TEST)) {}
+      : StaticLocalDataSet(sid_t(objectId, imtq::SetIds::NEGATIVE_Y_TEST)) {}
 
   /** INIT block */
   lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_NEG_Y_ERR, this);
@@ -842,10 +953,10 @@ class NegYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 class PosZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
  public:
   PosZSelfTestSet(HasLocalDataPoolIF* owner)
-      : StaticLocalDataSet(owner, IMTQ::SetIds::POS_Z_TEST) {}
+      : StaticLocalDataSet(owner, imtq::SetIds::POSITIVE_Z_TEST) {}
 
   PosZSelfTestSet(object_id_t objectId)
-      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_Z_TEST)) {}
+      : StaticLocalDataSet(sid_t(objectId, imtq::SetIds::POSITIVE_Y_TEST)) {}
 
   /** INIT block */
   lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_POS_Z_ERR, this);
@@ -919,10 +1030,10 @@ class PosZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 class NegZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
  public:
   NegZSelfTestSet(HasLocalDataPoolIF* owner)
-      : StaticLocalDataSet(owner, IMTQ::SetIds::NEG_Z_TEST) {}
+      : StaticLocalDataSet(owner, imtq::SetIds::NEGATIVE_Z_TEST) {}
 
   NegZSelfTestSet(object_id_t objectId)
-      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_Z_TEST)) {}
+      : StaticLocalDataSet(sid_t(objectId, imtq::SetIds::NEGATIVE_Z_TEST)) {}
 
   /** INIT block */
   lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_NEG_Z_ERR, this);
@@ -979,6 +1090,153 @@ class NegZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
       lp_var_t<int16_t>(sid.objectId, FINA_NEG_Z_COIL_Z_TEMPERATURE, this);
 };
 
-}  // namespace IMTQ
+}  // namespace imtq
 
+struct ImtqRequest {
+  friend class ImtqHandler;
+
+ public:
+  static constexpr size_t REQUEST_LEN = 10;
+
+  ImtqRequest(const uint8_t* data, size_t maxSize)
+      : rawData(const_cast<uint8_t*>(data)), maxSize(maxSize) {}
+
+  imtq::RequestType getRequestType() const { return static_cast<imtq::RequestType>(rawData[0]); }
+
+  void setMeasureRequest(imtq::SpecialRequest specialRequest) {
+    rawData[0] = static_cast<uint8_t>(imtq::RequestType::MEASURE);
+    rawData[1] = static_cast<uint8_t>(specialRequest);
+  }
+
+  void setActuateRequest(int16_t dipoleX, int16_t dipoleY, int16_t dipoleZ,
+                         uint16_t torqueDuration) {
+    size_t dummy = 0;
+    rawData[0] = static_cast<uint8_t>(imtq::RequestType::ACTUATE);
+    rawData[1] = static_cast<uint8_t>(imtq::SpecialRequest::NONE);
+    uint8_t* serPtr = rawData + 2;
+    SerializeAdapter::serialize(&dipoleX, &serPtr, &dummy, maxSize,
+                                SerializeIF::Endianness::MACHINE);
+    SerializeAdapter::serialize(&dipoleY, &serPtr, &dummy, maxSize,
+                                SerializeIF::Endianness::MACHINE);
+    SerializeAdapter::serialize(&dipoleZ, &serPtr, &dummy, maxSize,
+                                SerializeIF::Endianness::MACHINE);
+    SerializeAdapter::serialize(&torqueDuration, &serPtr, &dummy, maxSize,
+                                SerializeIF::Endianness::MACHINE);
+  }
+
+  uint8_t* startOfActuateDataPtr() { return rawData + 2; }
+
+  int16_t* getDipoles() { return reinterpret_cast<int16_t*>(rawData + 2); }
+
+  uint16_t getTorqueDuration() {
+    uint8_t* data = rawData + 2 + 6;
+    uint16_t value;
+    size_t dummy = 0;
+    SerializeAdapter::deSerialize(&value, data, &dummy, SerializeIF::Endianness::MACHINE);
+    return value;
+  }
+
+  void setSpecialRequest(imtq::SpecialRequest specialRequest) {
+    rawData[1] = static_cast<uint8_t>(specialRequest);
+  }
+
+  imtq::SpecialRequest getSpecialRequest() const {
+    return static_cast<imtq::SpecialRequest>(rawData[1]);
+  }
+
+ private:
+  ImtqRequest(uint8_t* rawData, size_t maxLen) : rawData(rawData) {
+    if (rawData != nullptr) {
+      rawData[0] = static_cast<uint8_t>(imtq::RequestType::MEASURE);
+    }
+  }
+  uint8_t* rawData;
+  size_t maxSize = 0;
+};
+
+struct ImtqRepliesDefault {
+  friend class ImtqPollingTask;
+
+ public:
+  static constexpr size_t BASE_LEN =
+      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();
+  }
+
+  uint8_t* getCalibMgmMeasurement() const { return calibMgmMeasurement + 1; }
+  bool wasCalibMgmMeasurementRead() const { return calibMgmMeasurement[0]; };
+
+  uint8_t* getEngHk() const { return engHk + 1; }
+  bool wasEngHkRead() const { return engHk[0]; };
+
+  uint8_t* getRawMgmMeasurement() const { return rawMgmMeasurement + 1; }
+  bool wasGetRawMgmMeasurementRead() const { return rawMgmMeasurement[0]; };
+
+  uint8_t* getSpecialRequest() const { return specialRequestReply + 1; }
+  bool wasSpecialRequestRead() const { return specialRequestReply[0]; }
+  uint8_t* getStartMtmMeasurement() const { return startMtmMeasurement + 1; }
+  bool wasStartMtmMeasurementRead() const { return startMtmMeasurement[0]; }
+
+  uint8_t* getSwReset() const { return swReset + 1; }
+
+  uint8_t* getSystemState() const { return systemState + 1; }
+  bool wasGetSystemStateRead() const { return systemState[0]; }
+
+ private:
+  void initPointers() {
+    swReset = rawData;
+    systemState = swReset + imtq::replySize::DEFAULT_MIN_LEN + 1;
+    startMtmMeasurement = systemState + imtq::replySize::SYSTEM_STATE + 1;
+    rawMgmMeasurement = startMtmMeasurement + imtq::replySize::DEFAULT_MIN_LEN + 1;
+    engHk = rawMgmMeasurement + imtq::replySize::RAW_MTM_MEASUREMENT + 1;
+    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;
+  uint8_t* startMtmMeasurement;
+  uint8_t* rawMgmMeasurement;
+  uint8_t* engHk;
+  uint8_t* calibMgmMeasurement;
+  // Share this to reduce amount of copied code for each transfer.
+  uint8_t* specialRequestReply;
+};
+
+struct ImtqRepliesWithTorque {
+  friend class ImtqPollingTask;
+
+ public:
+  static constexpr size_t BASE_LEN =
+      imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::ENG_HK_DATA_REPLY + 1 +
+      imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::RAW_MTM_MEASUREMENT + 1;
+  ImtqRepliesWithTorque(const uint8_t* rawData) : rawData(const_cast<uint8_t*>(rawData)) {
+    initPointers();
+  }
+
+  uint8_t* getDipoleActuation() const { return dipoleActuation + 1; }
+  bool wasDipoleActuationRead() const { return dipoleActuation[0]; }
+
+  uint8_t* getEngHk() const { return engHk + 1; }
+  bool wasEngHkRead() const { return engHk[0]; };
+
+  uint8_t* getRawMgmMeasurement() const { return rawMgmMeasurement + 1; }
+  bool wasGetRawMgmMeasurementRead() const { return rawMgmMeasurement[0]; };
+
+ private:
+  void initPointers() {
+    dipoleActuation = rawData;
+    engHk = dipoleActuation + imtq::replySize::DEFAULT_MIN_LEN + 1;
+    startMtmMeasurement = engHk + imtq::replySize::ENG_HK_DATA_REPLY + 1;
+    rawMgmMeasurement = startMtmMeasurement + imtq::replySize::DEFAULT_MIN_LEN + 1;
+  }
+  uint8_t* rawData;
+  uint8_t* dipoleActuation;
+  uint8_t* engHk;
+  uint8_t* startMtmMeasurement;
+  uint8_t* rawMgmMeasurement;
+};
 #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ */

tmtc

@@ -1 +1 @@
-Subproject commit 201e08afe13777ef06d0da1cd3a58ba087ea04df
+Subproject commit fbf507b2a893f3db4d95f3694190b6d62f513aa8