vieles

CHANGELOG.md

@@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release:
 
 # [unreleased]
 
+## Changed
+
+- Move ACS board polling to separate worker thread.
+
 # [v1.32.0]
 
 eive-tmtc: v2.16.1

bsp_q7s/core/ObjectFactory.cpp

@@ -1,8 +1,10 @@
 #include "ObjectFactory.h"
 
 #include <fsfw/subsystem/Subsystem.h>
+#include <linux/devices/AcsBoardPolling.h>
 #include <linux/devices/ImtqPollingTask.h>
 #include <linux/devices/RwPollingTask.h>
+#include <mission/devices/GyrL3gCustomHandler.h>
 #include <mission/system/objects/CamSwitcher.h>
 
 #include "OBSWConfig.h"
@@ -57,6 +59,7 @@
 #if OBSW_TEST_LIBGPIOD == 1
 #include "linux/boardtest/LibgpiodTest.h"
 #endif
+#include <mission/devices/GyrAdis1650XHandler.h>
 #include <mission/devices/ImtqHandler.h>
 #include <mission/devices/PcduHandler.h>
 #include <mission/devices/SyrlinksHandler.h>
@@ -83,7 +86,6 @@
 #include "mission/core/GenericFactory.h"
 #include "mission/devices/ACUHandler.h"
 #include "mission/devices/BpxBatteryHandler.h"
-#include "mission/devices/GyroADIS1650XHandler.h"
 #include "mission/devices/HeaterHandler.h"
 #include "mission/devices/Max31865PT1000Handler.h"
 #include "mission/devices/P60DockHandler.h"
@@ -239,8 +241,8 @@ ReturnValue_t ObjectFactory::createRadSensorComponent(LinuxLibgpioIF* gpioComIF,
   return returnvalue::OK;
 }
 
-void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF,
+void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF,
-                                             PowerSwitchIF& pwrSwitcher) {
+                                             SerialComIF* uartComIF, PowerSwitchIF& pwrSwitcher) {
   using namespace gpio;
   GpioCookie* gpioCookieAcsBoard = new GpioCookie();
 
@@ -343,6 +345,7 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
   static_cast<void>(fdir);
 
 #if OBSW_ADD_ACS_BOARD == 1
+  new AcsBoardPolling(objects::ACS_BOARD_POLLING_TASK, spiComIF, *gpioComIF);
   std::string spiDev = q7s::SPI_DEFAULT_DEV;
   std::array<DeviceHandlerBase*, 8> assemblyChildren;
   SpiCookie* spiCookie =
@@ -414,12 +417,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
   // Commented until ACS board V2 in in clean room again
   // Gyro 0 Side A
   spiCookie =
-      new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, ADIS1650X::MAXIMUM_REPLY_SIZE,
+      new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, adis1650x::MAXIMUM_REPLY_SIZE,
                     spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
   auto adisHandler =
-      new GyroADIS1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie,
+      new GyrAdis1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::ACS_BOARD_POLLING_TASK,
-                               ADIS1650X::Type::ADIS16505);
+                              spiCookie, adis1650x::Type::ADIS16505);
   fdir = new AcsBoardFdir(objects::GYRO_0_ADIS_HANDLER);
   adisHandler->setCustomFdir(fdir);
   assemblyChildren[4] = adisHandler;
@@ -431,11 +434,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
   adisHandler->enablePeriodicPrintouts(true, 10);
 #endif
   // Gyro 1 Side A
-  spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, L3GD20H::MAX_BUFFER_SIZE,
+  spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, l3gd20h::MAX_BUFFER_SIZE,
                             spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
-  auto gyroL3gHandler1 = new GyroHandlerL3GD20H(
+  auto gyroL3gHandler1 =
-      objects::GYRO_1_L3G_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY);
+      new GyrL3gCustomHandler(objects::GYRO_1_L3G_HANDLER, objects::ACS_BOARD_POLLING_TASK,
+                              spiCookie, spi::L3G_TRANSITION_DELAY);
   fdir = new AcsBoardFdir(objects::GYRO_1_L3G_HANDLER);
   gyroL3gHandler1->setCustomFdir(fdir);
   assemblyChildren[5] = gyroL3gHandler1;
@@ -448,11 +452,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
 #endif
   // Gyro 2 Side B
   spiCookie =
-      new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, ADIS1650X::MAXIMUM_REPLY_SIZE,
+      new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, adis1650x::MAXIMUM_REPLY_SIZE,
                     spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
-  adisHandler = new GyroADIS1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_MAIN_COM_IF,
+  adisHandler =
-                                         spiCookie, ADIS1650X::Type::ADIS16505);
+      new GyrAdis1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::ACS_BOARD_POLLING_TASK,
+                              spiCookie, adis1650x::Type::ADIS16505);
   fdir = new AcsBoardFdir(objects::GYRO_2_ADIS_HANDLER);
   adisHandler->setCustomFdir(fdir);
   assemblyChildren[6] = adisHandler;
@@ -461,11 +466,12 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
   adisHandler->setToGoToNormalModeImmediately();
 #endif
   // Gyro 3 Side B
-  spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, L3GD20H::MAX_BUFFER_SIZE,
+  spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, l3gd20h::MAX_BUFFER_SIZE,
                             spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED);
   spiCookie->setMutexParams(MutexIF::TimeoutType::WAITING, spi::ACS_BOARD_CS_TIMEOUT);
-  auto gyroL3gHandler3 = new GyroHandlerL3GD20H(
+  auto gyroL3gHandler3 =
-      objects::GYRO_3_L3G_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, spi::L3G_TRANSITION_DELAY);
+      new GyrL3gCustomHandler(objects::GYRO_3_L3G_HANDLER, objects::ACS_BOARD_POLLING_TASK,
+                              spiCookie, spi::L3G_TRANSITION_DELAY);
   fdir = new AcsBoardFdir(objects::GYRO_3_L3G_HANDLER);
   gyroL3gHandler3->setCustomFdir(fdir);
   assemblyChildren[7] = gyroL3gHandler3;

bsp_q7s/core/ObjectFactory.h

@@ -2,6 +2,7 @@
 #define BSP_Q7S_OBJECTFACTORY_H_
 
 #include <fsfw/returnvalues/returnvalue.h>
+#include <fsfw_hal/linux/spi/SpiComIF.h>
 #include <mission/devices/HeaterHandler.h>
 #include <mission/system/objects/Stack5VHandler.h>
 #include <mission/tmtc/CcsdsIpCoreHandler.h>
@@ -31,7 +32,7 @@ void createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF,
                             PowerSwitchIF* pwrSwitcher, Stack5VHandler& stackHandler);
 void createTmpComponents();
 ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF, Stack5VHandler& handler);
-void createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF,
+void createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF,
                               PowerSwitchIF& pwrSwitcher);
 void createHeaterComponents(GpioIF* gpioIF, PowerSwitchIF* pwrSwitcher, HealthTableIF* healthTable,
                             HeaterHandler*& heaterHandler);

bsp_q7s/core/scheduling.cpp

@@ -194,6 +194,15 @@ void scheduling::initTasks() {
     scheduling::printAddObjectError("GPS_CTRL", objects::GPS_CONTROLLER);
   }
 
+#if OBSW_ADD_ACS_BOARD == 1
+  PeriodicTaskIF* acsBrdPolling = factory->createPeriodicTask(
+      "ACS_BOARD_POLLING", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
+  result = acsBrdPolling->addComponent(objects::ACS_BOARD_POLLING_TASK);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("ACS_BOARD_POLLING", objects::ACS_BOARD_POLLING_TASK);
+  }
+#endif
+
 #if OBSW_ADD_RW == 1
   PeriodicTaskIF* rwPolling = factory->createPeriodicTask(
       "RW_POLLING_TASK", 85, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
@@ -351,6 +360,9 @@ void scheduling::initTasks() {
 #if OBSW_ADD_SA_DEPL == 1
   solarArrayDeplTask->startTask();
 #endif
+#if OBSW_ADD_ACS_BOARD == 1
+  acsBrdPolling->startTask();
+#endif
 #if OBSW_ADD_MGT == 1
   imtqPolling->startTask();
 #endif

bsp_q7s/fmObjectFactory.cpp

@@ -43,7 +43,7 @@ void ObjectFactory::produce(void* args) {
 #endif
 
 #if OBSW_ADD_ACS_BOARD == 1
-  createAcsBoardComponents(gpioComIF, uartComIF, *pwrSwitcher);
+  createAcsBoardComponents(*spiMainComIF, gpioComIF, uartComIF, *pwrSwitcher);
 #endif
   HeaterHandler* heaterHandler;
   createHeaterComponents(gpioComIF, pwrSwitcher, healthTable, heaterHandler);

common/config/eive/objects.h

@@ -123,7 +123,9 @@ enum commonObjects : uint32_t {
   // CCSDS_IP_CORE_BRIDGE = 0x73500000,
 
   /* 0x49 ('I') for Communication Interfaces */
-  SPI_RTD_COM_IF = 0x49020006,
+  ACS_BOARD_POLLING_TASK = 0x49060004,
+  RW_POLLING_TASK = 0x49060005,
+  SPI_RTD_COM_IF = 0x49060006,
 
   // 0x60 for other stuff
   HEATER_0_PLOC_PROC_BRD = 0x60000000,

dummies/GyroAdisDummy.cpp

@@ -1,6 +1,6 @@
 #include "GyroAdisDummy.h"
 
-#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
+#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
 
 GyroAdisDummy::GyroAdisDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
     : DeviceHandlerBase(objectId, comif, comCookie), dataset(this) {}
@@ -40,13 +40,13 @@ uint32_t GyroAdisDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { r
 
 ReturnValue_t GyroAdisDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                      LocalDataPoolManager &poolManager) {
-  localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry<double>({-0.5}, true));
+  localDataPoolMap.emplace(adis1650x::ANG_VELOC_X, new PoolEntry<double>({-0.5}, true));
-  localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry<double>({0.2}, true));
+  localDataPoolMap.emplace(adis1650x::ANG_VELOC_Y, new PoolEntry<double>({0.2}, true));
-  localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry<double>({-1.2}, true));
+  localDataPoolMap.emplace(adis1650x::ANG_VELOC_Z, new PoolEntry<double>({-1.2}, true));
-  localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::ACCELERATION_X, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::ACCELERATION_Y, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::ACCELERATION_Z, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(adis1650x::TEMPERATURE, new PoolEntry<float>({0.0}));
 
   return returnvalue::OK;
 }

dummies/GyroAdisDummy.h

@@ -2,8 +2,7 @@
 #define DUMMIES_GYROADISDUMMY_H_
 
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
-
+#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
-#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
 
 class GyroAdisDummy : public DeviceHandlerBase {
  public:

dummies/GyroL3GD20Dummy.cpp

@@ -1,6 +1,6 @@
 #include "GyroL3GD20Dummy.h"
 
-#include "fsfw_hal/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h"
+#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
 
 GyroL3GD20Dummy::GyroL3GD20Dummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
     : DeviceHandlerBase(objectId, comif, comCookie) {}
@@ -40,9 +40,9 @@ uint32_t GyroL3GD20Dummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
 
 ReturnValue_t GyroL3GD20Dummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                        LocalDataPoolManager &poolManager) {
-  localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry<float>({1.2}, true));
+  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_X, new PoolEntry<float>({1.2}, true));
-  localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, new PoolEntry<float>({-0.1}, true));
+  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Y, new PoolEntry<float>({-0.1}, true));
-  localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, new PoolEntry<float>({0.7}, true));
+  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Z, new PoolEntry<float>({0.7}, true));
-  localDataPoolMap.emplace(L3GD20H::TEMPERATURE, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(l3gd20h::TEMPERATURE, new PoolEntry<float>({0.0}));
   return returnvalue::OK;
 }

fsfw

@@ -1 +1 @@
-Subproject commit bdfe31dba48039b60fe700e7d03bfb95e9549688
+Subproject commit cf735143fe4b79db2fc7faba2b1cd239474e2cfc

linux/devices/AcsBoardPolling.cpp

@@ -0,0 +1,453 @@
+#include "AcsBoardPolling.h"
+
+#include <fcntl.h>
+#include <fsfw/globalfunctions/arrayprinter.h>
+#include <fsfw/tasks/SemaphoreFactory.h>
+#include <fsfw/tasks/TaskFactory.h>
+#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
+#include <fsfw_hal/linux/UnixFileGuard.h>
+#include <fsfw_hal/linux/spi/SpiCookie.h>
+#include <fsfw_hal/linux/utility.h>
+#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
+#include <sys/ioctl.h>
+
+#include "devices/gpioIds.h"
+
+using namespace returnvalue;
+
+AcsBoardPolling::AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF, GpioIF& gpioIF)
+    : SystemObject(objectId), spiComIF(lowLevelComIF), gpioIF(gpioIF) {
+  semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
+  semaphore->acquire();
+  ipcLock = MutexFactory::instance()->createMutex();
+}
+
+ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) {
+  while (true) {
+    ipcLock->lockMutex();
+    state = InternalState::IDLE;
+    ipcLock->unlockMutex();
+    semaphore->acquire();
+    // Give all tasks or the PST some time to submit all consecutive requests.
+    TaskFactory::delayTask(2);
+    gyroAdisHandler(gyro0Adis);
+    gyroAdisHandler(gyro2Adis);
+    gyroL3gHandler(gyro1L3g);
+    gyroL3gHandler(gyro3L3g);
+    // To prevent task being not reactivated by tardy tasks
+    TaskFactory::delayTask(20);
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t AcsBoardPolling::initialize() { return returnvalue::OK; }
+
+ReturnValue_t AcsBoardPolling::initializeInterface(CookieIF* cookie) {
+  SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
+  if (spiCookie == nullptr) {
+    return returnvalue::FAILED;
+  }
+  switch (spiCookie->getChipSelectPin()) {
+    case (gpioIds::MGM_0_LIS3_CS): {
+      mgm0L3Cookie = spiCookie;
+      break;
+    }
+    case (gpioIds::MGM_1_RM3100_CS): {
+      mgm1Rm3100Cookie = spiCookie;
+      break;
+    }
+    case (gpioIds::MGM_2_LIS3_CS): {
+      mgm2L3Cookie = spiCookie;
+      break;
+    }
+    case (gpioIds::MGM_3_RM3100_CS): {
+      mgm3Rm3100Cookie = spiCookie;
+      break;
+    }
+    case (gpioIds::GYRO_0_ADIS_CS): {
+      gyro0Adis.cookie = spiCookie;
+      break;
+    }
+    case (gpioIds::GYRO_1_L3G_CS): {
+      gyro1L3g.cookie = spiCookie;
+      break;
+    }
+    case (gpioIds::GYRO_2_ADIS_CS): {
+      gyro2Adis.cookie = spiCookie;
+      break;
+    }
+    case (gpioIds::GYRO_3_L3G_CS): {
+      gyro3L3g.cookie = spiCookie;
+      break;
+    }
+    default: {
+      sif::error << "AcsBoardPollingTask: invalid spi cookie" << std::endl;
+    }
+  }
+  return spiComIF.initializeInterface(cookie);
+}
+
+ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* sendData,
+                                           size_t sendLen) {
+  SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
+  if (spiCookie == nullptr) {
+    return returnvalue::FAILED;
+  }
+  auto handleAdisRequest = [&](GyroAdis& adis) {
+    if (sendLen != sizeof(acs::Adis1650XRequest)) {
+      sif::error << "AcsBoardPolling: invalid adis request send length";
+      adis.replyResult = returnvalue::FAILED;
+      return returnvalue::FAILED;
+    }
+    auto* req = reinterpret_cast<const acs::Adis1650XRequest*>(sendData);
+    MutexGuard mg(ipcLock);
+    if (req->mode != adis.mode) {
+      if (req->mode == acs::SimpleSensorMode::NORMAL) {
+        adis.type = req->type;
+        adis.countdown.setTimeout(adis1650x::START_UP_TIME);
+        adis.countdown.resetTimer();
+        if (adis.type == adis1650x::Type::ADIS16507) {
+          adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16507;
+        } else if (adis.type == adis1650x::Type::ADIS16505) {
+          adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16505;
+        } else {
+          sif::warning << "AcsBoardPolling: Unknown ADIS type" << std::endl;
+        }
+        adis.performStartup = true;
+      } else if (req->mode == acs::SimpleSensorMode::OFF) {
+        adis.performStartup = false;
+        adis.ownReply.cfgWasSet = false;
+        adis.ownReply.dataWasSet = false;
+      }
+      adis.mode = req->mode;
+    }
+    return returnvalue::OK;
+  };
+  auto handleL3gRequest = [&](GyroL3g& gyro) {
+    if (sendLen != sizeof(acs::GyroL3gRequest)) {
+      sif::error << "AcsBoardPolling: invalid l3g request send length";
+      gyro.replyResult = returnvalue::FAILED;
+      return returnvalue::FAILED;
+    }
+    auto* req = reinterpret_cast<const acs::GyroL3gRequest*>(sendData);
+    MutexGuard mg(ipcLock);
+    if (req->mode != gyro.mode) {
+      if (req->mode == acs::SimpleSensorMode::NORMAL) {
+        gyro.performStartup = true;
+      } else {
+        gyro.ownReply.cfgWasSet = false;
+      }
+      gyro.mode = req->mode;
+    }
+    return returnvalue::OK;
+  };
+  switch (spiCookie->getChipSelectPin()) {
+    case (gpioIds::GYRO_0_ADIS_CS): {
+      handleAdisRequest(gyro0Adis);
+      break;
+    }
+    case (gpioIds::GYRO_2_ADIS_CS): {
+      handleAdisRequest(gyro2Adis);
+      break;
+    }
+    case (gpioIds::GYRO_1_L3G_CS): {
+      handleL3gRequest(gyro1L3g);
+      break;
+    }
+    case (gpioIds::GYRO_3_L3G_CS): {
+      handleL3gRequest(gyro3L3g);
+      break;
+    }
+  }
+  MutexGuard mg(ipcLock);
+  if (state == InternalState::IDLE) {
+    state = InternalState::BUSY;
+    semaphore->release();
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t AcsBoardPolling::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }
+
+ReturnValue_t AcsBoardPolling::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
+  return returnvalue::OK;
+}
+
+ReturnValue_t AcsBoardPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
+                                                   size_t* size) {
+  SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
+  if (spiCookie == nullptr) {
+    return returnvalue::FAILED;
+  }
+  MutexGuard mg(ipcLock);
+  auto handleAdisReply = [&](GyroAdis& gyro) {
+    std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::Adis1650XReply));
+    *buffer = reinterpret_cast<uint8_t*>(&gyro.readerReply);
+    *size = sizeof(acs::Adis1650XReply);
+  };
+  auto handleL3gReply = [&](GyroL3g& gyro) {
+    std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::GyroL3gReply));
+    *buffer = reinterpret_cast<uint8_t*>(&gyro.readerReply);
+    *size = sizeof(acs::GyroL3gReply);
+  };
+  switch (spiCookie->getChipSelectPin()) {
+    case (gpioIds::GYRO_0_ADIS_CS): {
+      handleAdisReply(gyro0Adis);
+      return gyro0Adis.replyResult;
+    }
+    case (gpioIds::GYRO_2_ADIS_CS): {
+      handleAdisReply(gyro2Adis);
+      return gyro2Adis.replyResult;
+    }
+    case (gpioIds::GYRO_1_L3G_CS): {
+      handleL3gReply(gyro1L3g);
+      return gyro1L3g.replyResult;
+    }
+    case (gpioIds::GYRO_3_L3G_CS): {
+      handleL3gReply(gyro3L3g);
+      return gyro3L3g.replyResult;
+    }
+  }
+  return returnvalue::OK;
+}
+
+void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
+  ReturnValue_t result;
+  acs::SimpleSensorMode mode;
+  bool gyroPerformStartup;
+  {
+    MutexGuard mg(ipcLock);
+    mode = l3g.mode;
+    gyroPerformStartup = l3g.performStartup;
+  }
+  if (mode == acs::SimpleSensorMode::NORMAL) {
+    if (gyroPerformStartup) {
+      cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK;
+      std::memcpy(cmdBuf.data() + 1, l3g.sensorCfg, 5);
+      result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
+      if (result != returnvalue::OK) {
+        l3g.replyResult = returnvalue::OK;
+      }
+      // Ignore useless reply and red config
+      cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK;
+      std::memset(cmdBuf.data() + 1, 0, 5);
+      result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
+      if (result != returnvalue::OK) {
+        l3g.replyResult = returnvalue::OK;
+      }
+      result = spiComIF.readReceivedMessage(l3g.cookie, &rawReply, &dummy);
+      if (result != returnvalue::OK) {
+        l3g.replyResult = returnvalue::OK;
+      }
+      MutexGuard mg(ipcLock);
+      // Cross check configuration as verification that communication is working
+      for (uint8_t idx = 0; idx < 5; idx++) {
+        if (rawReply[idx + 1] != l3g.sensorCfg[idx]) {
+          sif::warning << "AcsBoardPolling: l3g config check missmatch" << std::endl;
+          l3g.replyResult = returnvalue::FAILED;
+          return;
+        }
+      }
+      l3g.ownReply.cfgWasSet = true;
+      l3g.ownReply.sensitivity = l3gd20h::ctrlReg4ToSensitivity(l3g.sensorCfg[3]);
+    }
+    cmdBuf[0] = l3gd20h::READ_START | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK;
+    std::memset(cmdBuf.data() + 1, 0, l3gd20h::READ_LEN);
+    result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
+    if (result != returnvalue::OK) {
+      l3g.replyResult = returnvalue::FAILED;
+      return;
+    }
+    // The regular read function always returns the full sensor config as well. Use that
+    // to verify communications.
+    for (uint8_t idx = 0; idx < 5; idx++) {
+      if (rawReply[idx + 1] != l3g.sensorCfg[idx]) {
+        sif::warning << "AcsBoardPolling: l3g config check missmatch" << std::endl;
+        l3g.replyResult = returnvalue::FAILED;
+        return;
+      }
+    }
+    l3g.ownReply.statusReg = rawReply[l3gd20h::STATUS_IDX];
+    l3g.ownReply.angVelocities[0] = (rawReply[l3gd20h::OUT_X_H] << 8) | rawReply[l3gd20h::OUT_X_L];
+    l3g.ownReply.angVelocities[1] = (rawReply[l3gd20h::OUT_Y_H] << 8) | rawReply[l3gd20h::OUT_Y_L];
+    l3g.ownReply.angVelocities[2] = (rawReply[l3gd20h::OUT_Z_H] << 8) | rawReply[l3gd20h::OUT_Z_L];
+    l3g.ownReply.tempOffsetRaw = rawReply[l3gd20h::TEMPERATURE_IDX];
+  }
+}
+
+ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen) {
+  ReturnValue_t result = returnvalue::OK;
+  int retval = 0;
+  // Prepare transfer
+  int fileDescriptor = 0;
+  std::string device = spiComIF.getSpiDev();
+  UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
+  if (fileHelper.getOpenResult() != returnvalue::OK) {
+    return SpiComIF::OPENING_FILE_FAILED;
+  }
+  spi::SpiModes spiMode = spi::SpiModes::MODE_0;
+  uint32_t spiSpeed = 0;
+  cookie.getSpiParameters(spiMode, spiSpeed, nullptr);
+  spiComIF.setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
+  cookie.assignWriteBuffer(cmdBuf.data());
+  cookie.setTransferSize(2);
+
+  gpioId_t gpioId = cookie.getChipSelectPin();
+  MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
+  uint32_t timeoutMs = 0;
+  MutexIF* mutex = spiComIF.getCsMutex();
+  cookie.getMutexParams(timeoutType, timeoutMs);
+  if (mutex == nullptr) {
+    sif::warning << "GyroADIS16507Handler::spiSendCallback: "
+                    "Mutex or GPIO interface invalid"
+                 << std::endl;
+    return returnvalue::FAILED;
+  }
+
+  size_t idx = 0;
+  spi_ioc_transfer* transferStruct = cookie.getTransferStructHandle();
+  uint64_t origTx = transferStruct->tx_buf;
+  uint64_t origRx = transferStruct->rx_buf;
+  while (idx < transferLen) {
+    result = mutex->lockMutex(timeoutType, timeoutMs);
+    if (result != returnvalue::OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+      sif::error << "AcsBoardPolling: Failed to lock mutex" << std::endl;
+#endif
+      return result;
+    }
+    // Pull SPI CS low. For now, no support for active high given
+    if (gpioId != gpio::NO_GPIO) {
+      gpioIF.pullLow(gpioId);
+    }
+
+    // Execute transfer
+    // Initiate a full duplex SPI transfer.
+    retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie.getTransferStructHandle());
+    if (retval < 0) {
+      utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
+      result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
+    }
+#if FSFW_HAL_SPI_WIRETAPPING == 1
+    comIf->performSpiWiretapping(cookie);
+#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
+
+    if (gpioId != gpio::NO_GPIO) {
+      gpioIF.pullHigh(gpioId);
+    }
+    mutex->unlockMutex();
+
+    idx += 2;
+    transferStruct->tx_buf += 2;
+    transferStruct->rx_buf += 2;
+    if (idx < transferLen) {
+      usleep(adis1650x::STALL_TIME_MICROSECONDS);
+    }
+  }
+  transferStruct->tx_buf = origTx;
+  transferStruct->rx_buf = origRx;
+  cookie.setTransferSize(transferLen);
+  return returnvalue::OK;
+}
+
+void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
+  ReturnValue_t result;
+  acs::SimpleSensorMode mode;
+  bool cdHasTimedOut = false;
+  bool mustPerformStartup = false;
+  {
+    MutexGuard mg(ipcLock);
+    mode = gyro.mode;
+    cdHasTimedOut = gyro.countdown.hasTimedOut();
+    mustPerformStartup = gyro.performStartup;
+  }
+  if (mode == acs::SimpleSensorMode::NORMAL and cdHasTimedOut) {
+    if (mustPerformStartup) {
+      uint8_t regList[6];
+      // Read configuration
+      regList[0] = adis1650x::DIAG_STAT_REG;
+      regList[1] = adis1650x::FILTER_CTRL_REG;
+      regList[2] = adis1650x::RANG_MDL_REG;
+      regList[3] = adis1650x::MSC_CTRL_REG;
+      regList[4] = adis1650x::DEC_RATE_REG;
+      regList[5] = adis1650x::PROD_ID_REG;
+      size_t transferLen =
+          adis1650x::prepareReadCommand(regList, sizeof(regList), cmdBuf.data(), cmdBuf.size());
+      result = readAdisCfg(*gyro.cookie, transferLen);
+      if (result != returnvalue::OK) {
+        gyro.replyResult = result;
+        return;
+      }
+      result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy);
+      if (result != returnvalue::OK or rawReply == nullptr) {
+        gyro.replyResult = result;
+        return;
+      }
+      uint16_t prodId = (rawReply[12] << 8) | rawReply[13];
+      if (((gyro.type == adis1650x::Type::ADIS16505) and (prodId != adis1650x::PROD_ID_16505)) or
+          ((gyro.type == adis1650x::Type::ADIS16507) and (prodId != adis1650x::PROD_ID_16507))) {
+        sif::warning << "AcsPollingTask: Invalid  ADIS product ID " << prodId << std::endl;
+        gyro.replyResult = returnvalue::FAILED;
+        return;
+      }
+      MutexGuard mg(ipcLock);
+      gyro.ownReply.cfgWasSet = true;
+      gyro.ownReply.cfg.diagStat = (rawReply[2] << 8) | rawReply[3];
+      gyro.ownReply.cfg.filterSetting = (rawReply[4] << 8) | rawReply[5];
+      gyro.ownReply.cfg.rangMdl = (rawReply[6] << 8) | rawReply[7];
+      gyro.ownReply.cfg.mscCtrlReg = (rawReply[8] << 8) | rawReply[9];
+      gyro.ownReply.cfg.decRateReg = (rawReply[10] << 8) | rawReply[11];
+      gyro.ownReply.cfg.prodId = prodId;
+      gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl);
+      gyro.performStartup = false;
+    }
+    // Read regular registers
+    std::memcpy(cmdBuf.data(), adis1650x::BURST_READ_ENABLE.data(),
+                adis1650x::BURST_READ_ENABLE.size());
+    std::memset(cmdBuf.data() + 2, 0, 10 * 2);
+    result = spiComIF.sendMessage(gyro.cookie, cmdBuf.data(), adis1650x::SENSOR_READOUT_SIZE);
+    if (result != returnvalue::OK) {
+      gyro.replyResult = returnvalue::FAILED;
+      return;
+    }
+    result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy);
+    if (result != returnvalue::OK or rawReply == nullptr) {
+      gyro.replyResult = returnvalue::FAILED;
+      return;
+    }
+    uint16_t checksum = (rawReply[20] << 8) | rawReply[21];
+
+    // Now verify the read checksum with the expected checksum according to datasheet p. 20
+    uint16_t calcChecksum = 0;
+    for (size_t idx = 2; idx < 20; idx++) {
+      calcChecksum += rawReply[idx];
+    }
+    if (checksum != calcChecksum) {
+      sif::warning << "AcsPollingTask: Invalid ADIS reply checksum" << std::endl;
+      gyro.replyResult = returnvalue::FAILED;
+      return;
+    }
+
+    auto burstMode = adis1650x::burstModeFromMscCtrl(gyro.ownReply.cfg.mscCtrlReg);
+    if (burstMode != adis1650x::BurstModes::BURST_16_BURST_SEL_0) {
+      sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Analysis for select burst mode"
+                    " not implemented!"
+                 << std::endl;
+      gyro.replyResult = returnvalue::FAILED;
+      return;
+    }
+
+    MutexGuard mg(ipcLock);
+    gyro.ownReply.dataWasSet = true;
+    gyro.ownReply.cfg.diagStat = rawReply[2] << 8 | rawReply[3];
+    gyro.ownReply.data.angVelocities[0] = (rawReply[4] << 8) | rawReply[5];
+    gyro.ownReply.data.angVelocities[1] = (rawReply[6] << 8) | rawReply[7];
+    gyro.ownReply.data.angVelocities[2] = (rawReply[8] << 8) | rawReply[9];
+
+    gyro.ownReply.data.accelerations[0] = (rawReply[10] << 8) | rawReply[11];
+    gyro.ownReply.data.accelerations[1] = (rawReply[12] << 8) | rawReply[13];
+    gyro.ownReply.data.accelerations[2] = (rawReply[14] << 8) | rawReply[15];
+
+    gyro.ownReply.data.temperatureRaw = (rawReply[16] << 8) | rawReply[17];
+  }
+}

linux/devices/AcsBoardPolling.h

@@ -0,0 +1,81 @@
+#ifndef LINUX_DEVICES_ACSBOARDPOLLING_H_
+#define LINUX_DEVICES_ACSBOARDPOLLING_H_
+
+#include <fsfw/devicehandlers/DeviceCommunicationIF.h>
+#include <fsfw/objectmanager/SystemObject.h>
+#include <fsfw/tasks/ExecutableObjectIF.h>
+#include <fsfw/tasks/SemaphoreIF.h>
+#include <fsfw_hal/linux/spi/SpiComIF.h>
+#include <mission/devices/devicedefinitions/acsPolling.h>
+#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
+
+class AcsBoardPolling : public SystemObject,
+                        public ExecutableObjectIF,
+                        public DeviceCommunicationIF {
+ public:
+  AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF, GpioIF& gpioIF);
+
+  ReturnValue_t performOperation(uint8_t operationCode) override;
+  ReturnValue_t initialize() override;
+
+ private:
+  enum class InternalState { IDLE, BUSY } state = InternalState::IDLE;
+  MutexIF* ipcLock;
+  SemaphoreIF* semaphore;
+  std::array<uint8_t, 32> cmdBuf;
+  std::array<uint8_t, 32> replyBuf;
+
+  bool mgm0L3IsOn = false;
+  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;
+    acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
+    ReturnValue_t replyResult;
+    acs::Adis1650XReply ownReply;
+    acs::Adis1650XReply readerReply;
+  };
+  GyroAdis gyro0Adis{};
+  GyroAdis gyro2Adis{};
+
+  struct GyroL3g {
+    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{};
+
+  uint8_t* rawReply = nullptr;
+  size_t dummy = 0;
+
+  SpiComIF& spiComIF;
+  GpioIF& gpioIF;
+
+  ReturnValue_t initializeInterface(CookieIF* cookie) override;
+  ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override;
+  ReturnValue_t getSendSuccess(CookieIF* cookie) override;
+  ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override;
+  ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override;
+
+  void gyroL3gHandler(GyroL3g& l3g);
+  void gyroAdisHandler(GyroAdis& gyro);
+  // Special readout: 16us stall time between small 2 byte transfers.
+  ReturnValue_t readAdisCfg(SpiCookie& spiCookie, size_t transferLen);
+};
+
+#endif /* LINUX_DEVICES_ACSBOARDPOLLING_H_ */

linux/devices/CMakeLists.txt

@@ -4,8 +4,13 @@ endif()
 
 target_sources(
   ${OBSW_NAME}
-  PRIVATE Max31865RtdPolling.cpp ScexUartReader.cpp ImtqPollingTask.cpp
+  PRIVATE Max31865RtdPolling.cpp
-          ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp)
+          ScexUartReader.cpp
+          ImtqPollingTask.cpp
+          ScexDleParser.cpp
+          ScexHelper.cpp
+          RwPollingTask.cpp
+          AcsBoardPolling.cpp)
 
 add_subdirectory(ploc)
 

linux/fsfwconfig/objects/systemObjectList.h

@@ -47,7 +47,6 @@ enum sourceObjects : uint32_t {
   GPIO_IF = 0x49010005,
 
   /* Custom device handler */
-  RW_POLLING_TASK = 0x49020005,
 
   /* 0x54 ('T') for test handlers */
   TEST_TASK = 0x54694269,

mission/controller/ThermalController.cpp

@@ -7,9 +7,9 @@
 #include <linux/devices/devicedefinitions/StarTrackerDefinitions.h>
 #include <mission/devices/devicedefinitions/BpxBatteryDefinitions.h>
 #include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
-#include <mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h>
 #include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
 #include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
+#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
 #include <mission/devices/devicedefinitions/imtqHelpers.h>
 #include <mission/devices/devicedefinitions/payloadPcduDefinitions.h>
 #include <mission/devices/devicedefinitions/rwHelpers.h>
@@ -854,7 +854,7 @@ void ThermalController::copyDevices() {
 
   {
     lp_var_t<float> tempGyro0 =
-        lp_var_t<float>(objects::GYRO_0_ADIS_HANDLER, ADIS1650X::TEMPERATURE);
+        lp_var_t<float>(objects::GYRO_0_ADIS_HANDLER, adis1650x::TEMPERATURE);
     PoolReadGuard pg(&tempGyro0, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg.getReadResult() != returnvalue::OK) {
       sif::warning << "ThermalController: Failed to read gyro 0 temperature" << std::endl;
@@ -867,7 +867,7 @@ void ThermalController::copyDevices() {
   }
 
   {
-    lp_var_t<float> tempGyro1 = lp_var_t<float>(objects::GYRO_1_L3G_HANDLER, L3GD20H::TEMPERATURE);
+    lp_var_t<float> tempGyro1 = lp_var_t<float>(objects::GYRO_1_L3G_HANDLER, l3gd20h::TEMPERATURE);
     PoolReadGuard pg(&tempGyro1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg.getReadResult() != returnvalue::OK) {
       sif::warning << "ThermalController: Failed to read gyro 1 temperature" << std::endl;
@@ -881,7 +881,7 @@ void ThermalController::copyDevices() {
 
   {
     lp_var_t<float> tempGyro2 =
-        lp_var_t<float>(objects::GYRO_2_ADIS_HANDLER, ADIS1650X::TEMPERATURE);
+        lp_var_t<float>(objects::GYRO_2_ADIS_HANDLER, adis1650x::TEMPERATURE);
     PoolReadGuard pg(&tempGyro2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg.getReadResult() != returnvalue::OK) {
       sif::warning << "ThermalController: Failed to read gyro 2 temperature" << std::endl;
@@ -894,7 +894,7 @@ void ThermalController::copyDevices() {
   }
 
   {
-    lp_var_t<float> tempGyro3 = lp_var_t<float>(objects::GYRO_3_L3G_HANDLER, L3GD20H::TEMPERATURE);
+    lp_var_t<float> tempGyro3 = lp_var_t<float>(objects::GYRO_3_L3G_HANDLER, l3gd20h::TEMPERATURE);
     PoolReadGuard pg(&tempGyro3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
     if (pg.getReadResult() != returnvalue::OK) {
       sif::warning << "ThermalController: Failed to read gyro 3 temperature" << std::endl;

mission/controller/acs/SensorValues.h

@@ -1,6 +1,7 @@
 #ifndef SENSORVALUES_H_
 #define SENSORVALUES_H_
 
+#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
 #include <mission/devices/devicedefinitions/imtqHelpers.h>
 #include <mission/devices/devicedefinitions/rwHelpers.h>
 
@@ -9,7 +10,6 @@
 #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
 #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
 #include "mission/devices/devicedefinitions/GPSDefinitions.h"
-#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
 #include "mission/devices/devicedefinitions/SusDefinitions.h"
 
 namespace ACS {

mission/core/pollingSeqTables.cpp

@@ -189,110 +189,7 @@ ReturnValue_t pst::pstTest(FixedTimeslotTaskIF *thisSequence) {
 ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg) {
   /* Length of a communication cycle */
   uint32_t length = thisSequence->getPeriodMs();
-  bool enableAside = true;
-  bool enableBside = true;
-  if (cfg.scheduleAcsBoard) {
-    if (enableAside) {
-      // A side
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            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,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::GET_READ);
 
-      thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      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);
-      thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::GET_READ);
-    }
-    if (enableBside) {
-      // B side
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            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,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      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);
-      thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
-                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
-
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::PERFORM_OPERATION);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_WRITE);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::GET_WRITE);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::SEND_READ);
-      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
-                            DeviceHandlerIF::GET_READ);
-    }
-  }
   // SUS: 16 ms
   bool addSus0 = true;
   bool addSus1 = true;
@@ -583,6 +480,113 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
     thisSequence->addSlot(objects::STAR_TRACKER, length * 0, DeviceHandlerIF::GET_READ);
   }
 
+  bool enableAside = true;
+  bool enableBside = true;
+  if (cfg.scheduleAcsBoard) {
+    if (enableAside) {
+      // A side
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            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,
+                            DeviceHandlerIF::SEND_READ);
+      thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::GET_READ);
+
+      thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
+                            length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
+                            length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::SEND_WRITE);
+      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);
+      thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
+                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
+    }
+    if (enableBside) {
+      // B side
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            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,
+                            DeviceHandlerIF::SEND_READ);
+      thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::GET_READ);
+
+      thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
+                            length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
+                            length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::SEND_WRITE);
+      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);
+      thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
+                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
+    }
+    if (enableAside) {
+      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
+                            length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
+                            length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::SEND_WRITE);
+      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
+                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
+      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
+                            length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
+      thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
+                            length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
+
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::SEND_WRITE);
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::GET_WRITE);
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::SEND_READ);
+      thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::GET_READ);
+    }
+    if (enableBside) {
+      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
+                            length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
+                            length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::SEND_WRITE);
+      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
+                            length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
+      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
+                            length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::SEND_READ);
+      thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
+                            length * config::acs::SCHED_BLOCK_3_PERIOD, DeviceHandlerIF::GET_READ);
+
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::PERFORM_OPERATION);
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::SEND_WRITE);
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
+                            DeviceHandlerIF::GET_WRITE);
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::SEND_READ);
+      thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_3_PERIOD,
+                            DeviceHandlerIF::GET_READ);
+    }
+  }
+
   if (cfg.scheduleImtq) {
     // This is the MTM measurement cycle
     thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD,

mission/devices/CMakeLists.txt

@@ -14,7 +14,8 @@ target_sources(
           ImtqHandler.cpp
           HeaterHandler.cpp
           RadiationSensorHandler.cpp
-          GyroADIS1650XHandler.cpp
+          GyrAdis1650XHandler.cpp
+          GyrL3gCustomHandler.cpp
           RwHandler.cpp
           max1227.cpp
           SusHandler.cpp

mission/devices/GyrAdis1650XHandler.cpp

@@ -0,0 +1,515 @@
+#include "mission/devices/GyrAdis1650XHandler.h"
+
+#include "fsfw/action/HasActionsIF.h"
+#include "fsfw/datapool/PoolReadGuard.h"
+#include "mission/devices/devicedefinitions/acsPolling.h"
+
+GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t deviceCommunication,
+                                         CookieIF *comCookie, adis1650x::Type type)
+    : DeviceHandlerBase(objectId, deviceCommunication, comCookie),
+      adisType(type),
+      primaryDataset(this),
+      configDataset(this),
+      breakCountdown() {
+  adisRequest.type = adisType;
+}
+
+void GyrAdis1650XHandler::doStartUp() {
+  // Initial 310 ms start up time after power-up
+  if (internalState == InternalState::STARTUP) {
+    if (not commandExecuted) {
+      warningSwitch = true;
+      breakCountdown.setTimeout(adis1650x::START_UP_TIME);
+      commandExecuted = true;
+    }
+    if (breakCountdown.hasTimedOut()) {
+      updatePeriodicReply(true, adis1650x::REPLY);
+      if (goToNormalMode) {
+        setMode(MODE_NORMAL);
+      } else {
+        setMode(MODE_ON);
+      }
+    }
+  }
+
+  //  // Read all configuration registers first
+  //  if (internalState == InternalState::CONFIG) {
+  //    if (commandExecuted) {
+  //      commandExecuted = false;
+  //      internalState = InternalState::IDLE;
+  //    }
+  //  }
+  //
+  //  if (internalState == InternalState::IDLE) {
+  //  }
+}
+
+void GyrAdis1650XHandler::doShutDown() {
+  if (internalState != InternalState::SHUTDOWN) {
+    commandExecuted = false;
+    primaryDataset.setValidity(false, true);
+    internalState = InternalState::SHUTDOWN;
+  }
+  if (commandExecuted) {
+    updatePeriodicReply(false, adis1650x::REPLY);
+    setMode(_MODE_POWER_DOWN);
+  }
+}
+
+ReturnValue_t GyrAdis1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
+  *id = adis1650x::REQUEST;
+  return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL);
+}
+
+ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
+  switch (internalState) {
+    case (InternalState::STARTUP): {
+      *id = adis1650x::REQUEST;
+      return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL);
+    }
+    case (InternalState::SHUTDOWN): {
+      *id = adis1650x::REQUEST;
+      acs::Adis1650XRequest *request = reinterpret_cast<acs::Adis1650XRequest *>(cmdBuf.data());
+      request->mode = acs::SimpleSensorMode::OFF;
+      request->type = adisType;
+      return returnvalue::OK;
+    }
+    default: {
+      // Might be a configuration error
+      sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: "
+                    "Unknown internal state!"
+                 << std::endl;
+      return returnvalue::OK;
+    }
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t GyrAdis1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
+                                                           const uint8_t *commandData,
+                                                           size_t commandDataLen) {
+  // switch (deviceCommand) {
+  //     case (adis1650x::READ_OUT_CONFIG): {
+  //       this->rawPacketLen = adis1650x::CONFIG_READOUT_SIZE;
+  //       uint8_t regList[6] = {};
+  //       regList[0] = adis1650x::DIAG_STAT_REG;
+  //       regList[1] = adis1650x::FILTER_CTRL_REG;
+  //       regList[2] = adis1650x::RANG_MDL_REG;
+  //       regList[3] = adis1650x::MSC_CTRL_REG;
+  //       regList[4] = adis1650x::DEC_RATE_REG;
+  //       regList[5] = adis1650x::PROD_ID_REG;
+  //       adis1650x::prepareReadCommand(regList, sizeof(regList), commandBuffer.data(),
+  //                                     commandBuffer.size());
+  //       this->rawPacket = commandBuffer.data();
+  //       break;
+  //     }
+  //     case (adis1650x::READ_SENSOR_DATA): {
+  //       if (breakCountdown.isBusy()) {
+  //         // A glob command is pending and sensor data can't be read currently
+  //         return NO_REPLY_EXPECTED;
+  //       }
+  //       std::memcpy(commandBuffer.data(), adis1650x::BURST_READ_ENABLE.data(),
+  //                   adis1650x::BURST_READ_ENABLE.size());
+  //       std::memset(commandBuffer.data() + 2, 0, 10 * 2);
+  //       this->rawPacketLen = adis1650x::SENSOR_READOUT_SIZE;
+  //       this->rawPacket = commandBuffer.data();
+  //       break;
+  //     }
+  //  TODO: Convert to special request
+
+  //    case (adis1650x::SELF_TEST_SENSORS): {
+  //      if (breakCountdown.isBusy()) {
+  //        // Another glob command is pending
+  //        return HasActionsIF::IS_BUSY;
+  //      }
+  //      prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::SENSOR_SELF_TEST, 0x00);
+  //      breakCountdown.setTimeout(adis1650x::SELF_TEST_BREAK);
+  //      break;
+  //    }
+  //    case (adis1650x::SELF_TEST_MEMORY): {
+  //      if (breakCountdown.isBusy()) {
+  //        // Another glob command is pending
+  //        return HasActionsIF::IS_BUSY;
+  //      }
+  //      prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::FLASH_MEMORY_TEST, 0x00);
+  //      breakCountdown.setTimeout(adis1650x::FLASH_MEMORY_TEST_BREAK);
+  //      break;
+  //    }
+  //    case (adis1650x::UPDATE_NV_CONFIGURATION): {
+  //      if (breakCountdown.isBusy()) {
+  //        // Another glob command is pending
+  //        return HasActionsIF::IS_BUSY;
+  //      }
+  //      prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::FLASH_MEMORY_UPDATE,
+  //      0x00); breakCountdown.setTimeout(adis1650x::FLASH_MEMORY_UPDATE_BREAK); break;
+  //    }
+  //    case (adis1650x::RESET_SENSOR_CONFIGURATION): {
+  //      if (breakCountdown.isBusy()) {
+  //        // Another glob command is pending
+  //        return HasActionsIF::IS_BUSY;
+  //      }
+  //      prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::FACTORY_CALIBRATION,
+  //      0x00); breakCountdown.setTimeout(adis1650x::FACTORY_CALIBRATION_BREAK); break;
+  //    }
+  //    case (adis1650x::SW_RESET): {
+  //      if (breakCountdown.isBusy()) {
+  //        // Another glob command is pending
+  //        return HasActionsIF::IS_BUSY;
+  //      }
+  //      prepareWriteCommand(adis1650x::GLOB_CMD, adis1650x::GlobCmds::SOFTWARE_RESET, 0x00);
+  //      breakCountdown.setTimeout(adis1650x::SW_RESET_BREAK);
+  //      break;
+  //    }
+  //    case (adis1650x::PRINT_CURRENT_CONFIGURATION): {
+  //#if OBSW_VERBOSE_LEVEL >= 1
+  //      PoolReadGuard pg(&configDataset);
+  //      sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4)
+  //                << std::setfill('0') << "0x" << configDataset.diagStatReg.value << std::endl;
+  //      sif::info << "MSC_CTRL: " << std::hex << std::setw(4) << "0x"
+  //                << configDataset.mscCtrlReg.value << " | FILT_CTRL: 0x"
+  //                << configDataset.filterSetting.value << " | DEC_RATE: 0x"
+  //                << configDataset.decRateReg.value << std::setfill(' ') << std::endl;
+  //#endif /* OBSW_VERBOSE_LEVEL >= 1 */
+  //
+  //}
+  //}
+  // return returnvalue::OK;
+  return NOTHING_TO_SEND;
+}
+
+void GyrAdis1650XHandler::fillCommandAndReplyMap() {
+  insertInCommandMap(adis1650x::REQUEST);
+  insertInReplyMap(adis1650x::REPLY, 5, nullptr, 0, true);
+  // insertInCommandAndReplyMap(adis1650x::READ_SENSOR_DATA, 1, &primaryDataset);
+  // insertInCommandAndReplyMap(adis1650x::READ_OUT_CONFIG, 1, &configDataset);
+  //  insertInCommandAndReplyMap(adis1650x::SELF_TEST_SENSORS, 1);
+  //  insertInCommandAndReplyMap(adis1650x::SELF_TEST_MEMORY, 1);
+  //  insertInCommandAndReplyMap(adis1650x::UPDATE_NV_CONFIGURATION, 1);
+  //  insertInCommandAndReplyMap(adis1650x::RESET_SENSOR_CONFIGURATION, 1);
+  //  insertInCommandAndReplyMap(adis1650x::SW_RESET, 1);
+  //  insertInCommandAndReplyMap(adis1650x::PRINT_CURRENT_CONFIGURATION, 1);
+}
+
+ReturnValue_t GyrAdis1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize,
+                                                DeviceCommandId_t *foundId, size_t *foundLen) {
+  if (breakCountdown.isBusy() or getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) {
+    return IGNORE_FULL_PACKET;
+  }
+  if (remainingSize != sizeof(acs::Adis1650XReply)) {
+    *foundLen = remainingSize;
+    return returnvalue::FAILED;
+  }
+  *foundId = adis1650x::REPLY;
+  *foundLen = remainingSize;
+
+  return returnvalue::OK;
+}
+
+ReturnValue_t GyrAdis1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
+                                                        const uint8_t *packet) {
+  using namespace adis1650x;
+  const acs::Adis1650XReply *reply = reinterpret_cast<const acs::Adis1650XReply *>(packet);
+  if (internalState == InternalState::STARTUP and reply->cfgWasSet) {
+    switch (adisType) {
+      case (adis1650x::Type::ADIS16507): {
+        if (reply->cfg.prodId != adis1650x::PROD_ID_16507) {
+          sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
+                     << reply->cfg.prodId << "for ADIS type 16507" << std::endl;
+          return returnvalue::FAILED;
+        }
+        break;
+      }
+      case (adis1650x::Type::ADIS16505): {
+        if (reply->cfg.prodId != adis1650x::PROD_ID_16505) {
+          sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
+                     << reply->cfg.prodId << "for ADIS type 16505" << std::endl;
+          return returnvalue::FAILED;
+        }
+        break;
+      }
+    }
+    PoolReadGuard rg(&configDataset);
+    configDataset.setValidity(true, true);
+    configDataset.mscCtrlReg = reply->cfg.mscCtrlReg;
+    configDataset.rangMdl = reply->cfg.rangMdl;
+    configDataset.diagStatReg = reply->cfg.diagStat;
+    configDataset.filterSetting = reply->cfg.filterSetting;
+    configDataset.decRateReg = reply->cfg.decRateReg;
+    commandExecuted = true;
+  }
+  if (reply->dataWasSet) {
+    {
+      PoolReadGuard rg(&configDataset);
+      configDataset.diagStatReg = reply->cfg.diagStat;
+    }
+    auto sensitivity = reply->data.sensitivity;
+    auto accelSensitivity = reply->data.accelScaling;
+    PoolReadGuard pg(&primaryDataset);
+    primaryDataset.setValidity(true, true);
+    primaryDataset.angVelocX = static_cast<double>(reply->data.angVelocities[0]) * sensitivity;
+    primaryDataset.angVelocY = static_cast<double>(reply->data.angVelocities[1]) * sensitivity;
+    primaryDataset.angVelocZ = static_cast<double>(reply->data.angVelocities[2]) * sensitivity;
+    // TODO: Check whether we need to divide by INT16_MAX
+    primaryDataset.accelX = static_cast<double>(reply->data.accelerations[0]) * accelSensitivity;
+    primaryDataset.accelY = static_cast<double>(reply->data.accelerations[1]) * accelSensitivity;
+    primaryDataset.accelZ = static_cast<double>(reply->data.accelerations[2]) * accelSensitivity;
+    primaryDataset.temperature.value = static_cast<float>(reply->data.temperatureRaw) * 0.1;
+  }
+  return returnvalue::OK;
+}
+
+LocalPoolDataSetBase *GyrAdis1650XHandler::getDataSetHandle(sid_t sid) {
+  if (sid.ownerSetId == adis1650x::ADIS_DATASET_ID) {
+    return &primaryDataset;
+  } else if (sid.ownerSetId == adis1650x::ADIS_CFG_DATASET_ID) {
+    return &configDataset;
+  }
+  return nullptr;
+}
+
+// ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) {
+//   using namespace adis1650x;
+//   BurstModes burstMode = getBurstMode();
+//   switch (burstMode) {
+//     case (BurstModes::BURST_16_BURST_SEL_1):
+//     case (BurstModes::BURST_32_BURST_SEL_1): {
+//       sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Analysis with BURST_SEL1"
+//                       " not implemented!"
+//                    << std::endl;
+//       return returnvalue::OK;
+//     }
+//     case (adis1650x::BurstModes::BURST_16_BURST_SEL_0): {
+//       uint16_t checksum = packet[20] << 8 | packet[21];
+//       // Now verify the read checksum with the expected checksum according to datasheet p. 20
+//       uint16_t calcChecksum = 0;
+//       for (size_t idx = 2; idx < 20; idx++) {
+//         calcChecksum += packet[idx];
+//       }
+//       if (checksum != calcChecksum) {
+//#if OBSW_VERBOSE_LEVEL >= 1
+//         sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: "
+//                         "Invalid checksum detected!"
+//                      << std::endl;
+//#endif
+//         return returnvalue::FAILED;
+//       }
+//
+//       ReturnValue_t result = configDataset.diagStatReg.read();
+//       if (result == returnvalue::OK) {
+//         configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
+//         configDataset.diagStatReg.setValid(true);
+//       }
+//       configDataset.diagStatReg.commit();
+//
+//       {
+//         PoolReadGuard pg(&primaryDataset);
+//         int16_t angVelocXRaw = packet[4] << 8 | packet[5];
+//         primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) * sensitivity;
+//         int16_t angVelocYRaw = packet[6] << 8 | packet[7];
+//         primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) * sensitivity;
+//         int16_t angVelocZRaw = packet[8] << 8 | packet[9];
+//         primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) * sensitivity;
+//
+//         float accelScaling = 0;
+//         if (adisType == adis1650x::Type::ADIS16507) {
+//           accelScaling = adis1650x::ACCELEROMETER_RANGE_16507;
+//         } else if (adisType == adis1650x::Type::ADIS16505) {
+//           accelScaling = adis1650x::ACCELEROMETER_RANGE_16505;
+//         } else {
+//           sif::warning << "GyroADIS1650XHandler::handleSensorData: "
+//                           "Unknown ADIS type"
+//                        << std::endl;
+//         }
+//         int16_t accelXRaw = packet[10] << 8 | packet[11];
+//         primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX * accelScaling;
+//         int16_t accelYRaw = packet[12] << 8 | packet[13];
+//         primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX * accelScaling;
+//         int16_t accelZRaw = packet[14] << 8 | packet[15];
+//         primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX * accelScaling;
+//
+//         int16_t temperatureRaw = packet[16] << 8 | packet[17];
+//         primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
+//         // Ignore data counter for now
+//         primaryDataset.setValidity(true, true);
+//       }
+//
+//       if (periodicPrintout) {
+//         if (debugDivider.checkAndIncrement()) {
+//           sif::info << "GyroADIS1650XHandler: Angular velocities in deg / s" << std::endl;
+//           sif::info << "X: " << primaryDataset.angVelocX.value << std::endl;
+//           sif::info << "Y: " << primaryDataset.angVelocY.value << std::endl;
+//           sif::info << "Z: " << primaryDataset.angVelocZ.value << std::endl;
+//           sif::info << "GyroADIS1650XHandler: Accelerations in m / s^2: " << std::endl;
+//           sif::info << "X: " << primaryDataset.accelX.value << std::endl;
+//           sif::info << "Y: " << primaryDataset.accelY.value << std::endl;
+//           sif::info << "Z: " << primaryDataset.accelZ.value << std::endl;
+//         }
+//       }
+//
+//       break;
+//     }
+//     case (BurstModes::BURST_32_BURST_SEL_0): {
+//       break;
+//     }
+//   }
+//   return returnvalue::OK;
+// }
+
+uint32_t GyrAdis1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; }
+
+void GyrAdis1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
+                                              uint8_t valueTwo) {
+  uint8_t secondReg = startReg + 1;
+  startReg |= adis1650x::WRITE_MASK;
+  secondReg |= adis1650x::WRITE_MASK;
+  cmdBuf[0] = startReg;
+  cmdBuf[1] = valueOne;
+  cmdBuf[2] = secondReg;
+  cmdBuf[3] = valueTwo;
+  this->rawPacketLen = 4;
+  this->rawPacket = cmdBuf.data();
+}
+
+ReturnValue_t GyrAdis1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+                                                           LocalDataPoolManager &poolManager) {
+  localDataPoolMap.emplace(adis1650x::ANG_VELOC_X, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::ACCELERATION_X, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::ACCELERATION_Y, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::ACCELERATION_Z, new PoolEntry<double>({0.0}));
+  localDataPoolMap.emplace(adis1650x::TEMPERATURE, new PoolEntry<float>({0.0}));
+
+  localDataPoolMap.emplace(adis1650x::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
+  localDataPoolMap.emplace(adis1650x::FILTER_SETTINGS, new PoolEntry<uint8_t>());
+  localDataPoolMap.emplace(adis1650x::RANG_MDL, &rangMdl);
+  localDataPoolMap.emplace(adis1650x::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
+  localDataPoolMap.emplace(adis1650x::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
+  poolManager.subscribeForRegularPeriodicPacket(
+      subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 5.0));
+  return returnvalue::OK;
+}
+
+adis1650x::BurstModes GyrAdis1650XHandler::getBurstMode() {
+  using namespace adis1650x;
+  configDataset.mscCtrlReg.read();
+  uint16_t currentCtrlReg = configDataset.mscCtrlReg.value;
+  configDataset.mscCtrlReg.commit();
+  return adis1650x::burstModeFromMscCtrl(currentCtrlReg);
+}
+
+//#ifdef FSFW_OSAL_LINUX
+
+// ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
+//                                                     const uint8_t *sendData, size_t sendLen,
+//                                                     void *args) {
+//   GyroADIS1650XHandler *handler = reinterpret_cast<GyroADIS1650XHandler *>(args);
+//   if (handler == nullptr) {
+//     sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
+//                << std::endl;
+//     return returnvalue::FAILED;
+//   }
+//   DeviceCommandId_t currentCommand = handler->getPendingCommand();
+//   switch (currentCommand) {
+//     case (adis1650x::READ_SENSOR_DATA): {
+//       return comIf->performRegularSendOperation(cookie, sendData, sendLen);
+//     }
+//     case (adis1650x::READ_OUT_CONFIG):
+//     default: {
+//       ReturnValue_t result = returnvalue::OK;
+//       int retval = 0;
+//       // Prepare transfer
+//       int fileDescriptor = 0;
+//       std::string device = comIf->getSpiDev();
+//       UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
+//       if (fileHelper.getOpenResult() != returnvalue::OK) {
+//         return SpiComIF::OPENING_FILE_FAILED;
+//       }
+//       spi::SpiModes spiMode = spi::SpiModes::MODE_0;
+//       uint32_t spiSpeed = 0;
+//       cookie->getSpiParameters(spiMode, spiSpeed, nullptr);
+//       comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
+//       cookie->assignWriteBuffer(sendData);
+//       cookie->setTransferSize(2);
+//
+//       gpioId_t gpioId = cookie->getChipSelectPin();
+//       GpioIF &gpioIF = comIf->getGpioInterface();
+//       MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
+//       uint32_t timeoutMs = 0;
+//       MutexIF *mutex = comIf->getCsMutex();
+//       cookie->getMutexParams(timeoutType, timeoutMs);
+//       if (mutex == nullptr) {
+//#if OBSW_VERBOSE_LEVEL >= 1
+//         sif::warning << "GyroADIS16507Handler::spiSendCallback: "
+//                         "Mutex or GPIO interface invalid"
+//                      << std::endl;
+//         return returnvalue::FAILED;
+//#endif
+//       }
+//
+//       if (gpioId != gpio::NO_GPIO) {
+//         result = mutex->lockMutex(timeoutType, timeoutMs);
+//         if (result != returnvalue::OK) {
+//#if FSFW_CPP_OSTREAM_ENABLED == 1
+//           sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
+//#endif
+//           return result;
+//         }
+//       }
+//
+//       size_t idx = 0;
+//       spi_ioc_transfer *transferStruct = cookie->getTransferStructHandle();
+//       uint64_t origTx = transferStruct->tx_buf;
+//       uint64_t origRx = transferStruct->rx_buf;
+//       while (idx < sendLen) {
+//         // Pull SPI CS low. For now, no support for active high given
+//         if (gpioId != gpio::NO_GPIO) {
+//           gpioIF.pullLow(gpioId);
+//         }
+//
+//         // Execute transfer
+//         // Initiate a full duplex SPI transfer.
+//         retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle());
+//         if (retval < 0) {
+//           utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
+//           result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
+//         }
+//#if FSFW_HAL_SPI_WIRETAPPING == 1
+//         comIf->performSpiWiretapping(cookie);
+//#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
+//
+//         if (gpioId != gpio::NO_GPIO) {
+//           gpioIF.pullHigh(gpioId);
+//         }
+//
+//         idx += 2;
+//         if (idx < sendLen) {
+//           usleep(adis1650x::STALL_TIME_MICROSECONDS);
+//         }
+//
+//         transferStruct->tx_buf += 2;
+//         transferStruct->rx_buf += 2;
+//       }
+//       transferStruct->tx_buf = origTx;
+//       transferStruct->rx_buf = origRx;
+//       if (gpioId != gpio::NO_GPIO) {
+//         mutex->unlockMutex();
+//       }
+//     }
+//   }
+//   return returnvalue::OK;
+// }
+
+void GyrAdis1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; }
+
+void GyrAdis1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
+  periodicPrintout = enable;
+  debugDivider.setDivider(divider);
+}
+
+ReturnValue_t GyrAdis1650XHandler::preparePeriodicRequest(acs::SimpleSensorMode mode) {
+  adisRequest.mode = mode;
+  rawPacket = reinterpret_cast<uint8_t *>(&adisRequest);
+  rawPacketLen = sizeof(acs::Adis1650XRequest);
+  return returnvalue::OK;
+}

mission/devices/GyrAdis1650XHandler.h

@@ -1,27 +1,24 @@
 #ifndef MISSION_DEVICES_GYROADIS16507HANDLER_H_
 #define MISSION_DEVICES_GYROADIS16507HANDLER_H_
 
+#include <mission/devices/devicedefinitions/acsPolling.h>
+#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
+
 #include "FSFWConfig.h"
 #include "OBSWConfig.h"
-#include "devicedefinitions/GyroADIS1650XDefinitions.h"
 #include "fsfw/devicehandlers/DeviceHandlerBase.h"
 #include "fsfw/globalfunctions/PeriodicOperationDivider.h"
 
-#ifdef FSFW_OSAL_LINUX
-class SpiComIF;
-class SpiCookie;
-#endif
-
 /**
  * @brief   Device handle for the ADIS16507 Gyroscope by Analog Devices
  * @details
  * Flight manual:
  * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/ADIS16507_Gyro
  */
-class GyroADIS1650XHandler : public DeviceHandlerBase {
+class GyrAdis1650XHandler : public DeviceHandlerBase {
  public:
-  GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
+  GyrAdis1650XHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
-                       ADIS1650X::Type type);
+                      adis1650x::Type type);
 
   void enablePeriodicPrintouts(bool enable, uint8_t divider);
   void setToGoToNormalModeImmediately();
@@ -40,42 +37,31 @@ class GyroADIS1650XHandler : public DeviceHandlerBase {
   uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
   ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                         LocalDataPoolManager &poolManager) override;
+  LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override;
 
  private:
-  std::array<uint8_t, 32> commandBuffer;
+  std::array<uint8_t, 32> cmdBuf;
-  ADIS1650X::Type adisType;
+  acs::Adis1650XRequest adisRequest;
+  adis1650x::Type adisType;
   AdisGyroPrimaryDataset primaryDataset;
   AdisGyroConfigDataset configDataset;
-  double sensitivity = ADIS1650X::SENSITIVITY_UNSET;
+  double sensitivity = adis1650x::SENSITIVITY_UNSET;
 
   bool goToNormalMode = false;
   bool warningSwitch = true;
 
-  enum class InternalState { STARTUP, CONFIG, IDLE };
+  enum class InternalState { STARTUP, SHUTDOWN, IDLE };
-
-  enum class BurstModes {
-    BURST_16_BURST_SEL_0,
-    BURST_16_BURST_SEL_1,
-    BURST_32_BURST_SEL_0,
-    BURST_32_BURST_SEL_1
-  };
 
   InternalState internalState = InternalState::STARTUP;
   bool commandExecuted = false;
 
   PoolEntry<uint16_t> rangMdl = PoolEntry<uint16_t>();
 
-  void prepareReadCommand(uint8_t *regList, size_t len);
+  adis1650x::BurstModes getBurstMode();
-
-  BurstModes getBurstMode();
-
-#ifdef FSFW_OSAL_LINUX
-  static ReturnValue_t spiSendCallback(SpiComIF *comIf, SpiCookie *cookie, const uint8_t *sendData,
-                                       size_t sendLen, void *args);
-#endif
 
   Countdown breakCountdown;
   void prepareWriteCommand(uint8_t startReg, uint8_t valueOne, uint8_t valueTwo);
+  ReturnValue_t preparePeriodicRequest(acs::SimpleSensorMode mode);
 
   ReturnValue_t handleSensorData(const uint8_t *packet);
 

mission/devices/GyrL3gCustomHandler.cpp

@@ -0,0 +1,216 @@
+
+#include <mission/devices/GyrL3gCustomHandler.h>
+
+#include <cmath>
+
+#include "fsfw/datapool/PoolReadGuard.h"
+#include "mission/devices/devicedefinitions/acsPolling.h"
+
+GyrL3gCustomHandler::GyrL3gCustomHandler(object_id_t objectId, object_id_t deviceCommunication,
+                                         CookieIF *comCookie, uint32_t transitionDelayMs)
+    : DeviceHandlerBase(objectId, deviceCommunication, comCookie),
+      transitionDelayMs(transitionDelayMs),
+      dataset(this) {}
+
+GyrL3gCustomHandler::~GyrL3gCustomHandler() = default;
+
+void GyrL3gCustomHandler::doStartUp() {
+  if (internalState == InternalState::NONE) {
+    internalState = InternalState::STARTUP;
+    updatePeriodicReply(true, l3gd20h::REPLY);
+    commandExecuted = false;
+  }
+
+  if (internalState == InternalState::STARTUP) {
+    if (commandExecuted) {
+      if (goNormalModeImmediately) {
+        setMode(MODE_NORMAL);
+      } else {
+        setMode(_MODE_TO_ON);
+      }
+    }
+  }
+}
+
+void GyrL3gCustomHandler::doShutDown() {
+  if (internalState != InternalState::SHUTDOWN) {
+    internalState = InternalState::SHUTDOWN;
+    commandExecuted = false;
+  }
+  if (commandExecuted) {
+    internalState = InternalState::NONE;
+    updatePeriodicReply(false, l3gd20h::REPLY);
+    commandExecuted = false;
+    setMode(_MODE_POWER_DOWN);
+  }
+}
+
+ReturnValue_t GyrL3gCustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
+  switch (internalState) {
+    case (InternalState::NONE):
+    case (InternalState::NORMAL): {
+      return NOTHING_TO_SEND;
+    }
+    case (InternalState::STARTUP): {
+      *id = l3gd20h::REQUEST;
+      gyrRequest.ctrlRegs[0] = l3gd20h::CTRL_REG_1_VAL;
+      gyrRequest.ctrlRegs[1] = l3gd20h::CTRL_REG_2_VAL;
+      gyrRequest.ctrlRegs[2] = l3gd20h::CTRL_REG_3_VAL;
+      gyrRequest.ctrlRegs[3] = l3gd20h::CTRL_REG_4_VAL;
+      gyrRequest.ctrlRegs[4] = l3gd20h::CTRL_REG_5_VAL;
+      return doPeriodicRequest(acs::SimpleSensorMode::NORMAL);
+    }
+    case (InternalState::SHUTDOWN): {
+      *id = l3gd20h::REQUEST;
+      return doPeriodicRequest(acs::SimpleSensorMode::OFF);
+    }
+    default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+      /* Might be a configuration error. */
+      sif::warning << "GyroL3GD20Handler::buildTransitionDeviceCommand: "
+                      "Unknown internal state!"
+                   << std::endl;
+#else
+      sif::printDebug(
+          "GyroL3GD20Handler::buildTransitionDeviceCommand: "
+          "Unknown internal state!\n");
+#endif
+      return returnvalue::OK;
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t GyrL3gCustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
+  *id = l3gd20h::REQUEST;
+  return doPeriodicRequest(acs::SimpleSensorMode::NORMAL);
+}
+
+ReturnValue_t GyrL3gCustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
+                                                           const uint8_t *commandData,
+                                                           size_t commandDataLen) {
+  switch (deviceCommand) {
+    default:
+      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t GyrL3gCustomHandler::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::GyroL3gReply)) {
+    *foundLen = len;
+    return returnvalue::FAILED;
+  }
+  *foundId = l3gd20h::REPLY;
+  *foundLen = len;
+  return returnvalue::OK;
+}
+
+ReturnValue_t GyrL3gCustomHandler::interpretDeviceReply(DeviceCommandId_t id,
+                                                        const uint8_t *packet) {
+  const acs::GyroL3gReply *reply = reinterpret_cast<const acs::GyroL3gReply *>(packet);
+  if (reply->cfgWasSet) {
+    if (internalState == InternalState::STARTUP) {
+      commandExecuted = true;
+    }
+    PoolReadGuard readSet(&dataset);
+    dataset.setValidity(true, true);
+    dataset.angVelocX = reply->angVelocities[0];
+    dataset.angVelocY = reply->angVelocities[1];
+    dataset.angVelocZ = reply->angVelocities[2];
+    if (std::abs(dataset.angVelocX.value) > absLimitX) {
+      dataset.angVelocX.setValid(false);
+    }
+    if (std::abs(dataset.angVelocY.value) > absLimitY) {
+      dataset.angVelocY.setValid(false);
+    }
+    if (std::abs(dataset.angVelocZ.value) > absLimitZ) {
+      dataset.angVelocZ.setValid(false);
+    }
+    dataset.temperature = 25.0 - reply->tempOffsetRaw;
+  }
+  //      PoolReadGuard readSet(&dataset);
+  //      if (readSet.getReadResult() == returnvalue::OK) {
+  //        if (std::abs(angVelocX) < this->absLimitX) {
+  //          dataset.angVelocX = angVelocX;
+  //          dataset.angVelocX.setValid(true);
+  //        } else {
+  //          dataset.angVelocX.setValid(false);
+  //        }
+  //
+  //        if (std::abs(angVelocY) < this->absLimitY) {
+  //          dataset.angVelocY = angVelocY;
+  //          dataset.angVelocY.setValid(true);
+  //        } else {
+  //          dataset.angVelocY.setValid(false);
+  //        }
+  //
+  //        if (std::abs(angVelocZ) < this->absLimitZ) {
+  //          dataset.angVelocZ = angVelocZ;
+  //          dataset.angVelocZ.setValid(true);
+  //        } else {
+  //          dataset.angVelocZ.setValid(false);
+  //        }
+  //
+  //        dataset.temperature = temperature;
+  //        dataset.temperature.setValid(true);
+  //      }
+  //      break;
+  //    }
+  return returnvalue::OK;
+}
+
+uint32_t GyrL3gCustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
+  return this->transitionDelayMs;
+}
+
+void GyrL3gCustomHandler::setToGoToNormalMode(bool enable) { this->goNormalModeImmediately = true; }
+
+ReturnValue_t GyrL3gCustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+                                                           LocalDataPoolManager &poolManager) {
+  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_X, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Y, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Z, new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(l3gd20h::TEMPERATURE, new PoolEntry<float>({0.0}));
+  poolManager.subscribeForRegularPeriodicPacket(
+      subdp::RegularHkPeriodicParams(dataset.getSid(), false, 10.0));
+  return returnvalue::OK;
+}
+
+void GyrL3gCustomHandler::fillCommandAndReplyMap() {
+  insertInCommandMap(l3gd20h::REQUEST);
+  insertInReplyMap(l3gd20h::REPLY, 5, nullptr, 0, true);
+  // insertInCommandAndReplyMap(l3gd20h::READ_REGS, 1, &dataset);
+  // insertInCommandAndReplyMap(l3gd20h::CONFIGURE_CTRL_REGS, 1);
+  // insertInCommandAndReplyMap(l3gd20h::READ_CTRL_REGS, 1);
+}
+
+void GyrL3gCustomHandler::modeChanged() { internalState = InternalState::NONE; }
+
+void GyrL3gCustomHandler::setAbsoluteLimits(float limitX, float limitY, float limitZ) {
+  this->absLimitX = limitX;
+  this->absLimitY = limitY;
+  this->absLimitZ = limitZ;
+}
+
+void GyrL3gCustomHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
+  periodicPrintout = enable;
+  debugDivider.setDivider(divider);
+}
+
+LocalPoolDataSetBase *GyrL3gCustomHandler::getDataSetHandle(sid_t sid) {
+  if (sid == dataset.getSid()) {
+    return &dataset;
+  }
+  return nullptr;
+}
+
+ReturnValue_t GyrL3gCustomHandler::doPeriodicRequest(acs::SimpleSensorMode mode) {
+  gyrRequest.mode = mode;
+  rawPacket = reinterpret_cast<uint8_t *>(&gyrRequest);
+  rawPacketLen = sizeof(acs::GyroL3gRequest);
+  return returnvalue::OK;
+}

mission/devices/GyrL3gCustomHandler.h

@@ -0,0 +1,92 @@
+#ifndef MISSION_DEVICES_GYROL3GD20HANDLER_H_
+#define MISSION_DEVICES_GYROL3GD20HANDLER_H_
+
+#include <fsfw/devicehandlers/DeviceHandlerBase.h>
+#include <fsfw/globalfunctions/PeriodicOperationDivider.h>
+#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
+#include <mission/devices/devicedefinitions/acsPolling.h>
+
+/**
+ * @brief 	Device Handler for the L3GD20H gyroscope sensor
+ *          (https://www.st.com/en/mems-and-sensors/l3gd20h.html)
+ * @details
+ * Advanced documentation:
+ * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/L3GD20H_Gyro
+ *
+ * Data is read big endian with the smallest possible range of 245 degrees per second.
+ */
+class GyrL3gCustomHandler : public DeviceHandlerBase {
+ public:
+  GyrL3gCustomHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie,
+                      uint32_t transitionDelayMs);
+  virtual ~GyrL3gCustomHandler();
+
+  void enablePeriodicPrintouts(bool enable, uint8_t divider);
+
+  /**
+   * Set the absolute limit for the values on the axis in degrees per second.
+   * The dataset values will be marked as invalid if that limit is exceeded
+   * @param xLimit
+   * @param yLimit
+   * @param zLimit
+   */
+  void setAbsoluteLimits(float limitX, float limitY, float limitZ);
+
+  /**
+   * @brief   Configure device handler to go to normal mode immediately
+   */
+  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;
+  virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override;
+
+  void fillCommandAndReplyMap() override;
+  void modeChanged() override;
+  virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override;
+  ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+                                        LocalDataPoolManager &poolManager) override;
+  LocalPoolDataSetBase *getDataSetHandle(sid_t sid) override;
+
+ private:
+  uint32_t transitionDelayMs = 0;
+  GyroPrimaryDataset dataset;
+
+  float absLimitX = l3gd20h::RANGE_DPS_00;
+  float absLimitY = l3gd20h::RANGE_DPS_00;
+  float absLimitZ = l3gd20h::RANGE_DPS_00;
+
+  enum class InternalState { NONE, STARTUP, SHUTDOWN, NORMAL };
+  InternalState internalState = InternalState::NONE;
+  bool commandExecuted = false;
+
+  uint8_t statusReg = 0;
+  bool goNormalModeImmediately = false;
+
+  uint8_t ctrlReg1Value = l3gd20h::CTRL_REG_1_VAL;
+  uint8_t ctrlReg2Value = l3gd20h::CTRL_REG_2_VAL;
+  uint8_t ctrlReg3Value = l3gd20h::CTRL_REG_3_VAL;
+  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
+  float sensitivity = l3gd20h::SENSITIVITY_00;
+
+  bool periodicPrintout = false;
+  PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3);
+
+  ReturnValue_t doPeriodicRequest(acs::SimpleSensorMode mode);
+};
+
+#endif /* MISSION_DEVICES_GYROL3GD20HANDLER_H_ */

mission/devices/GyroADIS1650XHandler.cpp

@@ -1,524 +0,0 @@
-#include "GyroADIS1650XHandler.h"
-
-#include <fsfw/action/HasActionsIF.h>
-#include <fsfw/datapool/PoolReadGuard.h>
-
-#include "fsfw/FSFW.h"
-
-#ifdef FSFW_OSAL_LINUX
-#include <sys/ioctl.h>
-#include <unistd.h>
-
-#include "fsfw_hal/linux/UnixFileGuard.h"
-#include "fsfw_hal/linux/spi/SpiComIF.h"
-#include "fsfw_hal/linux/spi/SpiCookie.h"
-#include "fsfw_hal/linux/utility.h"
-#endif
-
-GyroADIS1650XHandler::GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication,
-                                           CookieIF *comCookie, ADIS1650X::Type type)
-    : DeviceHandlerBase(objectId, deviceCommunication, comCookie),
-      adisType(type),
-      primaryDataset(this),
-      configDataset(this),
-      breakCountdown() {
-#ifdef FSFW_OSAL_LINUX
-  SpiCookie *cookie = dynamic_cast<SpiCookie *>(comCookie);
-  if (cookie != nullptr) {
-    cookie->setCallbackMode(&spiSendCallback, this);
-  }
-#endif
-}
-
-void GyroADIS1650XHandler::doStartUp() {
-  // Initial 310 ms start up time after power-up
-  if (internalState == InternalState::STARTUP) {
-    if (not commandExecuted) {
-      warningSwitch = true;
-      breakCountdown.setTimeout(ADIS1650X::START_UP_TIME);
-      commandExecuted = true;
-    }
-    if (breakCountdown.hasTimedOut()) {
-      internalState = InternalState::CONFIG;
-      commandExecuted = false;
-    }
-  }
-
-  // Read all configuration registers first
-  if (internalState == InternalState::CONFIG) {
-    if (commandExecuted) {
-      commandExecuted = false;
-      internalState = InternalState::IDLE;
-    }
-  }
-
-  if (internalState == InternalState::IDLE) {
-    if (goToNormalMode) {
-      setMode(MODE_NORMAL);
-    } else {
-      setMode(MODE_ON);
-    }
-  }
-}
-
-void GyroADIS1650XHandler::doShutDown() {
-  commandExecuted = false;
-  internalState = InternalState::STARTUP;
-  setMode(_MODE_POWER_DOWN);
-}
-
-ReturnValue_t GyroADIS1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
-  *id = ADIS1650X::READ_SENSOR_DATA;
-  return buildCommandFromCommand(*id, nullptr, 0);
-}
-
-ReturnValue_t GyroADIS1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
-  switch (internalState) {
-    case (InternalState::CONFIG): {
-      *id = ADIS1650X::READ_OUT_CONFIG;
-      buildCommandFromCommand(*id, nullptr, 0);
-      break;
-    }
-    case (InternalState::STARTUP): {
-      return NOTHING_TO_SEND;
-      break;
-    }
-    default: {
-      // Might be a configuration error
-      sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: "
-                    "Unknown internal state!"
-                 << std::endl;
-      return returnvalue::OK;
-    }
-  }
-  return returnvalue::OK;
-}
-
-ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
-                                                            const uint8_t *commandData,
-                                                            size_t commandDataLen) {
-  switch (deviceCommand) {
-    case (ADIS1650X::READ_OUT_CONFIG): {
-      this->rawPacketLen = ADIS1650X::CONFIG_READOUT_SIZE;
-      uint8_t regList[6] = {};
-      regList[0] = ADIS1650X::DIAG_STAT_REG;
-      regList[1] = ADIS1650X::FILTER_CTRL_REG;
-      regList[2] = ADIS1650X::RANG_MDL_REG;
-      regList[3] = ADIS1650X::MSC_CTRL_REG;
-      regList[4] = ADIS1650X::DEC_RATE_REG;
-      regList[5] = ADIS1650X::PROD_ID_REG;
-      prepareReadCommand(regList, sizeof(regList));
-      this->rawPacket = commandBuffer.data();
-      break;
-    }
-    case (ADIS1650X::READ_SENSOR_DATA): {
-      if (breakCountdown.isBusy()) {
-        // A glob command is pending and sensor data can't be read currently
-        return NO_REPLY_EXPECTED;
-      }
-      std::memcpy(commandBuffer.data(), ADIS1650X::BURST_READ_ENABLE.data(),
-                  ADIS1650X::BURST_READ_ENABLE.size());
-      std::memset(commandBuffer.data() + 2, 0, 10 * 2);
-      this->rawPacketLen = ADIS1650X::SENSOR_READOUT_SIZE;
-      this->rawPacket = commandBuffer.data();
-      break;
-    }
-    case (ADIS1650X::SELF_TEST_SENSORS): {
-      if (breakCountdown.isBusy()) {
-        // Another glob command is pending
-        return HasActionsIF::IS_BUSY;
-      }
-      prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SENSOR_SELF_TEST, 0x00);
-      breakCountdown.setTimeout(ADIS1650X::SELF_TEST_BREAK);
-      break;
-    }
-    case (ADIS1650X::SELF_TEST_MEMORY): {
-      if (breakCountdown.isBusy()) {
-        // Another glob command is pending
-        return HasActionsIF::IS_BUSY;
-      }
-      prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_TEST, 0x00);
-      breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_TEST_BREAK);
-      break;
-    }
-    case (ADIS1650X::UPDATE_NV_CONFIGURATION): {
-      if (breakCountdown.isBusy()) {
-        // Another glob command is pending
-        return HasActionsIF::IS_BUSY;
-      }
-      prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_UPDATE, 0x00);
-      breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_UPDATE_BREAK);
-      break;
-    }
-    case (ADIS1650X::RESET_SENSOR_CONFIGURATION): {
-      if (breakCountdown.isBusy()) {
-        // Another glob command is pending
-        return HasActionsIF::IS_BUSY;
-      }
-      prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FACTORY_CALIBRATION, 0x00);
-      breakCountdown.setTimeout(ADIS1650X::FACTORY_CALIBRATION_BREAK);
-      break;
-    }
-    case (ADIS1650X::SW_RESET): {
-      if (breakCountdown.isBusy()) {
-        // Another glob command is pending
-        return HasActionsIF::IS_BUSY;
-      }
-      prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SOFTWARE_RESET, 0x00);
-      breakCountdown.setTimeout(ADIS1650X::SW_RESET_BREAK);
-      break;
-    }
-    case (ADIS1650X::PRINT_CURRENT_CONFIGURATION): {
-#if OBSW_VERBOSE_LEVEL >= 1
-      PoolReadGuard pg(&configDataset);
-      sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4)
-                << std::setfill('0') << "0x" << configDataset.diagStatReg.value << std::endl;
-      sif::info << "MSC_CTRL: " << std::hex << std::setw(4) << "0x"
-                << configDataset.mscCtrlReg.value << " | FILT_CTRL: 0x"
-                << configDataset.filterSetting.value << " | DEC_RATE: 0x"
-                << configDataset.decRateReg.value << std::setfill(' ') << std::endl;
-#endif /* OBSW_VERBOSE_LEVEL >= 1 */
-    }
-  }
-  return returnvalue::OK;
-}
-
-void GyroADIS1650XHandler::fillCommandAndReplyMap() {
-  insertInCommandAndReplyMap(ADIS1650X::READ_SENSOR_DATA, 1, &primaryDataset);
-  insertInCommandAndReplyMap(ADIS1650X::READ_OUT_CONFIG, 1, &configDataset);
-  insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_SENSORS, 1);
-  insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_MEMORY, 1);
-  insertInCommandAndReplyMap(ADIS1650X::UPDATE_NV_CONFIGURATION, 1);
-  insertInCommandAndReplyMap(ADIS1650X::RESET_SENSOR_CONFIGURATION, 1);
-  insertInCommandAndReplyMap(ADIS1650X::SW_RESET, 1);
-  insertInCommandAndReplyMap(ADIS1650X::PRINT_CURRENT_CONFIGURATION, 1);
-}
-
-ReturnValue_t GyroADIS1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize,
-                                                 DeviceCommandId_t *foundId, size_t *foundLen) {
-  // For SPI, the ID will always be the one of the last sent command
-  *foundId = this->getPendingCommand();
-  *foundLen = this->rawPacketLen;
-
-  return returnvalue::OK;
-}
-
-ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
-                                                         const uint8_t *packet) {
-  using namespace ADIS1650X;
-  switch (id) {
-    case (ADIS1650X::READ_OUT_CONFIG): {
-      uint16_t readProdId = packet[12] << 8 | packet[13];
-      if (((adisType == ADIS1650X::Type::ADIS16507) and (readProdId != ADIS1650X::PROD_ID_16507)) or
-          ((adisType == ADIS1650X::Type::ADIS16505) and (readProdId != ADIS1650X::PROD_ID_16505))) {
-#if OBSW_VERBOSE_LEVEL >= 1
-        if (warningSwitch) {
-          sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
-                       << readProdId << std::endl;
-        }
-        warningSwitch = false;
-#endif
-        return returnvalue::FAILED;
-      }
-      PoolReadGuard rg(&configDataset);
-      configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
-      configDataset.filterSetting.value = packet[4] << 8 | packet[5];
-      uint16_t rangMdlRaw = packet[6] << 8 | packet[7];
-      ADIS1650X::RangMdlBitfield bitfield =
-          static_cast<ADIS1650X::RangMdlBitfield>((rangMdlRaw >> 2) & 0b11);
-      switch (bitfield) {
-        case (ADIS1650X::RangMdlBitfield::RANGE_125_1BMLZ): {
-          sensitivity = SENSITIVITY_1BMLZ;
-          break;
-        }
-        case (ADIS1650X::RangMdlBitfield::RANGE_500_2BMLZ): {
-          sensitivity = SENSITIVITY_2BMLZ;
-          break;
-        }
-        case (ADIS1650X::RangMdlBitfield::RANGE_2000_3BMLZ): {
-          sensitivity = SENSITIVITY_3BMLZ;
-          break;
-        }
-        case (RangMdlBitfield::RESERVED): {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-          sif::error << "ADIS1650X: Unexpected value for RANG_MDL register" << std::endl;
-#endif
-          break;
-        }
-      }
-      configDataset.rangMdl.value = rangMdlRaw;
-      configDataset.mscCtrlReg.value = packet[8] << 8 | packet[9];
-      configDataset.decRateReg.value = packet[10] << 8 | packet[11];
-      configDataset.setValidity(true, true);
-      if (internalState == InternalState::CONFIG) {
-        commandExecuted = true;
-      }
-      break;
-    }
-    case (ADIS1650X::READ_SENSOR_DATA): {
-      return handleSensorData(packet);
-    }
-  }
-  return returnvalue::OK;
-}
-
-ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) {
-  BurstModes burstMode = getBurstMode();
-  switch (burstMode) {
-    case (BurstModes::BURST_16_BURST_SEL_1):
-    case (BurstModes::BURST_32_BURST_SEL_1): {
-      sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Analysis with BURST_SEL1"
-                      " not implemented!"
-                   << std::endl;
-      return returnvalue::OK;
-    }
-    case (BurstModes::BURST_16_BURST_SEL_0): {
-      uint16_t checksum = packet[20] << 8 | packet[21];
-      // Now verify the read checksum with the expected checksum according to datasheet p. 20
-      uint16_t calcChecksum = 0;
-      for (size_t idx = 2; idx < 20; idx++) {
-        calcChecksum += packet[idx];
-      }
-      if (checksum != calcChecksum) {
-#if OBSW_VERBOSE_LEVEL >= 1
-        sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: "
-                        "Invalid checksum detected!"
-                     << std::endl;
-#endif
-        return returnvalue::FAILED;
-      }
-
-      ReturnValue_t result = configDataset.diagStatReg.read();
-      if (result == returnvalue::OK) {
-        configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
-        configDataset.diagStatReg.setValid(true);
-      }
-      configDataset.diagStatReg.commit();
-
-      {
-        PoolReadGuard pg(&primaryDataset);
-        int16_t angVelocXRaw = packet[4] << 8 | packet[5];
-        primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) * sensitivity;
-        int16_t angVelocYRaw = packet[6] << 8 | packet[7];
-        primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) * sensitivity;
-        int16_t angVelocZRaw = packet[8] << 8 | packet[9];
-        primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) * sensitivity;
-
-        float accelScaling = 0;
-        if (adisType == ADIS1650X::Type::ADIS16507) {
-          accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16507;
-        } else if (adisType == ADIS1650X::Type::ADIS16505) {
-          accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16505;
-        } else {
-          sif::warning << "GyroADIS1650XHandler::handleSensorData: "
-                          "Unknown ADIS type"
-                       << std::endl;
-        }
-        int16_t accelXRaw = packet[10] << 8 | packet[11];
-        primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX * accelScaling;
-        int16_t accelYRaw = packet[12] << 8 | packet[13];
-        primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX * accelScaling;
-        int16_t accelZRaw = packet[14] << 8 | packet[15];
-        primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX * accelScaling;
-
-        int16_t temperatureRaw = packet[16] << 8 | packet[17];
-        primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
-        // Ignore data counter for now
-        primaryDataset.setValidity(true, true);
-      }
-
-      if (periodicPrintout) {
-        if (debugDivider.checkAndIncrement()) {
-          sif::info << "GyroADIS1650XHandler: Angular velocities in deg / s" << std::endl;
-          sif::info << "X: " << primaryDataset.angVelocX.value << std::endl;
-          sif::info << "Y: " << primaryDataset.angVelocY.value << std::endl;
-          sif::info << "Z: " << primaryDataset.angVelocZ.value << std::endl;
-          sif::info << "GyroADIS1650XHandler: Accelerations in m / s^2: " << std::endl;
-          sif::info << "X: " << primaryDataset.accelX.value << std::endl;
-          sif::info << "Y: " << primaryDataset.accelY.value << std::endl;
-          sif::info << "Z: " << primaryDataset.accelZ.value << std::endl;
-        }
-      }
-
-      break;
-    }
-    case (BurstModes::BURST_32_BURST_SEL_0): {
-      break;
-    }
-  }
-  return returnvalue::OK;
-}
-
-uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; }
-
-void GyroADIS1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
-                                               uint8_t valueTwo) {
-  uint8_t secondReg = startReg + 1;
-  startReg |= ADIS1650X::WRITE_MASK;
-  secondReg |= ADIS1650X::WRITE_MASK;
-  commandBuffer[0] = startReg;
-  commandBuffer[1] = valueOne;
-  commandBuffer[2] = secondReg;
-  commandBuffer[3] = valueTwo;
-  this->rawPacketLen = 4;
-  this->rawPacket = commandBuffer.data();
-}
-
-void GyroADIS1650XHandler::prepareReadCommand(uint8_t *regList, size_t len) {
-  for (size_t idx = 0; idx < len; idx++) {
-    commandBuffer[idx * 2] = regList[idx];
-    commandBuffer[idx * 2 + 1] = 0x00;
-  }
-  commandBuffer[len * 2] = 0x00;
-  commandBuffer[len * 2 + 1] = 0x00;
-}
-
-ReturnValue_t GyroADIS1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
-                                                            LocalDataPoolManager &poolManager) {
-  localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry<double>({0.0}));
-  localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry<float>({0.0}));
-
-  localDataPoolMap.emplace(ADIS1650X::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
-  localDataPoolMap.emplace(ADIS1650X::FILTER_SETTINGS, new PoolEntry<uint8_t>());
-  localDataPoolMap.emplace(ADIS1650X::RANG_MDL, &rangMdl);
-  localDataPoolMap.emplace(ADIS1650X::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
-  localDataPoolMap.emplace(ADIS1650X::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
-  poolManager.subscribeForRegularPeriodicPacket(
-      subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 5.0));
-  return returnvalue::OK;
-}
-
-GyroADIS1650XHandler::BurstModes GyroADIS1650XHandler::getBurstMode() {
-  configDataset.mscCtrlReg.read();
-  uint16_t currentCtrlReg = configDataset.mscCtrlReg.value;
-  configDataset.mscCtrlReg.commit();
-  if ((currentCtrlReg & ADIS1650X::BURST_32_BIT) == ADIS1650X::BURST_32_BIT) {
-    if ((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) {
-      return BurstModes::BURST_32_BURST_SEL_1;
-    } else {
-      return BurstModes::BURST_32_BURST_SEL_0;
-    }
-  } else {
-    if ((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) {
-      return BurstModes::BURST_16_BURST_SEL_1;
-    } else {
-      return BurstModes::BURST_16_BURST_SEL_0;
-    }
-  }
-}
-
-#ifdef FSFW_OSAL_LINUX
-
-ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
-                                                    const uint8_t *sendData, size_t sendLen,
-                                                    void *args) {
-  GyroADIS1650XHandler *handler = reinterpret_cast<GyroADIS1650XHandler *>(args);
-  if (handler == nullptr) {
-    sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
-               << std::endl;
-    return returnvalue::FAILED;
-  }
-  DeviceCommandId_t currentCommand = handler->getPendingCommand();
-  switch (currentCommand) {
-    case (ADIS1650X::READ_SENSOR_DATA): {
-      return comIf->performRegularSendOperation(cookie, sendData, sendLen);
-    }
-    case (ADIS1650X::READ_OUT_CONFIG):
-    default: {
-      ReturnValue_t result = returnvalue::OK;
-      int retval = 0;
-      // Prepare transfer
-      int fileDescriptor = 0;
-      std::string device = comIf->getSpiDev();
-      UnixFileGuard fileHelper(device, fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
-      if (fileHelper.getOpenResult() != returnvalue::OK) {
-        return SpiComIF::OPENING_FILE_FAILED;
-      }
-      spi::SpiModes spiMode = spi::SpiModes::MODE_0;
-      uint32_t spiSpeed = 0;
-      cookie->getSpiParameters(spiMode, spiSpeed, nullptr);
-      comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
-      cookie->assignWriteBuffer(sendData);
-      cookie->setTransferSize(2);
-
-      gpioId_t gpioId = cookie->getChipSelectPin();
-      GpioIF &gpioIF = comIf->getGpioInterface();
-      MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
-      uint32_t timeoutMs = 0;
-      MutexIF *mutex = comIf->getCsMutex();
-      cookie->getMutexParams(timeoutType, timeoutMs);
-      if (mutex == nullptr) {
-#if OBSW_VERBOSE_LEVEL >= 1
-        sif::warning << "GyroADIS16507Handler::spiSendCallback: "
-                        "Mutex or GPIO interface invalid"
-                     << std::endl;
-        return returnvalue::FAILED;
-#endif
-      }
-
-      if (gpioId != gpio::NO_GPIO) {
-        result = mutex->lockMutex(timeoutType, timeoutMs);
-        if (result != returnvalue::OK) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-          sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
-#endif
-          return result;
-        }
-      }
-
-      size_t idx = 0;
-      spi_ioc_transfer *transferStruct = cookie->getTransferStructHandle();
-      uint64_t origTx = transferStruct->tx_buf;
-      uint64_t origRx = transferStruct->rx_buf;
-      while (idx < sendLen) {
-        // Pull SPI CS low. For now, no support for active high given
-        if (gpioId != gpio::NO_GPIO) {
-          gpioIF.pullLow(gpioId);
-        }
-
-        // Execute transfer
-        // Initiate a full duplex SPI transfer.
-        retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle());
-        if (retval < 0) {
-          utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
-          result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
-        }
-#if FSFW_HAL_SPI_WIRETAPPING == 1
-        comIf->performSpiWiretapping(cookie);
-#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */
-
-        if (gpioId != gpio::NO_GPIO) {
-          gpioIF.pullHigh(gpioId);
-        }
-
-        idx += 2;
-        if (idx < sendLen) {
-          usleep(ADIS1650X::STALL_TIME_MICROSECONDS);
-        }
-
-        transferStruct->tx_buf += 2;
-        transferStruct->rx_buf += 2;
-      }
-      transferStruct->tx_buf = origTx;
-      transferStruct->rx_buf = origRx;
-      if (gpioId != gpio::NO_GPIO) {
-        mutex->unlockMutex();
-      }
-    }
-  }
-  return returnvalue::OK;
-}
-
-void GyroADIS1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; }
-
-void GyroADIS1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
-  periodicPrintout = enable;
-  debugDivider.setDivider(divider);
-}
-
-#endif /* OBSW_ADIS1650X_LINUX_COM_IF == 1 */

mission/devices/devicedefinitions/CMakeLists.txt

@@ -1,2 +1,2 @@
 target_sources(${LIB_EIVE_MISSION} PRIVATE ScexDefinitions.cpp rwHelpers.cpp
-                                           imtqHelpers.cpp)
+                                           imtqHelpers.cpp gyroAdisHelpers.cpp)

mission/devices/devicedefinitions/GyroL3GD20Definitions.h

@@ -6,7 +6,7 @@
 
 #include <cstdint>
 
-namespace L3GD20H {
+namespace l3gd20h {
 
 /* Actual size is 15 but we round up a bit */
 static constexpr size_t MAX_BUFFER_SIZE = 16;
@@ -104,27 +104,27 @@ static constexpr uint32_t GYRO_DATASET_ID = READ_REGS;
 
 enum GyroPoolIds : lp_id_t { ANG_VELOC_X, ANG_VELOC_Y, ANG_VELOC_Z, TEMPERATURE };
 
-}  // namespace L3GD20H
+}  // namespace l3gd20h
 
 class GyroPrimaryDataset : public StaticLocalDataSet<5> {
  public:
   /** Constructor  for data users like controllers */
   GyroPrimaryDataset(object_id_t mgmId)
-      : StaticLocalDataSet(sid_t(mgmId, L3GD20H::GYRO_DATASET_ID)) {
+      : StaticLocalDataSet(sid_t(mgmId, l3gd20h::GYRO_DATASET_ID)) {
     setAllVariablesReadOnly();
   }
 
   /* Angular velocities in degrees per second (DPS) */
-  lp_var_t<float> angVelocX = lp_var_t<float>(sid.objectId, L3GD20H::ANG_VELOC_X, this);
+  lp_var_t<float> angVelocX = lp_var_t<float>(sid.objectId, l3gd20h::ANG_VELOC_X, this);
-  lp_var_t<float> angVelocY = lp_var_t<float>(sid.objectId, L3GD20H::ANG_VELOC_Y, this);
+  lp_var_t<float> angVelocY = lp_var_t<float>(sid.objectId, l3gd20h::ANG_VELOC_Y, this);
-  lp_var_t<float> angVelocZ = lp_var_t<float>(sid.objectId, L3GD20H::ANG_VELOC_Z, this);
+  lp_var_t<float> angVelocZ = lp_var_t<float>(sid.objectId, l3gd20h::ANG_VELOC_Z, this);
-  lp_var_t<float> temperature = lp_var_t<float>(sid.objectId, L3GD20H::TEMPERATURE, this);
+  lp_var_t<float> temperature = lp_var_t<float>(sid.objectId, l3gd20h::TEMPERATURE, this);
 
  private:
   friend class GyroHandlerL3GD20H;
   /** Constructor  for the data creator */
   GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner)
-      : StaticLocalDataSet(hkOwner, L3GD20H::GYRO_DATASET_ID) {}
+      : StaticLocalDataSet(hkOwner, l3gd20h::GYRO_DATASET_ID) {}
 };
 
 #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ */

mission/devices/devicedefinitions/acsPolling.h

@@ -0,0 +1,58 @@
+#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_
+#define MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_
+
+#include "gyroAdisHelpers.h"
+
+namespace acs {
+
+enum SimpleSensorMode { NORMAL = 0, OFF = 1 };
+
+struct Adis1650XRequest {
+  SimpleSensorMode mode;
+  adis1650x::Type type;
+};
+
+struct Adis1650XConfig {
+  uint16_t diagStat;
+  uint16_t filterSetting;
+  uint16_t rangMdl;
+  uint16_t mscCtrlReg;
+  uint16_t decRateReg;
+  uint16_t prodId;
+};
+
+struct Adis1650XData {
+  double sensitivity;
+  // Angular velocities in all axes (X, Y and Z)
+  int16_t angVelocities[3];
+  double accelScaling;
+  // Accelerations in all axes
+  int16_t accelerations[3];
+  int16_t temperatureRaw;
+};
+
+struct Adis1650XReply {
+  bool cfgWasSet = false;
+  Adis1650XConfig cfg;
+  bool dataWasSet = false;
+  Adis1650XData data;
+};
+
+struct GyroL3gRequest {
+  SimpleSensorMode mode;
+  uint8_t ctrlRegs[5];
+};
+
+struct GyroL3gReply {
+  bool cfgWasSet;
+  uint8_t statusReg;
+  // Angular velocities in all axes (X, Y and Z)
+  int16_t angVelocities[3];
+  int8_t tempOffsetRaw;
+  uint8_t ctrlRegs[5];
+  float sensitivity;
+};
+
+}  // namespace acs
+
+#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_ACSPOLLING_H_ */

mission/devices/devicedefinitions/gyroAdisHelpers.cpp

@@ -0,0 +1,54 @@
+#include "gyroAdisHelpers.h"
+
+size_t adis1650x::prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf,
+                                     size_t maxLen) {
+  if (len * 2 + 2 > maxLen) {
+    return 0;
+  }
+  for (size_t idx = 0; idx < len; idx++) {
+    outBuf[idx * 2] = regList[idx];
+    outBuf[idx * 2 + 1] = 0x00;
+  }
+  outBuf[len * 2] = 0x00;
+  outBuf[len * 2 + 1] = 0x00;
+  return len * 2 + 2;
+}
+
+adis1650x::BurstModes adis1650x::burstModeFromMscCtrl(uint16_t mscCtrl) {
+  if ((mscCtrl & adis1650x::BURST_32_BIT) == adis1650x::BURST_32_BIT) {
+    if ((mscCtrl & adis1650x::BURST_SEL_BIT) == adis1650x::BURST_SEL_BIT) {
+      return BurstModes::BURST_32_BURST_SEL_1;
+    } else {
+      return BurstModes::BURST_32_BURST_SEL_0;
+    }
+  } else {
+    if ((mscCtrl & adis1650x::BURST_SEL_BIT) == adis1650x::BURST_SEL_BIT) {
+      return BurstModes::BURST_16_BURST_SEL_1;
+    } else {
+      return BurstModes::BURST_16_BURST_SEL_0;
+    }
+  }
+}
+
+double adis1650x::rangMdlToSensitivity(uint16_t rangMdl) {
+  adis1650x::RangMdlBitfield bitfield =
+      static_cast<adis1650x::RangMdlBitfield>((rangMdl >> 2) & 0b11);
+  switch (bitfield) {
+    case (adis1650x::RangMdlBitfield::RANGE_125_1BMLZ): {
+      return SENSITIVITY_1BMLZ;
+    }
+    case (adis1650x::RangMdlBitfield::RANGE_500_2BMLZ): {
+      return SENSITIVITY_2BMLZ;
+    }
+    case (adis1650x::RangMdlBitfield::RANGE_2000_3BMLZ): {
+      return SENSITIVITY_3BMLZ;
+    }
+    case (RangMdlBitfield::RESERVED):
+    default: {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+      sif::error << "ADIS1650X: Unexpected value for RANG_MDL register" << std::endl;
+#endif
+      return 0.0;
+    }
+  }
+}

mission/devices/devicedefinitions/gyroAdisHelpers.h

@@ -6,9 +6,20 @@
 #include "fsfw/datapoollocal/StaticLocalDataSet.h"
 #include "fsfw/devicehandlers/DeviceHandlerIF.h"
 
-namespace ADIS1650X {
+namespace adis1650x {
 
-enum class Type { ADIS16505, ADIS16507 };
+enum class BurstModes {
+  BURST_16_BURST_SEL_0,
+  BURST_16_BURST_SEL_1,
+  BURST_32_BURST_SEL_0,
+  BURST_32_BURST_SEL_1
+};
+
+size_t prepareReadCommand(const uint8_t* regList, size_t len, uint8_t* outBuf, size_t maxLen);
+BurstModes burstModeFromMscCtrl(uint16_t mscCtrl);
+double rangMdlToSensitivity(uint16_t rangMdl);
+
+enum class Type : uint8_t { ADIS16505, ADIS16507 };
 
 static constexpr size_t MAXIMUM_REPLY_SIZE = 64;
 static constexpr uint8_t WRITE_MASK = 0b1000'0000;
@@ -67,6 +78,9 @@ static constexpr DeviceCommandId_t RESET_SENSOR_CONFIGURATION = 30;
 static constexpr DeviceCommandId_t SW_RESET = 31;
 static constexpr DeviceCommandId_t PRINT_CURRENT_CONFIGURATION = 32;
 
+static constexpr DeviceCommandId_t REQUEST = 0x70;
+static constexpr DeviceCommandId_t REPLY = 0x77;
+
 static constexpr uint16_t BURST_32_BIT = 1 << 9;
 static constexpr uint16_t BURST_SEL_BIT = 1 << 8;
 static constexpr uint16_t LIN_ACCEL_COMPENSATION_BIT = 1 << 7;
@@ -111,57 +125,57 @@ enum FilterSettings : uint8_t {
   SIXTYFOUR_TAPS = 6
 };
 
-}  // namespace ADIS1650X
+}  // namespace adis1650x
 
 class AdisGyroPrimaryDataset : public StaticLocalDataSet<8> {
  public:
   /** Constructor  for data users like controllers */
   AdisGyroPrimaryDataset(object_id_t adisId)
-      : StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_DATASET_ID)) {
+      : StaticLocalDataSet(sid_t(adisId, adis1650x::ADIS_DATASET_ID)) {
     setAllVariablesReadOnly();
   }
 
   /* Angular velocities in degrees per second (DPS) */
-  lp_var_t<double> angVelocX = lp_var_t<double>(sid.objectId, ADIS1650X::ANG_VELOC_X, this);
+  lp_var_t<double> angVelocX = lp_var_t<double>(sid.objectId, adis1650x::ANG_VELOC_X, this);
-  lp_var_t<double> angVelocY = lp_var_t<double>(sid.objectId, ADIS1650X::ANG_VELOC_Y, this);
+  lp_var_t<double> angVelocY = lp_var_t<double>(sid.objectId, adis1650x::ANG_VELOC_Y, this);
-  lp_var_t<double> angVelocZ = lp_var_t<double>(sid.objectId, ADIS1650X::ANG_VELOC_Z, this);
+  lp_var_t<double> angVelocZ = lp_var_t<double>(sid.objectId, adis1650x::ANG_VELOC_Z, this);
-  lp_var_t<double> accelX = lp_var_t<double>(sid.objectId, ADIS1650X::ACCELERATION_X, this);
+  lp_var_t<double> accelX = lp_var_t<double>(sid.objectId, adis1650x::ACCELERATION_X, this);
-  lp_var_t<double> accelY = lp_var_t<double>(sid.objectId, ADIS1650X::ACCELERATION_Y, this);
+  lp_var_t<double> accelY = lp_var_t<double>(sid.objectId, adis1650x::ACCELERATION_Y, this);
-  lp_var_t<double> accelZ = lp_var_t<double>(sid.objectId, ADIS1650X::ACCELERATION_Z, this);
+  lp_var_t<double> accelZ = lp_var_t<double>(sid.objectId, adis1650x::ACCELERATION_Z, this);
-  lp_var_t<float> temperature = lp_var_t<float>(sid.objectId, ADIS1650X::TEMPERATURE, this);
+  lp_var_t<float> temperature = lp_var_t<float>(sid.objectId, adis1650x::TEMPERATURE, this);
 
  private:
-  friend class GyroADIS1650XHandler;
+  friend class GyrAdis1650XHandler;
   friend class GyroAdisDummy;
 
   /** Constructor for the data creator */
   AdisGyroPrimaryDataset(HasLocalDataPoolIF* hkOwner)
-      : StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_DATASET_ID) {}
+      : StaticLocalDataSet(hkOwner, adis1650x::ADIS_DATASET_ID) {}
 };
 
 class AdisGyroConfigDataset : public StaticLocalDataSet<5> {
  public:
   /** Constructor  for data users like controllers */
   AdisGyroConfigDataset(object_id_t adisId)
-      : StaticLocalDataSet(sid_t(adisId, ADIS1650X::ADIS_CFG_DATASET_ID)) {
+      : StaticLocalDataSet(sid_t(adisId, adis1650x::ADIS_CFG_DATASET_ID)) {
     setAllVariablesReadOnly();
   }
 
   lp_var_t<uint16_t> diagStatReg =
-      lp_var_t<uint16_t>(sid.objectId, ADIS1650X::DIAG_STAT_REGISTER, this);
+      lp_var_t<uint16_t>(sid.objectId, adis1650x::DIAG_STAT_REGISTER, this);
   lp_var_t<uint8_t> filterSetting =
-      lp_var_t<uint8_t>(sid.objectId, ADIS1650X::FILTER_SETTINGS, this);
+      lp_var_t<uint8_t>(sid.objectId, adis1650x::FILTER_SETTINGS, this);
-  lp_var_t<uint16_t> rangMdl = lp_var_t<uint16_t>(sid.objectId, ADIS1650X::RANG_MDL, this);
+  lp_var_t<uint16_t> rangMdl = lp_var_t<uint16_t>(sid.objectId, adis1650x::RANG_MDL, this);
   lp_var_t<uint16_t> mscCtrlReg =
-      lp_var_t<uint16_t>(sid.objectId, ADIS1650X::MSC_CTRL_REGISTER, this);
+      lp_var_t<uint16_t>(sid.objectId, adis1650x::MSC_CTRL_REGISTER, this);
   lp_var_t<uint16_t> decRateReg =
-      lp_var_t<uint16_t>(sid.objectId, ADIS1650X::DEC_RATE_REGISTER, this);
+      lp_var_t<uint16_t>(sid.objectId, adis1650x::DEC_RATE_REGISTER, this);
 
  private:
-  friend class GyroADIS1650XHandler;
+  friend class GyrAdis1650XHandler;
   /** Constructor for the data creator */
   AdisGyroConfigDataset(HasLocalDataPoolIF* hkOwner)
-      : StaticLocalDataSet(hkOwner, ADIS1650X::ADIS_CFG_DATASET_ID) {}
+      : StaticLocalDataSet(hkOwner, adis1650x::ADIS_CFG_DATASET_ID) {}
 };
 
 #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROADIS16507DEFINITIONS_H_ */

mission/system/objects/AcsBoardAssembly.cpp

@@ -274,4 +274,9 @@ void AcsBoardAssembly::refreshHelperModes() {
   }
 }
 
-ReturnValue_t AcsBoardAssembly::initialize() { return AssemblyBase::initialize(); }
+ReturnValue_t AcsBoardAssembly::initialize() {
+  for (const auto& child : childrenMap) {
+    updateChildModeByObjId(child.first, MODE_OFF, 0);
+  }
+  return AssemblyBase::initialize();
+}

mission/system/objects/SusAssembly.cpp

@@ -120,7 +120,12 @@ ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wan
   return returnvalue::OK;
 }
 
-ReturnValue_t SusAssembly::initialize() { return AssemblyBase::initialize(); }
+ReturnValue_t SusAssembly::initialize() {
+  for (const auto& child : childrenMap) {
+    updateChildModeByObjId(child.first, MODE_OFF, 0);
+  }
+  return AssemblyBase::initialize();
+}
 
 bool SusAssembly::isUseable(object_id_t object, Mode_t mode) {
   if (healthHelper.healthTable->isFaulty(object)) {

tmtc

@@ -1 +1 @@
-Subproject commit 13014eb25053368f4fb9a445788aba71ff98de19
+Subproject commit 9720fcddecb04b228dc5eb0d064f15a12ef8daca