eive-obsw/mission/acs/imtqHelpers.h

#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>
#include <mission/acs/defs.h>

class ImtqHandler;

// C garbage which can be included from gps.h
#undef STATUS_SET

namespace imtq {

uint16_t integrationTimeFromSelectValue(uint8_t value);

enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE, DO_NOTHING };

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

struct Request {
  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
  imtq::RequestType requestType = imtq::RequestType::MEASURE_NO_ACTUATION;
  imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE;
  uint8_t integrationTimeSel = 3;
  int16_t dipoles[3]{};
  uint16_t torqueDuration = 0;
};

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

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);
static constexpr ReturnValue_t STARTUP_CFG_ERROR = MAKE_RETURN_CODE(9);
//! [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(10);

namespace cmdIds {

static constexpr DeviceCommandId_t REQUEST = 0x70;
static constexpr DeviceCommandId_t REPLY_NO_TORQUE = 0x71;
static constexpr DeviceCommandId_t REPLY_WITH_TORQUE = 0x72;
static const DeviceCommandId_t START_ACTUATION_DIPOLE = 0x2;
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;
static const DeviceCommandId_t NEG_Y_SELF_TEST = 0xA;
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;

}  // namespace cmdIds

static const uint8_t POINTER_REG_SIZE = 1;

enum SetIds : uint32_t {
  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,
};

namespace replySize {

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 HK_SET_POOL_ENTRIES = 20;
static const uint8_t CAL_MTM_POOL_ENTRIES = 4;
static const uint8_t SELF_TEST_DATASET_ENTRIES = 104;

/** Error codes for interpreting the self test error byte */
static const uint8_t I2C_FAILURE_MASK = 0x1;
static const uint8_t SPI_FAILURE_MASK = 0x2;            // MTM connectivity
static const uint8_t ADC_FAILURE_MASK = 0x4;            // Current/Temp measurement
static const uint8_t PWM_FAILURE_MASK = 0x8;            // Coil actuation
static const uint8_t TC_FAILURE_MASK = 0x10;            // System failure
static const uint8_t MTM_RANGE_FAILURE_MASK = 0x20;     // MTM values outside of expected range
static const uint8_t COIL_CURRENT_FAILURE_MASK = 0x40;  // Coil currents outside of expected range
static const uint8_t INVALID_ERROR_BYTE =
    0x80;  // This is an invalid error byte and should be never replied by the IMTQ

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

namespace param {

static constexpr uint16_t SEL_MTM = 0x2002;
static constexpr uint16_t INTEGRATION_TIME_SELECT = 0x2003;

}  // namespace param

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,
  ANALOG_CURRENT,
  COIL_CURRENTS,
  COIL_TEMPERATURES,
  MCU_TEMPERATURE,

  DIGITAL_VOLTAGE_MV_WT,
  ANALOG_VOLTAGE_MV_WT,
  DIGITAL_CURRENT_WT,
  ANALOG_CURRENT_WT,
  COIL_CURRENTS_WT,
  COIL_TEMPERATURES_WT,
  MCU_TEMPERATURE_WT,

  MGM_CAL_NT,
  ACTUATION_CAL_STATUS,

  MTM_RAW,
  ACTUATION_RAW_STATUS,

  MTM_RAW_WT,
  ACTUATION_RAW_STATUS_WT,

  DIPOLES_ID,
  CURRENT_TORQUE_DURATION,

  INIT_POS_X_ERR,
  INIT_POS_X_RAW_MAG_X,
  INIT_POS_X_RAW_MAG_Y,
  INIT_POS_X_RAW_MAG_Z,
  INIT_POS_X_CAL_MAG_X,
  INIT_POS_X_CAL_MAG_Y,
  INIT_POS_X_CAL_MAG_Z,
  INIT_POS_X_COIL_X_CURRENT,
  INIT_POS_X_COIL_Y_CURRENT,
  INIT_POS_X_COIL_Z_CURRENT,
  INIT_POS_X_COIL_X_TEMPERATURE,
  INIT_POS_X_COIL_Y_TEMPERATURE,
  INIT_POS_X_COIL_Z_TEMPERATURE,

  INIT_NEG_X_ERR,
  INIT_NEG_X_RAW_MAG_X,
  INIT_NEG_X_RAW_MAG_Y,
  INIT_NEG_X_RAW_MAG_Z,
  INIT_NEG_X_CAL_MAG_X,
  INIT_NEG_X_CAL_MAG_Y,
  INIT_NEG_X_CAL_MAG_Z,
  INIT_NEG_X_COIL_X_CURRENT,
  INIT_NEG_X_COIL_Y_CURRENT,
  INIT_NEG_X_COIL_Z_CURRENT,
  INIT_NEG_X_COIL_X_TEMPERATURE,
  INIT_NEG_X_COIL_Y_TEMPERATURE,
  INIT_NEG_X_COIL_Z_TEMPERATURE,

  INIT_POS_Y_ERR,
  INIT_POS_Y_RAW_MAG_X,
  INIT_POS_Y_RAW_MAG_Y,
  INIT_POS_Y_RAW_MAG_Z,
  INIT_POS_Y_CAL_MAG_X,
  INIT_POS_Y_CAL_MAG_Y,
  INIT_POS_Y_CAL_MAG_Z,
  INIT_POS_Y_COIL_X_CURRENT,
  INIT_POS_Y_COIL_Y_CURRENT,
  INIT_POS_Y_COIL_Z_CURRENT,
  INIT_POS_Y_COIL_X_TEMPERATURE,
  INIT_POS_Y_COIL_Y_TEMPERATURE,
  INIT_POS_Y_COIL_Z_TEMPERATURE,

  INIT_NEG_Y_ERR,
  INIT_NEG_Y_RAW_MAG_X,
  INIT_NEG_Y_RAW_MAG_Y,
  INIT_NEG_Y_RAW_MAG_Z,
  INIT_NEG_Y_CAL_MAG_X,
  INIT_NEG_Y_CAL_MAG_Y,
  INIT_NEG_Y_CAL_MAG_Z,
  INIT_NEG_Y_COIL_X_CURRENT,
  INIT_NEG_Y_COIL_Y_CURRENT,
  INIT_NEG_Y_COIL_Z_CURRENT,
  INIT_NEG_Y_COIL_X_TEMPERATURE,
  INIT_NEG_Y_COIL_Y_TEMPERATURE,
  INIT_NEG_Y_COIL_Z_TEMPERATURE,

  INIT_POS_Z_ERR,
  INIT_POS_Z_RAW_MAG_X,
  INIT_POS_Z_RAW_MAG_Y,
  INIT_POS_Z_RAW_MAG_Z,
  INIT_POS_Z_CAL_MAG_X,
  INIT_POS_Z_CAL_MAG_Y,
  INIT_POS_Z_CAL_MAG_Z,
  INIT_POS_Z_COIL_X_CURRENT,
  INIT_POS_Z_COIL_Y_CURRENT,
  INIT_POS_Z_COIL_Z_CURRENT,
  INIT_POS_Z_COIL_X_TEMPERATURE,
  INIT_POS_Z_COIL_Y_TEMPERATURE,
  INIT_POS_Z_COIL_Z_TEMPERATURE,

  INIT_NEG_Z_ERR,
  INIT_NEG_Z_RAW_MAG_X,
  INIT_NEG_Z_RAW_MAG_Y,
  INIT_NEG_Z_RAW_MAG_Z,
  INIT_NEG_Z_CAL_MAG_X,
  INIT_NEG_Z_CAL_MAG_Y,
  INIT_NEG_Z_CAL_MAG_Z,
  INIT_NEG_Z_COIL_X_CURRENT,
  INIT_NEG_Z_COIL_Y_CURRENT,
  INIT_NEG_Z_COIL_Z_CURRENT,
  INIT_NEG_Z_COIL_X_TEMPERATURE,
  INIT_NEG_Z_COIL_Y_TEMPERATURE,
  INIT_NEG_Z_COIL_Z_TEMPERATURE,

  POS_X_ERR,
  POS_X_RAW_MAG_X,
  POS_X_RAW_MAG_Y,
  POS_X_RAW_MAG_Z,
  POS_X_CAL_MAG_X,
  POS_X_CAL_MAG_Y,
  POS_X_CAL_MAG_Z,
  POS_X_COIL_X_CURRENT,
  POS_X_COIL_Y_CURRENT,
  POS_X_COIL_Z_CURRENT,
  POS_X_COIL_X_TEMPERATURE,
  POS_X_COIL_Y_TEMPERATURE,
  POS_X_COIL_Z_TEMPERATURE,

  NEG_X_ERR,
  NEG_X_RAW_MAG_X,
  NEG_X_RAW_MAG_Y,
  NEG_X_RAW_MAG_Z,
  NEG_X_CAL_MAG_X,
  NEG_X_CAL_MAG_Y,
  NEG_X_CAL_MAG_Z,
  NEG_X_COIL_X_CURRENT,
  NEG_X_COIL_Y_CURRENT,
  NEG_X_COIL_Z_CURRENT,
  NEG_X_COIL_X_TEMPERATURE,
  NEG_X_COIL_Y_TEMPERATURE,
  NEG_X_COIL_Z_TEMPERATURE,

  POS_Y_ERR,
  POS_Y_RAW_MAG_X,
  POS_Y_RAW_MAG_Y,
  POS_Y_RAW_MAG_Z,
  POS_Y_CAL_MAG_X,
  POS_Y_CAL_MAG_Y,
  POS_Y_CAL_MAG_Z,
  POS_Y_COIL_X_CURRENT,
  POS_Y_COIL_Y_CURRENT,
  POS_Y_COIL_Z_CURRENT,
  POS_Y_COIL_X_TEMPERATURE,
  POS_Y_COIL_Y_TEMPERATURE,
  POS_Y_COIL_Z_TEMPERATURE,

  NEG_Y_ERR,
  NEG_Y_RAW_MAG_X,
  NEG_Y_RAW_MAG_Y,
  NEG_Y_RAW_MAG_Z,
  NEG_Y_CAL_MAG_X,
  NEG_Y_CAL_MAG_Y,
  NEG_Y_CAL_MAG_Z,
  NEG_Y_COIL_X_CURRENT,
  NEG_Y_COIL_Y_CURRENT,
  NEG_Y_COIL_Z_CURRENT,
  NEG_Y_COIL_X_TEMPERATURE,
  NEG_Y_COIL_Y_TEMPERATURE,
  NEG_Y_COIL_Z_TEMPERATURE,

  POS_Z_ERR,
  POS_Z_RAW_MAG_X,
  POS_Z_RAW_MAG_Y,
  POS_Z_RAW_MAG_Z,
  POS_Z_CAL_MAG_X,
  POS_Z_CAL_MAG_Y,
  POS_Z_CAL_MAG_Z,
  POS_Z_COIL_X_CURRENT,
  POS_Z_COIL_Y_CURRENT,
  POS_Z_COIL_Z_CURRENT,
  POS_Z_COIL_X_TEMPERATURE,
  POS_Z_COIL_Y_TEMPERATURE,
  POS_Z_COIL_Z_TEMPERATURE,

  NEG_Z_ERR,
  NEG_Z_RAW_MAG_X,
  NEG_Z_RAW_MAG_Y,
  NEG_Z_RAW_MAG_Z,
  NEG_Z_CAL_MAG_X,
  NEG_Z_CAL_MAG_Y,
  NEG_Z_CAL_MAG_Z,
  NEG_Z_COIL_X_CURRENT,
  NEG_Z_COIL_Y_CURRENT,
  NEG_Z_COIL_Z_CURRENT,
  NEG_Z_COIL_X_TEMPERATURE,
  NEG_Z_COIL_Y_TEMPERATURE,
  NEG_Z_COIL_Z_TEMPERATURE,

  FINA_POS_X_ERR,
  FINA_POS_X_RAW_MAG_X,
  FINA_POS_X_RAW_MAG_Y,
  FINA_POS_X_RAW_MAG_Z,
  FINA_POS_X_CAL_MAG_X,
  FINA_POS_X_CAL_MAG_Y,
  FINA_POS_X_CAL_MAG_Z,
  FINA_POS_X_COIL_X_CURRENT,
  FINA_POS_X_COIL_Y_CURRENT,
  FINA_POS_X_COIL_Z_CURRENT,
  FINA_POS_X_COIL_X_TEMPERATURE,
  FINA_POS_X_COIL_Y_TEMPERATURE,
  FINA_POS_X_COIL_Z_TEMPERATURE,

  FINA_NEG_X_ERR,
  FINA_NEG_X_RAW_MAG_X,
  FINA_NEG_X_RAW_MAG_Y,
  FINA_NEG_X_RAW_MAG_Z,
  FINA_NEG_X_CAL_MAG_X,
  FINA_NEG_X_CAL_MAG_Y,
  FINA_NEG_X_CAL_MAG_Z,
  FINA_NEG_X_COIL_X_CURRENT,
  FINA_NEG_X_COIL_Y_CURRENT,
  FINA_NEG_X_COIL_Z_CURRENT,
  FINA_NEG_X_COIL_X_TEMPERATURE,
  FINA_NEG_X_COIL_Y_TEMPERATURE,
  FINA_NEG_X_COIL_Z_TEMPERATURE,

  FINA_POS_Y_ERR,
  FINA_POS_Y_RAW_MAG_X,
  FINA_POS_Y_RAW_MAG_Y,
  FINA_POS_Y_RAW_MAG_Z,
  FINA_POS_Y_CAL_MAG_X,
  FINA_POS_Y_CAL_MAG_Y,
  FINA_POS_Y_CAL_MAG_Z,
  FINA_POS_Y_COIL_X_CURRENT,
  FINA_POS_Y_COIL_Y_CURRENT,
  FINA_POS_Y_COIL_Z_CURRENT,
  FINA_POS_Y_COIL_X_TEMPERATURE,
  FINA_POS_Y_COIL_Y_TEMPERATURE,
  FINA_POS_Y_COIL_Z_TEMPERATURE,

  FINA_NEG_Y_ERR,
  FINA_NEG_Y_RAW_MAG_X,
  FINA_NEG_Y_RAW_MAG_Y,
  FINA_NEG_Y_RAW_MAG_Z,
  FINA_NEG_Y_CAL_MAG_X,
  FINA_NEG_Y_CAL_MAG_Y,
  FINA_NEG_Y_CAL_MAG_Z,
  FINA_NEG_Y_COIL_X_CURRENT,
  FINA_NEG_Y_COIL_Y_CURRENT,
  FINA_NEG_Y_COIL_Z_CURRENT,
  FINA_NEG_Y_COIL_X_TEMPERATURE,
  FINA_NEG_Y_COIL_Y_TEMPERATURE,
  FINA_NEG_Y_COIL_Z_TEMPERATURE,

  FINA_POS_Z_ERR,
  FINA_POS_Z_RAW_MAG_X,
  FINA_POS_Z_RAW_MAG_Y,
  FINA_POS_Z_RAW_MAG_Z,
  FINA_POS_Z_CAL_MAG_X,
  FINA_POS_Z_CAL_MAG_Y,
  FINA_POS_Z_CAL_MAG_Z,
  FINA_POS_Z_COIL_X_CURRENT,
  FINA_POS_Z_COIL_Y_CURRENT,
  FINA_POS_Z_COIL_Z_CURRENT,
  FINA_POS_Z_COIL_X_TEMPERATURE,
  FINA_POS_Z_COIL_Y_TEMPERATURE,
  FINA_POS_Z_COIL_Z_TEMPERATURE,

  FINA_NEG_Z_ERR,
  FINA_NEG_Z_RAW_MAG_X,
  FINA_NEG_Z_RAW_MAG_Y,
  FINA_NEG_Z_RAW_MAG_Z,
  FINA_NEG_Z_CAL_MAG_X,
  FINA_NEG_Z_CAL_MAG_Y,
  FINA_NEG_Z_CAL_MAG_Z,
  FINA_NEG_Z_COIL_X_CURRENT,
  FINA_NEG_Z_COIL_Y_CURRENT,
  FINA_NEG_Z_COIL_Z_CURRENT,
  FINA_NEG_Z_COIL_X_TEMPERATURE,
  FINA_NEG_Z_COIL_Y_TEMPERATURE,
  FINA_NEG_Z_COIL_Z_TEMPERATURE,
};

class StatusDataset : public StaticLocalDataSet<4> {
 public:
  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);
};

class HkDataset : public StaticLocalDataSet<HK_SET_POOL_ENTRIES> {
 public:
  HkDataset(HasLocalDataPoolIF* owner, uint32_t setId, std::array<lp_id_t, 7> pids)
      : StaticLocalDataSet(owner, setId),
        digitalVoltageMv(sid.objectId, pids[0], this),
        analogVoltageMv(sid.objectId, pids[1], this),
        digitalCurrentmA(sid.objectId, pids[2], this),
        analogCurrentmA(sid.objectId, pids[3], this),
        coilCurrentsMilliamps(sid.objectId, pids[4], this),
        /** All temperatures in [C] for X, Y, Z */
        coilTemperatures(sid.objectId, pids[5], this),
        mcuTemperature(sid.objectId, pids[6], this) {}

  HkDataset(object_id_t objectId, uint32_t setId, std::array<lp_id_t, 7> pids)
      : StaticLocalDataSet(sid_t(objectId, setId)),
        digitalVoltageMv(sid.objectId, pids[0], this),
        analogVoltageMv(sid.objectId, pids[1], this),
        digitalCurrentmA(sid.objectId, pids[2], this),
        analogCurrentmA(sid.objectId, pids[3], this),
        coilCurrentsMilliamps(sid.objectId, pids[4], this),
        /** All temperatures in [C] for X, Y, Z */
        coilTemperatures(sid.objectId, pids[5], this),
        mcuTemperature(sid.objectId, pids[6], this) {}

  // Engineering HK variables
  lp_var_t<uint16_t> digitalVoltageMv;
  lp_var_t<uint16_t> analogVoltageMv;
  lp_var_t<float> digitalCurrentmA;
  lp_var_t<float> analogCurrentmA;
  lp_vec_t<float, 3> coilCurrentsMilliamps;
  /** All temperatures in [C] for X, Y, Z */
  lp_vec_t<int16_t, 3> coilTemperatures;
  lp_var_t<int16_t> mcuTemperature;

 private:
};

class HkDatasetNoTorque : public HkDataset {
 public:
  HkDatasetNoTorque(HasLocalDataPoolIF* owner)
      : HkDataset(owner, imtq::SetIds::ENG_HK_NO_TORQUE,
                  {DIGITAL_VOLTAGE_MV, ANALOG_VOLTAGE_MV, DIGITAL_CURRENT, ANALOG_CURRENT,
                   COIL_CURRENTS, COIL_TEMPERATURES, MCU_TEMPERATURE}) {}
};

class HkDatasetWithTorque : public HkDataset {
 public:
  HkDatasetWithTorque(HasLocalDataPoolIF* owner)
      : HkDataset(owner, imtq::SetIds::ENG_HK_SET_WITH_TORQUE,
                  {DIGITAL_VOLTAGE_MV_WT, ANALOG_VOLTAGE_MV_WT, DIGITAL_CURRENT_WT,
                   ANALOG_CURRENT_WT, COIL_CURRENTS_WT, COIL_TEMPERATURES_WT, MCU_TEMPERATURE_WT}) {
  }
};
/**
 *
 * @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_MTM_SET) {}

  CalibratedMtmMeasurementSet(object_id_t objectId)
      : 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);
  /** 1 if coils were actuating during measurement otherwise 0 */
  lp_var_t<uint8_t> coilActuationStatus =
      lp_var_t<uint8_t>(sid.objectId, ACTUATION_CAL_STATUS, this);
};

/**
 * @brief   This dataset holds the raw MTM measurements.
 */
class RawMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
 public:
  RawMtmMeasurementSet(object_id_t objectId, uint32_t setId, std::array<lp_id_t, 2> pids)
      : StaticLocalDataSet(sid_t(objectId, setId)),
        mtmRawNt(sid.objectId, pids.at(0), this),
        coilActuationStatus(sid.objectId, pids.at(1), this) {}

  RawMtmMeasurementSet(HasLocalDataPoolIF* owner, uint32_t setId, std::array<lp_id_t, 2> pids)
      : StaticLocalDataSet(owner, setId),
        mtmRawNt(sid.objectId, pids.at(0), this),
        coilActuationStatus(sid.objectId, pids.at(1), this) {}

  /** The unit of all measurements is nT */
  lp_vec_t<float, 3> mtmRawNt;
  /** 1 if coils were actuating during measurement otherwise 0 */
  lp_var_t<uint8_t> coilActuationStatus;
};

class RawMtmMeasurementNoTorque : public RawMtmMeasurementSet {
 public:
  RawMtmMeasurementNoTorque(HasLocalDataPoolIF* owner)
      : RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_NO_TORQUE,
                             {PoolIds::MTM_RAW, PoolIds::ACTUATION_RAW_STATUS}) {}
  RawMtmMeasurementNoTorque(object_id_t objectId)
      : RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_NO_TORQUE,
                             {PoolIds::MTM_RAW, PoolIds::ACTUATION_RAW_STATUS}) {}
};
class RawMtmMeasurementWithTorque : public RawMtmMeasurementSet {
 public:
  RawMtmMeasurementWithTorque(HasLocalDataPoolIF* owner)
      : RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_WITH_TORQUE,
                             {PoolIds::MTM_RAW_WT, PoolIds::ACTUATION_RAW_STATUS_WT}) {}
  RawMtmMeasurementWithTorque(object_id_t objectId)
      : RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_WITH_TORQUE,
                             {PoolIds::MTM_RAW_WT, PoolIds::ACTUATION_RAW_STATUS_WT}) {}
};

/**
 * @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.
 *
 * @details Deserialize the packet, write the deserialized data to the ipc store and store the
 *          the ipc store address in the action message.
 */
class CommandDipolePacket : public SerialLinkedListAdapter<SerializeIF> {
 public:
  CommandDipolePacket() { setLinks(); }

  SerializeElement<uint16_t> xDipole;
  SerializeElement<uint16_t> yDipole;
  SerializeElement<uint16_t> zDipole;
  SerializeElement<uint16_t> duration;

 private:
  /**
   * @brief   Constructor
   *
   * @param xDipole   The dipole of the x coil in 10 ^ -4 * Am^2
   * @param yDipole   The dipole of the y coil in 10 ^ -4 * Am^2
   * @param zDipole   The dipole of the z coil in 10 ^ -4 * Am^2
   * @param duration  The duration in milliseconds the dipole will be generated by the coils.
   *                  When set to 0, the dipole will be generated until a new dipole actuation
   *                  command is sent.
   */
  CommandDipolePacket(uint16_t xDipole, uint16_t yDipole, uint16_t zDipole, uint16_t duration)
      : xDipole(xDipole), yDipole(yDipole), zDipole(zDipole), duration(duration) {}
  void setLinks() {
    setStart(&xDipole);
    xDipole.setNext(&yDipole);
    yDipole.setNext(&zDipole);
    zDipole.setNext(&duration);
  }
};

class DipoleActuationSet : public StaticLocalDataSet<4> {
  friend class ::ImtqHandler;

 public:
  DipoleActuationSet(HasLocalDataPoolIF& owner)
      : StaticLocalDataSet(&owner, imtq::SetIds::DIPOLES) {}
  DipoleActuationSet(object_id_t objectId)
      : StaticLocalDataSet(sid_t(objectId, imtq::SetIds::DIPOLES)) {}

  // Refresh torque command without changing any of the set dipoles.
  void refreshTorqueing(uint16_t durationMs_) { currentTorqueDurationMs = durationMs_; }

  void setDipoles(int16_t xDipole_, int16_t yDipole_, int16_t zDipole_,
                  uint16_t currentTorqueDurationMs_) {
    dipoles[0] = xDipole_;
    dipoles[1] = yDipole_;
    dipoles[2] = zDipole_;
    currentTorqueDurationMs = currentTorqueDurationMs_;
  }

  const int16_t* getDipoles() const { return dipoles.value; }

 private:
  lp_vec_t<int16_t, 3> dipoles = lp_vec_t<int16_t, 3>(sid.objectId, DIPOLES_ID, this);
  lp_var_t<uint16_t> currentTorqueDurationMs =
      lp_var_t<uint16_t>(sid.objectId, CURRENT_TORQUE_DURATION, this);
};

/**
 * @brief   This dataset can be used to store the self test results of the +X self test.
 *
 * @details Units of measurements:
 *          Raw magnetic field: [nT]
 *          Calibrated magnetic field: [nT]
 *          Coil currents: [mA]
 *          Temperature: [C]
 *          The +X self test generates a positive dipole in X direction and measures the magnetic
 *          field with the built-in MTM. The procedure of the test is as follows:
 *          1. All coils off (INIT step)
 *          2. +X actuation
 *          3. All coils off (FINA step)
 */
class PosXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 public:
  PosXSelfTestSet(HasLocalDataPoolIF* owner)
      : StaticLocalDataSet(owner, imtq::SetIds::POSITIVE_X_TEST) {}

  PosXSelfTestSet(object_id_t objectId)
      : 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);
  lp_var_t<float> initRawMagX = lp_var_t<float>(sid.objectId, INIT_POS_X_RAW_MAG_X, this);
  lp_var_t<float> initRawMagY = lp_var_t<float>(sid.objectId, INIT_POS_X_RAW_MAG_Y, this);
  lp_var_t<float> initRawMagZ = lp_var_t<float>(sid.objectId, INIT_POS_X_RAW_MAG_Z, this);
  lp_var_t<float> initCalMagX = lp_var_t<float>(sid.objectId, INIT_POS_X_CAL_MAG_X, this);
  lp_var_t<float> initCalMagY = lp_var_t<float>(sid.objectId, INIT_POS_X_CAL_MAG_Y, this);
  lp_var_t<float> initCalMagZ = lp_var_t<float>(sid.objectId, INIT_POS_X_CAL_MAG_Z, this);
  lp_var_t<float> initCoilXCurrent = lp_var_t<float>(sid.objectId, INIT_POS_X_COIL_X_CURRENT, this);
  lp_var_t<float> initCoilYCurrent = lp_var_t<float>(sid.objectId, INIT_POS_X_COIL_Y_CURRENT, this);
  lp_var_t<float> initCoilZCurrent = lp_var_t<float>(sid.objectId, INIT_POS_X_COIL_Z_CURRENT, this);
  lp_var_t<uint16_t> initCoilXTemperature =
      lp_var_t<uint16_t>(sid.objectId, INIT_POS_X_COIL_X_TEMPERATURE, this);
  lp_var_t<uint16_t> initCoilYTemperature =
      lp_var_t<uint16_t>(sid.objectId, INIT_POS_X_COIL_Y_TEMPERATURE, this);
  lp_var_t<uint16_t> initCoilZTemperature =
      lp_var_t<uint16_t>(sid.objectId, INIT_POS_X_COIL_Z_TEMPERATURE, this);

  /** +X block */
  lp_var_t<uint8_t> err = lp_var_t<uint8_t>(sid.objectId, POS_X_ERR, this);
  lp_var_t<float> rawMagX = lp_var_t<float>(sid.objectId, POS_X_RAW_MAG_X, this);
  lp_var_t<float> rawMagY = lp_var_t<float>(sid.objectId, POS_X_RAW_MAG_Y, this);
  lp_var_t<float> rawMagZ = lp_var_t<float>(sid.objectId, POS_X_RAW_MAG_Z, this);
  lp_var_t<float> calMagX = lp_var_t<float>(sid.objectId, POS_X_CAL_MAG_X, this);
  lp_var_t<float> calMagY = lp_var_t<float>(sid.objectId, POS_X_CAL_MAG_Y, this);
  lp_var_t<float> calMagZ = lp_var_t<float>(sid.objectId, POS_X_CAL_MAG_Z, this);
  lp_var_t<float> coilXCurrent = lp_var_t<float>(sid.objectId, POS_X_COIL_X_CURRENT, this);
  lp_var_t<float> coilYCurrent = lp_var_t<float>(sid.objectId, POS_X_COIL_Y_CURRENT, this);
  lp_var_t<float> coilZCurrent = lp_var_t<float>(sid.objectId, POS_X_COIL_Z_CURRENT, this);
  lp_var_t<int16_t> coilXTemperature =
      lp_var_t<int16_t>(sid.objectId, POS_X_COIL_X_TEMPERATURE, this);
  lp_var_t<int16_t> coilYTemperature =
      lp_var_t<int16_t>(sid.objectId, POS_X_COIL_Y_TEMPERATURE, this);
  lp_var_t<int16_t> coilZTemperature =
      lp_var_t<int16_t>(sid.objectId, POS_X_COIL_Z_TEMPERATURE, this);

  /** FINA block */
  lp_var_t<uint8_t> finaErr = lp_var_t<uint8_t>(sid.objectId, FINA_POS_X_ERR, this);
  lp_var_t<float> finaRawMagX = lp_var_t<float>(sid.objectId, FINA_POS_X_RAW_MAG_X, this);
  lp_var_t<float> finaRawMagY = lp_var_t<float>(sid.objectId, FINA_POS_X_RAW_MAG_Y, this);
  lp_var_t<float> finaRawMagZ = lp_var_t<float>(sid.objectId, FINA_POS_X_RAW_MAG_Z, this);
  lp_var_t<float> finaCalMagX = lp_var_t<float>(sid.objectId, FINA_POS_X_CAL_MAG_X, this);
  lp_var_t<float> finaCalMagY = lp_var_t<float>(sid.objectId, FINA_POS_X_CAL_MAG_Y, this);
  lp_var_t<float> finaCalMagZ = lp_var_t<float>(sid.objectId, FINA_POS_X_CAL_MAG_Z, this);
  lp_var_t<float> finaCoilXCurrent = lp_var_t<float>(sid.objectId, FINA_POS_X_COIL_X_CURRENT, this);
  lp_var_t<float> finaCoilYCurrent = lp_var_t<float>(sid.objectId, FINA_POS_X_COIL_Y_CURRENT, this);
  lp_var_t<float> finaCoilZCurrent = lp_var_t<float>(sid.objectId, FINA_POS_X_COIL_Z_CURRENT, this);
  lp_var_t<int16_t> finaCoilXTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_POS_X_COIL_X_TEMPERATURE, this);
  lp_var_t<int16_t> finaCoilYTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_POS_X_COIL_Y_TEMPERATURE, this);
  lp_var_t<int16_t> finaCoilZTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_POS_X_COIL_Z_TEMPERATURE, this);
};

/**
 * @brief   This dataset can be used to store the self test results of the -X self test.
 *
 * @details Units of measurements:
 *          Raw magnetic field: [nT]
 *          Calibrated magnetic field: [nT]
 *          Coil currents: [mA]
 *          Temperature: [C]
 *          The -X self test generates a negative dipole in X direction and measures the magnetic
 *          field with the built-in MTM. The procedure of the test is as follows:
 *          1. All coils off (INIT step)
 *          2. -X actuation
 *          3. All coils off (FINA step)
 */
class NegXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 public:
  NegXSelfTestSet(HasLocalDataPoolIF* owner)
      : StaticLocalDataSet(owner, imtq::SetIds::NEGATIVE_X_TEST) {}

  NegXSelfTestSet(object_id_t objectId)
      : 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);
  lp_var_t<float> initRawMagX = lp_var_t<float>(sid.objectId, INIT_NEG_X_RAW_MAG_X, this);
  lp_var_t<float> initRawMagY = lp_var_t<float>(sid.objectId, INIT_NEG_X_RAW_MAG_Y, this);
  lp_var_t<float> initRawMagZ = lp_var_t<float>(sid.objectId, INIT_NEG_X_RAW_MAG_Z, this);
  lp_var_t<float> initCalMagX = lp_var_t<float>(sid.objectId, INIT_NEG_X_CAL_MAG_X, this);
  lp_var_t<float> initCalMagY = lp_var_t<float>(sid.objectId, INIT_NEG_X_CAL_MAG_Y, this);
  lp_var_t<float> initCalMagZ = lp_var_t<float>(sid.objectId, INIT_NEG_X_CAL_MAG_Z, this);
  lp_var_t<float> initCoilXCurrent = lp_var_t<float>(sid.objectId, INIT_NEG_X_COIL_X_CURRENT, this);
  lp_var_t<float> initCoilYCurrent = lp_var_t<float>(sid.objectId, INIT_NEG_X_COIL_Y_CURRENT, this);
  lp_var_t<float> initCoilZCurrent = lp_var_t<float>(sid.objectId, INIT_NEG_X_COIL_Z_CURRENT, this);
  lp_var_t<int16_t> initCoilXTemperature =
      lp_var_t<int16_t>(sid.objectId, INIT_NEG_X_COIL_X_TEMPERATURE, this);
  lp_var_t<int16_t> initCoilYTemperature =
      lp_var_t<int16_t>(sid.objectId, INIT_NEG_X_COIL_Y_TEMPERATURE, this);
  lp_var_t<int16_t> initCoilZTemperature =
      lp_var_t<int16_t>(sid.objectId, INIT_NEG_X_COIL_Z_TEMPERATURE, this);

  /** -X block */
  lp_var_t<uint8_t> err = lp_var_t<uint8_t>(sid.objectId, NEG_X_ERR, this);
  lp_var_t<float> rawMagX = lp_var_t<float>(sid.objectId, NEG_X_RAW_MAG_X, this);
  lp_var_t<float> rawMagY = lp_var_t<float>(sid.objectId, NEG_X_RAW_MAG_Y, this);
  lp_var_t<float> rawMagZ = lp_var_t<float>(sid.objectId, NEG_X_RAW_MAG_Z, this);
  lp_var_t<float> calMagX = lp_var_t<float>(sid.objectId, NEG_X_CAL_MAG_X, this);
  lp_var_t<float> calMagY = lp_var_t<float>(sid.objectId, NEG_X_CAL_MAG_Y, this);
  lp_var_t<float> calMagZ = lp_var_t<float>(sid.objectId, NEG_X_CAL_MAG_Z, this);
  lp_var_t<float> coilXCurrent = lp_var_t<float>(sid.objectId, NEG_X_COIL_X_CURRENT, this);
  lp_var_t<float> coilYCurrent = lp_var_t<float>(sid.objectId, NEG_X_COIL_Y_CURRENT, this);
  lp_var_t<float> coilZCurrent = lp_var_t<float>(sid.objectId, NEG_X_COIL_Z_CURRENT, this);
  lp_var_t<int16_t> coilXTemperature =
      lp_var_t<int16_t>(sid.objectId, NEG_X_COIL_X_TEMPERATURE, this);
  lp_var_t<int16_t> coilYTemperature =
      lp_var_t<int16_t>(sid.objectId, NEG_X_COIL_Y_TEMPERATURE, this);
  lp_var_t<int16_t> coilZTemperature =
      lp_var_t<int16_t>(sid.objectId, NEG_X_COIL_Z_TEMPERATURE, this);

  /** FINA block */
  lp_var_t<uint8_t> finaErr = lp_var_t<uint8_t>(sid.objectId, FINA_NEG_X_ERR, this);
  lp_var_t<float> finaRawMagX = lp_var_t<float>(sid.objectId, FINA_NEG_X_RAW_MAG_X, this);
  lp_var_t<float> finaRawMagY = lp_var_t<float>(sid.objectId, FINA_NEG_X_RAW_MAG_Y, this);
  lp_var_t<float> finaRawMagZ = lp_var_t<float>(sid.objectId, FINA_NEG_X_RAW_MAG_Z, this);
  lp_var_t<float> finaCalMagX = lp_var_t<float>(sid.objectId, FINA_NEG_X_CAL_MAG_X, this);
  lp_var_t<float> finaCalMagY = lp_var_t<float>(sid.objectId, FINA_NEG_X_CAL_MAG_Y, this);
  lp_var_t<float> finaCalMagZ = lp_var_t<float>(sid.objectId, FINA_NEG_X_CAL_MAG_Z, this);
  lp_var_t<float> finaCoilXCurrent = lp_var_t<float>(sid.objectId, FINA_NEG_X_COIL_X_CURRENT, this);
  lp_var_t<float> finaCoilYCurrent = lp_var_t<float>(sid.objectId, FINA_NEG_X_COIL_Y_CURRENT, this);
  lp_var_t<float> finaCoilZCurrent = lp_var_t<float>(sid.objectId, FINA_NEG_X_COIL_Z_CURRENT, this);
  lp_var_t<int16_t> finaCoilXTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_NEG_X_COIL_X_TEMPERATURE, this);
  lp_var_t<int16_t> finaCoilYTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_NEG_X_COIL_Y_TEMPERATURE, this);
  lp_var_t<int16_t> finaCoilZTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_NEG_X_COIL_Z_TEMPERATURE, this);
};

/**
 * @brief   This dataset can be used to store the self test results of the +Y self test.
 *
 * @details Units of measurements:
 *          Raw magnetic field: [nT]
 *          Calibrated magnetic field: [nT]
 *          Coil currents: [mA]
 *          Temperature: [C]
 *          The +Y self test generates a positive dipole in y direction and measures the magnetic
 *          field with the built-in MTM. The procedure of the test is as follows:
 *          1. All coils off (INIT step)
 *          2. +Y actuation
 *          3. All coils off (FINA step)
 */
class PosYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 public:
  PosYSelfTestSet(HasLocalDataPoolIF* owner)
      : StaticLocalDataSet(owner, imtq::SetIds::POSITIVE_Y_TEST) {}

  PosYSelfTestSet(object_id_t objectId)
      : 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);
  lp_var_t<float> initRawMagX = lp_var_t<float>(sid.objectId, INIT_POS_Y_RAW_MAG_X, this);
  lp_var_t<float> initRawMagY = lp_var_t<float>(sid.objectId, INIT_POS_Y_RAW_MAG_Y, this);
  lp_var_t<float> initRawMagZ = lp_var_t<float>(sid.objectId, INIT_POS_Y_RAW_MAG_Z, this);
  lp_var_t<float> initCalMagX = lp_var_t<float>(sid.objectId, INIT_POS_Y_CAL_MAG_X, this);
  lp_var_t<float> initCalMagY = lp_var_t<float>(sid.objectId, INIT_POS_Y_CAL_MAG_Y, this);
  lp_var_t<float> initCalMagZ = lp_var_t<float>(sid.objectId, INIT_POS_Y_CAL_MAG_Z, this);
  lp_var_t<float> initCoilXCurrent = lp_var_t<float>(sid.objectId, INIT_POS_Y_COIL_X_CURRENT, this);
  lp_var_t<float> initCoilYCurrent = lp_var_t<float>(sid.objectId, INIT_POS_Y_COIL_Y_CURRENT, this);
  lp_var_t<float> initCoilZCurrent = lp_var_t<float>(sid.objectId, INIT_POS_Y_COIL_Z_CURRENT, this);
  lp_var_t<int16_t> initCoilXTemperature =
      lp_var_t<int16_t>(sid.objectId, INIT_POS_Y_COIL_X_TEMPERATURE, this);
  lp_var_t<int16_t> initCoilYTemperature =
      lp_var_t<int16_t>(sid.objectId, INIT_POS_Y_COIL_Y_TEMPERATURE, this);
  lp_var_t<int16_t> initCoilZTemperature =
      lp_var_t<int16_t>(sid.objectId, INIT_POS_Y_COIL_Z_TEMPERATURE, this);

  /** +Y block */
  lp_var_t<uint8_t> err = lp_var_t<uint8_t>(sid.objectId, POS_Y_ERR, this);
  lp_var_t<float> rawMagX = lp_var_t<float>(sid.objectId, POS_Y_RAW_MAG_X, this);
  lp_var_t<float> rawMagY = lp_var_t<float>(sid.objectId, POS_Y_RAW_MAG_Y, this);
  lp_var_t<float> rawMagZ = lp_var_t<float>(sid.objectId, POS_Y_RAW_MAG_Z, this);
  lp_var_t<float> calMagX = lp_var_t<float>(sid.objectId, POS_Y_CAL_MAG_X, this);
  lp_var_t<float> calMagY = lp_var_t<float>(sid.objectId, POS_Y_CAL_MAG_Y, this);
  lp_var_t<float> calMagZ = lp_var_t<float>(sid.objectId, POS_Y_CAL_MAG_Z, this);
  lp_var_t<float> coilXCurrent = lp_var_t<float>(sid.objectId, POS_Y_COIL_X_CURRENT, this);
  lp_var_t<float> coilYCurrent = lp_var_t<float>(sid.objectId, POS_Y_COIL_Y_CURRENT, this);
  lp_var_t<float> coilZCurrent = lp_var_t<float>(sid.objectId, POS_Y_COIL_Z_CURRENT, this);
  lp_var_t<int16_t> coilXTemperature =
      lp_var_t<int16_t>(sid.objectId, POS_Y_COIL_X_TEMPERATURE, this);
  lp_var_t<int16_t> coilYTemperature =
      lp_var_t<int16_t>(sid.objectId, POS_Y_COIL_Y_TEMPERATURE, this);
  lp_var_t<int16_t> coilZTemperature =
      lp_var_t<int16_t>(sid.objectId, POS_Y_COIL_Z_TEMPERATURE, this);

  /** FINA block */
  lp_var_t<uint8_t> finaErr = lp_var_t<uint8_t>(sid.objectId, FINA_POS_Y_ERR, this);
  lp_var_t<float> finaRawMagX = lp_var_t<float>(sid.objectId, FINA_POS_Y_RAW_MAG_X, this);
  lp_var_t<float> finaRawMagY = lp_var_t<float>(sid.objectId, FINA_POS_Y_RAW_MAG_Y, this);
  lp_var_t<float> finaRawMagZ = lp_var_t<float>(sid.objectId, FINA_POS_Y_RAW_MAG_Z, this);
  lp_var_t<float> finaCalMagX = lp_var_t<float>(sid.objectId, FINA_POS_Y_CAL_MAG_X, this);
  lp_var_t<float> finaCalMagY = lp_var_t<float>(sid.objectId, FINA_POS_Y_CAL_MAG_Y, this);
  lp_var_t<float> finaCalMagZ = lp_var_t<float>(sid.objectId, FINA_POS_Y_CAL_MAG_Z, this);
  lp_var_t<float> finaCoilXCurrent = lp_var_t<float>(sid.objectId, FINA_POS_Y_COIL_X_CURRENT, this);
  lp_var_t<float> finaCoilYCurrent = lp_var_t<float>(sid.objectId, FINA_POS_Y_COIL_Y_CURRENT, this);
  lp_var_t<float> finaCoilZCurrent = lp_var_t<float>(sid.objectId, FINA_POS_Y_COIL_Z_CURRENT, this);
  lp_var_t<int16_t> finaCoilXTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_POS_Y_COIL_X_TEMPERATURE, this);
  lp_var_t<int16_t> finaCoilYTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_POS_Y_COIL_Y_TEMPERATURE, this);
  lp_var_t<int16_t> finaCoilZTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_POS_Y_COIL_Z_TEMPERATURE, this);
};

/**
 * @brief   This dataset can be used to store the self test results of the -Y self test.
 *
 * @details Units of measurements:
 *          Raw magnetic field: [nT]
 *          Calibrated magnetic field: [nT]
 *          Coil currents: [mA]
 *          Temperature: [C]
 *          The -Y self test generates a negative dipole in y direction and measures the magnetic
 *          field with the built-in MTM. The procedure of the test is as follows:
 *          1. All coils off (INIT step)
 *          2. -Y actuation
 *          3. All coils off (FINA step)
 */
class NegYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 public:
  NegYSelfTestSet(HasLocalDataPoolIF* owner)
      : StaticLocalDataSet(owner, imtq::SetIds::NEGATIVE_Y_TEST) {}

  NegYSelfTestSet(object_id_t objectId)
      : 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);
  lp_var_t<float> initRawMagX = lp_var_t<float>(sid.objectId, INIT_NEG_Y_RAW_MAG_X, this);
  lp_var_t<float> initRawMagY = lp_var_t<float>(sid.objectId, INIT_NEG_Y_RAW_MAG_Y, this);
  lp_var_t<float> initRawMagZ = lp_var_t<float>(sid.objectId, INIT_NEG_Y_RAW_MAG_Z, this);
  lp_var_t<float> initCalMagX = lp_var_t<float>(sid.objectId, INIT_NEG_Y_CAL_MAG_X, this);
  lp_var_t<float> initCalMagY = lp_var_t<float>(sid.objectId, INIT_NEG_Y_CAL_MAG_Y, this);
  lp_var_t<float> initCalMagZ = lp_var_t<float>(sid.objectId, INIT_NEG_Y_CAL_MAG_Z, this);
  lp_var_t<float> initCoilXCurrent = lp_var_t<float>(sid.objectId, INIT_NEG_Y_COIL_X_CURRENT, this);
  lp_var_t<float> initCoilYCurrent = lp_var_t<float>(sid.objectId, INIT_NEG_Y_COIL_Y_CURRENT, this);
  lp_var_t<float> initCoilZCurrent = lp_var_t<float>(sid.objectId, INIT_NEG_Y_COIL_Z_CURRENT, this);
  lp_var_t<int16_t> initCoilXTemperature =
      lp_var_t<int16_t>(sid.objectId, INIT_NEG_Y_COIL_X_TEMPERATURE, this);
  lp_var_t<int16_t> initCoilYTemperature =
      lp_var_t<int16_t>(sid.objectId, INIT_NEG_Y_COIL_Y_TEMPERATURE, this);
  lp_var_t<int16_t> initCoilZTemperature =
      lp_var_t<int16_t>(sid.objectId, INIT_NEG_Y_COIL_Z_TEMPERATURE, this);

  /** -Y block */
  lp_var_t<uint8_t> err = lp_var_t<uint8_t>(sid.objectId, NEG_Y_ERR, this);
  lp_var_t<float> rawMagX = lp_var_t<float>(sid.objectId, NEG_Y_RAW_MAG_X, this);
  lp_var_t<float> rawMagY = lp_var_t<float>(sid.objectId, NEG_Y_RAW_MAG_Y, this);
  lp_var_t<float> rawMagZ = lp_var_t<float>(sid.objectId, NEG_Y_RAW_MAG_Z, this);
  lp_var_t<float> calMagX = lp_var_t<float>(sid.objectId, NEG_Y_CAL_MAG_X, this);
  lp_var_t<float> calMagY = lp_var_t<float>(sid.objectId, NEG_Y_CAL_MAG_Y, this);
  lp_var_t<float> calMagZ = lp_var_t<float>(sid.objectId, NEG_Y_CAL_MAG_Z, this);
  lp_var_t<float> coilXCurrent = lp_var_t<float>(sid.objectId, NEG_Y_COIL_X_CURRENT, this);
  lp_var_t<float> coilYCurrent = lp_var_t<float>(sid.objectId, NEG_Y_COIL_Y_CURRENT, this);
  lp_var_t<float> coilZCurrent = lp_var_t<float>(sid.objectId, NEG_Y_COIL_Z_CURRENT, this);
  lp_var_t<int16_t> coilXTemperature =
      lp_var_t<int16_t>(sid.objectId, NEG_Y_COIL_X_TEMPERATURE, this);
  lp_var_t<int16_t> coilYTemperature =
      lp_var_t<int16_t>(sid.objectId, NEG_Y_COIL_Y_TEMPERATURE, this);
  lp_var_t<int16_t> coilZTemperature =
      lp_var_t<int16_t>(sid.objectId, NEG_Y_COIL_Z_TEMPERATURE, this);

  /** FINA block */
  lp_var_t<uint8_t> finaErr = lp_var_t<uint8_t>(sid.objectId, FINA_NEG_Y_ERR, this);
  lp_var_t<float> finaRawMagX = lp_var_t<float>(sid.objectId, FINA_NEG_Y_RAW_MAG_X, this);
  lp_var_t<float> finaRawMagY = lp_var_t<float>(sid.objectId, FINA_NEG_Y_RAW_MAG_Y, this);
  lp_var_t<float> finaRawMagZ = lp_var_t<float>(sid.objectId, FINA_NEG_Y_RAW_MAG_Z, this);
  lp_var_t<float> finaCalMagX = lp_var_t<float>(sid.objectId, FINA_NEG_Y_CAL_MAG_X, this);
  lp_var_t<float> finaCalMagY = lp_var_t<float>(sid.objectId, FINA_NEG_Y_CAL_MAG_Y, this);
  lp_var_t<float> finaCalMagZ = lp_var_t<float>(sid.objectId, FINA_NEG_Y_CAL_MAG_Z, this);
  lp_var_t<float> finaCoilXCurrent = lp_var_t<float>(sid.objectId, FINA_NEG_Y_COIL_X_CURRENT, this);
  lp_var_t<float> finaCoilYCurrent = lp_var_t<float>(sid.objectId, FINA_NEG_Y_COIL_Y_CURRENT, this);
  lp_var_t<float> finaCoilZCurrent = lp_var_t<float>(sid.objectId, FINA_NEG_Y_COIL_Z_CURRENT, this);
  lp_var_t<int16_t> finaCoilXTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_NEG_Y_COIL_X_TEMPERATURE, this);
  lp_var_t<int16_t> finaCoilYTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_NEG_Y_COIL_Y_TEMPERATURE, this);
  lp_var_t<int16_t> finaCoilZTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_NEG_Y_COIL_Z_TEMPERATURE, this);
};

/**
 * @brief   This dataset can be used to store the self test results of the +Z self test.
 *
 * @details Units of measurements:
 *          Raw magnetic field: [nT]
 *          Calibrated magnetic field: [nT]
 *          Coil currents: [mA]
 *          Temperature: [C]
 *          The +Z self test generates a positive dipole in z direction and measures the magnetic
 *          field with the built-in MTM. The procedure of the test is as follows:
 *          1. All coils off (INIT step)
 *          2. +Z actuation
 *          3. All coils off (FINA step)
 */
class PosZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 public:
  PosZSelfTestSet(HasLocalDataPoolIF* owner)
      : StaticLocalDataSet(owner, imtq::SetIds::POSITIVE_Z_TEST) {}

  PosZSelfTestSet(object_id_t objectId)
      : 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);
  lp_var_t<float> initRawMagX = lp_var_t<float>(sid.objectId, INIT_POS_Z_RAW_MAG_X, this);
  lp_var_t<float> initRawMagY = lp_var_t<float>(sid.objectId, INIT_POS_Z_RAW_MAG_Y, this);
  lp_var_t<float> initRawMagZ = lp_var_t<float>(sid.objectId, INIT_POS_Z_RAW_MAG_Z, this);
  lp_var_t<float> initCalMagX = lp_var_t<float>(sid.objectId, INIT_POS_Z_CAL_MAG_X, this);
  lp_var_t<float> initCalMagY = lp_var_t<float>(sid.objectId, INIT_POS_Z_CAL_MAG_Y, this);
  lp_var_t<float> initCalMagZ = lp_var_t<float>(sid.objectId, INIT_POS_Z_CAL_MAG_Z, this);
  lp_var_t<float> initCoilXCurrent = lp_var_t<float>(sid.objectId, INIT_POS_Z_COIL_X_CURRENT, this);
  lp_var_t<float> initCoilYCurrent = lp_var_t<float>(sid.objectId, INIT_POS_Z_COIL_Y_CURRENT, this);
  lp_var_t<float> initCoilZCurrent = lp_var_t<float>(sid.objectId, INIT_POS_Z_COIL_Z_CURRENT, this);
  lp_var_t<uint16_t> initCoilXTemperature =
      lp_var_t<uint16_t>(sid.objectId, INIT_POS_Z_COIL_X_TEMPERATURE, this);
  lp_var_t<uint16_t> initCoilYTemperature =
      lp_var_t<uint16_t>(sid.objectId, INIT_POS_Z_COIL_Y_TEMPERATURE, this);
  lp_var_t<uint16_t> initCoilZTemperature =
      lp_var_t<uint16_t>(sid.objectId, INIT_POS_Z_COIL_Z_TEMPERATURE, this);

  /** +Z block */
  lp_var_t<uint8_t> err = lp_var_t<uint8_t>(sid.objectId, POS_Z_ERR, this);
  lp_var_t<float> rawMagX = lp_var_t<float>(sid.objectId, POS_Z_RAW_MAG_X, this);
  lp_var_t<float> rawMagY = lp_var_t<float>(sid.objectId, POS_Z_RAW_MAG_Y, this);
  lp_var_t<float> rawMagZ = lp_var_t<float>(sid.objectId, POS_Z_RAW_MAG_Z, this);
  lp_var_t<float> calMagX = lp_var_t<float>(sid.objectId, POS_Z_CAL_MAG_X, this);
  lp_var_t<float> calMagY = lp_var_t<float>(sid.objectId, POS_Z_CAL_MAG_Y, this);
  lp_var_t<float> calMagZ = lp_var_t<float>(sid.objectId, POS_Z_CAL_MAG_Z, this);
  lp_var_t<float> coilXCurrent = lp_var_t<float>(sid.objectId, POS_Z_COIL_X_CURRENT, this);
  lp_var_t<float> coilYCurrent = lp_var_t<float>(sid.objectId, POS_Z_COIL_Y_CURRENT, this);
  lp_var_t<float> coilZCurrent = lp_var_t<float>(sid.objectId, POS_Z_COIL_Z_CURRENT, this);
  lp_var_t<int16_t> coilXTemperature =
      lp_var_t<int16_t>(sid.objectId, POS_Z_COIL_X_TEMPERATURE, this);
  lp_var_t<int16_t> coilYTemperature =
      lp_var_t<int16_t>(sid.objectId, POS_Z_COIL_Y_TEMPERATURE, this);
  lp_var_t<int16_t> coilZTemperature =
      lp_var_t<int16_t>(sid.objectId, POS_Z_COIL_Z_TEMPERATURE, this);

  /** FINA block */
  lp_var_t<uint8_t> finaErr = lp_var_t<uint8_t>(sid.objectId, FINA_POS_Z_ERR, this);
  lp_var_t<float> finaRawMagX = lp_var_t<float>(sid.objectId, FINA_POS_Z_RAW_MAG_X, this);
  lp_var_t<float> finaRawMagY = lp_var_t<float>(sid.objectId, FINA_POS_Z_RAW_MAG_Y, this);
  lp_var_t<float> finaRawMagZ = lp_var_t<float>(sid.objectId, FINA_POS_Z_RAW_MAG_Z, this);
  lp_var_t<float> finaCalMagX = lp_var_t<float>(sid.objectId, FINA_POS_Z_CAL_MAG_X, this);
  lp_var_t<float> finaCalMagY = lp_var_t<float>(sid.objectId, FINA_POS_Z_CAL_MAG_Y, this);
  lp_var_t<float> finaCalMagZ = lp_var_t<float>(sid.objectId, FINA_POS_Z_CAL_MAG_Z, this);
  lp_var_t<float> finaCoilXCurrent = lp_var_t<float>(sid.objectId, FINA_POS_Z_COIL_X_CURRENT, this);
  lp_var_t<float> finaCoilYCurrent = lp_var_t<float>(sid.objectId, FINA_POS_Z_COIL_Y_CURRENT, this);
  lp_var_t<float> finaCoilZCurrent = lp_var_t<float>(sid.objectId, FINA_POS_Z_COIL_Z_CURRENT, this);
  lp_var_t<int16_t> finaCoilXTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_POS_Z_COIL_X_TEMPERATURE, this);
  lp_var_t<int16_t> finaCoilYTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_POS_Z_COIL_Y_TEMPERATURE, this);
  lp_var_t<int16_t> finaCoilZTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_POS_Z_COIL_Z_TEMPERATURE, this);
};

/**
 * @brief   This dataset can be used to store the self test results of the -Z self test.
 *
 * @details Units of measurements:
 *          Raw magnetic field: [nT]
 *          Calibrated magnetic field: [nT]
 *          Coil currents: [mA]
 *          Temperature: [C]
 *          The -Z self test generates a negative dipole in z direction and measures the magnetic
 *          field with the built-in MTM. The procedure of the test is as follows:
 *          1. All coils off (INIT step)
 *          2. -Z actuation
 *          3. All coils off (FINA step)
 */
class NegZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 public:
  NegZSelfTestSet(HasLocalDataPoolIF* owner)
      : StaticLocalDataSet(owner, imtq::SetIds::NEGATIVE_Z_TEST) {}

  NegZSelfTestSet(object_id_t objectId)
      : 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);
  lp_var_t<float> initRawMagX = lp_var_t<float>(sid.objectId, INIT_NEG_Z_RAW_MAG_X, this);
  lp_var_t<float> initRawMagY = lp_var_t<float>(sid.objectId, INIT_NEG_Z_RAW_MAG_Y, this);
  lp_var_t<float> initRawMagZ = lp_var_t<float>(sid.objectId, INIT_NEG_Z_RAW_MAG_Z, this);
  lp_var_t<float> initCalMagX = lp_var_t<float>(sid.objectId, INIT_NEG_Z_CAL_MAG_X, this);
  lp_var_t<float> initCalMagY = lp_var_t<float>(sid.objectId, INIT_NEG_Z_CAL_MAG_Y, this);
  lp_var_t<float> initCalMagZ = lp_var_t<float>(sid.objectId, INIT_NEG_Z_CAL_MAG_Z, this);
  lp_var_t<float> initCoilXCurrent = lp_var_t<float>(sid.objectId, INIT_NEG_Z_COIL_X_CURRENT, this);
  lp_var_t<float> initCoilYCurrent = lp_var_t<float>(sid.objectId, INIT_NEG_Z_COIL_Y_CURRENT, this);
  lp_var_t<float> initCoilZCurrent = lp_var_t<float>(sid.objectId, INIT_NEG_Z_COIL_Z_CURRENT, this);
  lp_var_t<int16_t> initCoilXTemperature =
      lp_var_t<int16_t>(sid.objectId, INIT_NEG_Z_COIL_X_TEMPERATURE, this);
  lp_var_t<int16_t> initCoilYTemperature =
      lp_var_t<int16_t>(sid.objectId, INIT_NEG_Z_COIL_Y_TEMPERATURE, this);
  lp_var_t<int16_t> initCoilZTemperature =
      lp_var_t<int16_t>(sid.objectId, INIT_NEG_Z_COIL_Z_TEMPERATURE, this);

  /** +Z block */
  lp_var_t<uint8_t> err = lp_var_t<uint8_t>(sid.objectId, NEG_Z_ERR, this);
  lp_var_t<float> rawMagX = lp_var_t<float>(sid.objectId, NEG_Z_RAW_MAG_X, this);
  lp_var_t<float> rawMagY = lp_var_t<float>(sid.objectId, NEG_Z_RAW_MAG_Y, this);
  lp_var_t<float> rawMagZ = lp_var_t<float>(sid.objectId, NEG_Z_RAW_MAG_Z, this);
  lp_var_t<float> calMagX = lp_var_t<float>(sid.objectId, NEG_Z_CAL_MAG_X, this);
  lp_var_t<float> calMagY = lp_var_t<float>(sid.objectId, NEG_Z_CAL_MAG_Y, this);
  lp_var_t<float> calMagZ = lp_var_t<float>(sid.objectId, NEG_Z_CAL_MAG_Z, this);
  lp_var_t<float> coilXCurrent = lp_var_t<float>(sid.objectId, NEG_Z_COIL_X_CURRENT, this);
  lp_var_t<float> coilYCurrent = lp_var_t<float>(sid.objectId, NEG_Z_COIL_Y_CURRENT, this);
  lp_var_t<float> coilZCurrent = lp_var_t<float>(sid.objectId, NEG_Z_COIL_Z_CURRENT, this);
  lp_var_t<int16_t> coilXTemperature =
      lp_var_t<int16_t>(sid.objectId, NEG_Z_COIL_X_TEMPERATURE, this);
  lp_var_t<int16_t> coilYTemperature =
      lp_var_t<int16_t>(sid.objectId, NEG_Z_COIL_Y_TEMPERATURE, this);
  lp_var_t<int16_t> coilZTemperature =
      lp_var_t<int16_t>(sid.objectId, NEG_Z_COIL_Z_TEMPERATURE, this);

  /** FINA block */
  lp_var_t<uint8_t> finaErr = lp_var_t<uint8_t>(sid.objectId, FINA_NEG_Z_ERR, this);
  lp_var_t<float> finaRawMagX = lp_var_t<float>(sid.objectId, FINA_NEG_Z_RAW_MAG_X, this);
  lp_var_t<float> finaRawMagY = lp_var_t<float>(sid.objectId, FINA_NEG_Z_RAW_MAG_Y, this);
  lp_var_t<float> finaRawMagZ = lp_var_t<float>(sid.objectId, FINA_NEG_Z_RAW_MAG_Z, this);
  lp_var_t<float> finaCalMagX = lp_var_t<float>(sid.objectId, FINA_NEG_Z_CAL_MAG_X, this);
  lp_var_t<float> finaCalMagY = lp_var_t<float>(sid.objectId, FINA_NEG_Z_CAL_MAG_Y, this);
  lp_var_t<float> finaCalMagZ = lp_var_t<float>(sid.objectId, FINA_NEG_Z_CAL_MAG_Z, this);
  lp_var_t<float> finaCoilXCurrent = lp_var_t<float>(sid.objectId, FINA_NEG_Z_COIL_X_CURRENT, this);
  lp_var_t<float> finaCoilYCurrent = lp_var_t<float>(sid.objectId, FINA_NEG_Z_COIL_Y_CURRENT, this);
  lp_var_t<float> finaCoilZCurrent = lp_var_t<float>(sid.objectId, FINA_NEG_Z_COIL_Z_CURRENT, this);
  lp_var_t<int16_t> finaCoilXTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_NEG_Z_COIL_X_TEMPERATURE, this);
  lp_var_t<int16_t> finaCoilYTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_NEG_Z_COIL_Y_TEMPERATURE, this);
  lp_var_t<int16_t> finaCoilZTemperature =
      lp_var_t<int16_t>(sid.objectId, FINA_NEG_Z_COIL_Z_TEMPERATURE, this);
};

}  // namespace imtq

struct ImtqRepliesDefault {
  friend class ImtqPollingTask;

 public:
  static constexpr size_t BASE_LEN =
      1 + 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();
  }

  void setConfigured() { rawData[0] = true; }
  bool devWasConfigured() const { return rawData[0]; }

  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 + 1;
    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_ */