Merge pull request 'Refactor IMTQ handling' (#384) from refactor_imtq_handling into develop

Reviewed-on: #384 Reviewed-by: Marius Eggert <eggertm@irs.uni-stuttgart.de>
2023-02-21 11:04:28 +01:00 · 2023-02-21 11:04:28 +01:00 · eefdc8ff5b
commit eefdc8ff5b
parent 80f8df3ee4 a91e6b7dfe
34 changed files with 1740 additions and 947 deletions
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@ -28,6 +28,13 @@ change warranting a new major release:
 ## Changed

 - Add `-Wshadow=local` shadowing warnings and fixed all of them
+- Refactored IMTQ handlers to also perform low level I2C communication tasks in separate thread.
+  This avoids the various delays needed for I2C communication with that device inside the ACS PST.
+  (e.g. 1 ms delay between each transfer, or 10 ms integration delay for MGM measurements).
+- Updated generated CSV files: Support for skip directive and explicit
+  "No description" info string
+- The polling threads for actuator polling now have a slightly higher priority than the ACS PST
+  to ensure timing requirements are met.

 ## Added

--- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp
+++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp
@ -1,7 +1,7 @@
 /**
 * @brief    Auto-generated event translation file. Contains 257 translations.
 * @details
- * Generated on: 2023-02-21 00:24:44
+ * Generated on: 2023-02-21 10:44:59
 */
 #include "translateEvents.h"

--- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp
+++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp
@ -1,8 +1,8 @@
 /**
 * @brief  Auto-generated object translation file.
 * @details
- * Contains 147 translations.
- * Generated on: 2023-02-21 00:24:44
+ * Contains 148 translations.
+ * Generated on: 2023-02-21 10:44:59
 */
 #include "translateObjects.h"

@ -38,6 +38,7 @@ const char *GYRO_3_L3G_HANDLER_STRING = "GYRO_3_L3G_HANDLER";
 const char *RW4_STRING = "RW4";
 const char *STAR_TRACKER_STRING = "STAR_TRACKER";
 const char *GPS_CONTROLLER_STRING = "GPS_CONTROLLER";
+const char *IMTQ_POLLING_STRING = "IMTQ_POLLING";
 const char *IMTQ_HANDLER_STRING = "IMTQ_HANDLER";
 const char *PCDU_HANDLER_STRING = "PCDU_HANDLER";
 const char *P60DOCK_HANDLER_STRING = "P60DOCK_HANDLER";
@ -220,6 +221,8 @@ const char *translateObject(object_id_t object) {
      return STAR_TRACKER_STRING;
    case 0x44130045:
      return GPS_CONTROLLER_STRING;
+    case 0x44140013:
+      return IMTQ_POLLING_STRING;
    case 0x44140014:
      return IMTQ_HANDLER_STRING;
    case 0x442000A1:
--- a/bsp_q7s/core/ObjectFactory.cpp
+++ b/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);
-  auto imtqHandler = new ImtqHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie,
+  new ImtqPollingTask(objects::IMTQ_POLLING);
+  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);
--- a/bsp_q7s/core/scheduling.cpp
+++ b/bsp_q7s/core/scheduling.cpp
@ -198,12 +198,20 @@ void scheduling::initTasks() {

 #if OBSW_ADD_RW == 1
  PeriodicTaskIF* rwPolling = factory->createPeriodicTask(
-      "RW_POLLING_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
+      "RW_POLLING_TASK", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
  result = rwPolling->addComponent(objects::RW_POLLING_TASK);
  if (result != returnvalue::OK) {
    scheduling::printAddObjectError("RW_POLLING_TASK", objects::RW_POLLING_TASK);
  }
 #endif
+#if OBSW_ADD_MGT == 1
+  PeriodicTaskIF* imtqPolling = factory->createPeriodicTask(
+      "IMTQ_POLLING_TASK", 85, 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");
@ -396,7 +407,7 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction
  static constexpr float acsPstPeriod = 0.8;
 #endif
  FixedTimeslotTaskIF* acsTcsPst = factory.createFixedTimeslotTask(
-      "ACS_TCS_PST", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, acsPstPeriod, missedDeadlineFunc);
+      "ACS_TCS_PST", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, acsPstPeriod, missedDeadlineFunc);
  result = pst::pstTcsAndAcs(acsTcsPst, cfg);
  if (result != returnvalue::OK) {
    if (result == FixedTimeslotTaskIF::SLOT_LIST_EMPTY) {
--- a/common/config/eive/definitions.h
+++ b/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_4_ACTUATOR_MS = 50;
-static constexpr uint32_t SCHED_BLOCK_5_RW_READ_MS = 300;
+static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 42;
+static constexpr uint32_t SCHED_BLOCK_4_ACS_CTRL_MS = 45;
+static constexpr uint32_t SCHED_BLOCK_5_ACTUATOR_MS = 50;
+static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 75;
+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_4_PERIOD = static_cast<float>(SCHED_BLOCK_4_ACTUATOR_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_3_PERIOD =
+    static_cast<float>(SCHED_BLOCK_3_READ_IMTQ_MGM_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

--- a/common/config/eive/objects.h
+++ b/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,
--- a/dummies/ImtqDummy.cpp
+++ b/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::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::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::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::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::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>({0}));
  return DeviceHandlerBase::initializeLocalDataPool(localDataPoolMap, poolManager);
 }
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit c32798522283d89028b7c1ecd3bd33b8391e1a39
+Subproject commit 2efff4d2c5ef82b5b62567ab1bb0ee53aeed6a5a
--- a/generators/bsp_hosted_objects.csv
+++ b/generators/bsp_hosted_objects.csv
@ -30,6 +30,7 @@
 0x44120350;RW4
 0x44130001;STAR_TRACKER
 0x44130045;GPS_CONTROLLER
+0x44140013;IMTQ_POLLING
 0x44140014;IMTQ_HANDLER
 0x442000A1;PCDU_HANDLER
 0x44250000;P60DOCK_HANDLER
--- a/generators/bsp_hosted_returnvalues.csv
+++ b/generators/bsp_hosted_returnvalues.csv
@ -3,6 +3,16 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x0001;Failed;Unspecified system-wide code for failed.;1;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h
 0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NVMParameterBase.h
 0x6300;NVMB_Busy;No description;0;NVM_PARAM_BASE;mission/system/objects/Stack5VHandler.h
+0x5100;IMTQ_InvalidCommandCode;No description;0;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5101;IMTQ_MgmMeasurementLowLevelError;No description;1;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5102;IMTQ_ActuateCmdLowLevelError;No description;2;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5103;IMTQ_ParameterMissing;No description;3;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5104;IMTQ_ParameterInvalid;No description;4;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5105;IMTQ_CcUnavailable;No description;5;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5106;IMTQ_InternalProcessingError;No description;6;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5107;IMTQ_RejectedWithoutReason;No description;7;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5108;IMTQ_CmdErrUnknown;No description;8;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x51a7;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;167;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
 0x52b0;RWHA_SpiWriteFailure;No description;176;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
 0x52b1;RWHA_SpiReadFailure;Used by the spi send function to tell a failing read call;177;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
 0x52b2;RWHA_MissingStartSign;Can be used by the HDLC decoding mechanism to inform about a missing start sign 0x7E;178;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
@ -13,19 +23,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x52b7;RWHA_SpiReadTimeout;Timeout when reading reply;183;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
 0x58a0;SUSS_ErrorUnlockMutex;No description;160;SUS_HANDLER;mission/devices/SusHandler.h
 0x58a1;SUSS_ErrorLockMutex;No description;161;SUS_HANDLER;mission/devices/SusHandler.h
-0x66a0;SADPL_CommandNotSupported;No description;160;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x66a1;SADPL_DeploymentAlreadyExecuting;No description;161;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x66a2;SADPL_MainSwitchTimeoutFailure;No description;162;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x66a3;SADPL_SwitchingDeplSa1Failed;No description;163;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x66a4;SADPL_SwitchingDeplSa2Failed;No description;164;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x51a0;IMTQ_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a1;IMTQ_InvalidRampTime;Action Message with invalid ramp time was received.;161;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a2;IMTQ_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a3;IMTQ_ExecutionFailed;Command execution failed;163;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a4;IMTQ_CrcError;Reaction wheel reply has invalid crc;164;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a5;IMTQ_ValueNotRead;No description;165;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a6;IMTQ_CmdErrUnknown;No description;166;IMTQ_HANDLER;mission/devices/ImtqHandler.h
-0x51a7;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;167;IMTQ_HANDLER;mission/devices/ImtqHandler.h
+0x66a0;SADPL_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;SA_DEPL_HANDLER;mission/devices/RwHandler.h
+0x66a1;SADPL_InvalidRampTime;Action Message with invalid ramp time was received.;161;SA_DEPL_HANDLER;mission/devices/RwHandler.h
+0x66a2;SADPL_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;SA_DEPL_HANDLER;mission/devices/RwHandler.h
+0x66a3;SADPL_ExecutionFailed;Command execution failed;163;SA_DEPL_HANDLER;mission/devices/RwHandler.h
+0x66a4;SADPL_CrcError;Reaction wheel reply has invalid crc;164;SA_DEPL_HANDLER;mission/devices/RwHandler.h
+0x66a5;SADPL_ValueNotRead;No description;165;SA_DEPL_HANDLER;mission/devices/RwHandler.h
 0x50a0;SYRLINKS_CrcFailure;No description;160;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h
 0x50a1;SYRLINKS_UartFraminOrParityErrorAck;No description;161;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h
 0x50a2;SYRLINKS_BadCharacterAck;No description;162;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h
--- a/generators/bsp_q7s_objects.csv
+++ b/generators/bsp_q7s_objects.csv
@ -29,6 +29,7 @@
 0x44120350;RW4
 0x44130001;STAR_TRACKER
 0x44130045;GPS_CONTROLLER
+0x44140013;IMTQ_POLLING
 0x44140014;IMTQ_HANDLER
 0x442000A1;PCDU_HANDLER
 0x44250000;P60DOCK_HANDLER
--- a/generators/bsp_q7s_returnvalues.csv
+++ b/generators/bsp_q7s_returnvalues.csv
@ -3,6 +3,16 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x0001;Failed;Unspecified system-wide code for failed.;1;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h
 0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NVMParameterBase.h
 0x6300;NVMB_Busy;No description;0;NVM_PARAM_BASE;mission/system/objects/Stack5VHandler.h
+0x5100;IMTQ_InvalidCommandCode;No description;0;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5101;IMTQ_MgmMeasurementLowLevelError;No description;1;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5102;IMTQ_ActuateCmdLowLevelError;No description;2;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5103;IMTQ_ParameterMissing;No description;3;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5104;IMTQ_ParameterInvalid;No description;4;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5105;IMTQ_CcUnavailable;No description;5;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5106;IMTQ_InternalProcessingError;No description;6;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5107;IMTQ_RejectedWithoutReason;No description;7;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x5108;IMTQ_CmdErrUnknown;No description;8;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
+0x51a7;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;167;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
 0x52b0;RWHA_SpiWriteFailure;No description;176;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
 0x52b1;RWHA_SpiReadFailure;Used by the spi send function to tell a failing read call;177;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
 0x52b2;RWHA_MissingStartSign;Can be used by the HDLC decoding mechanism to inform about a missing start sign 0x7E;178;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
@ -13,19 +23,12 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x52b7;RWHA_SpiReadTimeout;Timeout when reading reply;183;RW_HANDLER;mission/devices/devicedefinitions/rwHelpers.h
 0x58a0;SUSS_ErrorUnlockMutex;No description;160;SUS_HANDLER;mission/devices/SusHandler.h
 0x58a1;SUSS_ErrorLockMutex;No description;161;SUS_HANDLER;mission/devices/SusHandler.h
-0x66a0;SADPL_CommandNotSupported;No description;160;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x66a1;SADPL_DeploymentAlreadyExecuting;No description;161;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x66a2;SADPL_MainSwitchTimeoutFailure;No description;162;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x66a3;SADPL_SwitchingDeplSa1Failed;No description;163;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x66a4;SADPL_SwitchingDeplSa2Failed;No description;164;SA_DEPL_HANDLER;mission/devices/SolarArrayDeploymentHandler.h
-0x51a0;IMTQ_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a1;IMTQ_InvalidRampTime;Action Message with invalid ramp time was received.;161;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a2;IMTQ_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a3;IMTQ_ExecutionFailed;Command execution failed;163;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a4;IMTQ_CrcError;Reaction wheel reply has invalid crc;164;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a5;IMTQ_ValueNotRead;No description;165;IMTQ_HANDLER;mission/devices/RwHandler.h
-0x51a6;IMTQ_CmdErrUnknown;No description;166;IMTQ_HANDLER;mission/devices/ImtqHandler.h
-0x51a7;IMTQ_UnexpectedSelfTestReply;The status reply to a self test command was received but no self test command has been sent. This should normally never happen.;167;IMTQ_HANDLER;mission/devices/ImtqHandler.h
+0x66a0;SADPL_InvalidSpeed;Action Message with invalid speed was received. Valid speeds must be in the range of [-65000, 1000] or [1000, 65000];160;SA_DEPL_HANDLER;mission/devices/RwHandler.h
+0x66a1;SADPL_InvalidRampTime;Action Message with invalid ramp time was received.;161;SA_DEPL_HANDLER;mission/devices/RwHandler.h
+0x66a2;SADPL_SetSpeedCommandInvalidLength;Received set speed command has invalid length. Should be 6.;162;SA_DEPL_HANDLER;mission/devices/RwHandler.h
+0x66a3;SADPL_ExecutionFailed;Command execution failed;163;SA_DEPL_HANDLER;mission/devices/RwHandler.h
+0x66a4;SADPL_CrcError;Reaction wheel reply has invalid crc;164;SA_DEPL_HANDLER;mission/devices/RwHandler.h
+0x66a5;SADPL_ValueNotRead;No description;165;SA_DEPL_HANDLER;mission/devices/RwHandler.h
 0x50a0;SYRLINKS_CrcFailure;No description;160;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h
 0x50a1;SYRLINKS_UartFraminOrParityErrorAck;No description;161;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h
 0x50a2;SYRLINKS_BadCharacterAck;No description;162;SYRLINKS_HANDLER;mission/devices/SyrlinksHandler.h
@ -566,6 +569,7 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x53b6;STRH_StartrackerAlreadyBooted;Star tracker is already in firmware mode;182;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h
 0x53b7;STRH_StartrackerRunningFirmware;Star tracker is in firmware mode but must be in bootloader mode to execute this command;183;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h
 0x53b8;STRH_StartrackerRunningBootloader;Star tracker is in bootloader mode but must be in firmware mode to execute this command;184;STR_HANDLER;linux/devices/startracker/StarTrackerHandler.h
+0x5300;STRH_NoReplyAvailable;No description;0;STR_HANDLER;linux/devices/ImtqPollingTask.h
 0x5302;STRH_InvalidCrc;No description;2;STR_HANDLER;linux/devices/ScexHelper.h
 0x5aa0;PTME_UnknownVcId;No description;160;PTME;linux/ipcore/Ptme.h
 0x5fa0;PDEC_AbandonedCltuRetval;No description;160;PDEC_HANDLER;linux/ipcore/PdecHandler.h
--- a/generators/events/translateEvents.cpp
+++ b/generators/events/translateEvents.cpp
@ -1,7 +1,7 @@
 /**
 * @brief    Auto-generated event translation file. Contains 257 translations.
 * @details
- * Generated on: 2023-02-21 00:24:44
+ * Generated on: 2023-02-21 10:44:59
 */
 #include "translateEvents.h"

--- a/generators/objects/translateObjects.cpp
+++ b/generators/objects/translateObjects.cpp
@ -1,8 +1,8 @@
 /**
 * @brief  Auto-generated object translation file.
 * @details
- * Contains 152 translations.
- * Generated on: 2023-02-21 00:24:44
+ * Contains 153 translations.
+ * Generated on: 2023-02-21 10:44:59
 */
 #include "translateObjects.h"

@ -37,6 +37,7 @@ const char *GYRO_3_L3G_HANDLER_STRING = "GYRO_3_L3G_HANDLER";
 const char *RW4_STRING = "RW4";
 const char *STAR_TRACKER_STRING = "STAR_TRACKER";
 const char *GPS_CONTROLLER_STRING = "GPS_CONTROLLER";
+const char *IMTQ_POLLING_STRING = "IMTQ_POLLING";
 const char *IMTQ_HANDLER_STRING = "IMTQ_HANDLER";
 const char *PCDU_HANDLER_STRING = "PCDU_HANDLER";
 const char *P60DOCK_HANDLER_STRING = "P60DOCK_HANDLER";
@ -223,6 +224,8 @@ const char *translateObject(object_id_t object) {
      return STAR_TRACKER_STRING;
    case 0x44130045:
      return GPS_CONTROLLER_STRING;
+    case 0x44140013:
+      return IMTQ_POLLING_STRING;
    case 0x44140014:
      return IMTQ_HANDLER_STRING;
    case 0x442000A1:
--- a/generators/requirements.txt
+++ b/generators/requirements.txt
@ -1,2 +1,2 @@
 colorlog==6.7.0
-git+https://egit.irs.uni-stuttgart.de/fsfw/fsfwgen@66e31885a7c87fbb4340cd2a51a13e1196f377af#egg=fsfwgen
+git+https://egit.irs.uni-stuttgart.de/fsfw/fsfwgen@v0.3.0#egg=fsfwgen
--- a/linux/devices/CMakeLists.txt
+++ b/linux/devices/CMakeLists.txt
@ -3,8 +3,9 @@ if(EIVE_BUILD_GPSD_GPS_HANDLER)
 endif()

 target_sources(
-  ${OBSW_NAME} PRIVATE Max31865RtdLowlevelHandler.cpp ScexUartReader.cpp
-                       ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp)
+  ${OBSW_NAME}
+  PRIVATE Max31865RtdLowlevelHandler.cpp ScexUartReader.cpp ScexDleParser.cpp
+          ScexHelper.cpp RwPollingTask.cpp ImtqPollingTask.cpp)

 add_subdirectory(ploc)

--- a/linux/devices/ImtqPollingTask.cpp
+++ b/linux/devices/ImtqPollingTask.cpp
@ -0,0 +1,433 @@
+#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) {
+  while (true) {
+    ipcLock->lockMutex();
+    state = InternalState::IDLE;
+    ipcLock->unlockMutex();
+    semaphore->acquire();
+
+    comStatus = returnvalue::OK;
+    // Stopwatch watch;
+    switch (currentRequest) {
+      case imtq::RequestType::MEASURE_NO_ACTUATION: {
+        handleMeasureStep();
+        break;
+      }
+      case imtq::RequestType::ACTUATE: {
+        handleActuateStep();
+        break;
+      }
+    };
+  }
+  return returnvalue::OK;
+}
+
+void ImtqPollingTask::handleMeasureStep() {
+  size_t replyLen = 0;
+  uint8_t* replyPtr;
+  ImtqRepliesDefault replies(replyBuf.data());
+  // Can be used later to verify correct timing (e.g. all data has been read)
+  clearReadFlagsDefault(replies);
+  auto i2cCmdExecMeasure = [&](imtq::CC::CC cc) {
+    ccToReplyPtrMeasure(replies, cc, &replyPtr, replyLen);
+    return i2cCmdExecDefault(cc, replyPtr, replyLen, imtq::MGM_MEASUREMENT_LOW_LEVEL_ERROR);
+  };
+
+  cmdLen = 1;
+  cmdBuf[0] = imtq::CC::GET_SYSTEM_STATE;
+  if (i2cCmdExecMeasure(imtq::CC::GET_SYSTEM_STATE) != returnvalue::OK) {
+    return;
+  }
+
+  ignoreNextActuateRequest =
+      (replies.getSystemState()[2] == static_cast<uint8_t>(imtq::mode::SELF_TEST));
+  if (ignoreNextActuateRequest) {
+    // Do not command anything until self-test is done.
+    return;
+  }
+
+  if (specialRequest != imtq::SpecialRequest::NONE) {
+    auto executeSelfTest = [&](imtq::selfTest::Axis axis) {
+      cmdBuf[0] = imtq::CC::SELF_TEST_CMD;
+      cmdBuf[1] = axis;
+      return i2cCmdExecMeasure(imtq::CC::SELF_TEST_CMD);
+    };
+    // If a self-test is already ongoing, ignore the request.
+    if (replies.getSystemState()[2] != static_cast<uint8_t>(imtq::mode::SELF_TEST)) {
+      switch (specialRequest) {
+        case (imtq::SpecialRequest::DO_SELF_TEST_POS_X): {
+          executeSelfTest(imtq::selfTest::Axis::X_POSITIVE);
+          break;
+        }
+        case (imtq::SpecialRequest::DO_SELF_TEST_NEG_X): {
+          executeSelfTest(imtq::selfTest::Axis::X_NEGATIVE);
+          break;
+        }
+        case (imtq::SpecialRequest::DO_SELF_TEST_POS_Y): {
+          executeSelfTest(imtq::selfTest::Axis::Y_POSITIVE);
+          break;
+        }
+        case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Y): {
+          executeSelfTest(imtq::selfTest::Axis::Y_NEGATIVE);
+          break;
+        }
+        case (imtq::SpecialRequest::DO_SELF_TEST_POS_Z): {
+          executeSelfTest(imtq::selfTest::Axis::Z_POSITIVE);
+          break;
+        }
+        case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Z): {
+          executeSelfTest(imtq::selfTest::Axis::Z_NEGATIVE);
+          break;
+        }
+        case (imtq::SpecialRequest::GET_SELF_TEST_RESULT): {
+          cmdBuf[0] = imtq::CC::GET_SELF_TEST_RESULT;
+          i2cCmdExecMeasure(imtq::CC::GET_SELF_TEST_RESULT);
+          break;
+        }
+        default: {
+          // Should never happen
+          break;
+        }
+      }
+      // We are done. Only request self test or results here.
+      return;
+    }
+  }
+
+  cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
+  if (i2cCmdExecMeasure(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
+    return;
+  }
+  // Takes a bit of time to take measurements. Subtract a bit because of the delay of previous
+  // commands.
+  TaskFactory::delayTask(currentIntegrationTimeMs);
+
+  cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
+  if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
+    return;
+  }
+
+  cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
+  if (i2cCmdExecMeasure(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
+    return;
+  }
+
+  cmdBuf[0] = imtq::CC::GET_CAL_MTM_MEASUREMENT;
+  if (i2cCmdExecMeasure(imtq::CC::GET_CAL_MTM_MEASUREMENT) != returnvalue::OK) {
+    return;
+  }
+  // sif::debug << "measure done" << std::endl;
+  return;
+}
+
+void ImtqPollingTask::handleActuateStep() {
+  uint8_t* replyPtr = nullptr;
+  size_t replyLen = 0;
+  // No point when self-test mode is active.
+  if (ignoreNextActuateRequest) {
+    return;
+  }
+  ImtqRepliesWithTorque replies(replyBufActuation.data());
+  // Can be used later to verify correct timing (e.g. all data has been read)
+  clearReadFlagsWithTorque(replies);
+  auto i2cCmdExecActuate = [&](imtq::CC::CC cc) {
+    ccToReplyPtrActuate(replies, cc, &replyPtr, replyLen);
+    return i2cCmdExecDefault(cc, replyPtr, replyLen, imtq::ACTUATE_CMD_LOW_LEVEL_ERROR);
+  };
+  buildDipoleCommand();
+  if (i2cCmdExecActuate(imtq::CC::START_ACTUATION_DIPOLE) != returnvalue::OK) {
+    return;
+  }
+
+  cmdLen = 1;
+  cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
+  if (i2cCmdExecActuate(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
+    return;
+  }
+
+  TaskFactory::delayTask(currentIntegrationTimeMs);
+
+  cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
+  if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
+    return;
+  }
+  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;
+  return;
+}
+
+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_POS_X):
+    case (imtq::SpecialRequest::DO_SELF_TEST_NEG_X):
+    case (imtq::SpecialRequest::DO_SELF_TEST_POS_Y):
+    case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Y):
+    case (imtq::SpecialRequest::DO_SELF_TEST_POS_Z):
+    case (imtq::SpecialRequest::DO_SELF_TEST_NEG_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_NO_ACTUATION) {
+    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::clearReadFlagsDefault(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::i2cCmdExecDefault(imtq::CC::CC cc, uint8_t* replyPtr,
+                                                 size_t replyLen, ReturnValue_t comErrIfFails) {
+  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;
+}
+
+void ImtqPollingTask::clearReadFlagsWithTorque(ImtqRepliesWithTorque& replies) {
+  replies.dipoleActuation[0] = false;
+  replies.engHk[0] = false;
+  replies.rawMgmMeasurement[0] = false;
+  replies.startMtmMeasurement[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;
+}
--- a/linux/devices/ImtqPollingTask.h
+++ b/linux/devices/ImtqPollingTask.h
@ -0,0 +1,70 @@
+#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_NO_ACTUATION;
+
+  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;
+  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;
+  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 clearReadFlagsDefault(ImtqRepliesDefault& replies);
+  void clearReadFlagsWithTorque(ImtqRepliesWithTorque& replies);
+  size_t getExchangeBufLen(imtq::SpecialRequest specialRequest);
+  void buildDipoleCommand();
+  void handleMeasureStep();
+  void handleActuateStep();
+  ReturnValue_t i2cCmdExecDefault(imtq::CC::CC cc, uint8_t* replyPtr, size_t replyLen,
+                                  ReturnValue_t comErrIfFails);
+  ReturnValue_t performI2cFullRequest(uint8_t* reply, size_t replyLen);
+};
+
+#endif /* LINUX_DEVICES_IMTQPOLLINGTASK_H_ */
--- a/linux/devices/RwPollingTask.cpp
+++ b/linux/devices/RwPollingTask.cpp
@ -162,11 +162,15 @@ 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);
-    *buffer = rwCookie->replyBuf.data();
-    *size = rwCookie->replyBuf.size();
+  if (rwCookie == nullptr or rwCookie->bufLock == nullptr) {
+    return returnvalue::FAILED;
  }
+  {
+    MutexGuard mg(rwCookie->bufLock);
+    memcpy(rwCookie->exchangeBuf.data(), rwCookie->replyBuf.data(), rwCookie->replyBuf.size());
+  }
+  *buffer = rwCookie->exchangeBuf.data();
+  *size = rwCookie->exchangeBuf.size();
  return returnvalue::OK;
 }

@ -248,6 +252,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 +433,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;
+    }
  }
 }

--- a/linux/devices/RwPollingTask.h
+++ b/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);
--- a/linux/fsfwconfig/events/translateEvents.cpp
+++ b/linux/fsfwconfig/events/translateEvents.cpp
@ -1,7 +1,7 @@
 /**
 * @brief    Auto-generated event translation file. Contains 257 translations.
 * @details
- * Generated on: 2023-02-21 00:24:44
+ * Generated on: 2023-02-21 10:44:59
 */
 #include "translateEvents.h"

--- a/linux/fsfwconfig/objects/translateObjects.cpp
+++ b/linux/fsfwconfig/objects/translateObjects.cpp
@ -1,8 +1,8 @@
 /**
 * @brief  Auto-generated object translation file.
 * @details
- * Contains 152 translations.
- * Generated on: 2023-02-21 00:24:44
+ * Contains 153 translations.
+ * Generated on: 2023-02-21 10:44:59
 */
 #include "translateObjects.h"

@ -37,6 +37,7 @@ const char *GYRO_3_L3G_HANDLER_STRING = "GYRO_3_L3G_HANDLER";
 const char *RW4_STRING = "RW4";
 const char *STAR_TRACKER_STRING = "STAR_TRACKER";
 const char *GPS_CONTROLLER_STRING = "GPS_CONTROLLER";
+const char *IMTQ_POLLING_STRING = "IMTQ_POLLING";
 const char *IMTQ_HANDLER_STRING = "IMTQ_HANDLER";
 const char *PCDU_HANDLER_STRING = "PCDU_HANDLER";
 const char *P60DOCK_HANDLER_STRING = "P60DOCK_HANDLER";
@ -223,6 +224,8 @@ const char *translateObject(object_id_t object) {
      return STAR_TRACKER_STRING;
    case 0x44130045:
      return GPS_CONTROLLER_STRING;
+    case 0x44140013:
+      return IMTQ_POLLING_STRING;
    case 0x44140014:
      return IMTQ_HANDLER_STRING;
    case 0x442000A1:
--- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
+++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
@ -4,6 +4,7 @@
 #include <fsfw/objectmanager/ObjectManagerIF.h>
 #include <fsfw/serviceinterface/ServiceInterfaceStream.h>
 #include <fsfw/tasks/FixedTimeslotTaskIF.h>
+#include <mission/devices/devicedefinitions/imtqHelpers.h>

 #include "OBSWConfig.h"
 #include "eive/definitions.h"
@ -60,8 +61,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.25, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0.3, DeviceHandlerIF::SEND_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
@ -585,98 +586,76 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
  if (cfg.scheduleImtq) {
    // This is the MTM measurement cycle
    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::PERFORM_OPERATION);
+                          imtq::ComStep::DHB_OP);
    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,
-                          DeviceHandlerIF::SEND_WRITE);
+                          imtq::ComStep::START_MEASURE_SEND);
    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::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);
+                          imtq::ComStep::START_MEASURE_GET);
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
+                          imtq::ComStep::READ_MEASURE_SEND);
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
+                          imtq::ComStep::READ_MEASURE_GET);
  }

-  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,
-                          DeviceHandlerIF::PERFORM_OPERATION);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::SEND_WRITE);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::GET_WRITE);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::SEND_READ);
-    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
-                          DeviceHandlerIF::GET_READ);
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_5_PERIOD,
+                          imtq::ComStep::START_ACTUATE_SEND);
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_5_PERIOD,
+                          imtq::ComStep::START_ACTUATE_GET);
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_6_PERIOD,
+                          imtq::ComStep::READ_ACTUATE_SEND);
+    thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_6_PERIOD,
+                          imtq::ComStep::READ_ACTUATE_GET);
  }

  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);
  }

--- a/mission/controller/AcsController.h
+++ b/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);
--- a/mission/controller/ThermalController.cpp
+++ b/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;
--- a/mission/controller/acs/SensorValues.h
+++ b/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),
--- a/mission/devices/ImtqHandler.cpp
+++ b/mission/devices/ImtqHandler.cpp
--- a/mission/devices/ImtqHandler.h
+++ b/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"
@ -23,7 +23,6 @@ class ImtqHandler : public DeviceHandlerBase {

  void setPollingMode(NormalPollingMode pollMode);

-  void doSendRead() override;
  /**
   * @brief   Sets mode to MODE_NORMAL. Can be used for debugging.
   */
@ -32,6 +31,7 @@ class ImtqHandler : public DeviceHandlerBase {
  void setDebugMode(bool enable);

 protected:
+  ReturnValue_t performOperation(uint8_t opCode);
  void doStartUp() override;
  void doShutDown() override;
  ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override;
@ -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,43 @@ class ImtqHandler : public DeviceHandlerBase {
  //! link between IMTQ and OBC.
  static const Event INVALID_ERROR_BYTE = MAKE_EVENT(8, severity::LOW);

-  IMTQ::EngHkDataset engHkDataset;
-  IMTQ::CalibratedMtmMeasurementSet calMtmMeasurementSet;
-  IMTQ::RawMtmMeasurementSet rawMtmMeasurementSet;
-  IMTQ::DipoleActuationSet dipoleSet;
-  IMTQ::PosXSelfTestSet posXselfTestDataset;
-  IMTQ::NegXSelfTestSet negXselfTestDataset;
-  IMTQ::PosYSelfTestSet posYselfTestDataset;
-  IMTQ::NegYSelfTestSet negYselfTestDataset;
-  IMTQ::PosZSelfTestSet posZselfTestDataset;
-  IMTQ::NegZSelfTestSet negZselfTestDataset;
+  imtq::StatusDataset statusSet;
+  imtq::DipoleActuationSet dipoleSet;
+  imtq::RawMtmMeasurementNoTorque rawMtmNoTorque;
+  imtq::HkDatasetNoTorque hkDatasetNoTorque;
+
+  imtq::RawMtmMeasurementWithTorque rawMtmWithTorque;
+  imtq::HkDatasetWithTorque hkDatasetWithTorque;
+
+  imtq::CalibratedMtmMeasurementSet calMtmMeasurementSet;
+  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;
+  bool specialRequestActive = false;
+  bool firstReplyCycle = true;

-  enum class CommunicationStep {
-    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 };
-
-  StartupStep startupStep = StartupStep::COMMAND_SELF_TEST;
-
-  bool selfTestPerformed = false;
+  imtq::RequestType requestStep = imtq::RequestType::MEASURE_NO_ACTUATION;

  /**
   * @brief   In case of a status reply to a single axis self test command, this function
@ -155,7 +135,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,23 +143,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);

-  /**
-   * @brief   This function sends a command reply to the requesting queue.
-   *
-   * @param data  Pointer to the data to send.
-   * @param dataSize  Size of the data to send.
-   * @param relplyId  Reply id which will be inserted at the beginning of the action message.
-   */
-  void handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId);
-
-  /**
-   * @brief   This function handles the reply containing the commanded dipole.
-   *
-   * @param packet    Pointer to the reply data.
-   */
-  void handleGetCommandedDipoleReply(const uint8_t* packet);
+  void fillSystemStateIntoDataset(const uint8_t* packet);

  /**
   * @brief   This function parses the reply containing the calibrated MTM measurement and writes
@ -212,7 +178,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.
   *
--- a/mission/devices/devicedefinitions/CMakeLists.txt
+++ b/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)
--- a/mission/devices/devicedefinitions/imtqHelpers.cpp
+++ b/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;
+}
--- a/mission/devices/devicedefinitions/imtqHandlerDefinitions.h
+++ b/mission/devices/devicedefinitions/imtqHandlerDefinitions.h
@ -1,23 +1,60 @@
 #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_
 #define MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_

+#include <eive/resultClassIds.h>
 #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;
-static const DeviceCommandId_t GET_ENG_HK_DATA = 0x1;
+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 };
+
+enum class SpecialRequest : uint8_t {
+  NONE = 0,
+  DO_SELF_TEST_POS_X = 1,
+  DO_SELF_TEST_NEG_X = 2,
+  DO_SELF_TEST_POS_Y = 3,
+  DO_SELF_TEST_NEG_Y = 4,
+  DO_SELF_TEST_POS_Z = 5,
+  DO_SELF_TEST_NEG_Z = 6,
+  GET_SELF_TEST_RESULT = 7
+};
+
+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 +63,49 @@ 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;
-static const uint8_t CFGR_CMD_SIZE = 3;
+}  // namespace cmdIds
+
 static const uint8_t POINTER_REG_SIZE = 1;

 enum SetIds : uint32_t {
-  ENG_HK = 1,
-  CAL_MGM = 2,
-  RAW_MGM = 3,
-  POS_X_TEST = 4,
-  NEG_X_TEST = 5,
-  POS_Y_TEST = 6,
-  NEG_Y_TEST = 7,
-  POS_Z_TEST = 8,
-  NEG_Z_TEST = 9,
-  DIPOLES = 10
+  ENG_HK_NO_TORQUE = 1,
+  RAW_MTM_NO_TORQUE = 2,
+  ENG_HK_SET_WITH_TORQUE = 3,
+  RAW_MTM_WITH_TORQUE = 4,
+  STATUS_SET = 5,
+  DIPOLES = 6,
+
+  CAL_MTM_SET = 9,
+  POSITIVE_X_TEST = 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;
-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;
+namespace replySize {

-static const uint16_t MAX_REPLY_SIZE = SIZE_SELF_TEST_RESULTS;
-static const uint8_t MAX_COMMAND_SIZE = 9;
+static constexpr uint8_t GET_TEMP_REPLY_SIZE = 2;
+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 +122,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 {
-static const uint8_t START_MTM_MEASUREMENT = 0x4;
-static const uint8_t START_ACTUATION_DIPOLE = 0x6;
-static const uint8_t SELF_TEST_CMD = 0x8;
-static const uint8_t SOFTWARE_RESET = 0xAA;
-static const uint8_t GET_ENG_HK_DATA = 0x4A;
-static const uint8_t GET_COMMANDED_DIPOLE = 0x46;
-static const uint8_t GET_RAW_MTM_MEASUREMENT = 0x42;
-static const uint8_t GET_CAL_MTM_MEASUREMENT = 0x43;
-static const uint8_t GET_SELF_TEST_RESULT = 0x47;
-static const uint8_t PAST_AVAILABLE_RESPONSE_BYTES = 0xff;
-};  // namespace CC
+enum CC : uint8_t {
+  START_MTM_MEASUREMENT = 0x4,
+  START_ACTUATION_DIPOLE = 0x6,
+  SELF_TEST_CMD = 0x8,
+  GET_SYSTEM_STATE = 0x41,
+  GET_RAW_MTM_MEASUREMENT = 0x42,
+  GET_CAL_MTM_MEASUREMENT = 0x43,
+  GET_COIL_CURRENT = 0x44,
+  GET_COIL_TEMPERATURES = 0x45,
+  GET_COMMANDED_DIPOLE = 0x46,
+  GET_SELF_TEST_RESULT = 0x47,
+  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 {
-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 CC

-namespace SELF_TEST_STEPS {
-static const uint8_t INIT = 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;
-static const uint8_t FINA = 0x7;
-}  // namespace SELF_TEST_STEPS
+size_t getReplySize(CC::CC cc, size_t* optSecondSize = nullptr);
+
+namespace mode {
+enum Mode : uint8_t { IDLE = 0, SELF_TEST = 1, DETUMBLE = 2 };
+}
+
+namespace selfTest {
+
+enum Axis : uint8_t {
+
+  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 +464,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 +495,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 +529,11 @@ class CalibratedMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRI
 */
 class RawMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
 public:
-  RawMtmMeasurementSet(HasLocalDataPoolIF* owner)
-      : StaticLocalDataSet(owner, IMTQ::SetIds::RAW_MGM) {}
+  RawMtmMeasurementSet(HasLocalDataPoolIF* owner, uint32_t setId)
+      : StaticLocalDataSet(owner, setId) {}

-  RawMtmMeasurementSet(object_id_t objectId)
-      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::RAW_MGM)) {}
+  RawMtmMeasurementSet(object_id_t objectId, uint32_t setId)
+      : 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 +542,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 +599,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 +651,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 +728,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 +805,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 +882,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 +959,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 +1036,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 +1096,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_NO_ACTUATION);
+    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 = 0;
+    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_NO_ACTUATION);
+    }
+  }
+  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_ */
--- a/mission/tmtc/CfdpTmFunnel.cpp
+++ b/mission/tmtc/CfdpTmFunnel.cpp
@ -53,6 +53,7 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) {
 #endif
    return result;
  }
+
  size_t packetLen = 0;
  uint8_t* serPtr = newPacketData;
  result = spacePacketHeader.serializeBe(&serPtr, &packetLen, spacePacketHeader.getFullPacketLen());
@ -84,8 +85,8 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) {
          sif::error << "PusTmFunnel::handlePacket: Store too full to create data copy or store "
                        "error"
                     << std::endl;
-          break;
 #endif
+          break;
        }
      } else {
        msg.setStorageId(origStoreId);
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit 1bc38ea4b507ac0e395c52d0b142ad598bb0817c
+Subproject commit 5f082cd9fccdaa304f00d92c1f25cdd6a7d8ed0b