old code seems to work

dummies/ImtqDummy.cpp

@@ -1,6 +1,6 @@
 #include "ImtqDummy.h"
 
-#include <mission/devices/devicedefinitions/IMTQHandlerDefinitions.h>
+#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
 
 ImtqDummy::ImtqDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
     : DeviceHandlerBase(objectId, comif, comCookie) {}

fsfw

@@ -1 +1 @@
-Subproject commit 692be9df8d06beb3bfc83aad77cefd727d8f7c35
+Subproject commit 0c5c2f6c4f07959a73a083eb3c6e1f3125642477

mission/controller/AcsController.cpp

@@ -2,6 +2,8 @@
 
 #include <fsfw/datapool/PoolReadGuard.h>
 
+#include "mission/devices/torquer.h"
+
 AcsController::AcsController(object_id_t objectId)
     : ExtendedControllerBase(objectId, objects::NO_OBJECT), mgmData(this) {}
 
@@ -29,6 +31,15 @@ void AcsController::performControlOperation() {
       break;
   }
 
+  {
+    // TODO: Calculate actuator output
+    PoolReadGuard pg(&dipoleSet);
+    MutexGuard mg(torquer::lazyLock());
+    torquer::NEW_ACTUATION_FLAG = true;
+    // TODO: Insert correct values here
+    dipoleSet.setDipoles(500, 500, 500, 150);
+  }
+
   {
     PoolReadGuard pg(&mgmData);
     if (pg.getReadResult() == returnvalue::OK) {

mission/controller/AcsController.h

@@ -7,8 +7,8 @@
 #include "eive/objects.h"
 #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
 #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
-#include "mission/devices/devicedefinitions/IMTQHandlerDefinitions.h"
 #include "mission/devices/devicedefinitions/SusDefinitions.h"
+#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
 
 class AcsController : public ExtendedControllerBase {
  public:

mission/controller/ThermalController.cpp

@@ -8,9 +8,9 @@
 #include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
 #include <mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h>
 #include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
-#include <mission/devices/devicedefinitions/IMTQHandlerDefinitions.h>
 #include <mission/devices/devicedefinitions/RwDefinitions.h>
 #include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
+#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
 #include <mission/devices/devicedefinitions/payloadPcduDefinitions.h>
 #include <objects/systemObjectList.h>
 

mission/devices/CMakeLists.txt

@@ -20,6 +20,7 @@ target_sources(
           SusHandler.cpp
           PayloadPcduHandler.cpp
           SolarArrayDeploymentHandler.cpp
-          ScexDeviceHandler.cpp)
+          ScexDeviceHandler.cpp
+          torquer.cpp)
 
 add_subdirectory(devicedefinitions)

mission/devices/ImtqHandler.cpp

@@ -25,9 +25,7 @@
 #include <cmath>
 #include <fsfw/datapoollocal/LocalPoolVariable.tpp>
 
-MutexIF* ImtqHandler::TORQUE_LOCK = nullptr;
+#include "mission/devices/torquer.h"
-bool ImtqHandler::TORQUEING = false;
-Countdown ImtqHandler::TORQUE_COUNTDOWN = Countdown();
 
 ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
                          power::Switch_t pwrSwitcher)
@@ -46,7 +44,6 @@ ImtqHandler::ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comC
   if (comCookie == nullptr) {
     sif::error << "IMTQHandler: Invalid com cookie" << std::endl;
   }
-  TORQUE_LOCK = MutexFactory::instance()->createMutex();
 }
 
 ImtqHandler::~ImtqHandler() {}
@@ -62,6 +59,7 @@ void ImtqHandler::doStartUp() {
 void ImtqHandler::doShutDown() { setMode(_MODE_POWER_DOWN); }
 
 ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
+  bool buildCommand = true;
   // Depending on the normal polling mode configuration, 3-4 communication steps are recommended
   switch (communicationStep) {
     case CommunicationStep::GET_ENG_HK_DATA:
@@ -99,9 +97,12 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
       // If the dipole is not commanded but set by the ACS control algorithm,
       // the dipoles will be set by the ACS controller directly using the dipole local pool set.
       // This set has a flag to determine whether the ACS controller actually set any new input.
-      PoolReadGuard pg(&dipoleSet);
+      MutexGuard mg(torquer::lazyLock());
-      if (dipoleSet.newActuation) {
+      if (torquer::NEW_ACTUATION_FLAG) {
         *id = IMTQ::START_ACTUATION_DIPOLE;
+        torquer::NEW_ACTUATION_FLAG = false;
+      } else {
+        buildCommand = false;
       }
       communicationStep = CommunicationStep::GET_ENG_HK_DATA;
       break;
@@ -111,8 +112,11 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
                  << std::endl;
       break;
   }
+  if (buildCommand) {
     return buildCommandFromCommand(*id, nullptr, 0);
   }
+  return NOTHING_TO_SEND;
+}
 
 ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
   return NOTHING_TO_SEND;
@@ -176,23 +180,27 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
       if (commandData != nullptr && commandDataLen < 8) {
         return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
       }
-      PoolReadGuard pg(&dipoleSet);
       ReturnValue_t result;
       // Commands override anything which was set in the software
       if (commandBuffer != nullptr) {
+        dipoleSet.setValidityBufferGeneration(false);
         result =
             dipoleSet.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::NETWORK);
+        dipoleSet.setValidityBufferGeneration(true);
         if (result != returnvalue::OK) {
           return result;
         }
+      } else {
+        // Read set dipole values from local pool
+        PoolReadGuard pg(&dipoleSet);
       }
       result = buildDipoleActuationCommand();
       if (result != returnvalue::OK) {
         return result;
       }
-      MutexGuard mg(TORQUE_LOCK);
+      MutexGuard mg(torquer::lazyLock());
-      TORQUEING = true;
+      torquer::TORQUEING = true;
-      TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
+      torquer::TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
       return result;
     }
     case (IMTQ::GET_ENG_HK_DATA): {
@@ -249,25 +257,21 @@ ReturnValue_t ImtqHandler::buildDipoleActuationCommand() {
 }
 
 void ImtqHandler::fillCommandAndReplyMap() {
-  this->insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  insertInCommandAndReplyMap(IMTQ::POS_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  insertInCommandAndReplyMap(IMTQ::NEG_X_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  insertInCommandAndReplyMap(IMTQ::POS_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  insertInCommandAndReplyMap(IMTQ::NEG_Y_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  insertInCommandAndReplyMap(IMTQ::POS_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
+  insertInCommandAndReplyMap(IMTQ::NEG_Z_SELF_TEST, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr,
+  insertInCommandAndReplyMap(IMTQ::GET_SELF_TEST_RESULT, 1, nullptr, IMTQ::SIZE_SELF_TEST_RESULTS);
-                                   IMTQ::SIZE_SELF_TEST_RESULTS);
+  insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::START_ACTUATION_DIPOLE, 1, nullptr,
+  insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset, IMTQ::SIZE_ENG_HK_DATA_REPLY);
-                                   IMTQ::SIZE_STATUS_REPLY);
+  insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr,
-  this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset,
-                                   IMTQ::SIZE_ENG_HK_DATA_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr,
                              IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY);
-  this->insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr,
+  insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr, IMTQ::SIZE_STATUS_REPLY);
-                                   IMTQ::SIZE_STATUS_REPLY);
+  insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet,
-  this->insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet,
                              IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT);
-  this->insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet,
+  insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet,
                              IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT);
 }
 
@@ -2315,11 +2319,3 @@ ReturnValue_t ImtqHandler::getSwitches(const uint8_t** switches, uint8_t* number
   }
   return DeviceHandlerBase::NO_SWITCH;
 }
-
-bool ImtqHandler::mgtIsTorqueing(dur_millis_t* remainingTorqueDuration) {
-  MutexGuard mg(TORQUE_LOCK);
-  if (TORQUEING and remainingTorqueDuration != nullptr) {
-    *remainingTorqueDuration = TORQUE_COUNTDOWN.getRemainingMillis() + TORQUE_BUFFER_TIME_MS;
-  }
-  return TORQUEING;
-}

mission/devices/ImtqHandler.h

@@ -2,7 +2,7 @@
 #define MISSION_DEVICES_IMTQHANDLER_H_
 
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
-#include <mission/devices/devicedefinitions/IMTQHandlerDefinitions.h>
+#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
 #include <string.h>
 
 #include "events/subsystemIdRanges.h"
@@ -107,7 +107,7 @@ class ImtqHandler : public DeviceHandlerBase {
   IMTQ::PosZSelfTestSet posZselfTestDataset;
   IMTQ::NegZSelfTestSet negZselfTestDataset;
 
-  NormalPollingMode pollingMode = NormalPollingMode::BOTH;
+  NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED;
 
   PoolEntry<int32_t> mgmCalEntry = PoolEntry<int32_t>(3);
   PoolEntry<int16_t> dipoleXEntry = PoolEntry<int16_t>(0, false);
@@ -221,15 +221,6 @@ class ImtqHandler : public DeviceHandlerBase {
   void checkErrorByte(const uint8_t errorByte, const uint8_t step);
 
   std::string makeStepString(const uint8_t step);
-
-  static MutexIF* TORQUE_LOCK;
-  static bool TORQUEING;
-  static Countdown TORQUE_COUNTDOWN;
-  // Additional buffer time to accont for time until I2C command arrives and ramp up / ramp down
-  // time of the MGT
-  static constexpr dur_millis_t TORQUE_BUFFER_TIME_MS = 20;
-
-  static bool mgtIsTorqueing(dur_millis_t* remainingTorqueDuration);
 };
 
 #endif /* MISSION_DEVICES_IMTQHANDLER_H_ */

mission/devices/devicedefinitions/imtqHandlerDefinitions.h

@@ -486,24 +486,17 @@ class DipoleActuationSet : public StaticLocalDataSet<4> {
       : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::DIPOLES)) {}
 
   // Refresh torque command without changing any of the set dipoles.
-  void refreshTorqueing(uint16_t durationMs_) {
+  void refreshTorqueing(uint16_t durationMs_) { currentTorqueDurationMs = durationMs_; }
-    newActuation = true;
-    currentTorqueDurationMs = durationMs_;
-  }
 
   void setDipoles(uint16_t xDipole_, uint16_t yDipole_, uint16_t zDipole_,
                   uint16_t currentTorqueDurationMs_) {
-    newActuation = false;
     if (xDipole.value != xDipole_) {
-      newActuation = true;
     }
     xDipole = xDipole_;
     if (yDipole.value != yDipole_) {
-      newActuation = true;
     }
     yDipole = yDipole_;
     if (zDipole.value != zDipole_) {
-      newActuation = true;
     }
     zDipole = zDipole_;
     currentTorqueDurationMs = currentTorqueDurationMs_;
@@ -516,13 +509,11 @@ class DipoleActuationSet : public StaticLocalDataSet<4> {
   }
 
  private:
-  lp_var_t<uint16_t> xDipole = lp_var_t<uint16_t>(sid.objectId, DIPOLES_X, this);
+  lp_var_t<int16_t> xDipole = lp_var_t<int16_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<int16_t> yDipole = lp_var_t<int16_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<int16_t> zDipole = lp_var_t<int16_t>(sid.objectId, DIPOLES_Z, this);
   lp_var_t<uint16_t> currentTorqueDurationMs =
       lp_var_t<uint16_t>(sid.objectId, CURRENT_TORQUE_DURATION, this);
-
-  bool newActuation = false;
 };
 
 /**

mission/devices/torquer.cpp

@@ -0,0 +1,27 @@
+#include "torquer.h"
+
+#include <fsfw/ipc/MutexGuard.h>
+
+MutexIF* TORQUE_LOCK = nullptr;
+
+namespace torquer {
+
+bool TORQUEING = false;
+bool NEW_ACTUATION_FLAG = false;
+Countdown TORQUE_COUNTDOWN = Countdown();
+
+bool mgtIsTorqueing(dur_millis_t* remainingTorqueDuration) {
+  if (TORQUEING and remainingTorqueDuration != nullptr) {
+    *remainingTorqueDuration = TORQUE_COUNTDOWN.getRemainingMillis() + TORQUE_BUFFER_TIME_MS;
+  }
+  return TORQUEING;
+}
+
+MutexIF* lazyLock() {
+  if (TORQUE_LOCK == nullptr) {
+    TORQUE_LOCK = MutexFactory::instance()->createMutex();
+  }
+  return TORQUE_LOCK;
+}
+
+}  // namespace torquer

mission/devices/torquer.h

@@ -0,0 +1,22 @@
+#ifndef MISSION_DEVICES_TOQUER_H_
+#define MISSION_DEVICES_TOQUER_H_
+
+#include <fsfw/ipc/MutexIF.h>
+#include <fsfw/timemanager/Countdown.h>
+
+namespace torquer {
+
+// Additional buffer time to accont for time until I2C command arrives and ramp up / ramp down
+// time of the MGT
+static constexpr dur_millis_t TORQUE_BUFFER_TIME_MS = 20;
+
+MutexIF* lazyLock();
+extern bool TORQUEING;
+extern bool NEW_ACTUATION_FLAG;
+extern Countdown TORQUE_COUNTDOWN;
+
+bool mgtIsTorqueing(dur_millis_t* remainingTorqueDuration);
+
+}  // namespace torquer
+
+#endif /* MISSION_DEVICES_TOQUER_H_ */

tmtc

@@ -1 +1 @@
-Subproject commit 42b962dede547975488e72b22abe2360065c0404
+Subproject commit 1dfc2fca2f58f8d226fab01c87eb529ba7ec8376