a lot of bugfixes for IMTQ

mission/devices/devicedefinitions/imtqHelpers.h

@@ -22,7 +22,7 @@ enum ComStep : uint8_t {
   READ_ACTUATE_GET = 8,
 };
 
-enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE };
+enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE, DO_NOTHING };
 
 enum class SpecialRequest : uint8_t {
   NONE = 0,
@@ -195,20 +195,28 @@ enum PoolIds : lp_id_t {
   ANALOG_VOLTAGE_MV,
   DIGITAL_CURRENT,
   ANALOG_CURRENT,
-  COIL_X_CURRENT,
-  COIL_Y_CURRENT,
-  COIL_Z_CURRENT,
-  COIL_X_TEMPERATURE,
-  COIL_Y_TEMPERATURE,
-  COIL_Z_TEMPERATURE,
+  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,
-  DIPOLES_X,
-  DIPOLES_Y,
-  DIPOLES_Z,
+
+  MTM_RAW_WT,
+  ACTUATION_RAW_STATUS_WT,
+
+  DIPOLES_ID,
   CURRENT_TORQUE_DURATION,
 
   INIT_POS_X_ERR,
@@ -476,34 +484,56 @@ class StatusDataset : public StaticLocalDataSet<4> {
 
 class HkDataset : public StaticLocalDataSet<HK_SET_POOL_ENTRIES> {
  public:
-  HkDataset(HasLocalDataPoolIF* owner, uint32_t setId) : StaticLocalDataSet(owner, setId) {}
+  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) : StaticLocalDataSet(sid_t(objectId, setId)) {}
+  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>(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);
-  lp_var_t<float> analogCurrentmA = lp_var_t<float>(sid.objectId, ANALOG_CURRENT, this);
-  lp_var_t<float> coilXCurrentmA = lp_var_t<float>(sid.objectId, COIL_X_CURRENT, this);
-  lp_var_t<float> coilYCurrentmA = lp_var_t<float>(sid.objectId, COIL_Y_CURRENT, this);
-  lp_var_t<float> coilZCurrentmA = lp_var_t<float>(sid.objectId, COIL_Z_CURRENT, this);
-  /** All temperatures in [<5B>C] */
-  lp_var_t<int16_t> coilXTemperature = lp_var_t<int16_t>(sid.objectId, COIL_X_TEMPERATURE, this);
-  lp_var_t<int16_t> coilYTemperature = lp_var_t<int16_t>(sid.objectId, COIL_Y_TEMPERATURE, this);
-  lp_var_t<int16_t> coilZTemperature = lp_var_t<int16_t>(sid.objectId, COIL_Z_TEMPERATURE, this);
-  lp_var_t<int16_t> mcuTemperature = lp_var_t<int16_t>(sid.objectId, MCU_TEMPERATURE, this);
+  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) {}
+  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) {}
+      : 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}) {
+  }
 };
 /**
  *
@@ -529,32 +559,39 @@ class CalibratedMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRI
  */
 class RawMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
  public:
-  RawMtmMeasurementSet(HasLocalDataPoolIF* owner, uint32_t setId)
-      : StaticLocalDataSet(owner, setId) {}
+  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(object_id_t objectId, uint32_t setId)
-      : StaticLocalDataSet(sid_t(objectId, setId)) {}
+  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 = lp_vec_t<float, 3>(sid.objectId, MTM_RAW, this);
+  lp_vec_t<float, 3> mtmRawNt;
   /** 1 if coils were actuating during measurement otherwise 0 */
-  lp_var_t<uint8_t> coilActuationStatus =
-      lp_var_t<uint8_t>(sid.objectId, ACTUATION_RAW_STATUS, this);
+  lp_var_t<uint8_t> coilActuationStatus;
 };
 
 class RawMtmMeasurementNoTorque : public RawMtmMeasurementSet {
  public:
   RawMtmMeasurementNoTorque(HasLocalDataPoolIF* owner)
-      : RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_NO_TORQUE) {}
+      : 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) {}
+      : 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) {}
+      : 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) {}
+      : RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_WITH_TORQUE,
+                             {PoolIds::MTM_RAW_WT, PoolIds::ACTUATION_RAW_STATUS_WT}) {}
 };
 
 /**
@@ -608,28 +645,16 @@ class DipoleActuationSet : public StaticLocalDataSet<4> {
 
   void setDipoles(int16_t xDipole_, int16_t yDipole_, int16_t zDipole_,
                   uint16_t currentTorqueDurationMs_) {
-    if (xDipole.value != xDipole_) {
-      xDipole = xDipole_;
-    }
-    if (yDipole.value != yDipole_) {
-      yDipole = yDipole_;
-    }
-    if (zDipole.value != zDipole_) {
-      zDipole = zDipole_;
-    }
+    dipoles[0] = xDipole_;
+    dipoles[1] = yDipole_;
+    dipoles[2] = zDipole_;
     currentTorqueDurationMs = currentTorqueDurationMs_;
   }
 
-  void getDipoles(int16_t& xDipole_, int16_t& yDipole_, int16_t& zDipole_) {
-    xDipole_ = xDipole.value;
-    yDipole_ = yDipole.value;
-    zDipole_ = zDipole.value;
-  }
+  const int16_t* getDipoles() const { return dipoles.value; }
 
  private:
-  lp_var_t<int16_t> xDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_X, this);
-  lp_var_t<int16_t> yDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_Y, this);
-  lp_var_t<int16_t> zDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_Z, this);
+  lp_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);
 };
@@ -1099,65 +1124,10 @@ class NegZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
 }  // 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;
+  imtq::RequestType request = imtq::RequestType::MEASURE_NO_ACTUATION;
+  imtq::SpecialRequest specialRequest = imtq::SpecialRequest::NONE;
+  int16_t dipoles[3]{};
+  uint16_t torqueDuration = 0;
 };
 
 struct ImtqRepliesDefault {