bsp_q7s/core/ObjectFactory.cpp

@@ -47,6 +47,8 @@
 #if OBSW_TEST_LIBGPIOD == 1
 #include "linux/boardtest/LibgpiodTest.h"
 #endif
+#include <mission/devices/ImtqHandler.h>
+
 #include <sstream>
 
 #include "fsfw/datapoollocal/LocalDataPoolManager.h"
@@ -70,7 +72,6 @@
 #include "mission/devices/BpxBatteryHandler.h"
 #include "mission/devices/GyroADIS1650XHandler.h"
 #include "mission/devices/HeaterHandler.h"
-#include <mission/devices/ImtqHandler.h>
 #include "mission/devices/Max31865PT1000Handler.h"
 #include "mission/devices/P60DockHandler.h"
 #include "mission/devices/PCDUHandler.h"

mission/controller/AcsController.h

@@ -71,8 +71,6 @@ class AcsController : public ExtendedControllerBase {
       SUS::SusDataset(objects::SUS_11_R_LOC_XBYMZB_PT_ZB),
   };
 
-
-
   // Initial delay to make sure all pool variables have been initialized their owners
   Countdown initialCountdown = Countdown(INIT_DELAY);
 };

mission/devices/ImtqHandler.cpp

@@ -1,11 +1,29 @@
+#include <bits/stdint-intn.h>
+#include <commonConfig.h>
+#include <fsfw/datapool/PoolEntry.h>
 #include <fsfw/datapool/PoolReadGuard.h>
-#include <fsfw/globalfunctions/CRC.h>
+#include <fsfw/datapoollocal/LocalDataPoolManager.h>
+#include <fsfw/datapoollocal/LocalPoolVariable.h>
+#include <fsfw/datapoollocal/ProvidesDataPoolSubscriptionIF.h>
+#include <fsfw/datapoollocal/localPoolDefinitions.h>
+#include <fsfw/devicehandlers/DeviceHandlerIF.h>
+#include <fsfw/ipc/MutexFactory.h>
+#include <fsfw/ipc/MutexGuard.h>
+#include <fsfw/ipc/messageQueueDefinitions.h>
+#include <fsfw/modes/ModeMessage.h>
+#include <fsfw/objectmanager/SystemObjectIF.h>
+#include <fsfw/power/definitions.h>
+#include <fsfw/returnvalues/returnvalue.h>
 #include <fsfw/serialize/SerializeAdapter.h>
+#include <fsfw/serialize/SerializeIF.h>
+#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
+#include <fsfw/tasks/TaskFactory.h>
+#include <fsfw/timemanager/Countdown.h>
+#include <fsfw/timemanager/clockDefinitions.h>
 #include <mission/devices/ImtqHandler.h>
 
 #include <cmath>
-
+#include <fsfw/datapoollocal/LocalPoolVariable.tpp>
-#include "OBSWConfig.h"
 
 MutexIF* ImtqHandler::TORQUE_LOCK = nullptr;
 bool ImtqHandler::TORQUEING = false;
@@ -56,22 +74,43 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
       break;
     case CommunicationStep::START_MTM_MEASUREMENT:
       *id = IMTQ::START_MTM_MEASUREMENT;
-      communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT;
+      if (pollingMode == NormalPollingMode::BOTH or
-      break;
+          pollingMode == NormalPollingMode::UNCALIBRATED) {
-    case CommunicationStep::GET_CAL_MTM_MEASUREMENT:
-      *id = IMTQ::GET_CAL_MTM_MEASUREMENT;
         communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT;
+      } else {
+        communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT;
+      }
       break;
     case CommunicationStep::GET_RAW_MTM_MEASUREMENT:
+      if (integrationTimeCd.getRemainingMillis() > 0) {
+        TaskFactory::delayTask(integrationTimeCd.getRemainingMillis());
+      }
       *id = IMTQ::GET_RAW_MTM_MEASUREMENT;
+      if (pollingMode == NormalPollingMode::BOTH) {
+        communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT;
+      } else {
+        communicationStep = CommunicationStep::DIPOLE_ACTUATION;
+      }
+      break;
+    case CommunicationStep::GET_CAL_MTM_MEASUREMENT:
+      if (integrationTimeCd.getRemainingMillis() > 0) {
+        TaskFactory::delayTask(integrationTimeCd.getRemainingMillis());
+      }
+      *id = IMTQ::GET_CAL_MTM_MEASUREMENT;
+      communicationStep = CommunicationStep::DIPOLE_ACTUATION;
+      break;
+    case CommunicationStep::DIPOLE_ACTUATION: {
+      // TODO: Set correct ID if actuation is necessary
+      *id = IMTQ::START_ACTUATION_DIPOLE;
       communicationStep = CommunicationStep::GET_ENG_HK_DATA;
       break;
+    }
     default:
       sif::debug << "IMTQHandler::buildNormalDeviceCommand: Invalid communication step"
                  << std::endl;
       break;
   }
-  return buildCommandFromCommand(*id, NULL, 0);
+  return buildCommandFromCommand(*id, nullptr, 0);
 }
 
 ReturnValue_t ImtqHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
@@ -133,20 +172,27 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
     case (IMTQ::START_ACTUATION_DIPOLE): {
       /* IMTQ expects low byte first */
       commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
-      if (commandData == nullptr) {
+      if (commandData != nullptr && commandDataLen < 8) {
         return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
       }
-      commandBuffer[1] = commandData[1];
+      PoolReadGuard pg(&dipoleSet);
-      commandBuffer[2] = commandData[0];
+      ReturnValue_t result;
-      commandBuffer[3] = commandData[3];
+      // Commands override anything which was set in the software
-      commandBuffer[4] = commandData[2];
+      if (commandBuffer != nullptr) {
-      commandBuffer[5] = commandData[5];
+        result =
-      commandBuffer[6] = commandData[4];
+            dipoleSet.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::NETWORK);
-      commandBuffer[7] = commandData[7];
+        if (result != returnvalue::OK) {
-      commandBuffer[8] = commandData[6];
+          return result;
-      rawPacket = commandBuffer;
+        }
-      rawPacketLen = 9;
+      }
-      return returnvalue::OK;
+      result = buildDipoleActuationCommand();
+      if (result != returnvalue::OK) {
+        return result;
+      }
+      MutexGuard mg(TORQUE_LOCK);
+      TORQUEING = true;
+      TORQUE_COUNTDOWN.setTimeout(dipoleSet.currentTorqueDurationMs.value);
+      return result;
     }
     case (IMTQ::GET_ENG_HK_DATA): {
       commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA;
@@ -162,6 +208,7 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
     }
     case (IMTQ::START_MTM_MEASUREMENT): {
       commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT;
+      integrationTimeCd.resetTimer();
       rawPacket = commandBuffer;
       rawPacketLen = 1;
       return returnvalue::OK;
@@ -184,6 +231,22 @@ ReturnValue_t ImtqHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
   return returnvalue::FAILED;
 }
 
+ReturnValue_t ImtqHandler::buildDipoleActuationCommand() {
+  commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
+  uint8_t* serPtr = commandBuffer + 1;
+  size_t serSize = 1;
+  dipoleSet.setValidityBufferGeneration(false);
+  ReturnValue_t result = dipoleSet.serialize(&serPtr, &serSize, sizeof(commandBuffer),
+                                             SerializeIF::Endianness::LITTLE);
+  dipoleSet.setValidityBufferGeneration(true);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  rawPacket = commandBuffer;
+  rawPacketLen = 9;
+  return result;
+}
+
 void ImtqHandler::fillCommandAndReplyMap() {
   this->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);
@@ -2247,7 +2310,7 @@ ReturnValue_t ImtqHandler::getSwitches(const uint8_t** switches, uint8_t* number
   return DeviceHandlerBase::NO_SWITCH;
 }
 
-bool ImtqHandler::mgtIsTorqueing(dur_millis_t *remainingTorqueDuration) {
+bool ImtqHandler::mgtIsTorqueing(dur_millis_t* remainingTorqueDuration) {
   MutexGuard mg(TORQUE_LOCK);
   if (TORQUEING and remainingTorqueDuration != nullptr) {
     *remainingTorqueDuration = TORQUE_COUNTDOWN.getRemainingMillis() + TORQUE_BUFFER_TIME_MS;

mission/devices/ImtqHandler.h

@@ -15,10 +15,14 @@
  */
 class ImtqHandler : public DeviceHandlerBase {
  public:
+  enum NormalPollingMode { UNCALIBRATED = 0, CALIBRATED = 1, BOTH = 2 };
+
   ImtqHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
               power::Switch_t pwrSwitcher);
   virtual ~ImtqHandler();
 
+  void setPollingMode(NormalPollingMode pollMode);
+
   /**
    * @brief   Sets mode to MODE_NORMAL. Can be used for debugging.
    */
@@ -103,7 +107,12 @@ class ImtqHandler : public DeviceHandlerBase {
   IMTQ::PosZSelfTestSet posZselfTestDataset;
   IMTQ::NegZSelfTestSet negZselfTestDataset;
 
+  NormalPollingMode pollingMode = NormalPollingMode::BOTH;
+
   PoolEntry<int32_t> mgmCalEntry = PoolEntry<int32_t>(3);
+  // 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;
 
@@ -115,7 +124,8 @@ class ImtqHandler : public DeviceHandlerBase {
     GET_ENG_HK_DATA,
     START_MTM_MEASUREMENT,
     GET_CAL_MTM_MEASUREMENT,
-    GET_RAW_MTM_MEASUREMENT
+    GET_RAW_MTM_MEASUREMENT,
+    DIPOLE_ACTUATION
   };
 
   CommunicationStep communicationStep = CommunicationStep::GET_ENG_HK_DATA;
@@ -197,6 +207,7 @@ class ImtqHandler : public DeviceHandlerBase {
   void handlePositiveZSelfTestReply(const uint8_t* packet);
   void handleNegativeZSelfTestReply(const uint8_t* packet);
 
+  ReturnValue_t buildDipoleActuationCommand();
   /**
    * @brief   This function checks the error byte of a self test measurement.
    *
@@ -215,7 +226,6 @@ class ImtqHandler : public DeviceHandlerBase {
   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

@@ -448,6 +448,11 @@ 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
@@ -467,10 +472,6 @@ class CommandDipolePacket : public SerialLinkedListAdapter<SerializeIF> {
     yDipole.setNext(&zDipole);
     zDipole.setNext(&duration);
   }
-  SerializeElement<uint16_t> xDipole;
-  SerializeElement<uint16_t> yDipole;
-  SerializeElement<uint16_t> zDipole;
-  SerializeElement<uint16_t> duration;
 };
 
 class DipoleActuationSet : public StaticLocalDataSet<4> {
@@ -498,13 +499,13 @@ 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;
 };
 /**