Merge branch 'develop' into eggert/acs-dataset-stuff

CHANGELOG.md

@@ -17,6 +17,27 @@ change warranting a new major release:
 
 # [unreleased]
 
+# [v1.29.0]
+
+eive-tmtc: v2.13.0
+
+## Changed
+
+- 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).
+
+## Added
+
+- Added new heater info set for the TCS controller. This set contains the heater switch states
+  and the current draw.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/351
+- The HeaterHandler now exposes a mode which reflects whether the heater power
+  is on or off. It also triggers mode events for its heater children objects
+  which show whether the specific heaters are on or off. The heater handler
+  will be part of the TCS tree.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/351
+
 # [v1.28.1]
 
 ## Fixed
@@ -24,10 +45,18 @@ change warranting a new major release:
 - Patch version which compiles for EM
 - CFDP Funnel bugfix: CCSDS wrapping was buggy and works properly now.
 - CMakeLists.txt fix which broke CI/CD builds when server could not retrieve full git SHA.
+- Possible regression in the MAX31865 polling task: Using a `ManualCsLockGuard` for reconfiguring
+  and then polling the sensor is problematic, invalid sensor values will be read.
+  CS probably needs to be de-asserted or some other HW/SPI specific issue. Letting the SPI ComIF
+  do the locking does the job.
 
 ## Changed
 
 - Add `-Wshadow=local` shadowing warnings and fixed all of them
+- 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
 
@@ -158,6 +187,7 @@ eive-tmtc: v2.12.2
 ## Changed
 
 - Reworked dummy handling for the TCS controller.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/325
 - Generator scripts now generate files for hosted and for Q7S build.
 
 ## Fixed

CMakeLists.txt

@@ -10,8 +10,8 @@
 cmake_minimum_required(VERSION 3.13)
 
 set(OBSW_VERSION_MAJOR 1)
-set(OBSW_VERSION_MINOR 28)
-set(OBSW_VERSION_REVISION 1)
+set(OBSW_VERSION_MINOR 29)
+set(OBSW_VERSION_REVISION 0)
 
 # set(CMAKE_VERBOSE TRUE)
 

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"
 

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:

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

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

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
 

common/config/eive/objects.h

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

dummies/ImtqDummy.cpp

@@ -1,6 +1,6 @@
 #include "ImtqDummy.h"
 
-#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
+#include <mission/devices/devicedefinitions/imtqHelpers.h>
 
 ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
     : DeviceHandlerBase(objectId, comif, comCookie) {}
@@ -38,10 +38,10 @@ uint32_t ImtqDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur
 
 ReturnValue_t ImtqDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                  LocalDataPoolManager &poolManager) {
-  localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(IMTQ::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);
 }

fsfw

@@ -1 +1 @@
-Subproject commit c32798522283d89028b7c1ecd3bd33b8391e1a39
+Subproject commit 2efff4d2c5ef82b5b62567ab1bb0ee53aeed6a5a

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

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

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

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

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"
 

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:

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

linux/ObjectFactory.cpp

@@ -9,7 +9,7 @@
 #include <fsfw_hal/linux/spi/SpiComIF.h>
 #include <fsfw_hal/linux/spi/SpiCookie.h>
 #include <linux/callbacks/gpioCallbacks.h>
-#include <linux/devices/Max31865RtdLowlevelHandler.h>
+#include <linux/devices/Max31865RtdPolling.h>
 #include <mission/controller/AcsController.h>
 #include <mission/core/GenericFactory.h>
 #include <mission/devices/Max31865EiveHandler.h>
@@ -278,7 +278,7 @@ void ObjectFactory::createRtdComponents(std::string spiDev, GpioIF* gpioComIF,
   TcsBoardAssembly* tcsBoardAss = ObjectFactory::createTcsBoardAssy(*pwrSwitcher);
 
   // Create special low level reader communication interface
-  new Max31865RtdReader(objects::SPI_RTD_COM_IF, comIF, gpioComIF);
+  new Max31865RtdPolling(objects::SPI_RTD_COM_IF, comIF, gpioComIF);
   for (uint8_t idx = 0; idx < NUM_RTDS; idx++) {
     rtdCookies[idx] = new SpiCookie(cookieArgs[idx].first, cookieArgs[idx].second,
                                     MAX31865::MAX_REPLY_SIZE, spi::RTD_MODE, spi::RTD_SPEED);

linux/devices/CMakeLists.txt

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

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

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

linux/devices/Max31865RtdPolling.cpp

@@ -1,8 +1,7 @@
-#include "Max31865RtdLowlevelHandler.h"
-
 #include <fsfw/tasks/TaskFactory.h>
 #include <fsfw/timemanager/Stopwatch.h>
 #include <fsfw_hal/linux/spi/ManualCsLockGuard.h>
+#include <linux/devices/Max31865RtdPolling.h>
 
 #define OBSW_RTD_AUTO_MODE 1
 
@@ -17,16 +16,17 @@ static constexpr uint8_t BASE_CFG =
     (MAX31865::ConvMode::NORM_OFF << MAX31865::CfgBitPos::CONV_MODE);
 #endif
 
-Max31865RtdReader::Max31865RtdReader(object_id_t objectId, SpiComIF* lowLevelComIF, GpioIF* gpioIF)
+Max31865RtdPolling::Max31865RtdPolling(object_id_t objectId, SpiComIF* lowLevelComIF,
+                                       GpioIF* gpioIF)
     : SystemObject(objectId), rtds(EiveMax31855::NUM_RTDS), comIF(lowLevelComIF), gpioIF(gpioIF) {
   readerMutex = MutexFactory::instance()->createMutex();
 }
 
-ReturnValue_t Max31865RtdReader::performOperation(uint8_t operationCode) {
+ReturnValue_t Max31865RtdPolling::performOperation(uint8_t operationCode) {
   using namespace MAX31865;
   ReturnValue_t result = returnvalue::OK;
   static_cast<void>(result);
-  // Stopwatch watch;
+  Stopwatch watch;
   if (periodicInitHandling()) {
 #if OBSW_RTD_AUTO_MODE == 0
     // 10 ms delay for VBIAS startup
@@ -49,17 +49,16 @@ ReturnValue_t Max31865RtdReader::performOperation(uint8_t operationCode) {
   return periodicReadHandling();
 }
 
-bool Max31865RtdReader::rtdIsActive(uint8_t idx) {
+bool Max31865RtdPolling::rtdIsActive(uint8_t idx) {
   if (rtds[idx]->on and rtds[idx]->db.active and rtds[idx]->db.configured) {
     return true;
   }
   return false;
 }
 
-bool Max31865RtdReader::periodicInitHandling() {
+bool Max31865RtdPolling::periodicInitHandling() {
   using namespace MAX31865;
   ReturnValue_t result = returnvalue::OK;
-
   for (auto& rtd : rtds) {
     if (rtd == nullptr) {
       continue;
@@ -70,11 +69,9 @@ bool Max31865RtdReader::periodicInitHandling() {
       return false;
     }
     if ((rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut()) {
-      ManualCsLockWrapper mg1(csLock, gpioIF, rtd->spiCookie, csTimeoutType, csTimeoutMs);
-      if (mg1.lockResult != returnvalue::OK or mg1.gpioResult != returnvalue::OK) {
-        sif::error << "Max31865RtdReader::periodicInitHandling: Manual CS lock failed" << std::endl;
-        continue;
-      }
+      // Please note that using the manual CS lock wrapper here is problematic. Might be a SPI
+      // or hardware specific issue where the CS needs to be pulled high and then low again
+      // between transfers
       result = writeCfgReg(rtd->spiCookie, BASE_CFG);
       if (result != returnvalue::OK) {
         handleSpiError(rtd, result, "writeCfgReg");
@@ -115,7 +112,7 @@ bool Max31865RtdReader::periodicInitHandling() {
   return someRtdUsable;
 }
 
-ReturnValue_t Max31865RtdReader::periodicReadReqHandling() {
+ReturnValue_t Max31865RtdPolling::periodicReadReqHandling() {
   using namespace MAX31865;
   // Now request one shot config for all active RTDs
   for (auto& rtd : rtds) {
@@ -139,7 +136,7 @@ ReturnValue_t Max31865RtdReader::periodicReadReqHandling() {
   return returnvalue::OK;
 }
 
-ReturnValue_t Max31865RtdReader::periodicReadHandling() {
+ReturnValue_t Max31865RtdPolling::periodicReadHandling() {
   using namespace MAX31865;
   auto result = returnvalue::OK;
   // Now read the RTD values
@@ -153,11 +150,9 @@ ReturnValue_t Max31865RtdReader::periodicReadHandling() {
       return returnvalue::FAILED;
     }
     if (rtdIsActive(rtd->idx)) {
-      ManualCsLockWrapper mg1(csLock, gpioIF, rtd->spiCookie, csTimeoutType, csTimeoutMs);
-      if (mg1.lockResult != returnvalue::OK or mg1.gpioResult != returnvalue::OK) {
-        sif::error << "Max31865RtdReader::periodicInitHandling: Manual CS lock failed" << std::endl;
-        continue;
-      }
+      // Please note that using the manual CS lock wrapper here is problematic. Might be a SPI
+      // or hardware specific issue where the CS needs to be pulled high and then low again
+      // between transfers
       uint16_t rtdVal = 0;
       bool faultBitSet = false;
       result = writeCfgReg(rtd->spiCookie, BASE_CFG);
@@ -166,6 +161,7 @@ ReturnValue_t Max31865RtdReader::periodicReadHandling() {
         continue;
       }
       result = readRtdVal(rtd->spiCookie, rtdVal, faultBitSet);
+      // sif::debug << "RTD Val: " << rtdVal << std::endl;
       if (result != returnvalue::OK) {
         handleSpiError(rtd, result, "readRtdVal");
         continue;
@@ -191,7 +187,7 @@ ReturnValue_t Max31865RtdReader::periodicReadHandling() {
   return returnvalue::OK;
 }
 
-ReturnValue_t Max31865RtdReader::initializeInterface(CookieIF* cookie) {
+ReturnValue_t Max31865RtdPolling::initializeInterface(CookieIF* cookie) {
   if (cookie == nullptr) {
     throw std::invalid_argument("Invalid MAX31865 Reader Cookie");
   }
@@ -211,8 +207,8 @@ ReturnValue_t Max31865RtdReader::initializeInterface(CookieIF* cookie) {
   return returnvalue::OK;
 }
 
-ReturnValue_t Max31865RtdReader::sendMessage(CookieIF* cookie, const uint8_t* sendData,
-                                             size_t sendLen) {
+ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* sendData,
+                                              size_t sendLen) {
   if (cookie == nullptr) {
     return returnvalue::FAILED;
   }
@@ -308,14 +304,14 @@ ReturnValue_t Max31865RtdReader::sendMessage(CookieIF* cookie, const uint8_t* se
   return returnvalue::OK;
 }
 
-ReturnValue_t Max31865RtdReader::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }
+ReturnValue_t Max31865RtdPolling::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }
 
-ReturnValue_t Max31865RtdReader::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
+ReturnValue_t Max31865RtdPolling::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
   return returnvalue::OK;
 }
 
-ReturnValue_t Max31865RtdReader::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
-                                                     size_t* size) {
+ReturnValue_t Max31865RtdPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
+                                                      size_t* size) {
   MutexGuard mg(readerMutex);
   if (mg.getLockResult() != returnvalue::OK) {
     // TODO: Emit warning
@@ -338,13 +334,13 @@ ReturnValue_t Max31865RtdReader::readReceivedMessage(CookieIF* cookie, uint8_t**
   return returnvalue::OK;
 }
 
-ReturnValue_t Max31865RtdReader::writeCfgReg(SpiCookie* cookie, uint8_t cfg) {
+ReturnValue_t Max31865RtdPolling::writeCfgReg(SpiCookie* cookie, uint8_t cfg) {
   using namespace MAX31865;
   return writeNToReg(cookie, CONFIG, 1, &cfg, nullptr);
 }
 
-ReturnValue_t Max31865RtdReader::writeBiasSel(MAX31865::Bias bias, SpiCookie* cookie,
-                                              uint8_t baseCfg) {
+ReturnValue_t Max31865RtdPolling::writeBiasSel(MAX31865::Bias bias, SpiCookie* cookie,
+                                               uint8_t baseCfg) {
   using namespace MAX31865;
   if (bias == MAX31865::Bias::OFF) {
     baseCfg &= ~(1 << CfgBitPos::BIAS_SEL);
@@ -354,7 +350,7 @@ ReturnValue_t Max31865RtdReader::writeBiasSel(MAX31865::Bias bias, SpiCookie* co
   return writeCfgReg(cookie, baseCfg);
 }
 
-ReturnValue_t Max31865RtdReader::clearFaultStatus(SpiCookie* cookie) {
+ReturnValue_t Max31865RtdPolling::clearFaultStatus(SpiCookie* cookie) {
   using namespace MAX31865;
   // Read back the current configuration to avoid overwriting it when clearing te fault status
   uint8_t currentCfg = 0;
@@ -368,7 +364,7 @@ ReturnValue_t Max31865RtdReader::clearFaultStatus(SpiCookie* cookie) {
   return writeCfgReg(cookie, currentCfg);
 }
 
-ReturnValue_t Max31865RtdReader::readCfgReg(SpiCookie* cookie, uint8_t& cfg) {
+ReturnValue_t Max31865RtdPolling::readCfgReg(SpiCookie* cookie, uint8_t& cfg) {
   using namespace MAX31865;
   uint8_t* replyPtr = nullptr;
   auto result = readNFromReg(cookie, CONFIG, 1, &replyPtr);
@@ -378,19 +374,19 @@ ReturnValue_t Max31865RtdReader::readCfgReg(SpiCookie* cookie, uint8_t& cfg) {
   return result;
 }
 
-ReturnValue_t Max31865RtdReader::writeLowThreshold(SpiCookie* cookie, uint16_t val) {
+ReturnValue_t Max31865RtdPolling::writeLowThreshold(SpiCookie* cookie, uint16_t val) {
   using namespace MAX31865;
   uint8_t cmd[2] = {static_cast<uint8_t>((val >> 8) & 0xff), static_cast<uint8_t>(val & 0xff)};
   return writeNToReg(cookie, LOW_THRESHOLD, 2, cmd, nullptr);
 }
 
-ReturnValue_t Max31865RtdReader::writeHighThreshold(SpiCookie* cookie, uint16_t val) {
+ReturnValue_t Max31865RtdPolling::writeHighThreshold(SpiCookie* cookie, uint16_t val) {
   using namespace MAX31865;
   uint8_t cmd[2] = {static_cast<uint8_t>((val >> 8) & 0xff), static_cast<uint8_t>(val & 0xff)};
   return writeNToReg(cookie, HIGH_THRESHOLD, 2, cmd, nullptr);
 }
 
-ReturnValue_t Max31865RtdReader::readLowThreshold(SpiCookie* cookie, uint16_t& lowThreshold) {
+ReturnValue_t Max31865RtdPolling::readLowThreshold(SpiCookie* cookie, uint16_t& lowThreshold) {
   using namespace MAX31865;
   uint8_t* replyPtr = nullptr;
   auto result = readNFromReg(cookie, LOW_THRESHOLD, 2, &replyPtr);
@@ -400,7 +396,7 @@ ReturnValue_t Max31865RtdReader::readLowThreshold(SpiCookie* cookie, uint16_t& l
   return result;
 }
 
-ReturnValue_t Max31865RtdReader::readHighThreshold(SpiCookie* cookie, uint16_t& highThreshold) {
+ReturnValue_t Max31865RtdPolling::readHighThreshold(SpiCookie* cookie, uint16_t& highThreshold) {
   using namespace MAX31865;
   uint8_t* replyPtr = nullptr;
   auto result = readNFromReg(cookie, HIGH_THRESHOLD, 2, &replyPtr);
@@ -410,8 +406,8 @@ ReturnValue_t Max31865RtdReader::readHighThreshold(SpiCookie* cookie, uint16_t&
   return result;
 }
 
-ReturnValue_t Max31865RtdReader::writeNToReg(SpiCookie* cookie, uint8_t reg, size_t n, uint8_t* cmd,
-                                             uint8_t** reply) {
+ReturnValue_t Max31865RtdPolling::writeNToReg(SpiCookie* cookie, uint8_t reg, size_t n,
+                                              uint8_t* cmd, uint8_t** reply) {
   using namespace MAX31865;
   if (n > cmdBuf.size() - 1) {
     return returnvalue::FAILED;
@@ -423,7 +419,7 @@ ReturnValue_t Max31865RtdReader::writeNToReg(SpiCookie* cookie, uint8_t reg, siz
   return comIF->sendMessage(cookie, cmdBuf.data(), n + 1);
 }
 
-ReturnValue_t Max31865RtdReader::readRtdVal(SpiCookie* cookie, uint16_t& val, bool& faultBitSet) {
+ReturnValue_t Max31865RtdPolling::readRtdVal(SpiCookie* cookie, uint16_t& val, bool& faultBitSet) {
   using namespace MAX31865;
   uint8_t* replyPtr = nullptr;
   auto result = readNFromReg(cookie, RTD, 2, &replyPtr);
@@ -438,8 +434,8 @@ ReturnValue_t Max31865RtdReader::readRtdVal(SpiCookie* cookie, uint16_t& val, bo
   return result;
 }
 
-ReturnValue_t Max31865RtdReader::readNFromReg(SpiCookie* cookie, uint8_t reg, size_t n,
-                                              uint8_t** reply) {
+ReturnValue_t Max31865RtdPolling::readNFromReg(SpiCookie* cookie, uint8_t reg, size_t n,
+                                               uint8_t** reply) {
   using namespace MAX31865;
   if (n > 4) {
     return returnvalue::FAILED;
@@ -465,15 +461,15 @@ ReturnValue_t Max31865RtdReader::readNFromReg(SpiCookie* cookie, uint8_t reg, si
   return returnvalue::OK;
 }
 
-ReturnValue_t Max31865RtdReader::handleSpiError(Max31865ReaderCookie* cookie, ReturnValue_t result,
-                                                const char* ctx) {
+ReturnValue_t Max31865RtdPolling::handleSpiError(Max31865ReaderCookie* cookie, ReturnValue_t result,
+                                                 const char* ctx) {
   cookie->db.spiErrorCount.value += 1;
   sif::warning << "Max31865RtdReader::handleSpiError: " << ctx << " | Failed with result " << result
                << std::endl;
   return result;
 }
 
-ReturnValue_t Max31865RtdReader::initialize() {
+ReturnValue_t Max31865RtdPolling::initialize() {
   csLock = comIF->getCsMutex();
   return SystemObject::initialize();
 }

linux/devices/Max31865RtdPolling.h

@@ -35,11 +35,11 @@ struct Max31865ReaderCookie : public CookieIF {
   EiveMax31855::ReadOutStruct db;
 };
 
-class Max31865RtdReader : public SystemObject,
-                          public ExecutableObjectIF,
-                          public DeviceCommunicationIF {
+class Max31865RtdPolling : public SystemObject,
+                           public ExecutableObjectIF,
+                           public DeviceCommunicationIF {
  public:
-  Max31865RtdReader(object_id_t objectId, SpiComIF* lowLevelComIF, GpioIF* gpioIF);
+  Max31865RtdPolling(object_id_t objectId, SpiComIF* lowLevelComIF, GpioIF* gpioIF);
 
   ReturnValue_t performOperation(uint8_t operationCode) override;
   ReturnValue_t initialize() override;

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

linux/devices/RwPollingTask.h

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

linux/fsfwconfig/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"
 

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:

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

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 {
@@ -90,7 +90,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   ACS::SensorValues sensorValues;
 
   /* ACS Actuation Datasets */
-  IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
+  imtq::DipoleActuationSet dipoleSet = imtq::DipoleActuationSet(objects::IMTQ_HANDLER);
   rws::RwSpeedActuationSet rw1SpeedSet = rws::RwSpeedActuationSet(objects::RW1);
   rws::RwSpeedActuationSet rw2SpeedSet = rws::RwSpeedActuationSet(objects::RW2);
   rws::RwSpeedActuationSet rw3SpeedSet = rws::RwSpeedActuationSet(objects::RW3);

mission/controller/ThermalController.cpp

@@ -10,7 +10,7 @@
 #include <mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h>
 #include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
 #include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
-#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
+#include <mission/devices/devicedefinitions/imtqHelpers.h>
 #include <mission/devices/devicedefinitions/payloadPcduDefinitions.h>
 #include <mission/devices/devicedefinitions/rwHelpers.h>
 #include <objects/systemObjectList.h>
@@ -21,6 +21,7 @@ ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater
       sensorTemperatures(this),
       susTemperatures(this),
       deviceTemperatures(this),
+      heaterInfo(this),
       imtqThermalSet(objects::IMTQ_HANDLER, ThermalStateCfg()),
       max31865Set0(objects::RTD_0_IC3_PLOC_HEATSPREADER,
                    EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
@@ -112,108 +113,88 @@ void ThermalController::performControlOperation() {
     deviceTemperatures.commit();
   }
 
+  std::array<HeaterHandler::SwitchState, 8> heaterStates;
+  heaterHandler.getAllSwitchStates(heaterStates);
+  {
+    PoolReadGuard pg(&heaterInfo);
+    std::memcpy(heaterInfo.heaterSwitchState.value, heaterStates.data(), 8);
+    {
+      PoolReadGuard pg2(&currentVecPdu2);
+      if (pg.getReadResult() == returnvalue::OK and pg2.getReadResult() == returnvalue::OK) {
+        heaterInfo.heaterCurrent.value = currentVecPdu2.value[PDU2::Channels::TCS_HEATER_IN];
+      }
+    }
+  }
+
   // performThermalModuleCtrl();
 }
 
 ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                          LocalDataPoolManager& poolManager) {
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_PLOC_HEATSPREADER,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_PLOC_MISSIONBOARD,
-                           new PoolEntry<float>({1.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_4K_CAMERA,
-                           new PoolEntry<float>({2.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_DAC_HEATSPREADER,
-                           new PoolEntry<float>({3.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_STARTRACKER,
-                           new PoolEntry<float>({4.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_RW1, new PoolEntry<float>({5.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_DRO, new PoolEntry<float>({6.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_SCEX, new PoolEntry<float>({7.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_X8, new PoolEntry<float>({8.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_HPA, new PoolEntry<float>({9.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TX_MODUL,
-                           new PoolEntry<float>({10.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_MPA, new PoolEntry<float>({11.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_ACU, new PoolEntry<float>({12.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_PLPCDU_HEATSPREADER,
-                           new PoolEntry<float>({13.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TCS_BOARD,
-                           new PoolEntry<float>({14.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_MAGNETTORQUER,
-                           new PoolEntry<float>({15.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_TCS_0, &tmp1075Tcs0);
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_TCS_1, &tmp1075Tcs1);
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_PLPCDU_0, &tmp1075PlPcdu0);
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_PLPCDU_1, &tmp1075PlPcdu1);
-  localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_IF_BOARD, &tmp1075IfBrd);
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_PLOC_HEATSPREADER, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_PLOC_MISSIONBOARD, new PoolEntry<float>({1.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_4K_CAMERA, new PoolEntry<float>({2.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_DAC_HEATSPREADER, new PoolEntry<float>({3.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_STARTRACKER, new PoolEntry<float>({4.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_RW1, new PoolEntry<float>({5.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_DRO, new PoolEntry<float>({6.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_SCEX, new PoolEntry<float>({7.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_X8, new PoolEntry<float>({8.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_HPA, new PoolEntry<float>({9.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TX_MODUL, new PoolEntry<float>({10.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_MPA, new PoolEntry<float>({11.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_ACU, new PoolEntry<float>({12.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_PLPCDU_HEATSPREADER, new PoolEntry<float>({13.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TCS_BOARD, new PoolEntry<float>({14.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_MAGNETTORQUER, new PoolEntry<float>({15.0}));
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_TCS_0, &tmp1075Tcs0);
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_TCS_1, &tmp1075Tcs1);
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_PLPCDU_0, &tmp1075PlPcdu0);
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_PLPCDU_1, &tmp1075PlPcdu1);
+  localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_IF_BOARD, &tmp1075IfBrd);
 
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_0_N_LOC_XFYFZM_PT_XF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_6_R_LOC_XFYBZM_PT_XF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_1_N_LOC_XBYFZM_PT_XB,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_7_R_LOC_XBYBZM_PT_XB,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_2_N_LOC_XFYBZB_PT_YB,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_8_R_LOC_XBYBZB_PT_YB,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_3_N_LOC_XFYBZF_PT_YF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_9_R_LOC_XBYBZB_PT_YF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_4_N_LOC_XMYFZF_PT_ZF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_10_N_LOC_XMYBZF_PT_ZF,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_5_N_LOC_XFYMZB_PT_ZB,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::SUS_11_R_LOC_XBYMZB_PT_ZB,
-                           new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_0_N_LOC_XFYFZM_PT_XF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_6_R_LOC_XFYBZM_PT_XF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_1_N_LOC_XBYFZM_PT_XB, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_7_R_LOC_XBYBZM_PT_XB, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_2_N_LOC_XFYBZB_PT_YB, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_8_R_LOC_XBYBZB_PT_YB, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_3_N_LOC_XFYBZF_PT_YF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_9_R_LOC_XBYBZB_PT_YF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_4_N_LOC_XMYFZF_PT_ZF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_10_N_LOC_XMYBZF_PT_ZF, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_5_N_LOC_XFYMZB_PT_ZB, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::SUS_11_R_LOC_XBYMZB_PT_ZB, new PoolEntry<float>({0.0}));
 
-  localDataPoolMap.emplace(thermalControllerDefinitions::COMPONENT_RW, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::COMPONENT_RW, new PoolEntry<float>({0.0}));
 
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_Q7S, new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::BATTERY_TEMP_1,
-                           new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::BATTERY_TEMP_2,
-                           new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::BATTERY_TEMP_3,
-                           new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::BATTERY_TEMP_4,
-                           new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_RW1, new PoolEntry<int32_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_RW2, new PoolEntry<int32_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_RW3, new PoolEntry<int32_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_RW4, new PoolEntry<int32_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_STAR_TRACKER,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_SYRLINKS_POWER_AMPLIFIER,
-                           new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_SYRLINKS_BASEBAND_BOARD,
-                           new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_MGT, new PoolEntry<int16_t>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_ACU, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_PDU1, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_PDU2, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_1_P60DOCK, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_2_P60DOCK, new PoolEntry<float>({0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_GYRO_0_SIDE_A,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_GYRO_1_SIDE_A,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_GYRO_2_SIDE_B,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_GYRO_3_SIDE_B,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_MGM_0_SIDE_A,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_MGM_2_SIDE_B,
-                           new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_ADC_PAYLOAD_PCDU,
-                           new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_Q7S, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_1, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_2, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_3, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_4, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_RW1, new PoolEntry<int32_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_RW2, new PoolEntry<int32_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_RW3, new PoolEntry<int32_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_RW4, new PoolEntry<int32_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_STAR_TRACKER, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_SYRLINKS_POWER_AMPLIFIER, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_SYRLINKS_BASEBAND_BOARD, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_MGT, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_ACU, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_PDU1, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_PDU2, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_1_P60DOCK, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_2_P60DOCK, new PoolEntry<float>({0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_0_SIDE_A, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_1_SIDE_A, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_2_SIDE_B, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_3_SIDE_B, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_MGM_0_SIDE_A, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_MGM_2_SIDE_B, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::TEMP_ADC_PAYLOAD_PCDU, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(tcsCtrl::HEATER_SWITCH_LIST, &heaterSwitchStates);
+  localDataPoolMap.emplace(tcsCtrl::HEATER_CURRENT, &heaterCurrent);
 
   poolManager.subscribeForRegularPeriodicPacket(
       subdp::RegularHkPeriodicParams(sensorTemperatures.getSid(), false, 10.0));
@@ -221,17 +202,21 @@ ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& lo
       subdp::RegularHkPeriodicParams(susTemperatures.getSid(), false, 10.0));
   poolManager.subscribeForRegularPeriodicPacket(
       subdp::RegularHkPeriodicParams(deviceTemperatures.getSid(), false, 10.0));
+  poolManager.subscribeForDiagPeriodicPacket(
+      subdp::DiagnosticsHkPeriodicParams(heaterInfo.getSid(), false, 10.0));
   return returnvalue::OK;
 }
 
 LocalPoolDataSetBase* ThermalController::getDataSetHandle(sid_t sid) {
   switch (sid.ownerSetId) {
-    case thermalControllerDefinitions::SENSOR_TEMPERATURES:
+    case tcsCtrl::SENSOR_TEMPERATURES:
       return &sensorTemperatures;
-    case thermalControllerDefinitions::SUS_TEMPERATURES:
+    case tcsCtrl::SUS_TEMPERATURES:
       return &susTemperatures;
-    case thermalControllerDefinitions::DEVICE_TEMPERATURES:
+    case tcsCtrl::DEVICE_TEMPERATURES:
       return &deviceTemperatures;
+    case tcsCtrl::HEATER_SET:
+      return &heaterInfo;
     default:
       return nullptr;
   }
@@ -785,7 +770,7 @@ void ThermalController::copyDevices() {
   }
 
   {
-    lp_var_t<int16_t> tempMgt = lp_var_t<int16_t>(objects::IMTQ_HANDLER, IMTQ::MCU_TEMPERATURE);
+    lp_var_t<int16_t> tempMgt = lp_var_t<int16_t>(objects::IMTQ_HANDLER, imtq::MCU_TEMPERATURE);
     PoolReadGuard pg(&tempMgt, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg.getReadResult() != returnvalue::OK) {
       sif::warning << "ThermalController: Failed to read MGT temperature" << std::endl;

mission/controller/ThermalController.h

@@ -12,6 +12,7 @@
 #include <list>
 
 #include "mission/devices/HeaterHandler.h"
+#include "mission/devices/devicedefinitions/GomspaceDefinitions.h"
 #include "mission/trace.h"
 
 /**
@@ -76,9 +77,12 @@ class ThermalController : public ExtendedControllerBase {
 
   HeaterHandler& heaterHandler;
 
-  thermalControllerDefinitions::SensorTemperatures sensorTemperatures;
-  thermalControllerDefinitions::SusTemperatures susTemperatures;
-  thermalControllerDefinitions::DeviceTemperatures deviceTemperatures;
+  tcsCtrl::SensorTemperatures sensorTemperatures;
+  tcsCtrl::SusTemperatures susTemperatures;
+  tcsCtrl::DeviceTemperatures deviceTemperatures;
+  tcsCtrl::HeaterInfo heaterInfo;
+  lp_vec_t<int16_t, 9> currentVecPdu2 =
+      lp_vec_t<int16_t, 9>(gp_id_t(objects::PDU2_HANDLER, PDU::pool::PDU_CURRENTS));
 
   DeviceHandlerThermalSet imtqThermalSet;
 
@@ -160,11 +164,13 @@ class ThermalController : public ExtendedControllerBase {
   std::array<std::pair<bool, double>, 5> sensors;
   uint8_t numSensors = 0;
 
-  PoolEntry<float> tmp1075Tcs0 = PoolEntry<float>(10.0);
-  PoolEntry<float> tmp1075Tcs1 = PoolEntry<float>(10.0);
-  PoolEntry<float> tmp1075PlPcdu0 = PoolEntry<float>(10.0);
-  PoolEntry<float> tmp1075PlPcdu1 = PoolEntry<float>(10.0);
-  PoolEntry<float> tmp1075IfBrd = PoolEntry<float>(10.0);
+  PoolEntry<float> tmp1075Tcs0 = PoolEntry<float>({10.0});
+  PoolEntry<float> tmp1075Tcs1 = PoolEntry<float>({10.0});
+  PoolEntry<float> tmp1075PlPcdu0 = PoolEntry<float>({10.0});
+  PoolEntry<float> tmp1075PlPcdu1 = PoolEntry<float>({10.0});
+  PoolEntry<float> tmp1075IfBrd = PoolEntry<float>({10.0});
+  PoolEntry<uint8_t> heaterSwitchStates = PoolEntry<uint8_t>(heater::NUMBER_OF_SWITCHES);
+  PoolEntry<int16_t> heaterCurrent = PoolEntry<int16_t>();
 
   static constexpr dur_millis_t MUTEX_TIMEOUT = 50;
 

mission/controller/acs/SensorValues.h

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

mission/controller/controllerdefinitions/ThermalControllerDefinitions.h

@@ -4,13 +4,16 @@
 #include <fsfw/datapoollocal/LocalPoolVariable.h>
 #include <fsfw/datapoollocal/StaticLocalDataSet.h>
 
-namespace thermalControllerDefinitions {
+#include "devices/heaterSwitcherList.h"
 
-enum SetIds : uint32_t {
-  SENSOR_TEMPERATURES,
-  DEVICE_TEMPERATURES,
-  SUS_TEMPERATURES,
-  COMPONENT_TEMPERATURES
+namespace tcsCtrl {
+
+enum SetId : uint32_t {
+  SENSOR_TEMPERATURES = 0,
+  DEVICE_TEMPERATURES = 1,
+  SUS_TEMPERATURES = 2,
+  COMPONENT_TEMPERATURES = 3,
+  HEATER_SET = 4,
 };
 
 enum PoolIds : lp_id_t {
@@ -75,7 +78,10 @@ enum PoolIds : lp_id_t {
   TEMP_GYRO_3_SIDE_B,
   TEMP_MGM_0_SIDE_A,
   TEMP_MGM_2_SIDE_B,
-  TEMP_ADC_PAYLOAD_PCDU
+  TEMP_ADC_PAYLOAD_PCDU,
+
+  HEATER_SWITCH_LIST,
+  HEATER_CURRENT
 };
 
 static const uint8_t ENTRIES_SENSOR_TEMPERATURE_SET = 25;
@@ -202,6 +208,17 @@ class SusTemperatures : public StaticLocalDataSet<ENTRIES_SUS_TEMPERATURE_SET> {
       lp_var_t<float>(sid.objectId, PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, this);
 };
 
-}  // namespace thermalControllerDefinitions
+class HeaterInfo : public StaticLocalDataSet<3> {
+ public:
+  HeaterInfo(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, HEATER_SET) {}
+  HeaterInfo(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, HEATER_SET)) {}
+
+  lp_vec_t<uint8_t, heater::NUMBER_OF_SWITCHES> heaterSwitchState =
+      lp_vec_t<uint8_t, heater::NUMBER_OF_SWITCHES>(sid.objectId, PoolIds::HEATER_SWITCH_LIST,
+                                                    this);
+  lp_var_t<int16_t> heaterCurrent = lp_var_t<int16_t>(sid.objectId, PoolIds::HEATER_CURRENT, this);
+};
+
+}  // namespace tcsCtrl
 
 #endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_THERMALCONTROLLERDEFINITIONS_H_ */

mission/core/GenericFactory.cpp

@@ -20,6 +20,7 @@
 #include <fsfw/pus/Service8FunctionManagement.h>
 #include <fsfw/pus/Service9TimeManagement.h>
 #include <fsfw/storagemanager/PoolManager.h>
+#include <fsfw/subsystem/SubsystemBase.h>
 #include <fsfw/tcdistribution/CcsdsDistributor.h>
 #include <fsfw/tcdistribution/PusDistributor.h>
 #include <fsfw/timemanager/CdsShortTimeStamper.h>
@@ -219,6 +220,7 @@ void ObjectFactory::createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF&
   }});
   heaterHandler = new HeaterHandler(objects::HEATER_HANDLER, &gpioIF, helper, &pwrSwitcher,
                                     pcdu::Switches::PDU2_CH3_TCS_BOARD_HEATER_IN_8V);
+  heaterHandler->connectModeTreeParent(satsystem::tcs::SUBSYSTEM);
 }
 
 void ObjectFactory::createThermalController(HeaterHandler& heaterHandler) {

mission/devices/HeaterHandler.cpp

@@ -8,11 +8,13 @@
 #include <stdexcept>
 
 #include "devices/powerSwitcherList.h"
+#include "fsfw/subsystem/helper.h"
 
 HeaterHandler::HeaterHandler(object_id_t setObjectId_, GpioIF* gpioInterface_, HeaterHelper helper,
                              PowerSwitchIF* mainLineSwitcher_, power::Switch_t mainLineSwitch_)
     : SystemObject(setObjectId_),
       helper(helper),
+      modeHelper(this),
       gpioInterface(gpioInterface_),
       mainLineSwitcher(mainLineSwitcher_),
       mainLineSwitch(mainLineSwitch_),
@@ -28,8 +30,8 @@ HeaterHandler::HeaterHandler(object_id_t setObjectId_, GpioIF* gpioInterface_, H
   if (mainLineSwitcher == nullptr) {
     throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid PowerSwitchIF");
   }
-  heaterMutex = MutexFactory::instance()->createMutex();
-  if (heaterMutex == nullptr) {
+  heaterHealthAndStateMutex = MutexFactory::instance()->createMutex();
+  if (heaterHealthAndStateMutex == nullptr) {
     throw std::runtime_error("HeaterHandler::HeaterHandler: Creating Mutex failed");
   }
   auto mqArgs = MqArgs(setObjectId_, static_cast<void*>(this));
@@ -46,6 +48,13 @@ ReturnValue_t HeaterHandler::performOperation(uint8_t operationCode) {
       heater.first->performOperation(0);
     }
     handleSwitchHandling();
+    if (waitForSwitchOff) {
+      if (mainLineSwitcher->getSwitchState(mainLineSwitch) == SWITCH_OFF) {
+        waitForSwitchOff = false;
+        mode = MODE_OFF;
+        modeHelper.modeChanged(mode, submode);
+      }
+    }
   } catch (const std::out_of_range& e) {
     sif::warning << "HeaterHandler::performOperation: "
                     "Out of range error | "
@@ -76,6 +85,10 @@ ReturnValue_t HeaterHandler::initialize() {
     return ObjectManagerIF::CHILD_INIT_FAILED;
   }
 
+  result = modeHelper.initialize();
+  if (result != returnvalue::OK) {
+    return ObjectManagerIF::CHILD_INIT_FAILED;
+  }
   return returnvalue::OK;
 }
 
@@ -100,6 +113,10 @@ void HeaterHandler::readCommandQueue() {
     if (result == returnvalue::OK) {
       continue;
     }
+    result = modeHelper.handleModeCommand(&command);
+    if (result == returnvalue::OK) {
+      continue;
+    }
   } while (result == returnvalue::OK);
 }
 
@@ -124,7 +141,11 @@ ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t
   auto action = static_cast<SwitchAction>(data[1]);
   // Always accepts OFF commands
   if (action == SwitchAction::SET_SWITCH_ON) {
-    HasHealthIF::HealthState health = heater.healthDevice->getHealth();
+    HasHealthIF::HealthState health;
+    {
+      MutexGuard mg(heaterHealthAndStateMutex);
+      health = heater.healthDevice->getHealth();
+    }
     if (health == HasHealthIF::FAULTY or health == HasHealthIF::PERMANENT_FAULTY or
         health == HasHealthIF::NEEDS_RECOVERY) {
       return HasHealthIF::OBJECT_NOT_HEALTHY;
@@ -218,6 +239,9 @@ void HeaterHandler::handleSwitchHandling() {
 void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) {
   ReturnValue_t result = returnvalue::OK;
   auto& heater = heaterVec.at(heaterIdx);
+  if (waitForSwitchOff) {
+    waitForSwitchOff = false;
+  }
   /* Check if command waits for main switch being set on and whether the timeout has expired */
   if (heater.waitMainSwitchOn && heater.mainSwitchCountdown.hasTimedOut()) {
     // TODO - This requires the initiation of an FDIR procedure
@@ -244,11 +268,16 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) {
         triggerEvent(GPIO_PULL_HIGH_FAILED, result);
       } else {
         triggerEvent(HEATER_WENT_ON, heaterIdx, 0);
-        heater.switchState = ON;
+        {
+          MutexGuard mg(heaterHealthAndStateMutex);
+          heater.switchState = ON;
+        }
       }
     } else {
       triggerEvent(SWITCH_ALREADY_ON, heaterIdx);
     }
+    mode = HasModesIF::MODE_ON;
+    modeHelper.modeChanged(mode, submode);
     // There is no need to send action finish replies if the sender was the
     // HeaterHandler itself
     if (heater.replyQueue != commandQueue->getId()) {
@@ -289,15 +318,15 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) {
                  << " low" << std::endl;
       triggerEvent(GPIO_PULL_LOW_FAILED, result);
     } else {
-      result = heaterMutex->lockMutex();
-      heater.switchState = OFF;
-      if (result == returnvalue::OK) {
-        heaterMutex->unlockMutex();
+      {
+        MutexGuard mg(heaterHealthAndStateMutex);
+        heater.switchState = OFF;
       }
       triggerEvent(HEATER_WENT_OFF, heaterIdx, 0);
       // When all switches are off, also main line switch will be turned off
       if (allSwitchesOff()) {
         mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
+        waitForSwitchOff = true;
       }
     }
   } else {
@@ -316,7 +345,7 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) {
 }
 
 HeaterHandler::SwitchState HeaterHandler::checkSwitchState(heater::Switchers switchNr) const {
-  MutexGuard mg(heaterMutex);
+  MutexGuard mg(heaterHealthAndStateMutex);
   return heaterVec.at(switchNr).switchState;
 }
 
@@ -329,9 +358,57 @@ ReturnValue_t HeaterHandler::switchHeater(heater::Switchers heater, SwitchState
   return returnvalue::FAILED;
 }
 
+void HeaterHandler::announceMode(bool recursive) {
+  triggerEvent(MODE_INFO, mode, submode);
+
+  std::array<SwitchState, 8> states;
+  getAllSwitchStates(states);
+  for (unsigned idx = 0; idx < helper.heaters.size(); idx++) {
+    if (states[idx] == ON) {
+      EventManagerIF::triggerEvent(helper.heaters[idx].first->getObjectId(), MODE_INFO, MODE_ON, 0);
+    } else {
+      EventManagerIF::triggerEvent(helper.heaters[idx].first->getObjectId(), MODE_INFO, MODE_OFF,
+                                   0);
+    }
+  }
+}
+
+void HeaterHandler::getMode(Mode_t* mode, Submode_t* submode) {
+  if (!mode || !submode) {
+    return;
+  }
+  *mode = this->mode;
+  *submode = this->submode;
+}
+
+const HasHealthIF* HeaterHandler::getOptHealthIF() const { return nullptr; }
+
+const HasModesIF& HeaterHandler::getModeIF() const { return *this; }
+
+ReturnValue_t HeaterHandler::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
+  return modetree::connectModeTreeParent(parent, *this, nullptr, modeHelper);
+}
+
+ModeTreeChildIF& HeaterHandler::getModeTreeChildIF() { return *this; }
+
+object_id_t HeaterHandler::getObjectId() const { return SystemObject::getObjectId(); }
+
+ReturnValue_t HeaterHandler::getAllSwitchStates(std::array<SwitchState, 8>& statesBuf) {
+  {
+    MutexGuard mg(heaterHealthAndStateMutex);
+    if (mg.getLockResult() != returnvalue::OK) {
+      return returnvalue::FAILED;
+    }
+    for (unsigned idx = 0; idx < helper.heaters.size(); idx++) {
+      statesBuf[idx] = heaterVec[idx].switchState;
+    }
+  }
+  return returnvalue::OK;
+}
+
 bool HeaterHandler::allSwitchesOff() {
   bool allSwitchesOrd = false;
-  MutexGuard mg(heaterMutex);
+  MutexGuard mg(heaterHealthAndStateMutex);
   /* Or all switches. As soon one switch is on, allSwitchesOrd will be true */
   for (power::Switch_t switchNr = 0; switchNr < heater::NUMBER_OF_SWITCHES; switchNr++) {
     allSwitchesOrd = allSwitchesOrd || heaterVec.at(switchNr).switchState;
@@ -364,7 +441,7 @@ uint32_t HeaterHandler::getSwitchDelayMs(void) const { return 2000; }
 HasHealthIF::HealthState HeaterHandler::getHealth(heater::Switchers heater) {
   auto* healthDev = heaterVec.at(heater).healthDevice;
   if (healthDev != nullptr) {
-    MutexGuard mg(heaterMutex);
+    MutexGuard mg(heaterHealthAndStateMutex);
     return healthDev->getHealth();
   }
   return HasHealthIF::HealthState::FAULTY;

mission/devices/HeaterHandler.h

@@ -10,6 +10,8 @@
 #include <fsfw/objectmanager/SystemObject.h>
 #include <fsfw/power/PowerSwitchIF.h>
 #include <fsfw/returnvalues/returnvalue.h>
+#include <fsfw/subsystem/ModeTreeChildIF.h>
+#include <fsfw/subsystem/ModeTreeConnectionIF.h>
 #include <fsfw/tasks/ExecutableObjectIF.h>
 #include <fsfw/timemanager/Countdown.h>
 #include <fsfw_hal/common/gpio/GpioIF.h>
@@ -40,6 +42,9 @@ struct HeaterHelper {
  */
 class HeaterHandler : public ExecutableObjectIF,
                       public PowerSwitchIF,
+                      public HasModesIF,
+                      public ModeTreeChildIF,
+                      public ModeTreeConnectionIF,
                       public SystemObject,
                       public HasActionsIF {
   friend class ThermalController;
@@ -54,6 +59,7 @@ class HeaterHandler : public ExecutableObjectIF,
   static const ReturnValue_t COMMAND_ALREADY_WAITING = MAKE_RETURN_CODE(0xA5);
 
   enum CmdSourceParam : uint8_t { INTERNAL = 0, EXTERNAL = 1 };
+  enum SwitchState : uint8_t { ON = 1, OFF = 0 };
 
   /** Device command IDs */
   static const DeviceCommandId_t SWITCH_HEATER = 0x0;
@@ -61,10 +67,12 @@ class HeaterHandler : public ExecutableObjectIF,
   HeaterHandler(object_id_t setObjectId, GpioIF* gpioInterface_, HeaterHelper helper,
                 PowerSwitchIF* mainLineSwitcherObjectId, power::Switch_t mainLineSwitch);
 
+  ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent) override;
+  ReturnValue_t getAllSwitchStates(std::array<SwitchState, 8>& statesBuf);
+
   virtual ~HeaterHandler();
 
  protected:
-  enum SwitchState : bool { ON = true, OFF = false };
   enum SwitchAction : uint8_t { SET_SWITCH_OFF, SET_SWITCH_ON, NONE };
 
   ReturnValue_t switchHeater(heater::Switchers heater, SwitchState switchState);
@@ -128,12 +136,14 @@ class HeaterHandler : public ExecutableObjectIF,
 
   HeaterMap heaterVec = {};
 
-  MutexIF* heaterMutex = nullptr;
+  MutexIF* heaterHealthAndStateMutex = nullptr;
 
   HeaterHelper helper;
+  ModeHelper modeHelper;
 
   /** Size of command queue */
   size_t cmdQueueSize = 20;
+  bool waitForSwitchOff = true;
 
   GpioIF* gpioInterface = nullptr;
 
@@ -152,6 +162,9 @@ class HeaterHandler : public ExecutableObjectIF,
 
   StorageManagerIF* ipcStore = nullptr;
 
+  Mode_t mode = HasModesIF::MODE_OFF;
+  Submode_t submode = 0;
+
   void readCommandQueue();
 
   /**
@@ -172,6 +185,16 @@ class HeaterHandler : public ExecutableObjectIF,
    */
   void setInitialSwitchStates();
 
+  // HasModesIF implementation
+  void announceMode(bool recursive) override;
+  void getMode(Mode_t* mode, Submode_t* submode) override;
+
+  // Mode Tree helper overrides
+  object_id_t getObjectId() const override;
+  const HasHealthIF* getOptHealthIF() const override;
+  const HasModesIF& getModeIF() const override;
+  ModeTreeChildIF& getModeTreeChildIF() override;
+
   void handleSwitchOnCommand(heater::Switchers heaterIdx);
 
   void handleSwitchOffCommand(heater::Switchers heaterIdx);

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.
    *

mission/devices/devicedefinitions/CMakeLists.txt

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

mission/devices/devicedefinitions/imtqHelpers.cpp

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

mission/devices/devicedefinitions/imtqHelpers.h

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

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

tmtc

@@ -1 +1 @@
-Subproject commit 1bc38ea4b507ac0e395c52d0b142ad598bb0817c
+Subproject commit 4ced2d0b185f9949662c946ca71fe666ac25e411