Merge remote-tracking branch 'origin/develop' into mueller/acs-ss-init

2022-10-25 13:13:41 +02:00 · 2022-10-25 13:13:41 +02:00 · 9b8d2f3097
commit 9b8d2f3097
parent c54f52eff6 5204afb9bd
12 changed files with 379 additions and 130 deletions
--- a/bsp_q7s/core/ObjectFactory.cpp
+++ b/bsp_q7s/core/ObjectFactory.cpp
@ -49,6 +49,8 @@
 #if OBSW_TEST_LIBGPIOD == 1
 #include "linux/boardtest/LibgpiodTest.h"
 #endif
+#include <mission/devices/ImtqHandler.h>
+
 #include <sstream>

 #include "fsfw/datapoollocal/LocalDataPoolManager.h"
@ -72,7 +74,6 @@
 #include "mission/devices/BpxBatteryHandler.h"
 #include "mission/devices/GyroADIS1650XHandler.h"
 #include "mission/devices/HeaterHandler.h"
-#include "mission/devices/IMTQHandler.h"
 #include "mission/devices/Max31865PT1000Handler.h"
 #include "mission/devices/P60DockHandler.h"
 #include "mission/devices/PCDUHandler.h"
@ -897,7 +898,7 @@ void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher) {
 void ObjectFactory::createImtqComponents(PowerSwitchIF* pwrSwitcher) {
  I2cCookie* imtqI2cCookie =
      new I2cCookie(addresses::IMTQ, IMTQ::MAX_REPLY_SIZE, q7s::I2C_DEFAULT_DEV);
-  auto imtqHandler = new IMTQHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie,
+  auto imtqHandler = new ImtqHandler(objects::IMTQ_HANDLER, objects::I2C_COM_IF, imtqI2cCookie,
                                     pcdu::Switches::PDU1_CH3_MGT_5V);
  imtqHandler->setPowerSwitcher(pwrSwitcher);
  imtqHandler->connectModeTreeParent(satsystem::ACS_SUBSYSTEM);
--- 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/imtqHandlerDefinitions.h>

 ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
    : DeviceHandlerBase(objectId, comif, comCookie) {}
--- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
+++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
@ -430,11 +430,21 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) {
  uint32_t length = thisSequence->getPeriodMs();
  static_cast<void>(length);
 #if OBSW_ADD_MGT == 1
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::PERFORM_OPERATION);
  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.4, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.8, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::PERFORM_OPERATION);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::GET_READ);
 #endif
 #if OBSW_ADD_BPX_BATTERY_HANDLER == 1
  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * 0, DeviceHandlerIF::PERFORM_OPERATION);
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@ -2,6 +2,8 @@

 #include <fsfw/datapool/PoolReadGuard.h>

+#include "mission/devices/torquer.h"
+
 AcsController::AcsController(object_id_t objectId)
    : ExtendedControllerBase(objectId), mgmData(this) {}

@ -29,6 +31,15 @@ void AcsController::performControlOperation() {
      break;
  }

+  {
+      // TODO: Calculate actuator output
+      //    PoolReadGuard pg(&dipoleSet);
+      //    MutexGuard mg(torquer::lazyLock());
+      //    torquer::NEW_ACTUATION_FLAG = true;
+      // TODO: Insert correct values here
+      // dipoleSet.setDipoles(500, 500, 500, 150);
+  }
+
  {
    PoolReadGuard pg(&mgmData);
    if (pg.getReadResult() == returnvalue::OK) {
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@ -7,8 +7,8 @@
 #include "eive/objects.h"
 #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
 #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
-#include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h"
 #include "mission/devices/devicedefinitions/SusDefinitions.h"
+#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"

 class AcsController : public ExtendedControllerBase {
 public:
@ -20,7 +20,6 @@ class AcsController : public ExtendedControllerBase {
  enum class InternalState { STARTUP, INITIAL_DELAY, READY };

  InternalState internalState = InternalState::STARTUP;
-
  ReturnValue_t handleCommandMessage(CommandMessage* message) override;
  void performControlOperation() override;

@ -44,6 +43,7 @@ class AcsController : public ExtendedControllerBase {
  RM3100::Rm3100PrimaryDataset mgm3Rm3100Set =
      RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER);
  IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_HANDLER);
+  IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);

  PoolEntry<float> mgm0PoolVec = PoolEntry<float>(3);
  PoolEntry<float> mgm1PoolVec = PoolEntry<float>(3);
--- a/mission/controller/ThermalController.cpp
+++ b/mission/controller/ThermalController.cpp
@ -8,9 +8,9 @@
 #include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
 #include <mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h>
 #include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
-#include <mission/devices/devicedefinitions/IMTQHandlerDefinitions.h>
 #include <mission/devices/devicedefinitions/RwDefinitions.h>
 #include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
+#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
 #include <mission/devices/devicedefinitions/payloadPcduDefinitions.h>
 #include <objects/systemObjectList.h>

--- a/mission/devices/CMakeLists.txt
+++ b/mission/devices/CMakeLists.txt
@ -11,7 +11,7 @@ target_sources(
          SyrlinksHkHandler.cpp
          Max31865PT1000Handler.cpp
          Max31865EiveHandler.cpp
-          IMTQHandler.cpp
+          ImtqHandler.cpp
          HeaterHandler.cpp
          RadiationSensorHandler.cpp
          GyroADIS1650XHandler.cpp
@ -20,6 +20,7 @@ target_sources(
          SusHandler.cpp
          PayloadPcduHandler.cpp
          SolarArrayDeploymentHandler.cpp
-          ScexDeviceHandler.cpp)
+          ScexDeviceHandler.cpp
+          torquer.cpp)

 add_subdirectory(devicedefinitions)
--- a/mission/devices/ImtqHandler.cpp
+++ b/mission/devices/ImtqHandler.cpp
@ -1,18 +1,41 @@
-#include "IMTQHandler.h"
-
+#include <bits/stdint-intn.h>
+#include <commonConfig.h>
+#include <fsfw/datapool/PoolEntry.h>
 #include <fsfw/datapool/PoolReadGuard.h>
-#include <fsfw/globalfunctions/CRC.h>
+#include <fsfw/datapoollocal/LocalDataPoolManager.h>
+#include <fsfw/datapoollocal/LocalPoolVariable.h>
+#include <fsfw/datapoollocal/ProvidesDataPoolSubscriptionIF.h>
+#include <fsfw/datapoollocal/localPoolDefinitions.h>
+#include <fsfw/devicehandlers/DeviceHandlerIF.h>
+#include <fsfw/ipc/MutexFactory.h>
+#include <fsfw/ipc/MutexGuard.h>
+#include <fsfw/ipc/messageQueueDefinitions.h>
+#include <fsfw/modes/ModeMessage.h>
+#include <fsfw/objectmanager/SystemObjectIF.h>
+#include <fsfw/power/definitions.h>
+#include <fsfw/returnvalues/returnvalue.h>
+#include <fsfw/serialize/SerializeAdapter.h>
+#include <fsfw/serialize/SerializeIF.h>
+#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
+#include <fsfw/tasks/TaskFactory.h>
+#include <fsfw/timemanager/Countdown.h>
+#include <fsfw/timemanager/clockDefinitions.h>
+#include <mission/devices/ImtqHandler.h>

 #include <cmath>
+#include <fsfw/datapoollocal/LocalPoolVariable.tpp>

-#include "OBSWConfig.h"
+#include "mission/devices/torquer.h"

-IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
+static constexpr bool ACTUATION_WIRETAPPING = false;
+
+ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
                         power::Switch_t pwrSwitcher)
    : DeviceHandlerBase(objectId, comIF, comCookie),
      engHkDataset(this),
      calMtmMeasurementSet(this),
      rawMtmMeasurementSet(this),
+      dipoleSet(*this),
      posXselfTestDataset(this),
      negXselfTestDataset(this),
      posYselfTestDataset(this),
@ -25,9 +48,9 @@ IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC
  }
 }

-IMTQHandler::~IMTQHandler() {}
+ImtqHandler::~ImtqHandler() = default;

-void IMTQHandler::doStartUp() {
+void ImtqHandler::doStartUp() {
  if (goToNormalMode) {
    setMode(MODE_NORMAL);
  } else {
@ -35,9 +58,11 @@ void IMTQHandler::doStartUp() {
  }
 }

-void IMTQHandler::doShutDown() { setMode(_MODE_POWER_DOWN); }
+void ImtqHandler::doShutDown() { setMode(_MODE_POWER_DOWN); }

-ReturnValue_t IMTQHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
+ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
+  bool buildCommand = true;
+  // Depending on the normal polling mode configuration, 3-4 communication steps are recommended
  switch (communicationStep) {
    case CommunicationStep::GET_ENG_HK_DATA:
      *id = IMTQ::GET_ENG_HK_DATA;
@ -45,29 +70,61 @@ ReturnValue_t IMTQHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
      break;
    case CommunicationStep::START_MTM_MEASUREMENT:
      *id = IMTQ::START_MTM_MEASUREMENT;
-      communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT;
-      break;
-    case CommunicationStep::GET_CAL_MTM_MEASUREMENT:
-      *id = IMTQ::GET_CAL_MTM_MEASUREMENT;
-      communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT;
+      if (pollingMode == NormalPollingMode::BOTH or
+          pollingMode == NormalPollingMode::UNCALIBRATED) {
+        communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT;
+      } else {
+        communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT;
+      }
      break;
    case CommunicationStep::GET_RAW_MTM_MEASUREMENT:
+      if (integrationTimeCd.getRemainingMillis() > 0) {
+        TaskFactory::delayTask(integrationTimeCd.getRemainingMillis());
+      }
      *id = IMTQ::GET_RAW_MTM_MEASUREMENT;
+      if (pollingMode == NormalPollingMode::BOTH) {
+        communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT;
+      } else {
+        communicationStep = CommunicationStep::DIPOLE_ACTUATION;
+      }
+      break;
+    case CommunicationStep::GET_CAL_MTM_MEASUREMENT:
+      if (integrationTimeCd.getRemainingMillis() > 0) {
+        TaskFactory::delayTask(integrationTimeCd.getRemainingMillis());
+      }
+      *id = IMTQ::GET_CAL_MTM_MEASUREMENT;
+      communicationStep = CommunicationStep::DIPOLE_ACTUATION;
+      break;
+    case CommunicationStep::DIPOLE_ACTUATION: {
+      // If the dipole is not commanded but set by the ACS control algorithm,
+      // the dipoles will be set by the ACS controller directly using the dipole local pool set.
+      // This set has a flag to determine whether the ACS controller actually set any new input.
+      MutexGuard mg(torquer::lazyLock());
+      if (torquer::NEW_ACTUATION_FLAG) {
+        *id = IMTQ::START_ACTUATION_DIPOLE;
+        torquer::NEW_ACTUATION_FLAG = false;
+      } else {
+        buildCommand = false;
+      }
      communicationStep = CommunicationStep::GET_ENG_HK_DATA;
      break;
+    }
    default:
      sif::debug << "IMTQHandler::buildNormalDeviceCommand: Invalid communication step"
                 << std::endl;
      break;
  }
-  return buildCommandFromCommand(*id, NULL, 0);
-}
-
-ReturnValue_t IMTQHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
+  if (buildCommand) {
+    return buildCommandFromCommand(*id, nullptr, 0);
+  }
  return NOTHING_TO_SEND;
 }

-ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
+ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
+  return NOTHING_TO_SEND;
+}
+
+ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                   const uint8_t* commandData,
                                                   size_t commandDataLen) {
  switch (deviceCommand) {
@ -122,20 +179,36 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
    case (IMTQ::START_ACTUATION_DIPOLE): {
      /* IMTQ expects low byte first */
      commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
-      if (commandData == nullptr) {
+      if (commandData != nullptr && commandDataLen < 8) {
        return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
      }
-      commandBuffer[1] = commandData[1];
-      commandBuffer[2] = commandData[0];
-      commandBuffer[3] = commandData[3];
-      commandBuffer[4] = commandData[2];
-      commandBuffer[5] = commandData[5];
-      commandBuffer[6] = commandData[4];
-      commandBuffer[7] = commandData[7];
-      commandBuffer[8] = commandData[6];
-      rawPacket = commandBuffer;
-      rawPacketLen = 9;
-      return returnvalue::OK;
+      ReturnValue_t result;
+      // Commands override anything which was set in the software
+      if (commandData != nullptr) {
+        dipoleSet.setValidityBufferGeneration(false);
+        result =
+            dipoleSet.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::NETWORK);
+        dipoleSet.setValidityBufferGeneration(true);
+        if (result != returnvalue::OK) {
+          return result;
+        }
+      } else {
+        // Read set dipole values from local pool
+        PoolReadGuard pg(&dipoleSet);
+      }
+      if (ACTUATION_WIRETAPPING) {
+        sif::debug << "Actuating IMTQ with parameters x = " << dipoleSet.xDipole.value
+                   << ", y = " << dipoleSet.yDipole.value << ", z = " << dipoleSet.zDipole.value
+                   << ", duration = " << dipoleSet.currentTorqueDurationMs.value << std::endl;
+      }
+      result = buildDipoleActuationCommand();
+      if (result != returnvalue::OK) {
+        return result;
+      }
+      MutexGuard mg(torquer::lazyLock());
+      torquer::TORQUEING = true;
+      torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
+      return result;
    }
    case (IMTQ::GET_ENG_HK_DATA): {
      commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA;
@ -151,6 +224,7 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
    }
    case (IMTQ::START_MTM_MEASUREMENT): {
      commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT;
+      integrationTimeCd.resetTimer();
      rawPacket = commandBuffer;
      rawPacketLen = 1;
      return returnvalue::OK;
@ -173,30 +247,42 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
  return returnvalue::FAILED;
 }

-void IMTQHandler::fillCommandAndReplyMap() {
-  this->insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr,
-                                   IMTQ::SIZE_SELF_TEST_RESULTS);
-  this->insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr,
-                                   IMTQ::SIZE_STATUS_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset,
-                                   IMTQ::SIZE_ENG_HK_DATA_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr,
-                                   IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr,
-                                   IMTQ::SIZE_STATUS_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet,
-                                   IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT);
-  this->insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet,
-                                   IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT);
+ReturnValue_t ImtqHandler::buildDipoleActuationCommand() {
+  commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
+  uint8_t* serPtr = commandBuffer + 1;
+  size_t serSize = 1;
+  dipoleSet.setValidityBufferGeneration(false);
+  ReturnValue_t result = dipoleSet.serialize(&serPtr, &serSize, sizeof(commandBuffer),
+                                             SerializeIF::Endianness::LITTLE);
+  dipoleSet.setValidityBufferGeneration(true);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  rawPacket = commandBuffer;
+  rawPacketLen = 9;
+  return result;
 }

-ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSize,
+void ImtqHandler::fillCommandAndReplyMap() {
+  insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr, IMTQ::SIZE_SELF_TEST_RESULTS);
+  insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset, IMTQ::SIZE_ENG_HK_DATA_REPLY);
+  insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr,
+                             IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY);
+  insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet,
+                             IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT);
+  insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet,
+                             IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT);
+}
+
+ReturnValue_t ImtqHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                        DeviceCommandId_t* foundId, size_t* foundLen) {
  ReturnValue_t result = returnvalue::OK;

@ -233,8 +319,15 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSi
      *foundLen = IMTQ::SIZE_SELF_TEST_RESULTS;
      *foundId = IMTQ::GET_SELF_TEST_RESULT;
      break;
+    case (IMTQ::CC::PAST_AVAILABLE_RESPONSE_BYTES): {
+      sif::warning << "IMTQHandler::scanForReply: Read 0xFF command byte, reading past available "
+                      "bytes. Keep 1 ms delay between I2C send and read" << std::endl;
+      result = IGNORE_REPLY_DATA;
+      break;
+    }
    default:
-      sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl;
+      sif::debug << "IMTQHandler::scanForReply: Reply with length " << remainingSize
+                 << "contains invalid command code " << static_cast<int>(*start) << std::endl;
      result = IGNORE_REPLY_DATA;
      break;
  }
@ -242,7 +335,7 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSi
  return result;
 }

-ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
+ReturnValue_t ImtqHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
  ReturnValue_t result = returnvalue::OK;

  result = parseStatusByte(packet);
@ -286,9 +379,9 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
  return returnvalue::OK;
 }

-void IMTQHandler::setNormalDatapoolEntriesInvalid() {}
+void ImtqHandler::setNormalDatapoolEntriesInvalid() {}

-LocalPoolDataSetBase* IMTQHandler::getDataSetHandle(sid_t sid) {
+LocalPoolDataSetBase* ImtqHandler::getDataSetHandle(sid_t sid) {
  if (sid == engHkDataset.getSid()) {
    return &engHkDataset;
  } else if (sid == calMtmMeasurementSet.getSid()) {
@ -313,9 +406,9 @@ LocalPoolDataSetBase* IMTQHandler::getDataSetHandle(sid_t sid) {
  }
 }

-uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; }
+uint32_t ImtqHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; }

-ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
+ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                   LocalDataPoolManager& poolManager) {
  /** Entries of engineering housekeeping dataset */
  localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>({0}));
@ -330,6 +423,11 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat
  localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));

+  localDataPoolMap.emplace(IMTQ::DIPOLES_X, &dipoleXEntry);
+  localDataPoolMap.emplace(IMTQ::DIPOLES_Y, &dipoleYEntry);
+  localDataPoolMap.emplace(IMTQ::DIPOLES_Z, &dipoleZEntry);
+  localDataPoolMap.emplace(IMTQ::CURRENT_TORQUE_DURATION, &torqueDurationEntry);
+
  /** Entries of calibrated MTM measurement dataset */
  localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, &mgmCalEntry);
  localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
@ -611,7 +709,7 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat
  return returnvalue::OK;
 }

-ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
+ReturnValue_t ImtqHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
  DeviceCommandId_t commandId = getPendingCommand();
  switch (commandId) {
    case IMTQ::POS_X_SELF_TEST:
@ -630,7 +728,7 @@ ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
  return returnvalue::OK;
 }

-ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) {
+ReturnValue_t ImtqHandler::parseStatusByte(const uint8_t* packet) {
  uint8_t cmdErrorField = *(packet + 1) & 0xF;
  switch (cmdErrorField) {
    case 0:
@ -661,7 +759,7 @@ ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) {
  }
 }

-void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
+void ImtqHandler::fillEngHkDataset(const uint8_t* packet) {
  PoolReadGuard rg(&engHkDataset);
  uint8_t offset = 2;
  engHkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
@ -687,7 +785,9 @@ void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
  offset += 2;
  engHkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
  offset += 2;
-  engHkDataset.mcuTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
+  size_t dummy = 2;
+  SerializeAdapter::deSerialize(&engHkDataset.mcuTemperature.value, packet + offset, &dummy,
+                                SerializeIF::Endianness::LITTLE);

  engHkDataset.setValidity(true, true);

@ -708,9 +808,9 @@ void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
  }
 }

-void IMTQHandler::setToGoToNormal(bool enable) { this->goToNormalMode = enable; }
+void ImtqHandler::setToGoToNormal(bool enable) { this->goToNormalMode = enable; }

-void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) {
+void ImtqHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) {
  if (wiretappingMode == RAW) {
    /* Data already sent in doGetRead() */
    return;
@ -734,7 +834,7 @@ void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCom
  }
 }

-void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
+void ImtqHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
  uint8_t tmData[6];
  /* Switching endianess of received dipole values */
  tmData[0] = *(packet + 3);
@ -746,7 +846,7 @@ void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
  handleDeviceTM(tmData, sizeof(tmData), IMTQ::GET_COMMANDED_DIPOLE);
 }

-void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
+void ImtqHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
  PoolReadGuard rg(&calMtmMeasurementSet);
  calMtmMeasurementSet.setValidity(true, true);
  int8_t offset = 2;
@ -776,7 +876,7 @@ void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
  }
 }

-void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) {
+void ImtqHandler::fillRawMtmDataset(const uint8_t* packet) {
  PoolReadGuard rg(&rawMtmMeasurementSet);
  unsigned int offset = 2;
  size_t deSerLen = 16;
@ -824,7 +924,7 @@ void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) {
  }
 }

-void IMTQHandler::handleSelfTestReply(const uint8_t* packet) {
+void ImtqHandler::handleSelfTestReply(const uint8_t* packet) {
  uint16_t offset = 2;
  checkErrorByte(*(packet + offset), *(packet + offset + 1));

@ -858,7 +958,7 @@ void IMTQHandler::handleSelfTestReply(const uint8_t* packet) {
  }
 }

-void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) {
+void ImtqHandler::handlePositiveXSelfTestReply(const uint8_t* packet) {
  PoolReadGuard rg(&posXselfTestDataset);

  uint16_t offset = 2;
@ -1070,7 +1170,7 @@ void IMTQHandler::handlePositiveXSelfTestReply(const uint8_t* packet) {
  }
 }

-void IMTQHandler::handleNegativeXSelfTestReply(const uint8_t* packet) {
+void ImtqHandler::handleNegativeXSelfTestReply(const uint8_t* packet) {
  PoolReadGuard rg(&posXselfTestDataset);

  uint16_t offset = 2;
@ -1282,7 +1382,7 @@ void IMTQHandler::handleNegativeXSelfTestReply(const uint8_t* packet) {
  }
 }

-void IMTQHandler::handlePositiveYSelfTestReply(const uint8_t* packet) {
+void ImtqHandler::handlePositiveYSelfTestReply(const uint8_t* packet) {
  PoolReadGuard rg(&posXselfTestDataset);

  uint16_t offset = 2;
@ -1494,7 +1594,7 @@ void IMTQHandler::handlePositiveYSelfTestReply(const uint8_t* packet) {
  }
 }

-void IMTQHandler::handleNegativeYSelfTestReply(const uint8_t* packet) {
+void ImtqHandler::handleNegativeYSelfTestReply(const uint8_t* packet) {
  PoolReadGuard rg(&posXselfTestDataset);

  uint16_t offset = 2;
@ -1706,7 +1806,7 @@ void IMTQHandler::handleNegativeYSelfTestReply(const uint8_t* packet) {
  }
 }

-void IMTQHandler::handlePositiveZSelfTestReply(const uint8_t* packet) {
+void ImtqHandler::handlePositiveZSelfTestReply(const uint8_t* packet) {
  PoolReadGuard rg(&posXselfTestDataset);

  uint16_t offset = 2;
@ -1918,7 +2018,7 @@ void IMTQHandler::handlePositiveZSelfTestReply(const uint8_t* packet) {
  }
 }

-void IMTQHandler::handleNegativeZSelfTestReply(const uint8_t* packet) {
+void ImtqHandler::handleNegativeZSelfTestReply(const uint8_t* packet) {
  PoolReadGuard rg(&posXselfTestDataset);

  uint16_t offset = 2;
@ -2130,9 +2230,9 @@ void IMTQHandler::handleNegativeZSelfTestReply(const uint8_t* packet) {
  }
 }

-void IMTQHandler::setDebugMode(bool enable) { this->debugMode = enable; }
+void ImtqHandler::setDebugMode(bool enable) { this->debugMode = enable; }

-void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) {
+void ImtqHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) {
  std::string stepString("");
  if (step < 8) {
    stepString = makeStepString(step);
@ -2190,7 +2290,12 @@ void IMTQHandler::checkErrorByte(const uint8_t errorByte, const uint8_t step) {
  }
 }

-std::string IMTQHandler::makeStepString(const uint8_t step) {
+void ImtqHandler::doSendRead() {
+  TaskFactory::delayTask(1);
+  DeviceHandlerBase::doSendRead();
+}
+
+std::string ImtqHandler::makeStepString(const uint8_t step) {
  std::string stepString("");
  switch (step) {
    case IMTQ::SELF_TEST_STEPS::INIT:
@ -2225,7 +2330,7 @@ std::string IMTQHandler::makeStepString(const uint8_t step) {
  return stepString;
 }

-ReturnValue_t IMTQHandler::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) {
+ReturnValue_t ImtqHandler::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) {
  if (switcher != power::NO_SWITCH) {
    *numberOfSwitches = 1;
    *switches = &switcher;
--- 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/imtqHandlerDefinitions.h>
 #include <string.h>

 #include "events/subsystemIdRanges.h"
@ -13,12 +13,17 @@
 *
 * @author	J. Meier
 */
-class IMTQHandler : public DeviceHandlerBase {
+class ImtqHandler : public DeviceHandlerBase {
 public:
-  IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
-              power::Switch_t pwrSwitcher);
-  virtual ~IMTQHandler();
+  enum NormalPollingMode { UNCALIBRATED = 0, CALIBRATED = 1, BOTH = 2 };

+  ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
+              power::Switch_t pwrSwitcher);
+  virtual ~ImtqHandler();
+
+  void setPollingMode(NormalPollingMode pollMode);
+
+  void doSendRead() override;
  /**
   * @brief   Sets mode to MODE_NORMAL. Can be used for debugging.
   */
@ -95,6 +100,7 @@ class IMTQHandler : public DeviceHandlerBase {
  IMTQ::EngHkDataset engHkDataset;
  IMTQ::CalibratedMtmMeasurementSet calMtmMeasurementSet;
  IMTQ::RawMtmMeasurementSet rawMtmMeasurementSet;
+  IMTQ::DipoleActuationSet dipoleSet;
  IMTQ::PosXSelfTestSet posXselfTestDataset;
  IMTQ::NegXSelfTestSet negXselfTestDataset;
  IMTQ::PosYSelfTestSet posYselfTestDataset;
@ -102,7 +108,16 @@ class IMTQHandler : public DeviceHandlerBase {
  IMTQ::PosZSelfTestSet posZselfTestDataset;
  IMTQ::NegZSelfTestSet negZselfTestDataset;

+  NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED;
+
  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;

@ -114,7 +129,8 @@ class IMTQHandler : public DeviceHandlerBase {
    GET_ENG_HK_DATA,
    START_MTM_MEASUREMENT,
    GET_CAL_MTM_MEASUREMENT,
-    GET_RAW_MTM_MEASUREMENT
+    GET_RAW_MTM_MEASUREMENT,
+    DIPOLE_ACTUATION
  };

  CommunicationStep communicationStep = CommunicationStep::GET_ENG_HK_DATA;
@ -196,6 +212,7 @@ class IMTQHandler : public DeviceHandlerBase {
  void handlePositiveZSelfTestReply(const uint8_t* packet);
  void handleNegativeZSelfTestReply(const uint8_t* packet);

+  ReturnValue_t buildDipoleActuationCommand();
  /**
   * @brief   This function checks the error byte of a self test measurement.
   *
--- a/mission/devices/devicedefinitions/imtqHandlerDefinitions.h
+++ b/mission/devices/devicedefinitions/imtqHandlerDefinitions.h
@ -1,8 +1,11 @@
 #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_
 #define MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_

+#include <fsfw/datapool/PoolReadGuard.h>
 #include <fsfw/datapoollocal/StaticLocalDataSet.h>

+class ImtqHandler;
+
 namespace IMTQ {

 static const DeviceCommandId_t NONE = 0x0;
@ -27,15 +30,18 @@ static const uint8_t GET_TEMP_REPLY_SIZE = 2;
 static const uint8_t CFGR_CMD_SIZE = 3;
 static const uint8_t POINTER_REG_SIZE = 1;

-static const uint32_t ENG_HK_DATA_SET_ID = 1;
-static const uint32_t CAL_MTM_SET = 2;
-static const uint32_t RAW_MTM_SET = 3;
-static const uint32_t POS_X_TEST_DATASET = 4;
-static const uint32_t NEG_X_TEST_DATASET = 5;
-static const uint32_t POS_Y_TEST_DATASET = 6;
-static const uint32_t NEG_Y_TEST_DATASET = 7;
-static const uint32_t POS_Z_TEST_DATASET = 8;
-static const uint32_t NEG_Z_TEST_DATASET = 9;
+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
+};

 static const uint8_t SIZE_ENG_HK_COMMAND = 1;
 static const uint8_t SIZE_STATUS_REPLY = 2;
@ -80,6 +86,7 @@ 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

 namespace SELF_TEST_AXIS {
@ -103,7 +110,7 @@ static const uint8_t Z_NEGATIVE = 0x6;
 static const uint8_t FINA = 0x7;
 }  // namespace SELF_TEST_STEPS

-enum IMTQPoolIds : lp_id_t {
+enum PoolIds : lp_id_t {
  DIGITAL_VOLTAGE_MV,
  ANALOG_VOLTAGE_MV,
  DIGITAL_CURRENT,
@ -119,6 +126,10 @@ enum IMTQPoolIds : lp_id_t {
  ACTUATION_CAL_STATUS,
  MTM_RAW,
  ACTUATION_RAW_STATUS,
+  DIPOLES_X,
+  DIPOLES_Y,
+  DIPOLES_Z,
+  CURRENT_TORQUE_DURATION,

  INIT_POS_X_ERR,
  INIT_POS_X_RAW_MAG_X,
@ -375,9 +386,9 @@ enum IMTQPoolIds : lp_id_t {

 class EngHkDataset : public StaticLocalDataSet<ENG_HK_SET_POOL_ENTRIES> {
 public:
-  EngHkDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, ENG_HK_DATA_SET_ID) {}
+  EngHkDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, IMTQ::SetIds::ENG_HK) {}

-  EngHkDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, ENG_HK_DATA_SET_ID)) {}
+  EngHkDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::ENG_HK)) {}

  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);
@ -398,13 +409,14 @@ class EngHkDataset : public StaticLocalDataSet<ENG_HK_SET_POOL_ENTRIES> {
 */
 class CalibratedMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
 public:
-  CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CAL_MTM_SET) {}
+  CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner)
+      : StaticLocalDataSet(owner, IMTQ::SetIds::CAL_MGM) {}

  CalibratedMtmMeasurementSet(object_id_t objectId)
-      : StaticLocalDataSet(sid_t(objectId, CAL_MTM_SET)) {}
+      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::CAL_MGM)) {}

  /** 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);
+  lp_vec_t<int32_t, 3> mgmXyz = lp_vec_t<int32_t, 3>(sid.objectId, MGM_CAL_NT, this);
  /** 1 if coils were actuating during measurement otherwise 0 */
  lp_var_t<uint8_t> coilActuationStatus =
      lp_var_t<uint8_t>(sid.objectId, ACTUATION_CAL_STATUS, this);
@ -415,9 +427,11 @@ class CalibratedMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRI
 */
 class RawMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
 public:
-  RawMtmMeasurementSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, RAW_MTM_SET) {}
+  RawMtmMeasurementSet(HasLocalDataPoolIF* owner)
+      : StaticLocalDataSet(owner, IMTQ::SetIds::RAW_MGM) {}

-  RawMtmMeasurementSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, RAW_MTM_SET)) {}
+  RawMtmMeasurementSet(object_id_t objectId)
+      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::RAW_MGM)) {}

  /** The unit of all measurements is nT */
  lp_vec_t<float, 3> mtmRawNt = lp_vec_t<float, 3>(sid.objectId, MTM_RAW, this);
@ -437,6 +451,11 @@ class CommandDipolePacket : public SerialLinkedListAdapter<SerializeIF> {
 public:
  CommandDipolePacket() { setLinks(); }

+  SerializeElement<uint16_t> xDipole;
+  SerializeElement<uint16_t> yDipole;
+  SerializeElement<uint16_t> zDipole;
+  SerializeElement<uint16_t> duration;
+
 private:
  /**
   * @brief   Constructor
@ -456,10 +475,46 @@ class CommandDipolePacket : public SerialLinkedListAdapter<SerializeIF> {
    yDipole.setNext(&zDipole);
    zDipole.setNext(&duration);
  }
-  SerializeElement<uint16_t> xDipole;
-  SerializeElement<uint16_t> yDipole;
-  SerializeElement<uint16_t> zDipole;
-  SerializeElement<uint16_t> duration;
+};
+
+class DipoleActuationSet : public StaticLocalDataSet<4> {
+  friend class ::ImtqHandler;
+
+ public:
+  DipoleActuationSet(HasLocalDataPoolIF& owner)
+      : StaticLocalDataSet(&owner, IMTQ::SetIds::DIPOLES) {}
+  DipoleActuationSet(object_id_t objectId)
+      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::DIPOLES)) {}
+
+  // Refresh torque command without changing any of the set dipoles.
+  void refreshTorqueing(uint16_t durationMs_) { currentTorqueDurationMs = durationMs_; }
+
+  void setDipoles(uint16_t xDipole_, uint16_t yDipole_, uint16_t zDipole_,
+                  uint16_t currentTorqueDurationMs_) {
+    if (xDipole.value != xDipole_) {
+    }
+    xDipole = xDipole_;
+    if (yDipole.value != yDipole_) {
+    }
+    yDipole = yDipole_;
+    if (zDipole.value != zDipole_) {
+    }
+    zDipole = zDipole_;
+    currentTorqueDurationMs = currentTorqueDurationMs_;
+  }
+
+  void getDipoles(uint16_t& xDipole_, uint16_t& yDipole_, uint16_t& zDipole_) {
+    xDipole_ = xDipole.value;
+    yDipole_ = yDipole.value;
+    zDipole_ = zDipole.value;
+  }
+
+ private:
+  lp_var_t<int16_t> xDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_X, this);
+  lp_var_t<int16_t> yDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_Y, this);
+  lp_var_t<int16_t> zDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_Z, this);
+  lp_var_t<uint16_t> currentTorqueDurationMs =
+      lp_var_t<uint16_t>(sid.objectId, CURRENT_TORQUE_DURATION, this);
 };

 /**
@ -479,10 +534,10 @@ class CommandDipolePacket : public SerialLinkedListAdapter<SerializeIF> {
 class PosXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 public:
  PosXSelfTestSet(HasLocalDataPoolIF* owner)
-      : StaticLocalDataSet(owner, IMTQ::POS_X_TEST_DATASET) {}
+      : StaticLocalDataSet(owner, IMTQ::SetIds::POS_X_TEST) {}

  PosXSelfTestSet(object_id_t objectId)
-      : StaticLocalDataSet(sid_t(objectId, IMTQ::POS_X_TEST_DATASET)) {}
+      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_X_TEST)) {}

  /** INIT block */
  lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_POS_X_ERR, this);
@ -556,10 +611,10 @@ class PosXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 class NegXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 public:
  NegXSelfTestSet(HasLocalDataPoolIF* owner)
-      : StaticLocalDataSet(owner, IMTQ::NEG_X_TEST_DATASET) {}
+      : StaticLocalDataSet(owner, IMTQ::SetIds::NEG_X_TEST) {}

  NegXSelfTestSet(object_id_t objectId)
-      : StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_X_TEST_DATASET)) {}
+      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_X_TEST)) {}

  /** INIT block */
  lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_NEG_X_ERR, this);
@ -633,10 +688,10 @@ class NegXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 class PosYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 public:
  PosYSelfTestSet(HasLocalDataPoolIF* owner)
-      : StaticLocalDataSet(owner, IMTQ::POS_Y_TEST_DATASET) {}
+      : StaticLocalDataSet(owner, IMTQ::SetIds::POS_Y_TEST) {}

  PosYSelfTestSet(object_id_t objectId)
-      : StaticLocalDataSet(sid_t(objectId, IMTQ::POS_Y_TEST_DATASET)) {}
+      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_Y_TEST)) {}

  /** INIT block */
  lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_POS_Y_ERR, this);
@ -710,10 +765,10 @@ class PosYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 class NegYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 public:
  NegYSelfTestSet(HasLocalDataPoolIF* owner)
-      : StaticLocalDataSet(owner, IMTQ::NEG_Y_TEST_DATASET) {}
+      : StaticLocalDataSet(owner, IMTQ::SetIds::NEG_Y_TEST) {}

  NegYSelfTestSet(object_id_t objectId)
-      : StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_Y_TEST_DATASET)) {}
+      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_Y_TEST)) {}

  /** INIT block */
  lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_NEG_Y_ERR, this);
@ -787,10 +842,10 @@ class NegYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 class PosZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 public:
  PosZSelfTestSet(HasLocalDataPoolIF* owner)
-      : StaticLocalDataSet(owner, IMTQ::POS_Z_TEST_DATASET) {}
+      : StaticLocalDataSet(owner, IMTQ::SetIds::POS_Z_TEST) {}

  PosZSelfTestSet(object_id_t objectId)
-      : StaticLocalDataSet(sid_t(objectId, IMTQ::POS_Z_TEST_DATASET)) {}
+      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_Z_TEST)) {}

  /** INIT block */
  lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_POS_Z_ERR, this);
@ -864,10 +919,10 @@ class PosZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 class NegZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 public:
  NegZSelfTestSet(HasLocalDataPoolIF* owner)
-      : StaticLocalDataSet(owner, IMTQ::NEG_Z_TEST_DATASET) {}
+      : StaticLocalDataSet(owner, IMTQ::SetIds::NEG_Z_TEST) {}

  NegZSelfTestSet(object_id_t objectId)
-      : StaticLocalDataSet(sid_t(objectId, IMTQ::NEG_Z_TEST_DATASET)) {}
+      : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_Z_TEST)) {}

  /** INIT block */
  lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_NEG_Z_ERR, this);
--- a/mission/devices/torquer.cpp
+++ b/mission/devices/torquer.cpp
@ -0,0 +1,27 @@
+#include "torquer.h"
+
+#include <fsfw/ipc/MutexGuard.h>
+
+MutexIF* TORQUE_LOCK = nullptr;
+
+namespace torquer {
+
+bool TORQUEING = false;
+bool NEW_ACTUATION_FLAG = false;
+Countdown TORQUE_COUNTDOWN = Countdown();
+
+bool mgtIsTorqueing(dur_millis_t* remainingTorqueDuration) {
+  if (TORQUEING and remainingTorqueDuration != nullptr) {
+    *remainingTorqueDuration = TORQUE_COUNTDOWN.getRemainingMillis() + TORQUE_BUFFER_TIME_MS;
+  }
+  return TORQUEING;
+}
+
+MutexIF* lazyLock() {
+  if (TORQUE_LOCK == nullptr) {
+    TORQUE_LOCK = MutexFactory::instance()->createMutex();
+  }
+  return TORQUE_LOCK;
+}
+
+}  // namespace torquer
--- a/mission/devices/torquer.h
+++ b/mission/devices/torquer.h
@ -0,0 +1,22 @@
+#ifndef MISSION_DEVICES_TOQUER_H_
+#define MISSION_DEVICES_TOQUER_H_
+
+#include <fsfw/ipc/MutexIF.h>
+#include <fsfw/timemanager/Countdown.h>
+
+namespace torquer {
+
+// Additional buffer time to accont for time until I2C command arrives and ramp up / ramp down
+// time of the MGT
+static constexpr dur_millis_t TORQUE_BUFFER_TIME_MS = 20;
+
+MutexIF* lazyLock();
+extern bool TORQUEING;
+extern bool NEW_ACTUATION_FLAG;
+extern Countdown TORQUE_COUNTDOWN;
+
+bool mgtIsTorqueing(dur_millis_t* remainingTorqueDuration);
+
+}  // namespace torquer
+
+#endif /* MISSION_DEVICES_TOQUER_H_ */