From 2b6334b9dcbc2d49e650f0bf9d68b949713071e8 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 20 Oct 2022 10:44:28 +0200
Subject: [PATCH] this works

---
 mission/devices/ImtqHandler.cpp                       | 11 +++++++++--
 mission/devices/ImtqHandler.h                         |  4 ++++
 .../devicedefinitions/IMTQHandlerDefinitions.h        |  8 +++++++-
 3 files changed, 20 insertions(+), 3 deletions(-)

diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp
index 23de759b..5e78b90f 100644
--- a/mission/devices/ImtqHandler.cpp
+++ b/mission/devices/ImtqHandler.cpp
@@ -100,8 +100,10 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
       communicationStep = CommunicationStep::DIPOLE_ACTUATION;
       break;
     case CommunicationStep::DIPOLE_ACTUATION: {
-      // TODO: Set correct ID if actuation is necessary
-      *id = IMTQ::START_ACTUATION_DIPOLE;
+      // The dipoles will be set by the ACS controller directly using the dipole local pool set.
+      if (dipoleSet.newActuation) {
+        *id = IMTQ::START_ACTUATION_DIPOLE;
+      }
       communicationStep = CommunicationStep::GET_ENG_HK_DATA;
       break;
     }
@@ -404,6 +406,11 @@ ReturnValue_t ImtqHandler::initializeLocalDataPool(localpool::DataPool& localDat
   localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry<int16_t>({0}));
   localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<int16_t>({0}));
 
+  localDataPoolMap.emplace(IMTQ::DIPOLES_X, &dipoleXEntry);
+  localDataPoolMap.emplace(IMTQ::DIPOLES_Y, &dipoleYEntry);
+  localDataPoolMap.emplace(IMTQ::DIPOLES_Z, &dipoleZEntry);
+  localDataPoolMap.emplace(IMTQ::CURRENT_TORQUE_DURATION, &torqueDurationEntry);
+
   /** Entries of calibrated MTM measurement dataset */
   localDataPoolMap.emplace(IMTQ::MGM_CAL_NT, &mgmCalEntry);
   localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>({0}));
diff --git a/mission/devices/ImtqHandler.h b/mission/devices/ImtqHandler.h
index 401c6743..7ddb4e4c 100644
--- a/mission/devices/ImtqHandler.h
+++ b/mission/devices/ImtqHandler.h
@@ -110,6 +110,10 @@ class ImtqHandler : public DeviceHandlerBase {
   NormalPollingMode pollingMode = NormalPollingMode::BOTH;
 
   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);
diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h
index df9aa44e..3ec0aedd 100644
--- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h
+++ b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h
@@ -4,6 +4,8 @@
 #include <fsfw/datapool/PoolReadGuard.h>
 #include <fsfw/datapoollocal/StaticLocalDataSet.h>
 
+class ImtqHandler;
+
 namespace IMTQ {
 
 static const DeviceCommandId_t NONE = 0x0;
@@ -475,6 +477,8 @@ class CommandDipolePacket : public SerialLinkedListAdapter<SerializeIF> {
 };
 
 class DipoleActuationSet : public StaticLocalDataSet<4> {
+  friend class ::ImtqHandler;
+
  public:
   DipoleActuationSet(HasLocalDataPoolIF& owner)
       : StaticLocalDataSet(&owner, IMTQ::SetIds::DIPOLES) {}
@@ -499,15 +503,17 @@ class DipoleActuationSet : public StaticLocalDataSet<4> {
     zDipole = zDipole_;
     currentTorqueDurationMs = currentTorqueDurationMs_;
   }
+
+ private:
   lp_var_t<uint16_t> xDipole = lp_var_t<uint16_t>(sid.objectId, DIPOLES_X, this);
   lp_var_t<uint16_t> yDipole = lp_var_t<uint16_t>(sid.objectId, DIPOLES_Y, this);
   lp_var_t<uint16_t> zDipole = lp_var_t<uint16_t>(sid.objectId, DIPOLES_Z, this);
   lp_var_t<uint16_t> currentTorqueDurationMs =
       lp_var_t<uint16_t>(sid.objectId, CURRENT_TORQUE_DURATION, this);
 
- private:
   bool newActuation = false;
 };
+
 /**
  * @brief   This dataset can be used to store the self test results of the +X self test.
  *