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

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

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

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.2, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.6, DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.2, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::IMTQ_HANDLER, length * 0.8, DeviceHandlerIF::GET_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);

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) {

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

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>
 

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)

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;
+      if (pollingMode == NormalPollingMode::BOTH or
-      break;
+          pollingMode == NormalPollingMode::UNCALIBRATED) {
-    case CommunicationStep::GET_CAL_MTM_MEASUREMENT:
+        communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT;
-      *id = IMTQ::GET_CAL_MTM_MEASUREMENT;
+      } else {
-      communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT;
+        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);
+  if (buildCommand) {
-}
+    return buildCommandFromCommand(*id, nullptr, 0);
-
+  }
-ReturnValue_t IMTQHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
   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];
+      ReturnValue_t result;
-      commandBuffer[2] = commandData[0];
+      // Commands override anything which was set in the software
-      commandBuffer[3] = commandData[3];
+      if (commandData != nullptr) {
-      commandBuffer[4] = commandData[2];
+        dipoleSet.setValidityBufferGeneration(false);
-      commandBuffer[5] = commandData[5];
+        result =
-      commandBuffer[6] = commandData[4];
+            dipoleSet.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::NETWORK);
-      commandBuffer[7] = commandData[7];
+        dipoleSet.setValidityBufferGeneration(true);
-      commandBuffer[8] = commandData[6];
+        if (result != returnvalue::OK) {
-      rawPacket = commandBuffer;
+          return result;
-      rawPacketLen = 9;
+        }
-      return returnvalue::OK;
+      } 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() {
+ReturnValue_t ImtqHandler::buildDipoleActuationCommand() {
-  this->insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
-  this->insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  uint8_t* serPtr = commandBuffer + 1;
-  this->insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  size_t serSize = 1;
-  this->insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  dipoleSet.setValidityBufferGeneration(false);
-  this->insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  ReturnValue_t result = dipoleSet.serialize(&serPtr, &serSize, sizeof(commandBuffer),
-  this->insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+                                             SerializeIF::Endianness::LITTLE);
-  this->insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr,
+  dipoleSet.setValidityBufferGeneration(true);
-                                   IMTQ::SIZE_SELF_TEST_RESULTS);
+  if (result != returnvalue::OK) {
-  this->insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr,
+    return result;
-                                   IMTQ::SIZE_STATUS_REPLY);
+  }
-  this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset,
+  rawPacket = commandBuffer;
-                                   IMTQ::SIZE_ENG_HK_DATA_REPLY);
+  rawPacketLen = 9;
-  this->insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr,
+  return result;
-                                   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::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;

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,
+  enum NormalPollingMode { UNCALIBRATED = 0, CALIBRATED = 1, BOTH = 2 };
-              power::Switch_t pwrSwitcher);
-  virtual ~IMTQHandler();
 
+  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.
    *

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;
+enum SetIds : uint32_t {
-static const uint32_t CAL_MTM_SET = 2;
+  ENG_HK = 1,
-static const uint32_t RAW_MTM_SET = 3;
+  CAL_MGM = 2,
-static const uint32_t POS_X_TEST_DATASET = 4;
+  RAW_MGM = 3,
-static const uint32_t NEG_X_TEST_DATASET = 5;
+  POS_X_TEST = 4,
-static const uint32_t POS_Y_TEST_DATASET = 6;
+  NEG_X_TEST = 5,
-static const uint32_t NEG_Y_TEST_DATASET = 7;
+  POS_Y_TEST = 6,
-static const uint32_t POS_Z_TEST_DATASET = 8;
+  NEG_Y_TEST = 7,
-static const uint32_t NEG_Z_TEST_DATASET = 9;
+  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;
+class DipoleActuationSet : public StaticLocalDataSet<4> {
-  SerializeElement<uint16_t> duration;
+  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);

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

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_ */