basic MGM polling done

bsp_q7s/core/ObjectFactory.cpp

@@ -349,7 +349,7 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF*
   std::string spiDev = q7s::SPI_DEFAULT_DEV;
   std::array<DeviceHandlerBase*, 8> assemblyChildren;
   SpiCookie* spiCookie =
-      new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, MGMLIS3MDL::MAX_BUFFER_SIZE,
+      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(
@@ -365,7 +365,7 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF*
   mgmLis3Handler->enablePeriodicPrintouts(true, 10);
 #endif
   spiCookie =
-      new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, RM3100::MAX_BUFFER_SIZE,
+      new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, mgmRm3100::MAX_BUFFER_SIZE,
                     spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
   auto mgmRm3100Handler1 =
@@ -381,9 +381,8 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF*
 #if OBSW_DEBUG_ACS == 1
   mgmRm3100Handler->enablePeriodicPrintouts(true, 10);
 #endif
-  spiCookie =
+  spiCookie = new SpiCookie(addresses::MGM_2_LIS3, gpioIds::MGM_2_LIS3_CS, mgmLis3::MAX_BUFFER_SIZE,
-      new SpiCookie(addresses::MGM_2_LIS3, gpioIds::MGM_2_LIS3_CS, MGMLIS3MDL::MAX_BUFFER_SIZE,
+                            spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED);
-                    spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
   auto* mgmLis3Handler2 = new MgmLIS3MDLHandler(
       objects::MGM_2_LIS3_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::LIS3_TRANSITION_DELAY);
@@ -398,7 +397,7 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF*
   mgmLis3Handler->enablePeriodicPrintouts(true, 10);
 #endif
   spiCookie =
-      new SpiCookie(addresses::MGM_3_RM3100, gpioIds::MGM_3_RM3100_CS, RM3100::MAX_BUFFER_SIZE,
+      new SpiCookie(addresses::MGM_3_RM3100, gpioIds::MGM_3_RM3100_CS, mgmRm3100::MAX_BUFFER_SIZE,
                     spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
   auto* mgmRm3100Handler3 =

dummies/MgmLIS3MDLDummy.cpp

@@ -1,6 +1,6 @@
 #include "MgmLIS3MDLDummy.h"
 
-#include "fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h"
+#include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
 
 MgmLIS3MDLDummy::MgmLIS3MDLDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
     : DeviceHandlerBase(objectId, comif, comCookie), dataset(this) {}
@@ -40,8 +40,8 @@ uint32_t MgmLIS3MDLDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
 
 ReturnValue_t MgmLIS3MDLDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                        LocalDataPoolManager &poolManager) {
-  localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(mgmLis3::TEMPERATURE_CELCIUS, new PoolEntry<float>({0.0}));
-  localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS,
+  localDataPoolMap.emplace(mgmLis3::FIELD_STRENGTHS,
                            new PoolEntry<float>({1.02, 0.56, -0.78}, true));
   return returnvalue::OK;
 }

dummies/MgmLIS3MDLDummy.h

@@ -2,8 +2,7 @@
 #define DUMMIES_MGMLIS3MDLDUMMY_H_
 
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
-
+#include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
-#include "fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h"
 
 class MgmLIS3MDLDummy : public DeviceHandlerBase {
  public:
@@ -17,7 +16,7 @@ class MgmLIS3MDLDummy : public DeviceHandlerBase {
   virtual ~MgmLIS3MDLDummy();
 
  protected:
-  MGMLIS3MDL::MgmPrimaryDataset dataset;
+  mgmLis3::MgmPrimaryDataset dataset;
   void doStartUp() override;
   void doShutDown() override;
   ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override;

dummies/MgmRm3100Dummy.cpp

@@ -36,7 +36,7 @@ uint32_t MgmRm3100Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
 
 ReturnValue_t MgmRm3100Dummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                       LocalDataPoolManager& poolManager) {
-  localDataPoolMap.emplace(RM3100::FIELD_STRENGTHS,
+  localDataPoolMap.emplace(mgmRm3100::FIELD_STRENGTHS,
                            new PoolEntry<float>({0.87, -0.95, 0.11}, true));
   return returnvalue::OK;
 }

dummies/MgmRm3100Dummy.h

@@ -1,8 +1,9 @@
 #ifndef DUMMIES_MGMRM3100DUMMY_H_
 #define DUMMIES_MGMRM3100DUMMY_H_
 
+#include <fsfw_hal/devicehandlers/devicedefinitions/mgmRm3100Helpers.h>
+
 #include "fsfw/devicehandlers/DeviceHandlerBase.h"
-#include "fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h"
 
 class MgmRm3100Dummy : public DeviceHandlerBase {
  public:
@@ -10,7 +11,7 @@ class MgmRm3100Dummy : public DeviceHandlerBase {
   virtual ~MgmRm3100Dummy();
 
  protected:
-  RM3100::Rm3100PrimaryDataset dataset;
+  mgmRm3100::Rm3100PrimaryDataset dataset;
   void doStartUp() override;
   void doShutDown() override;
   ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override;

fsfw

@@ -1 +1 @@
-Subproject commit cf735143fe4b79db2fc7faba2b1cd239474e2cfc
+Subproject commit 511d07c0c78de7b1850e341dfcf8be7589f3c523

linux/devices/AcsBoardPolling.cpp

@@ -5,6 +5,7 @@
 #include <fsfw/tasks/SemaphoreFactory.h>
 #include <fsfw/tasks/TaskFactory.h>
 #include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
+#include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
 #include <fsfw_hal/linux/UnixFileGuard.h>
 #include <fsfw_hal/linux/spi/SpiCookie.h>
 #include <fsfw_hal/linux/utility.h>
@@ -34,6 +35,10 @@ ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) {
     gyroAdisHandler(gyro2Adis);
     gyroL3gHandler(gyro1L3g);
     gyroL3gHandler(gyro3L3g);
+    mgmRm3100Handler(mgm1Rm3100);
+    mgmRm3100Handler(mgm3Rm3100);
+    mgmLis3Handler(mgm0Lis3);
+    mgmLis3Handler(mgm2Lis3);
     // To prevent task being not reactivated by tardy tasks
     TaskFactory::delayTask(20);
   }
@@ -49,19 +54,19 @@ ReturnValue_t AcsBoardPolling::initializeInterface(CookieIF* cookie) {
   }
   switch (spiCookie->getChipSelectPin()) {
     case (gpioIds::MGM_0_LIS3_CS): {
-      mgm0L3Cookie = spiCookie;
+      mgm0Lis3.cookie = spiCookie;
       break;
     }
     case (gpioIds::MGM_1_RM3100_CS): {
-      mgm1Rm3100Cookie = spiCookie;
+      mgm1Rm3100.cookie = spiCookie;
       break;
     }
     case (gpioIds::MGM_2_LIS3_CS): {
-      mgm2L3Cookie = spiCookie;
+      mgm2Lis3.cookie = spiCookie;
       break;
     }
     case (gpioIds::MGM_3_RM3100_CS): {
-      mgm3Rm3100Cookie = spiCookie;
+      mgm3Rm3100.cookie = spiCookie;
       break;
     }
     case (gpioIds::GYRO_0_ADIS_CS): {
@@ -142,7 +147,59 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
     }
     return returnvalue::OK;
   };
+  auto handleLis3Request = [&](MgmLis3& mgm) {
+    if (sendLen != sizeof(acs::MgmLis3Request)) {
+      sif::error << "AcsBoardPolling: invalid lis3 request send length";
+      mgm.replyResult = returnvalue::FAILED;
+      return returnvalue::FAILED;
+    }
+    auto* req = reinterpret_cast<const acs::MgmLis3Request*>(sendData);
+    MutexGuard mg(ipcLock);
+    if (req->mode != mgm.mode) {
+      if (req->mode == acs::SimpleSensorMode::NORMAL) {
+        mgm.performStartup = true;
+      } else {
+        mgm.ownReply.dataWasSet = false;
+        mgm.ownReply.temperatureWasSet = false;
+      }
+      mgm.mode = req->mode;
+    }
+    return returnvalue::OK;
+  };
+  auto handleRm3100Request = [&](MgmRm3100& mgm) {
+    if (sendLen != sizeof(acs::MgmRm3100Request)) {
+      sif::error << "AcsBoardPolling: invalid rm3100 request send length";
+      mgm.replyResult = returnvalue::FAILED;
+      return returnvalue::FAILED;
+    }
+    auto* req = reinterpret_cast<const acs::MgmRm3100Request*>(sendData);
+    MutexGuard mg(ipcLock);
+    if (req->mode != mgm.mode) {
+      if (req->mode == acs::SimpleSensorMode::NORMAL) {
+        mgm.performStartup = true;
+      } else {
+      }
+      mgm.mode = req->mode;
+    }
+    return returnvalue::OK;
+  };
   switch (spiCookie->getChipSelectPin()) {
+    case (gpioIds::MGM_0_LIS3_CS): {
+      handleLis3Request(mgm0Lis3);
+      break;
+    }
+    case (gpioIds::MGM_1_RM3100_CS): {
+      handleRm3100Request(mgm1Rm3100);
+      break;
+    }
+    case (gpioIds::MGM_2_LIS3_CS): {
+      handleLis3Request(mgm2Lis3);
+      break;
+    }
+    case (gpioIds::MGM_3_RM3100_CS): {
+      handleRm3100Request(mgm3Rm3100);
+      break;
+    }
     case (gpioIds::GYRO_0_ADIS_CS): {
       handleAdisRequest(gyro0Adis);
       break;
@@ -185,28 +242,50 @@ ReturnValue_t AcsBoardPolling::readReceivedMessage(CookieIF* cookie, uint8_t** b
     std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::Adis1650XReply));
     *buffer = reinterpret_cast<uint8_t*>(&gyro.readerReply);
     *size = sizeof(acs::Adis1650XReply);
+    return gyro.replyResult;
   };
   auto handleL3gReply = [&](GyroL3g& gyro) {
     std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::GyroL3gReply));
     *buffer = reinterpret_cast<uint8_t*>(&gyro.readerReply);
     *size = sizeof(acs::GyroL3gReply);
+    return gyro.replyResult;
+  };
+  auto handleRm3100Reply = [&](MgmRm3100& mgm) {
+    std::memcpy(&mgm.readerReply, &mgm.ownReply, sizeof(acs::MgmRm3100Reply));
+    *buffer = reinterpret_cast<uint8_t*>(&mgm.readerReply);
+    *size = sizeof(acs::MgmRm3100Reply);
+    return mgm.replyResult;
+  };
+  auto handleLis3Reply = [&](MgmLis3& mgm) {
+    std::memcpy(&mgm.readerReply, &mgm.ownReply, sizeof(acs::MgmLis3Reply));
+    *buffer = reinterpret_cast<uint8_t*>(&mgm.readerReply);
+    *size = sizeof(acs::MgmLis3Reply);
+    return mgm.replyResult;
   };
   switch (spiCookie->getChipSelectPin()) {
+    case (gpioIds::MGM_0_LIS3_CS): {
+      return handleLis3Reply(mgm0Lis3);
+    }
+    case (gpioIds::MGM_1_RM3100_CS): {
+      return handleRm3100Reply(mgm1Rm3100);
+    }
+    case (gpioIds::MGM_2_LIS3_CS): {
+      return handleLis3Reply(mgm2Lis3);
+    }
+    case (gpioIds::MGM_3_RM3100_CS): {
+      return handleRm3100Reply(mgm3Rm3100);
+    }
     case (gpioIds::GYRO_0_ADIS_CS): {
-      handleAdisReply(gyro0Adis);
+      return handleAdisReply(gyro0Adis);
-      return gyro0Adis.replyResult;
     }
     case (gpioIds::GYRO_2_ADIS_CS): {
-      handleAdisReply(gyro2Adis);
+      return handleAdisReply(gyro2Adis);
-      return gyro2Adis.replyResult;
     }
     case (gpioIds::GYRO_1_L3G_CS): {
-      handleL3gReply(gyro1L3g);
+      return handleL3gReply(gyro1L3g);
-      return gyro1L3g.replyResult;
     }
     case (gpioIds::GYRO_3_L3G_CS): {
-      handleL3gReply(gyro3L3g);
+      return handleL3gReply(gyro3L3g);
-      return gyro3L3g.replyResult;
     }
   }
   return returnvalue::OK;
@@ -459,3 +538,190 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
     gyro.ownReply.data.temperatureRaw = (rawReply[16] << 8) | rawReply[17];
   }
 }
+
+void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
+  ReturnValue_t result;
+  acs::SimpleSensorMode mode;
+  bool mustPerformStartup = false;
+  {
+    MutexGuard mg(ipcLock);
+    mode = mgm.mode;
+    mustPerformStartup = mgm.performStartup;
+  }
+  if (mode == acs::SimpleSensorMode::NORMAL) {
+    if (mustPerformStartup) {
+      // To check valid communication, read back identification
+      // register which should always be the same value.
+      cmdBuf[0] = mgmLis3::readCommand(mgmLis3::IDENTIFY_DEVICE_REG_ADDR);
+      cmdBuf[1] = 0x00;
+      result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      if (rawReply[1] != mgmLis3::DEVICE_ID) {
+        sif::error << "AcsPollingTask: invalid MGM lis3 device ID" << std::endl;
+        mgm.replyResult = result;
+        return;
+      }
+      mgm.cfg[0] = mgmLis3::CTRL_REG1_DEFAULT;
+      mgm.cfg[1] = mgmLis3::CTRL_REG2_DEFAULT;
+      mgm.cfg[2] = mgmLis3::CTRL_REG3_DEFAULT;
+      mgm.cfg[3] = mgmLis3::CTRL_REG4_DEFAULT;
+      mgm.cfg[4] = mgmLis3::CTRL_REG5_DEFAULT;
+      cmdBuf[0] = mgmLis3::writeCommand(mgmLis3::CTRL_REG1, true);
+      std::memcpy(cmdBuf.data() + 1, mgm.cfg, 5);
+      result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 6);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      // Done here. We can always read back config and data during periodic handling
+      mgm.performStartup = false;
+    }
+    cmdBuf[0] = mgmLis3::readCommand(mgmLis3::CTRL_REG1, true);
+    std::memset(cmdBuf.data() + 1, 0, mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS);
+    result =
+        spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS + 1);
+    if (result != returnvalue::OK) {
+      mgm.replyResult = result;
+      return;
+    }
+    result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
+    if (result != returnvalue::OK) {
+      mgm.replyResult = result;
+      return;
+    }
+    // Verify communication by re-checking config
+    if (rawReply[1] != mgm.cfg[0] or rawReply[2] != mgm.cfg[1] or rawReply[3] != mgm.cfg[2] or
+        rawReply[4] != mgm.cfg[3] or rawReply[5] != mgm.cfg[4]) {
+      mgm.replyResult = result;
+      return;
+    }
+    {
+      MutexGuard mg(ipcLock);
+      mgm.ownReply.dataWasSet = true;
+      mgm.ownReply.sensitivity = mgmLis3::getSensitivityFactor(mgmLis3::getSensitivity(mgm.cfg[1]));
+      mgm.ownReply.mgmValuesRaw[0] =
+          (rawReply[mgmLis3::X_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::X_LOWBYTE_IDX];
+      mgm.ownReply.mgmValuesRaw[1] =
+          (rawReply[mgmLis3::Y_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::Y_LOWBYTE_IDX];
+      mgm.ownReply.mgmValuesRaw[2] =
+          (rawReply[mgmLis3::Z_HIGHBYTE_IDX] << 8) | rawReply[mgmLis3::Z_LOWBYTE_IDX];
+    }
+    // Read tempetature
+    cmdBuf[0] = mgmLis3::readCommand(mgmLis3::TEMP_LOWBYTE, true);
+    result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 3);
+    if (result != returnvalue::OK) {
+      mgm.replyResult = result;
+      return;
+    }
+    result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
+    if (result != returnvalue::OK) {
+      mgm.replyResult = result;
+      return;
+    }
+    MutexGuard mg(ipcLock);
+    mgm.ownReply.temperatureWasSet = true;
+    mgm.ownReply.temperatureRaw = (rawReply[2] << 8) | rawReply[1];
+  }
+}
+
+void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) {
+  ReturnValue_t result;
+  acs::SimpleSensorMode mode;
+  bool mustPerformStartup = false;
+  {
+    MutexGuard mg(ipcLock);
+    mode = mgm.mode;
+    mustPerformStartup = mgm.performStartup;
+  }
+  if (mode == acs::SimpleSensorMode::NORMAL) {
+    if (mustPerformStartup) {
+      // Configure CMM first
+      cmdBuf[0] = mgmRm3100::CMM_REGISTER;
+      cmdBuf[1] = mgmRm3100::CMM_VALUE;
+      result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      // Read back register
+      cmdBuf[0] = mgmRm3100::CMM_REGISTER | mgmRm3100::READ_MASK;
+      cmdBuf[1] = 0;
+      result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      if (rawReply[1] != mgmRm3100::CMM_VALUE) {
+        sif::error << "AcsBoardPolling: MGM RM3100 read back CMM invalid" << std::endl;
+        mgm.replyResult = result;
+        return;
+      }
+      // Configure TMRC register
+      cmdBuf[0] = mgmRm3100::TMRC_REGISTER;
+      // hardcoded for now
+      cmdBuf[1] = mgm.tmrcValue;
+      result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      // Read back and verify value
+      cmdBuf[0] = mgmRm3100::TMRC_REGISTER | mgmRm3100::READ_MASK;
+      cmdBuf[1] = 0;
+      result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 2);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
+      if (result != OK) {
+        mgm.replyResult = result;
+        return;
+      }
+      if (rawReply[1] != mgm.tmrcValue) {
+        sif::error << "AcsBoardPolling: MGM RM3100 read back TMRC invalid" << std::endl;
+        mgm.replyResult = result;
+        return;
+      }
+      mgm.performStartup = false;
+    }
+    // Regular read operation
+    cmdBuf[0] = mgmRm3100::MEASUREMENT_REG_START | mgmRm3100::READ_MASK;
+    std::memset(cmdBuf.data() + 1, 0, 9);
+    result = spiComIF.sendMessage(mgm.cookie, cmdBuf.data(), 10);
+    if (result != OK) {
+      mgm.replyResult = result;
+      return;
+    }
+    result = spiComIF.readReceivedMessage(mgm.cookie, &rawReply, &dummy);
+    if (result != OK) {
+      mgm.replyResult = result;
+      return;
+    }
+    MutexGuard mg(ipcLock);
+    for (uint8_t idx = 0; idx < 3; idx++) {
+      // Hardcoded, but note that the gain depends on the cycle count
+      // value which is configurable!
+      mgm.ownReply.scaleFactors[idx] = 1.0 / mgmRm3100::DEFAULT_GAIN;
+    }
+    mgm.ownReply.mgmValuesRaw[0] =
+        ((rawReply[1] << 24) | (rawReply[2] << 16) | (rawReply[3] << 8)) >> 8;
+    mgm.ownReply.mgmValuesRaw[1] =
+        ((rawReply[4] << 24) | (rawReply[5] << 16) | (rawReply[6] << 8)) >> 8;
+    mgm.ownReply.mgmValuesRaw[2] =
+        ((rawReply[7] << 24) | (rawReply[8] << 16) | (rawReply[9] << 8)) >> 8;
+  }
+}

linux/devices/AcsBoardPolling.h

@@ -5,6 +5,7 @@
 #include <fsfw/objectmanager/SystemObject.h>
 #include <fsfw/tasks/ExecutableObjectIF.h>
 #include <fsfw/tasks/SemaphoreIF.h>
+#include <fsfw_hal/devicehandlers/devicedefinitions/mgmRm3100Helpers.h>
 #include <fsfw_hal/linux/spi/SpiComIF.h>
 #include <mission/devices/devicedefinitions/acsPolling.h>
 #include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
@@ -25,41 +26,46 @@ class AcsBoardPolling : public SystemObject,
   std::array<uint8_t, 32> cmdBuf;
   std::array<uint8_t, 32> replyBuf;
 
-  bool mgm0L3IsOn = false;
+  struct DevBase {
-  SpiCookie* mgm0L3Cookie = nullptr;
-  bool mgm1Rm3100IsOn = false;
-  SpiCookie* mgm1Rm3100Cookie = nullptr;
-  bool mgm2L3IsOn = false;
-  SpiCookie* mgm2L3Cookie = nullptr;
-  bool mgm3Rm3100IsOn = false;
-  SpiCookie* mgm3Rm3100Cookie = nullptr;
-
-  struct GyroAdis {
-    adis1650x::Type type;
-    bool isOn = false;
-    bool performStartup = false;
     SpiCookie* cookie = nullptr;
-    Countdown countdown;
+    bool performStartup = false;
     acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
     ReturnValue_t replyResult;
+  };
+
+  struct GyroAdis : public DevBase {
+    adis1650x::Type type;
+    Countdown countdown;
     acs::Adis1650XReply ownReply;
     acs::Adis1650XReply readerReply;
   };
   GyroAdis gyro0Adis{};
   GyroAdis gyro2Adis{};
 
-  struct GyroL3g {
+  struct GyroL3g : public DevBase {
-    bool performStartup = false;
-    SpiCookie* cookie = nullptr;
-    acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
     uint8_t sensorCfg[5];
-    ReturnValue_t replyResult;
     acs::GyroL3gReply ownReply;
     acs::GyroL3gReply readerReply;
   };
   GyroL3g gyro1L3g{};
   GyroL3g gyro3L3g{};
 
+  struct MgmRm3100 : public DevBase {
+    uint8_t tmrcValue = mgmRm3100::TMRC_DEFAULT_37HZ_VALUE;
+    acs::MgmRm3100Reply ownReply;
+    acs::MgmRm3100Reply readerReply;
+  };
+  MgmRm3100 mgm1Rm3100;
+  MgmRm3100 mgm3Rm3100;
+
+  struct MgmLis3 : public DevBase {
+    uint8_t cfg[5]{};
+    acs::MgmLis3Reply ownReply;
+    acs::MgmLis3Reply readerReply;
+  };
+  MgmLis3 mgm0Lis3;
+  MgmLis3 mgm2Lis3;
+
   uint8_t* rawReply = nullptr;
   size_t dummy = 0;
 
@@ -74,6 +80,8 @@ class AcsBoardPolling : public SystemObject,
 
   void gyroL3gHandler(GyroL3g& l3g);
   void gyroAdisHandler(GyroAdis& gyro);
+  void mgmLis3Handler(MgmLis3& mgm);
+  void mgmRm3100Handler(MgmRm3100& mgm);
   // Special readout: 16us stall time between small 2 byte transfers.
   ReturnValue_t readAdisCfg(SpiCookie& spiCookie, size_t transferLen);
 };

mission/controller/ThermalController.cpp

@@ -3,7 +3,7 @@
 #include <bsp_q7s/core/CoreDefinitions.h>
 #include <fsfw/datapool/PoolReadGuard.h>
 #include <fsfw/thermal/ThermalComponentIF.h>
-#include <fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h>
+#include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
 #include <linux/devices/devicedefinitions/StarTrackerDefinitions.h>
 #include <mission/devices/devicedefinitions/BpxBatteryDefinitions.h>
 #include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
@@ -908,7 +908,7 @@ void ThermalController::copyDevices() {
 
   {
     lp_var_t<float> tempMgm0 =
-        lp_var_t<float>(objects::MGM_0_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS);
+        lp_var_t<float>(objects::MGM_0_LIS3_HANDLER, mgmLis3::TEMPERATURE_CELCIUS);
     PoolReadGuard pg(&tempMgm0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg.getReadResult() != returnvalue::OK) {
       sif::warning << "ThermalController: Failed to read MGM 0 temperature" << std::endl;
@@ -922,7 +922,7 @@ void ThermalController::copyDevices() {
 
   {
     lp_var_t<float> tempMgm2 =
-        lp_var_t<float>(objects::MGM_2_LIS3_HANDLER, MGMLIS3MDL::TEMPERATURE_CELCIUS);
+        lp_var_t<float>(objects::MGM_2_LIS3_HANDLER, mgmLis3::TEMPERATURE_CELCIUS);
     PoolReadGuard pg(&tempMgm2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg.getReadResult() != returnvalue::OK) {
       sif::warning << "ThermalController: Failed to read MGM 2 temperature" << std::endl;

mission/controller/acs/SensorValues.h

@@ -27,14 +27,12 @@ class SensorValues {
   ReturnValue_t updateStr();
   ReturnValue_t updateRw();
 
-  MGMLIS3MDL::MgmPrimaryDataset mgm0Lis3Set =
+  mgmLis3::MgmPrimaryDataset mgm0Lis3Set = mgmLis3::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER);
-      MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_0_LIS3_HANDLER);
+  mgmRm3100::Rm3100PrimaryDataset mgm1Rm3100Set =
-  RM3100::Rm3100PrimaryDataset mgm1Rm3100Set =
+      mgmRm3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER);
-      RM3100::Rm3100PrimaryDataset(objects::MGM_1_RM3100_HANDLER);
+  mgmLis3::MgmPrimaryDataset mgm2Lis3Set = mgmLis3::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER);
-  MGMLIS3MDL::MgmPrimaryDataset mgm2Lis3Set =
+  mgmRm3100::Rm3100PrimaryDataset mgm3Rm3100Set =
-      MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER);
+      mgmRm3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER);
-  RM3100::Rm3100PrimaryDataset mgm3Rm3100Set =
-      RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER);
   imtq::RawMtmMeasurementNoTorque imtqMgmSet =
       imtq::RawMtmMeasurementNoTorque(objects::IMTQ_HANDLER);
 

mission/devices/devicedefinitions/acsPolling.h

@@ -1,6 +1,7 @@
 #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_
 #define MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_
 
+#include "fsfw/thermal/tcsDefinitions.h"
 #include "gyroAdisHelpers.h"
 
 namespace acs {
@@ -22,13 +23,13 @@ struct Adis1650XConfig {
 };
 
 struct Adis1650XData {
-  double sensitivity;
+  double sensitivity = 0.0;
   // Angular velocities in all axes (X, Y and Z)
-  int16_t angVelocities[3];
+  int16_t angVelocities[3]{};
-  double accelScaling;
+  double accelScaling = 0.0;
   // Accelerations in all axes
-  int16_t accelerations[3];
+  int16_t accelerations[3]{};
-  int16_t temperatureRaw;
+  int16_t temperatureRaw = thermal::INVALID_TEMPERATURE;
 };
 
 struct Adis1650XReply {
@@ -39,18 +40,40 @@ struct Adis1650XReply {
 };
 
 struct GyroL3gRequest {
-  SimpleSensorMode mode;
+  SimpleSensorMode mode = SimpleSensorMode::OFF;
-  uint8_t ctrlRegs[5];
+  uint8_t ctrlRegs[5]{};
 };
 
 struct GyroL3gReply {
-  bool cfgWasSet;
+  bool cfgWasSet = false;
   uint8_t statusReg;
   // Angular velocities in all axes (X, Y and Z)
-  int16_t angVelocities[3];
+  int16_t angVelocities[3]{};
-  int8_t tempOffsetRaw;
+  int8_t tempOffsetRaw = 0;
-  uint8_t ctrlRegs[5];
+  uint8_t ctrlRegs[5]{};
-  float sensitivity;
+  float sensitivity = 0.0;
+};
+
+struct MgmRm3100Request {
+  SimpleSensorMode mode = SimpleSensorMode::OFF;
+};
+
+struct MgmRm3100Reply {
+  bool dataWasRead = false;
+  float scaleFactors[3]{};
+  int32_t mgmValuesRaw[3]{};
+};
+
+struct MgmLis3Request {
+  SimpleSensorMode mode = SimpleSensorMode::OFF;
+};
+
+struct MgmLis3Reply {
+  bool dataWasSet = false;
+  float sensitivity = 0.0;
+  int16_t mgmValuesRaw[3]{};
+  bool temperatureWasSet = false;
+  int16_t temperatureRaw = thermal::INVALID_TEMPERATURE;
 };
 
 }  // namespace acs