new MGM polling

bsp_q7s/core/ObjectFactory.cpp

@@ -5,6 +5,8 @@
 #include <linux/devices/ImtqPollingTask.h>
 #include <linux/devices/RwPollingTask.h>
 #include <mission/devices/GyrL3gCustomHandler.h>
+#include <mission/devices/MgmLis3CustomHandler.h>
+#include <mission/devices/MgmRm3100CustomHandler.h>
 #include <mission/system/objects/CamSwitcher.h>
 
 #include "OBSWConfig.h"
@@ -352,7 +354,7 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF*
       new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, mgmLis3::MAX_BUFFER_SIZE,
                     spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
-  auto mgmLis3Handler0 = new MgmLIS3MDLHandler(
+  auto mgmLis3Handler0 = new MgmLis3CustomHandler(
       objects::MGM_0_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY);
   fdir = new AcsBoardFdir(objects::MGM_0_LIS3_HANDLER);
   mgmLis3Handler0->setCustomFdir(fdir);
@@ -369,8 +371,8 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF*
                     spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
   auto mgmRm3100Handler1 =
-      new MgmRM3100Handler(objects::MGM_1_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie,
-                           spi::RM3100_TRANSITION_DELAY);
+      new MgmRm3100CustomHandler(objects::MGM_1_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie,
+                                 spi::RM3100_TRANSITION_DELAY);
   fdir = new AcsBoardFdir(objects::MGM_1_RM3100_HANDLER);
   mgmRm3100Handler1->setCustomFdir(fdir);
   assemblyChildren[1] = mgmRm3100Handler1;
@@ -384,7 +386,7 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF*
   spiCookie = new SpiCookie(addresses::MGM_2_LIS3, gpioIds::MGM_2_LIS3_CS, mgmLis3::MAX_BUFFER_SIZE,
                             spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
-  auto* mgmLis3Handler2 = new MgmLIS3MDLHandler(
+  auto* mgmLis3Handler2 = new MgmLis3CustomHandler(
       objects::MGM_2_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY);
   fdir = new AcsBoardFdir(objects::MGM_2_LIS3_HANDLER);
   mgmLis3Handler2->setCustomFdir(fdir);
@@ -401,8 +403,8 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF*
                     spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
   auto* mgmRm3100Handler3 =
-      new MgmRM3100Handler(objects::MGM_3_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie,
-                           spi::RM3100_TRANSITION_DELAY);
+      new MgmRm3100CustomHandler(objects::MGM_3_RM3100_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie,
+                                 spi::RM3100_TRANSITION_DELAY);
   fdir = new AcsBoardFdir(objects::MGM_3_RM3100_HANDLER);
   mgmRm3100Handler3->setCustomFdir(fdir);
   assemblyChildren[3] = mgmRm3100Handler3;

mission/core/pollingSeqTables.cpp

@@ -491,9 +491,9 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                             DeviceHandlerIF::SEND_WRITE);
       thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::GET_READ);
 
       thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
@@ -505,9 +505,9 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
       thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
                             length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
       thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
+                            length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
       thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
+                            length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
     }
     if (enableBside) {
       // B side
@@ -517,9 +517,9 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                             DeviceHandlerIF::SEND_WRITE);
       thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
                             DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
                             DeviceHandlerIF::GET_READ);
 
       thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
@@ -531,9 +531,9 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
       thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
                             length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
       thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
+                            length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
       thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
+                            length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
     }
     if (enableAside) {
       thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,

mission/devices/CMakeLists.txt

@@ -16,6 +16,8 @@ target_sources(
           RadiationSensorHandler.cpp
           GyrAdis1650XHandler.cpp
           GyrL3gCustomHandler.cpp
+          MgmRm3100CustomHandler.cpp
+          MgmLis3CustomHandler.cpp
           RwHandler.cpp
           max1227.cpp
           SusHandler.cpp

mission/devices/GyrAdis1650XHandler.cpp

@@ -15,6 +15,10 @@ GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t devic
 }
 
 void GyrAdis1650XHandler::doStartUp() {
+  if (internalState != InternalState::STARTUP) {
+    internalState = InternalState::STARTUP;
+    commandExecuted = false;
+  }
   // Initial 310 ms start up time after power-up
   if (internalState == InternalState::STARTUP) {
     if (not commandExecuted) {
@@ -29,19 +33,9 @@ void GyrAdis1650XHandler::doStartUp() {
       } else {
         setMode(MODE_ON);
       }
+      internalState = InternalState::NONE;
     }
   }
-
-  //  // Read all configuration registers first
-  //  if (internalState == InternalState::CONFIG) {
-  //    if (commandExecuted) {
-  //      commandExecuted = false;
-  //      internalState = InternalState::IDLE;
-  //    }
-  //  }
-  //
-  //  if (internalState == InternalState::IDLE) {
-  //  }
 }
 
 void GyrAdis1650XHandler::doShutDown() {
@@ -50,8 +44,10 @@ void GyrAdis1650XHandler::doShutDown() {
     primaryDataset.setValidity(false, true);
     internalState = InternalState::SHUTDOWN;
   }
-  if (commandExecuted) {
+  if (internalState == InternalState::SHUTDOWN and commandExecuted) {
     updatePeriodicReply(false, adis1650x::REPLY);
+    internalState = InternalState::NONE;
+    commandExecuted = false;
     setMode(_MODE_POWER_DOWN);
   }
 }

mission/devices/GyrAdis1650XHandler.h

@@ -50,7 +50,7 @@ class GyrAdis1650XHandler : public DeviceHandlerBase {
   bool goToNormalMode = false;
   bool warningSwitch = true;
 
-  enum class InternalState { STARTUP, SHUTDOWN, IDLE };
+  enum class InternalState { NONE, STARTUP, SHUTDOWN };
 
   InternalState internalState = InternalState::STARTUP;
   bool commandExecuted = false;

mission/devices/GyrL3gCustomHandler.cpp

@@ -15,7 +15,7 @@ GyrL3gCustomHandler::GyrL3gCustomHandler(object_id_t objectId, object_id_t devic
 GyrL3gCustomHandler::~GyrL3gCustomHandler() = default;
 
 void GyrL3gCustomHandler::doStartUp() {
-  if (internalState == InternalState::NONE) {
+  if (internalState != InternalState::STARTUP) {
     internalState = InternalState::STARTUP;
     updatePeriodicReply(true, l3gd20h::REPLY);
     commandExecuted = false;
@@ -28,6 +28,8 @@ void GyrL3gCustomHandler::doStartUp() {
       } else {
         setMode(_MODE_TO_ON);
       }
+      internalState == InternalState::NONE;
+      commandExecuted = false;
     }
   }
 }
@@ -37,7 +39,7 @@ void GyrL3gCustomHandler::doShutDown() {
     internalState = InternalState::SHUTDOWN;
     commandExecuted = false;
   }
-  if (commandExecuted) {
+  if (internalState == InternalState::SHUTDOWN and commandExecuted) {
     internalState = InternalState::NONE;
     updatePeriodicReply(false, l3gd20h::REPLY);
     commandExecuted = false;

mission/devices/GyrL3gCustomHandler.h

@@ -77,7 +77,6 @@ class GyrL3gCustomHandler : public DeviceHandlerBase {
   uint8_t ctrlReg4Value = l3gd20h::CTRL_REG_4_VAL;
   uint8_t ctrlReg5Value = l3gd20h::CTRL_REG_5_VAL;
 
-  std::array<uint8_t, 32> cmdBuf;
   acs::GyroL3gRequest gyrRequest{};
 
   // Set default value

mission/devices/MgmLis3CustomHandler.cpp

@@ -0,0 +1,146 @@
+#include <mission/devices/MgmLis3CustomHandler.h>
+
+#include <cmath>
+
+#include "fsfw/datapool/PoolReadGuard.h"
+
+MgmLis3CustomHandler::MgmLis3CustomHandler(object_id_t objectId, object_id_t deviceCommunication,
+                                           CookieIF *comCookie, uint32_t transitionDelay)
+    : DeviceHandlerBase(objectId, deviceCommunication, comCookie),
+      dataset(this),
+      transitionDelay(transitionDelay) {}
+
+MgmLis3CustomHandler::~MgmLis3CustomHandler() = default;
+
+void MgmLis3CustomHandler::doStartUp() {
+  if (internalState != InternalState::STARTUP) {
+    commandExecuted = false;
+    updatePeriodicReply(true, REPLY);
+    internalState = InternalState::STARTUP;
+  }
+  if (internalState == InternalState::STARTUP) {
+    if (commandExecuted) {
+      setMode(MODE_NORMAL);
+      internalState = InternalState::NONE;
+      commandExecuted = false;
+    }
+  }
+}
+
+void MgmLis3CustomHandler::doShutDown() {
+  if (internalState != InternalState::SHUTDOWN) {
+    internalState = InternalState::SHUTDOWN;
+    commandExecuted = false;
+  }
+  if (internalState == InternalState::SHUTDOWN and commandExecuted) {
+    updatePeriodicReply(false, REPLY);
+    commandExecuted = false;
+    internalState = InternalState::NONE;
+    setMode(_MODE_POWER_DOWN);
+  }
+}
+
+ReturnValue_t MgmLis3CustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
+  if (internalState == InternalState::STARTUP) {
+    *id = REQUEST;
+    return prepareRequest(acs::SimpleSensorMode::NORMAL);
+  } else if (internalState == InternalState::SHUTDOWN) {
+    *id = REQUEST;
+    return prepareRequest(acs::SimpleSensorMode::OFF);
+  }
+  return NOTHING_TO_SEND;
+}
+
+ReturnValue_t MgmLis3CustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
+  *id = REQUEST;
+  return prepareRequest(acs::SimpleSensorMode::NORMAL);
+}
+
+ReturnValue_t MgmLis3CustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
+                                                            const uint8_t *commandData,
+                                                            size_t commandDataLen) {
+  return NOTHING_TO_SEND;
+}
+
+ReturnValue_t MgmLis3CustomHandler::scanForReply(const uint8_t *start, size_t len,
+                                                 DeviceCommandId_t *foundId, size_t *foundLen) {
+  if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) {
+    return IGNORE_FULL_PACKET;
+  }
+  if (len != sizeof(acs::MgmRm3100Reply)) {
+    *foundLen = len;
+    return returnvalue::FAILED;
+  }
+  *foundId = REPLY;
+  *foundLen = len;
+  return returnvalue::OK;
+}
+ReturnValue_t MgmLis3CustomHandler::interpretDeviceReply(DeviceCommandId_t id,
+                                                         const uint8_t *packet) {
+  const auto *reply = reinterpret_cast<const acs::MgmLis3Reply *>(packet);
+  if (reply->dataWasSet) {
+    if (internalState == InternalState::STARTUP) {
+      commandExecuted = true;
+    }
+    PoolReadGuard pg(&dataset);
+    for (uint8_t idx = 0; idx < 3; idx++) {
+      dataset.fieldStrengths[idx] =
+          reply->mgmValuesRaw[idx] * reply->sensitivity * mgmLis3::GAUSS_TO_MICROTESLA_FACTOR;
+    }
+
+    dataset.setValidity(true, true);
+    if (std::abs(dataset.fieldStrengths[0]) > absLimitX or
+        std::abs(dataset.fieldStrengths[1]) > absLimitY or
+        std::abs(dataset.fieldStrengths[2]) > absLimitZ) {
+      dataset.fieldStrengths.setValid(false);
+    }
+    dataset.temperature = 25.0 + ((static_cast<float>(reply->temperatureRaw)) / 8.0);
+  }
+  return returnvalue::OK;
+}
+
+void MgmLis3CustomHandler::fillCommandAndReplyMap() {
+  insertInCommandMap(REQUEST);
+  insertInReplyMap(REPLY, 5, nullptr, 0, true);
+}
+
+void MgmLis3CustomHandler::setToGoToNormalMode(bool enable) { this->goToNormalMode = enable; }
+
+uint32_t MgmLis3CustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
+  return transitionDelay;
+}
+
+void MgmLis3CustomHandler::modeChanged(void) { internalState = InternalState::NONE; }
+
+ReturnValue_t MgmLis3CustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+                                                            LocalDataPoolManager &poolManager) {
+  localDataPoolMap.emplace(mgmLis3::FIELD_STRENGTHS, &mgmXYZ);
+  localDataPoolMap.emplace(mgmLis3::TEMPERATURE_CELCIUS, &temperature);
+  poolManager.subscribeForRegularPeriodicPacket({dataset.getSid(), false, 10.0});
+  return returnvalue::OK;
+}
+
+void MgmLis3CustomHandler::setAbsoluteLimits(float xLimit, float yLimit, float zLimit) {
+  this->absLimitX = xLimit;
+  this->absLimitY = yLimit;
+  this->absLimitZ = zLimit;
+}
+
+void MgmLis3CustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
+  periodicPrintout = enable;
+  debugDivider.setDivider(divider);
+}
+
+ReturnValue_t MgmLis3CustomHandler::prepareRequest(acs::SimpleSensorMode mode) {
+  request.mode = mode;
+  rawPacket = reinterpret_cast<uint8_t *>(&request);
+  rawPacketLen = sizeof(acs::MgmLis3Request);
+  return returnvalue::OK;
+}
+
+LocalPoolDataSetBase *MgmLis3CustomHandler::getDataSetHandle(sid_t sid) {
+  if (sid == dataset.getSid()) {
+    return &dataset;
+  }
+  return nullptr;
+}

mission/devices/MgmLis3CustomHandler.h

@@ -0,0 +1,105 @@
+#ifndef MISSION_DEVICES_MGMLIS3CUSTOMHANDLER_H_
+#define MISSION_DEVICES_MGMLIS3CUSTOMHANDLER_H_
+
+#include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
+
+#include "fsfw/devicehandlers/DeviceHandlerBase.h"
+#include "fsfw/globalfunctions/PeriodicOperationDivider.h"
+#include "mission/devices/devicedefinitions/acsPolling.h"
+
+class PeriodicOperationDivider;
+
+/**
+ * @brief   Device handler object for the LIS3MDL 3-axis magnetometer
+ *          by STMicroeletronics
+ * @details
+ * Datasheet can be found online by googling LIS3MDL.
+ * Flight manual:
+ * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/LIS3MDL_MGM
+ * @author  L. Loidold, R. Mueller
+ */
+class MgmLis3CustomHandler : public DeviceHandlerBase {
+ public:
+  static constexpr DeviceCommandId_t REQUEST = 0x70;
+  static constexpr DeviceCommandId_t REPLY = 0x77;
+
+  static const uint8_t INTERFACE_ID = CLASS_ID::MGM_LIS3MDL;
+  static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::MGM_LIS3MDL;
+  // Notifies a command to change the setup parameters
+  static const Event CHANGE_OF_SETUP_PARAMETER = MAKE_EVENT(0, severity::LOW);
+
+  MgmLis3CustomHandler(uint32_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
+                       uint32_t transitionDelay);
+  virtual ~MgmLis3CustomHandler();
+
+  void enablePeriodicPrintouts(bool enable, uint8_t divider);
+  /**
+   * Set the absolute limit for the values on the axis in microtesla. The dataset values will
+   * be marked as invalid if that limit is exceeded
+   * @param xLimit
+   * @param yLimit
+   * @param zLimit
+   */
+  void setAbsoluteLimits(float xLimit, float yLimit, float zLimit);
+  void setToGoToNormalMode(bool enable);
+
+ protected:
+  /** DeviceHandlerBase overrides */
+  void doShutDown() override;
+  void doStartUp() override;
+  virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override;
+  ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData,
+                                        size_t commandDataLen) override;
+  ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override;
+  ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override;
+  ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId,
+                             size_t *foundLen) override;
+  /**
+   * This implementation is tailored towards space applications and will flag values larger
+   * than 100 microtesla on X,Y and 150 microtesla on Z as invalid
+   * @param id
+   * @param packet
+   * @return
+   */
+  virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override;
+  void fillCommandAndReplyMap() override;
+  void modeChanged(void) override;
+  ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+                                        LocalDataPoolManager &poolManager) override;
+  LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override;
+
+ private:
+  mgmLis3::MgmPrimaryDataset dataset;
+  acs::MgmLis3Request request;
+
+  uint32_t transitionDelay;
+
+  float absLimitX = 100;
+  float absLimitY = 100;
+  float absLimitZ = 150;
+
+  uint8_t statusRegister = 0;
+  bool goToNormalMode = false;
+
+  enum class InternalState {
+    NONE,
+    STARTUP,
+    SHUTDOWN,
+  };
+
+  InternalState internalState = InternalState::NONE;
+  bool commandExecuted = false;
+
+  PoolEntry<float> mgmXYZ = PoolEntry<float>(3);
+  PoolEntry<float> temperature = PoolEntry<float>();
+  /*------------------------------------------------------------------------*/
+  /* Device specific commands and variables */
+  /*------------------------------------------------------------------------*/
+
+  bool periodicPrintout = false;
+  PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3);
+
+  ReturnValue_t prepareRequest(acs::SimpleSensorMode mode);
+};
+
+#endif /* MISSION_DEVICES_MGMLIS3CUSTOMHANDLER_H_ */

mission/devices/MgmRm3100CustomHandler.cpp

@@ -0,0 +1,127 @@
+#include <mission/devices/MgmRm3100CustomHandler.h>
+
+#include "fsfw/datapool/PoolReadGuard.h"
+#include "fsfw/devicehandlers/DeviceHandlerMessage.h"
+#include "fsfw/globalfunctions/bitutility.h"
+#include "fsfw/objectmanager/SystemObjectIF.h"
+#include "fsfw/returnvalues/returnvalue.h"
+
+MgmRm3100CustomHandler::MgmRm3100CustomHandler(object_id_t objectId,
+                                               object_id_t deviceCommunication, CookieIF *comCookie,
+                                               uint32_t transitionDelay)
+    : DeviceHandlerBase(objectId, deviceCommunication, comCookie),
+      primaryDataset(this),
+      transitionDelay(transitionDelay) {}
+
+MgmRm3100CustomHandler::~MgmRm3100CustomHandler() = default;
+
+void MgmRm3100CustomHandler::doStartUp() {
+  if (internalState != InternalState::STARTUP) {
+    commandExecuted = false;
+    updatePeriodicReply(true, REPLY);
+    internalState = InternalState::STARTUP;
+  }
+  if (internalState == InternalState::STARTUP) {
+    if (commandExecuted) {
+      commandExecuted = false;
+      internalState = InternalState::NONE;
+      setMode(MODE_NORMAL);
+    }
+  }
+}
+
+void MgmRm3100CustomHandler::doShutDown() {
+  if (internalState != InternalState::SHUTDOWN) {
+    commandExecuted = false;
+    internalState = InternalState::SHUTDOWN;
+  }
+  if (internalState == InternalState::SHUTDOWN and commandExecuted) {
+    updatePeriodicReply(false, REPLY);
+    setMode(_MODE_POWER_DOWN);
+    commandExecuted = false;
+  }
+}
+
+ReturnValue_t MgmRm3100CustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
+  if (internalState == InternalState::STARTUP) {
+    *id = REQUEST;
+    return prepareRequest(acs::SimpleSensorMode::NORMAL);
+  } else if (internalState == InternalState::SHUTDOWN) {
+    *id = REQUEST;
+    return prepareRequest(acs::SimpleSensorMode::OFF);
+  }
+  return NOTHING_TO_SEND;
+}
+
+ReturnValue_t MgmRm3100CustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
+                                                              const uint8_t *commandData,
+                                                              size_t commandDataLen) {
+  return NOTHING_TO_SEND;
+}
+
+ReturnValue_t MgmRm3100CustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
+  *id = REQUEST;
+  return prepareRequest(acs::SimpleSensorMode::NORMAL);
+}
+
+ReturnValue_t MgmRm3100CustomHandler::scanForReply(const uint8_t *start, size_t len,
+                                                   DeviceCommandId_t *foundId, size_t *foundLen) {
+  if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) {
+    return IGNORE_FULL_PACKET;
+  }
+  if (len != sizeof(acs::MgmRm3100Reply)) {
+    *foundLen = len;
+    return returnvalue::FAILED;
+  }
+  *foundId = REPLY;
+  *foundLen = len;
+  return returnvalue::OK;
+}
+
+ReturnValue_t MgmRm3100CustomHandler::interpretDeviceReply(DeviceCommandId_t id,
+                                                           const uint8_t *packet) {
+  const auto *reply = reinterpret_cast<const acs::MgmRm3100Reply *>(packet);
+  if (reply->dataWasRead) {
+    if (internalState == InternalState::STARTUP) {
+      commandExecuted = true;
+    }
+
+    PoolReadGuard pg(&primaryDataset);
+    for (uint8_t idx = 0; idx < 3; idx++) {
+      primaryDataset.fieldStrengths[idx] = reply->mgmValuesRaw[idx] * reply->scaleFactors[idx];
+    }
+  }
+  return returnvalue::OK;
+}
+
+void MgmRm3100CustomHandler::fillCommandAndReplyMap() {
+  insertInCommandMap(REQUEST);
+  insertInReplyMap(REPLY, 5, nullptr, 0, true);
+}
+
+void MgmRm3100CustomHandler::modeChanged() { internalState = InternalState::NONE; }
+
+ReturnValue_t MgmRm3100CustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+                                                              LocalDataPoolManager &poolManager) {
+  localDataPoolMap.emplace(mgmRm3100::FIELD_STRENGTHS, &mgmXYZ);
+  poolManager.subscribeForRegularPeriodicPacket({primaryDataset.getSid(), false, 10.0});
+  return returnvalue::OK;
+}
+
+uint32_t MgmRm3100CustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
+  return this->transitionDelay;
+}
+
+void MgmRm3100CustomHandler::setToGoToNormalMode(bool enable) { goToNormalModeAtStartup = enable; }
+
+void MgmRm3100CustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
+  periodicPrintout = enable;
+  debugDivider.setDivider(divider);
+}
+
+ReturnValue_t MgmRm3100CustomHandler::prepareRequest(acs::SimpleSensorMode mode) {
+  request.mode = mode;
+  rawPacket = reinterpret_cast<uint8_t *>(&request);
+  rawPacketLen = sizeof(acs::MgmRm3100Request);
+  return returnvalue::OK;
+}

mission/devices/MgmRm3100CustomHandler.h

@@ -0,0 +1,98 @@
+#ifndef MISSION_DEVICES_MGMRM3100CUSTOMHANDLER_H_
+#define MISSION_DEVICES_MGMRM3100CUSTOMHANDLER_H_
+
+#include <fsfw_hal/devicehandlers/devicedefinitions/mgmRm3100Helpers.h>
+
+#include "fsfw/devicehandlers/DeviceHandlerBase.h"
+#include "fsfw/globalfunctions/PeriodicOperationDivider.h"
+#include "mission/devices/devicedefinitions/acsPolling.h"
+
+/**
+ * @brief 	Device Handler for the RM3100 geomagnetic magnetometer sensor
+ *          (https://www.pnicorp.com/rm3100/)
+ * @details
+ * Flight manual:
+ * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/RM3100_MGM
+ */
+class MgmRm3100CustomHandler : public DeviceHandlerBase {
+ public:
+  static constexpr DeviceCommandId_t REQUEST = 0x70;
+  static constexpr DeviceCommandId_t REPLY = 0x77;
+
+  static const uint8_t INTERFACE_ID = CLASS_ID::MGM_RM3100;
+
+  //! [EXPORT] : [COMMENT] P1: TMRC value which was set, P2: 0
+  static constexpr Event TMRC_SET =
+      event::makeEvent(SUBSYSTEM_ID::MGM_RM3100, 0x00, severity::INFO);
+
+  //! [EXPORT] : [COMMENT] Cycle counter set. P1: First two bytes new Cycle Count X
+  //! P1: Second two bytes new Cycle Count Y
+  //! P2: New cycle count Z
+  static constexpr Event cycleCountersSet =
+      event::makeEvent(SUBSYSTEM_ID::MGM_RM3100, 0x01, severity::INFO);
+
+  MgmRm3100CustomHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
+                         uint32_t transitionDelay);
+  virtual ~MgmRm3100CustomHandler();
+
+  void enablePeriodicPrintouts(bool enable, uint8_t divider);
+  /**
+   * Configure device handler to go to normal mode after startup immediately
+   * @param enable
+   */
+  void setToGoToNormalMode(bool enable);
+
+ protected:
+  /* DeviceHandlerBase overrides */
+  ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t *id) override;
+  void doStartUp() override;
+  void doShutDown() override;
+  ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override;
+  ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData,
+                                        size_t commandDataLen) override;
+  ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId,
+                             size_t *foundLen) override;
+  ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override;
+
+  void fillCommandAndReplyMap() override;
+  void modeChanged(void) override;
+  virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override;
+  ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+                                        LocalDataPoolManager &poolManager) override;
+
+ private:
+  enum class InternalState { NONE, STARTUP, SHUTDOWN };
+  InternalState internalState = InternalState::NONE;
+  bool commandExecuted = false;
+  mgmRm3100::Rm3100PrimaryDataset primaryDataset;
+  acs::MgmRm3100Request request{};
+
+  //  uint8_t cmmRegValue = mgmRm3100::CMM_VALUE;
+  //  uint8_t tmrcRegValue = mgmRm3100::TMRC_DEFAULT_VALUE;
+  //  uint16_t cycleCountRegValueX = mgmRm3100::CYCLE_COUNT_VALUE;
+  //  uint16_t cycleCountRegValueY = mgmRm3100::CYCLE_COUNT_VALUE;
+  //  uint16_t cycleCountRegValueZ = mgmRm3100::CYCLE_COUNT_VALUE;
+  float scaleFactorX = 1.0 / mgmRm3100::DEFAULT_GAIN;
+  float scaleFactorY = 1.0 / mgmRm3100::DEFAULT_GAIN;
+  float scaleFactorZ = 1.0 / mgmRm3100::DEFAULT_GAIN;
+
+  bool goToNormalModeAtStartup = false;
+  uint32_t transitionDelay;
+  PoolEntry<float> mgmXYZ = PoolEntry<float>(3);
+
+  ReturnValue_t handleCycleCountConfigCommand(DeviceCommandId_t deviceCommand,
+                                              const uint8_t *commandData, size_t commandDataLen);
+  ReturnValue_t handleCycleCommand(bool oneCycleValue, const uint8_t *commandData,
+                                   size_t commandDataLen);
+
+  ReturnValue_t handleTmrcConfigCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData,
+                                        size_t commandDataLen);
+
+  // ReturnValue_t handleDataReadout(const uint8_t *packet);
+
+  bool periodicPrintout = false;
+  ReturnValue_t prepareRequest(acs::SimpleSensorMode mode);
+  PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3);
+};
+
+#endif /* MISSION_DEVICEHANDLING_MgmRm3100CustomHandler_H_ */