2023-04-03 15:12:49 +02:00
23 changed files with 371 additions and 243 deletions
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@ -16,6 +16,30 @@ will consitute of a breaking change warranting a new major release:

 # [unreleased]

+## Added
+
+- Added NaN and Inf check for the `MEKF`. If these are detected, the `AcsController` will reset
+  the `MEKF` on its own once. This way, there will not be an event spam and operators will have
+  to look into the reason of wrong outputs. To restore the reset ability, an action command has
+  been added.
+
+
+## Fixed
+
+- Fixed transition for dual power lane assemblies: When going from dual side submode to single side
+  submode, perform logical commanding first, similarly to when going to OFF mode.
+
+## Changed
+
+- Bugfixes for STR mode transitions: Booting to mode ON with submode FIRMWARE now works properly.
+  Furthermore, the submode in the NORMAL mode now should be 0 instead of some ON mode submode.
+- Updated GYR bias values to newest measurements. This also corrects the ADIS values to always
+  consit of just one digit.
+
+# [v1.38.0] 2023-03-17
+
+eive-tmtc: v2.19.2
+
 ## Fixed

 - SA deployment file handling: Use exceptionless API.
@ -53,8 +77,7 @@ eive-tmtc: v2.19.1
  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/462
 - Add `PcduHandlerDummy` component.
 - Added parameter for timeout until `MEKF_INVALID_MODE_VIOLATION` event is triggered.
- EIVE system: Add boot mode which is also the initial mode. The fallback mode of the boot mode
-  will be the SAFE mode. The boot mode can also be used to switch as many devices as possible OFF.
+
 ## Fixed

 - Pointing control of the `AcsController` was still expecting submodes instead of modes.
@ -80,9 +103,6 @@ eive-tmtc: v2.19.1
  commanding is done autonomously by the COM subsystem internally or by the operator. This prevents
  the transmitter from going off during fallbacks to the SAFE mode, which might not always be
  desired.
-
-## Changed
-
 - Initialize switch states to a special `SWITCH_STATE_UNKNOWN` (2) variable. Return
  `PowerSwitchIF::SWITCH_UNKNOWN` in switch state getter if this is the state.
 - Wait 1 second before commanding SAFE mode. This ensures or at least increases the chance that
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -10,8 +10,8 @@
 cmake_minimum_required(VERSION 3.13)

 set(OBSW_VERSION_MAJOR 1)
-set(OBSW_VERSION_MINOR 37)
-set(OBSW_VERSION_REVISION 2)
+set(OBSW_VERSION_MINOR 38)
+set(OBSW_VERSION_REVISION 0)

 # set(CMAKE_VERBOSE TRUE)

--- a/bsp_hosted/fsfwconfig/events/translateEvents.cpp
+++ b/bsp_hosted/fsfwconfig/events/translateEvents.cpp
@ -1,7 +1,7 @@
 /**
 * @brief    Auto-generated event translation file. Contains 277 translations.
 * @details
- * Generated on: 2023-03-16 17:47:14
+ * Generated on: 2023-03-21 14:18:44
 */
 #include "translateEvents.h"

--- a/bsp_hosted/fsfwconfig/objects/translateObjects.cpp
+++ b/bsp_hosted/fsfwconfig/objects/translateObjects.cpp
@ -2,7 +2,7 @@
 * @brief  Auto-generated object translation file.
 * @details
 * Contains 169 translations.
- * Generated on: 2023-03-16 17:47:14
+ * Generated on: 2023-03-21 14:18:44
 */
 #include "translateObjects.h"

--- a/generators/bsp_hosted_returnvalues.csv
+++ b/generators/bsp_hosted_returnvalues.csv
@ -470,8 +470,9 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x6a04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
 0x6a05;ACSMEKF_MekfNoSusMgmStrData;No description;5;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
 0x6a06;ACSMEKF_MekfCovarianceInversionFailed;No description;6;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
-0x6a07;ACSMEKF_MekfInitialized;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
-0x6a08;ACSMEKF_MekfRunning;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a07;ACSMEKF_MekfNotFinite;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a08;ACSMEKF_MekfInitialized;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a09;ACSMEKF_MekfRunning;No description;9;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
 0x6b01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h
 0x6c01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h
 0x6d01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h
--- a/generators/bsp_q7s_returnvalues.csv
+++ b/generators/bsp_q7s_returnvalues.csv
@ -581,8 +581,9 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
 0x6a04;ACSMEKF_MekfNoModelVectors;No description;4;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
 0x6a05;ACSMEKF_MekfNoSusMgmStrData;No description;5;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
 0x6a06;ACSMEKF_MekfCovarianceInversionFailed;No description;6;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
-0x6a07;ACSMEKF_MekfInitialized;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
-0x6a08;ACSMEKF_MekfRunning;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a07;ACSMEKF_MekfNotFinite;No description;7;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a08;ACSMEKF_MekfInitialized;No description;8;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
+0x6a09;ACSMEKF_MekfRunning;No description;9;ACS_MEKF;mission/controller/acs/MultiplicativeKalmanFilter.h
 0x6b01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h
 0x6c01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h
 0x6d01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h
--- a/generators/events/translateEvents.cpp
+++ b/generators/events/translateEvents.cpp
@ -1,7 +1,7 @@
 /**
 * @brief    Auto-generated event translation file. Contains 277 translations.
 * @details
- * Generated on: 2023-03-16 17:47:14
+ * Generated on: 2023-03-21 14:18:44
 */
 #include "translateEvents.h"

--- a/generators/objects/translateObjects.cpp
+++ b/generators/objects/translateObjects.cpp
@ -2,7 +2,7 @@
 * @brief  Auto-generated object translation file.
 * @details
 * Contains 173 translations.
- * Generated on: 2023-03-16 17:47:14
+ * Generated on: 2023-03-21 14:18:44
 */
 #include "translateObjects.h"

--- a/linux/devices/AcsBoardPolling.cpp
+++ b/linux/devices/AcsBoardPolling.cpp
@ -451,95 +451,99 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
    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, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
-      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 (mode == acs::SimpleSensorMode::OFF) {
+    return;
+  }
+  if (not cdHasTimedOut) {
+    return;
+  }
+  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 = returnvalue::FAILED;
+      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;
    }
-    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, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
-    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];
+    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, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+  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];
 }

 void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
--- a/linux/devices/startracker/StarTrackerHandler.cpp
+++ b/linux/devices/startracker/StarTrackerHandler.cpp
@ -268,18 +268,18 @@ void StarTrackerHandler::doStartUp() {
    default:
      return;
  }
-  setMode(_MODE_TO_ON, SUBMODE_BOOTLOADER);
+  startupState = StartupState::DONE;
+  internalState = InternalState::IDLE;
+  setMode(_MODE_TO_ON);
 }

 void StarTrackerHandler::doShutDown() {
  // If the star tracker is shutdown also stop all running processes in the image loader task
  strHelper->stopProcess();
-  setMode(_MODE_POWER_DOWN);
-}
-
-void StarTrackerHandler::doOffActivity() {
-  startupState = StartupState::IDLE;
  internalState = InternalState::IDLE;
+  startupState = StartupState::IDLE;
+  bootState = FwBootState::NONE;
+  setMode(_MODE_POWER_DOWN);
 }

 ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
@ -302,81 +302,103 @@ ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t* id

 ReturnValue_t StarTrackerHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
  switch (internalState) {
-    case InternalState::BOOT:
-      *id = startracker::BOOT;
-      bootCountdown.setTimeout(BOOT_TIMEOUT);
-      internalState = InternalState::BOOT_DELAY;
-      return buildCommandFromCommand(*id, nullptr, 0);
-    case InternalState::REQ_VERSION:
-      internalState = InternalState::VERIFY_BOOT;
-      // Again read program to check if firmware boot was successful
-      *id = startracker::REQ_VERSION;
-      return buildCommandFromCommand(*id, nullptr, 0);
-    case InternalState::LOGLEVEL:
-      internalState = InternalState::WAIT_FOR_EXECUTION;
-      *id = startracker::LOGLEVEL;
-      return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
-                                     paramJsonFile.size());
-    case InternalState::LIMITS:
-      internalState = InternalState::WAIT_FOR_EXECUTION;
-      *id = startracker::LIMITS;
-      return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
-                                     paramJsonFile.size());
-    case InternalState::TRACKING:
-      internalState = InternalState::WAIT_FOR_EXECUTION;
-      *id = startracker::TRACKING;
-      return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
-                                     paramJsonFile.size());
-    case InternalState::MOUNTING:
-      internalState = InternalState::WAIT_FOR_EXECUTION;
-      *id = startracker::MOUNTING;
-      return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
-                                     paramJsonFile.size());
-    case InternalState::IMAGE_PROCESSOR:
-      internalState = InternalState::WAIT_FOR_EXECUTION;
-      *id = startracker::IMAGE_PROCESSOR;
-      return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
-                                     paramJsonFile.size());
-    case InternalState::CAMERA:
-      internalState = InternalState::WAIT_FOR_EXECUTION;
-      *id = startracker::CAMERA;
-      return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
-                                     paramJsonFile.size());
-    case InternalState::CENTROIDING:
-      internalState = InternalState::WAIT_FOR_EXECUTION;
-      *id = startracker::CENTROIDING;
-      return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
-                                     paramJsonFile.size());
-    case InternalState::LISA:
-      internalState = InternalState::WAIT_FOR_EXECUTION;
-      *id = startracker::LISA;
-      return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
-                                     paramJsonFile.size());
-    case InternalState::MATCHING:
-      internalState = InternalState::WAIT_FOR_EXECUTION;
-      *id = startracker::MATCHING;
-      return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
-                                     paramJsonFile.size());
-    case InternalState::VALIDATION:
-      internalState = InternalState::WAIT_FOR_EXECUTION;
-      *id = startracker::VALIDATION;
-      return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
-                                     paramJsonFile.size());
-    case InternalState::ALGO:
-      internalState = InternalState::WAIT_FOR_EXECUTION;
-      *id = startracker::ALGO;
-      return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
-                                     paramJsonFile.size());
-    case InternalState::LOG_SUBSCRIPTION:
-      internalState = InternalState::WAIT_FOR_EXECUTION;
-      *id = startracker::LOGSUBSCRIPTION;
-      return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
-                                     paramJsonFile.size());
-    case InternalState::DEBUG_CAMERA:
-      internalState = InternalState::WAIT_FOR_EXECUTION;
-      *id = startracker::DEBUG_CAMERA;
-      return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
-                                     paramJsonFile.size());
+    case InternalState::BOOT_FIRMWARE: {
+      if (bootState == FwBootState::WAIT_FOR_EXECUTION or bootState == FwBootState::VERIFY_BOOT) {
+        return NOTHING_TO_SEND;
+      }
+      if (bootState == FwBootState::NONE) {
+        *id = startracker::BOOT;
+        bootCountdown.setTimeout(BOOT_TIMEOUT);
+        bootState = FwBootState::BOOT_DELAY;
+        return buildCommandFromCommand(*id, nullptr, 0);
+      }
+      if (bootState == FwBootState::BOOT_DELAY) {
+        if (bootCountdown.isBusy()) {
+          return NOTHING_TO_SEND;
+        }
+        bootState = FwBootState::REQ_VERSION;
+      }
+      switch (bootState) {
+        case (FwBootState::REQ_VERSION): {
+          bootState = FwBootState::VERIFY_BOOT;
+          // Again read program to check if firmware boot was successful
+          *id = startracker::REQ_VERSION;
+          return buildCommandFromCommand(*id, nullptr, 0);
+        }
+        case (FwBootState::LOGLEVEL): {
+          bootState = FwBootState::WAIT_FOR_EXECUTION;
+          *id = startracker::LOGLEVEL;
+          return buildCommandFromCommand(
+              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
+        }
+        case (FwBootState::LIMITS): {
+          bootState = FwBootState::WAIT_FOR_EXECUTION;
+          *id = startracker::LIMITS;
+          return buildCommandFromCommand(
+              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
+        }
+        case (FwBootState::TRACKING): {
+          bootState = FwBootState::WAIT_FOR_EXECUTION;
+          *id = startracker::TRACKING;
+          return buildCommandFromCommand(
+              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
+        }
+        case FwBootState::MOUNTING:
+          bootState = FwBootState::WAIT_FOR_EXECUTION;
+          *id = startracker::MOUNTING;
+          return buildCommandFromCommand(
+              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
+        case FwBootState::IMAGE_PROCESSOR:
+          bootState = FwBootState::WAIT_FOR_EXECUTION;
+          *id = startracker::IMAGE_PROCESSOR;
+          return buildCommandFromCommand(
+              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
+        case FwBootState::CAMERA:
+          bootState = FwBootState::WAIT_FOR_EXECUTION;
+          *id = startracker::CAMERA;
+          return buildCommandFromCommand(
+              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
+        case FwBootState::CENTROIDING:
+          bootState = FwBootState::WAIT_FOR_EXECUTION;
+          *id = startracker::CENTROIDING;
+          return buildCommandFromCommand(
+              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
+        case FwBootState::LISA:
+          bootState = FwBootState::WAIT_FOR_EXECUTION;
+          *id = startracker::LISA;
+          return buildCommandFromCommand(
+              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
+        case FwBootState::MATCHING:
+          bootState = FwBootState::WAIT_FOR_EXECUTION;
+          *id = startracker::MATCHING;
+          return buildCommandFromCommand(
+              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
+        case FwBootState::VALIDATION:
+          bootState = FwBootState::WAIT_FOR_EXECUTION;
+          *id = startracker::VALIDATION;
+          return buildCommandFromCommand(
+              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
+        case FwBootState::ALGO:
+          bootState = FwBootState::WAIT_FOR_EXECUTION;
+          *id = startracker::ALGO;
+          return buildCommandFromCommand(
+              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
+        case FwBootState::LOG_SUBSCRIPTION:
+          bootState = FwBootState::WAIT_FOR_EXECUTION;
+          *id = startracker::LOGSUBSCRIPTION;
+          return buildCommandFromCommand(
+              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
+        case FwBootState::DEBUG_CAMERA:
+          bootState = FwBootState::WAIT_FOR_EXECUTION;
+          *id = startracker::DEBUG_CAMERA;
+          return buildCommandFromCommand(
+              *id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()), paramJsonFile.size());
+        default: {
+          sif::error << "STR: Unexpected boot state" << (int)bootState << std::endl;
+          return NOTHING_TO_SEND;
+        }
+      }
+    }
    case InternalState::BOOT_BOOTLOADER:
      internalState = InternalState::BOOTLOADER_CHECK;
      *id = startracker::SWITCH_TO_BOOTLOADER_PROGRAM;
@ -707,6 +729,15 @@ void StarTrackerHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {

 void StarTrackerHandler::doOnTransition(Submode_t subModeFrom) {
  uint8_t dhbSubmode = getSubmode();
+  // We hide that the transition to submode firmware actually goes through the submode bootloader.
+  // This is because the startracker always starts in bootloader mode but we want to allow direct
+  // transitions to firmware mode.
+  if (startupState == StartupState::DONE) {
+    subModeFrom = SUBMODE_BOOTLOADER;
+  }
+  if (dhbSubmode == SUBMODE_NONE) {
+    bootFirmware(MODE_ON);
+  }
  if (dhbSubmode == SUBMODE_BOOTLOADER && subModeFrom == SUBMODE_FIRMWARE) {
    bootBootloader();
  } else if (dhbSubmode == SUBMODE_FIRMWARE && subModeFrom == SUBMODE_FIRMWARE) {
@ -736,19 +767,23 @@ void StarTrackerHandler::doNormalTransition(Mode_t modeFrom, Submode_t subModeFr
 void StarTrackerHandler::bootFirmware(Mode_t toMode) {
  switch (internalState) {
    case InternalState::IDLE:
-      internalState = InternalState::BOOT;
+      sif::info << "STR: Booting to firmware mode" << std::endl;
+      internalState = InternalState::BOOT_FIRMWARE;
      break;
-    case InternalState::BOOT_DELAY:
-      if (bootCountdown.hasTimedOut()) {
-        internalState = InternalState::REQ_VERSION;
-      }
+    case InternalState::BOOT_FIRMWARE:
      break;
    case InternalState::FAILED_FIRMWARE_BOOT:
      internalState = InternalState::IDLE;
      break;
    case InternalState::DONE:
-      setMode(toMode);
+      if (toMode == MODE_NORMAL) {
+        setMode(toMode, 0);
+      } else {
+        setMode(toMode, SUBMODE_FIRMWARE);
+      }
+      sif::info << "STR: Firmware boot success" << std::endl;
      internalState = InternalState::IDLE;
+      startupState = StartupState::IDLE;
      break;
    default:
      return;
@ -776,10 +811,11 @@ void StarTrackerHandler::setUpJsonCfgs(JsonConfigs& cfgs, const char* paramJsonF
 void StarTrackerHandler::bootBootloader() {
  if (internalState == InternalState::IDLE) {
    internalState = InternalState::BOOT_BOOTLOADER;
-  } else if (internalState == InternalState::BOOTING_BOOTLOADER_FAILED) {
+  } else if (internalState == InternalState::FAILED_BOOTLOADER_BOOT) {
    internalState = InternalState::IDLE;
  } else if (internalState == InternalState::DONE) {
    internalState = InternalState::IDLE;
+    startupState = StartupState::IDLE;
    setMode(MODE_ON);
  }
 }
@ -1934,7 +1970,7 @@ ReturnValue_t StarTrackerHandler::checkProgram() {
      if (startupState == StartupState::WAIT_CHECK_PROGRAM) {
        startupState = StartupState::DONE;
      }
-      if (internalState == InternalState::VERIFY_BOOT) {
+      if (bootState == FwBootState::VERIFY_BOOT) {
        sif::warning << "StarTrackerHandler::checkProgram: Failed to boot firmware" << std::endl;
        // Device handler will run into timeout and fall back to transition source mode
        triggerEvent(BOOTING_FIRMWARE_FAILED_EVENT);
@ -1947,11 +1983,11 @@ ReturnValue_t StarTrackerHandler::checkProgram() {
      if (startupState == StartupState::WAIT_CHECK_PROGRAM) {
        startupState = StartupState::BOOT_BOOTLOADER;
      }
-      if (internalState == InternalState::VERIFY_BOOT) {
-        internalState = InternalState::LOGLEVEL;
+      if (bootState == FwBootState::VERIFY_BOOT) {
+        bootState = FwBootState::LOGLEVEL;
      } else if (internalState == InternalState::BOOTLOADER_CHECK) {
        triggerEvent(BOOTING_BOOTLOADER_FAILED_EVENT);
-        internalState = InternalState::BOOTING_BOOTLOADER_FAILED;
+        internalState = InternalState::FAILED_BOOTLOADER_BOOT;
      }
      break;
    default:
@ -2025,54 +2061,55 @@ ReturnValue_t StarTrackerHandler::handleActionReplySet(LocalPoolDataSetBase& dat
 void StarTrackerHandler::handleStartup(const uint8_t* parameterId) {
  switch (*parameterId) {
    case (startracker::ID::LOG_LEVEL): {
-      internalState = InternalState::LIMITS;
+      bootState = FwBootState::LIMITS;
      break;
    }
    case (startracker::ID::LIMITS): {
-      internalState = InternalState::TRACKING;
+      bootState = FwBootState::TRACKING;
      break;
    }
    case (startracker::ID::TRACKING): {
-      internalState = InternalState::MOUNTING;
+      bootState = FwBootState::MOUNTING;
      break;
    }
    case (startracker::ID::MOUNTING): {
-      internalState = InternalState::IMAGE_PROCESSOR;
+      bootState = FwBootState::IMAGE_PROCESSOR;
      break;
    }
    case (startracker::ID::IMAGE_PROCESSOR): {
-      internalState = InternalState::CAMERA;
+      bootState = FwBootState::CAMERA;
      break;
    }
    case (startracker::ID::CAMERA): {
-      internalState = InternalState::CENTROIDING;
+      bootState = FwBootState::CENTROIDING;
      break;
    }
    case (startracker::ID::CENTROIDING): {
-      internalState = InternalState::LISA;
+      bootState = FwBootState::LISA;
      break;
    }
    case (startracker::ID::LISA): {
-      internalState = InternalState::MATCHING;
+      bootState = FwBootState::MATCHING;
      break;
    }
    case (startracker::ID::MATCHING): {
-      internalState = InternalState::VALIDATION;
+      bootState = FwBootState::VALIDATION;
      break;
    }
    case (startracker::ID::VALIDATION): {
-      internalState = InternalState::ALGO;
+      bootState = FwBootState::ALGO;
      break;
    }
    case (startracker::ID::ALGO): {
-      internalState = InternalState::LOG_SUBSCRIPTION;
+      bootState = FwBootState::LOG_SUBSCRIPTION;
      break;
    }
    case (startracker::ID::LOG_SUBSCRIPTION): {
-      internalState = InternalState::DEBUG_CAMERA;
+      bootState = FwBootState::DEBUG_CAMERA;
      break;
    }
    case (startracker::ID::DEBUG_CAMERA): {
+      bootState = FwBootState::NONE;
      internalState = InternalState::DONE;
      break;
    }
--- a/linux/devices/startracker/StarTrackerHandler.h
+++ b/linux/devices/startracker/StarTrackerHandler.h
@ -60,7 +60,6 @@ class StarTrackerHandler : public DeviceHandlerBase {
 protected:
  void doStartUp() override;
  void doShutDown() override;
-  void doOffActivity() override;
  ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override;
  ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t* id) override;
  void fillCommandAndReplyMap() override;
@ -247,14 +246,31 @@ class StarTrackerHandler : public DeviceHandlerBase {

  NormalState normalState = NormalState::TEMPERATURE_REQUEST;

+  enum class StartupState {
+    IDLE,
+    CHECK_PROGRAM,
+    WAIT_CHECK_PROGRAM,
+    BOOT_BOOTLOADER,
+    WAIT_JCFG,
+    DONE
+  };
+  StartupState startupState = StartupState::IDLE;
+
  enum class InternalState {
    IDLE,
-    BOOT,
+    BOOT_FIRMWARE,
+    DONE,
+    FAILED_FIRMWARE_BOOT,
+    BOOT_BOOTLOADER,
+    BOOTLOADER_CHECK,
+    FAILED_BOOTLOADER_BOOT
+  };
+
+  enum class FwBootState {
+    NONE,
+    BOOT_DELAY,
    REQ_VERSION,
    VERIFY_BOOT,
-    STARTUP_CHECK,
-    BOOT_DELAY,
-    FIRMWARE_CHECK,
    LOGLEVEL,
    LIMITS,
    TRACKING,
@ -270,26 +286,11 @@ class StarTrackerHandler : public DeviceHandlerBase {
    LOG_SUBSCRIPTION,
    DEBUG_CAMERA,
    WAIT_FOR_EXECUTION,
-    DONE,
-    FAILED_FIRMWARE_BOOT,
-    BOOT_BOOTLOADER,
-    BOOTLOADER_CHECK,
-    BOOTING_BOOTLOADER_FAILED
  };
+  FwBootState bootState = FwBootState::NONE;

  InternalState internalState = InternalState::IDLE;

-  enum class StartupState {
-    IDLE,
-    CHECK_PROGRAM,
-    WAIT_CHECK_PROGRAM,
-    BOOT_BOOTLOADER,
-    WAIT_JCFG,
-    DONE
-  };
-
-  StartupState startupState = StartupState::IDLE;
-
  bool strHelperExecuting = false;

  const power::Switch_t powerSwitch = power::NO_SWITCH;
--- a/linux/fsfwconfig/events/translateEvents.cpp
+++ b/linux/fsfwconfig/events/translateEvents.cpp
@ -1,7 +1,7 @@
 /**
 * @brief    Auto-generated event translation file. Contains 277 translations.
 * @details
- * Generated on: 2023-03-16 17:47:14
+ * Generated on: 2023-03-21 14:18:44
 */
 #include "translateEvents.h"

--- a/linux/fsfwconfig/objects/translateObjects.cpp
+++ b/linux/fsfwconfig/objects/translateObjects.cpp
@ -2,7 +2,7 @@
 * @brief  Auto-generated object translation file.
 * @details
 * Contains 173 translations.
- * Generated on: 2023-03-16 17:47:14
+ * Generated on: 2023-03-21 14:18:44
 */
 #include "translateObjects.h"

--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@ -55,6 +55,10 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
      navigation.resetMekf(&mekfData);
      return HasActionsIF::EXECUTION_FINISHED;
    }
+    case RESTORE_MEKF_NONFINITE_RECOVERY: {
+      mekfLost = false;
+      return HasActionsIF::EXECUTION_FINISHED;
+    }
    default: {
      return HasActionsIF::INVALID_ACTION_ID;
    }
@ -149,6 +153,10 @@ void AcsController::performSafe() {
      triggerEvent(acs::MEKF_INVALID_INFO);
      mekfInvalidFlag = true;
    }
+    if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
+      navigation.resetMekf(&mekfData);
+      mekfLost = true;
+    }
  } else {
    mekfInvalidFlag = false;
  }
@ -231,6 +239,9 @@ void AcsController::performDetumble() {
      triggerEvent(acs::MEKF_INVALID_INFO);
      mekfInvalidFlag = true;
    }
+    if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
+      navigation.resetMekf(&mekfData);
+    }
  } else {
    mekfInvalidFlag = false;
  }
@ -282,6 +293,9 @@ void AcsController::performPointingCtrl() {
      triggerEvent(acs::MEKF_INVALID_INFO);
      mekfInvalidFlag = true;
    }
+    if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
+      navigation.resetMekf(&mekfData);
+    }
    if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) {
      // Trigger this so STR FDIR can set the device faulty.
      EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0, 0);
--- a/mission/controller/AcsController.h
+++ b/mission/controller/AcsController.h
@ -64,6 +64,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
  uint16_t mekfInvalidCounter = 0;
  bool safeCtrlFailureFlag = false;
  uint8_t safeCtrlFailureCounter = 0;
+  uint8_t resetMekfCount = 0;
+  bool mekfLost = false;

  int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
  int16_t cmdDipolMtqs[3] = {0, 0, 0};
@ -78,6 +80,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
  /** Device command IDs */
  static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0;
  static const DeviceCommandId_t RESET_MEKF = 0x1;
+  static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2;

  static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL;
  static constexpr ReturnValue_t FILE_DELETION_FAILED = MAKE_RETURN_CODE(0);
--- a/mission/controller/acs/AcsParameters.h
+++ b/mission/controller/acs/AcsParameters.h
@ -768,10 +768,10 @@ class AcsParameters : public HasParametersIF {
    double gyr2orientationMatrix[3][3] = {{0, 0, -1}, {0, -1, 0}, {-1, 0, 0}};
    double gyr3orientationMatrix[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}};

-    double gyr0bias[3] = {0.06318149743589743, 0.4283235025641024, -0.16383500000000004};
-    double gyr1bias[3] = {-0.12855128205128205, 1.6737307692307695, 1.031724358974359};
-    double gyr2bias[3] = {0.15039212820512823, 0.7094475589743591, -0.22298363589743594};
-    double gyr3bias[3] = {0.0021730769230769217, -0.6655897435897435, 0.034096153846153845};
+    double gyr0bias[3] = {0.0, 0.4, -0.1};
+    double gyr1bias[3] = {0.0956745283018868, 2.0854575471698116, 1.2505990566037737};
+    double gyr2bias[3] = {0.1, 0.7, -0.2};
+    double gyr3bias[3] = {-0.10721698113207549, -0.6111650943396226, 0.1716462264150944};

    /* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
     * assumed to be equal for the same class of sensors */
--- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp
@ -1080,6 +1080,12 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(

  MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);

+  if (not(MathOperations<double>::checkVectorIsFinite(propagatedQuaternion, 4)) ||
+      not(MathOperations<double>::checkMatrixIsFinite(initialQuaternion, 6, 6))) {
+    updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE);
+    return MEKF_NOT_FINITE;
+  }
+
  updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst);
  return MEKF_RUNNING;
 }
--- a/mission/controller/acs/MultiplicativeKalmanFilter.h
+++ b/mission/controller/acs/MultiplicativeKalmanFilter.h
@ -62,6 +62,7 @@ class MultiplicativeKalmanFilter {
    NO_MODEL_VECTORS = 2,
    NO_SUS_MGM_STR_DATA = 3,
    COVARIANCE_INVERSION_FAILED = 4,
+    NOT_FINITE = 5,
    INITIALIZED = 10,
    RUNNING = 11,
  };
@ -74,8 +75,9 @@ class MultiplicativeKalmanFilter {
  static constexpr ReturnValue_t MEKF_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_MEKF_ID, 5);
  static constexpr ReturnValue_t MEKF_COVARIANCE_INVERSION_FAILED =
      returnvalue::makeCode(IF_MEKF_ID, 6);
-  static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 7);
-  static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 8);
+  static constexpr ReturnValue_t MEKF_NOT_FINITE = returnvalue::makeCode(IF_MEKF_ID, 7);
+  static constexpr ReturnValue_t MEKF_INITIALIZED = returnvalue::makeCode(IF_MEKF_ID, 8);
+  static constexpr ReturnValue_t MEKF_RUNNING = returnvalue::makeCode(IF_MEKF_ID, 9);

 private:
  /*Parameters*/
--- a/mission/controller/acs/util/MathOperations.h
+++ b/mission/controller/acs/util/MathOperations.h
@ -404,6 +404,26 @@ class MathOperations {
    std::memcpy(inverse, identity, sizeof(identity));
    return 0;  // successful inversion
  }
+
+  static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
+    for (uint8_t i = 0; i < size; i++) {
+      if (not isfinite(inputVector[i])) {
+        return false;
+      }
+    }
+    return true;
+  }
+
+  static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
+    for (uint8_t col = 0; col < cols; col++) {
+      for (uint8_t row = 0; row < rows; row++) {
+        if (not isfinite(inputMatrix[row * cols + cols])) {
+          return false;
+        }
+      }
+    }
+    return true;
+  }
 };

 #endif /* ACS_MATH_MATHOPERATIONS_H_ */
--- a/mission/core/pollingSeqTables.cpp
+++ b/mission/core/pollingSeqTables.cpp
@ -64,8 +64,7 @@ ReturnValue_t pst::pstI2cProcessingSystem(FixedTimeslotTaskIF *thisSequence) {
                        DeviceHandlerIF::GET_WRITE);
  thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.7,
                        DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.7,
-                        DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.7, DeviceHandlerIF::GET_READ);
  // damaged
  /*
  thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4,
@ -86,8 +85,7 @@ ReturnValue_t pst::pstI2cProcessingSystem(FixedTimeslotTaskIF *thisSequence) {
                        DeviceHandlerIF::GET_WRITE);
  thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.9,
                        DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.9,
-                        DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.9, DeviceHandlerIF::GET_READ);
  static_cast<void>(length);
  return thisSequence->checkSequence();
 }
@ -558,10 +556,14 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                        DeviceHandlerIF::GET_READ);
  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
                        DeviceHandlerIF::PERFORM_OPERATION);
-  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, DeviceHandlerIF::SEND_WRITE);
-  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD, DeviceHandlerIF::GET_WRITE);
-  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * config::spiSched::SCHED_BLOCK_9_PERIOD, DeviceHandlerIF::SEND_READ);
-  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * config::spiSched::SCHED_BLOCK_9_PERIOD, DeviceHandlerIF::GET_READ);
+  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
+                        DeviceHandlerIF::SEND_WRITE);
+  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * config::spiSched::SCHED_BLOCK_8_PERIOD,
+                        DeviceHandlerIF::GET_WRITE);
+  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
+                        DeviceHandlerIF::SEND_READ);
+  thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
+                        DeviceHandlerIF::GET_READ);

 #if OBSW_ADD_RAD_SENSORS == 1
  /* Radiation sensor */
--- a/mission/devices/GyrAdis1650XHandler.cpp
+++ b/mission/devices/GyrAdis1650XHandler.cpp
@ -65,10 +65,7 @@ ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_
    }
    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;
+      return preparePeriodicRequest(acs::SimpleSensorMode::OFF);
    }
    default: {
      return NOTHING_TO_SEND;
--- a/mission/system/objects/DualLaneAssemblyBase.cpp
+++ b/mission/system/objects/DualLaneAssemblyBase.cpp
@ -34,10 +34,17 @@ void DualLaneAssemblyBase::performChildOperation() {
 }

 void DualLaneAssemblyBase::startTransition(Mode_t mode, Submode_t submode) {
-  // doStartTransition(mode, submode);
  using namespace duallane;
  pwrStateMachine.reset();
  if (mode != MODE_OFF) {
+    // Special exception: A transition from dual side to single mode must be handled like
+    // going OFF.
+    if ((this->mode == MODE_ON or this->mode == DeviceHandlerIF::MODE_NORMAL) and
+        this->submode == DUAL_MODE and submode != DUAL_MODE) {
+      dualToSingleSideTransition = true;
+      AssemblyBase::startTransition(mode, submode);
+      return;
+    }
    // If anything other than MODE_OFF is commanded, perform power state machine first
    // Cache the target modes, required by power state machine
    pwrStateMachine.start(mode, submode);
@ -75,9 +82,13 @@ ReturnValue_t DualLaneAssemblyBase::pwrStateMachineWrapper() {
      // Will be called for transitions to MODE_OFF, where everything is done after power switching
      finishModeOp();
    } else if (opCode == OpCodes::TO_NOT_OFF_DONE) {
-      // Will be called for transitions from MODE_OFF to anything else, where the mode still has
-      // to be commanded after power switching
-      AssemblyBase::startTransition(targetMode, targetSubmode);
+      if (dualToSingleSideTransition) {
+        finishModeOp();
+      } else {
+        // Will be called for transitions from MODE_OFF to anything else, where the mode still has
+        // to be commanded after power switching
+        AssemblyBase::startTransition(targetMode, targetSubmode);
+      }
    } else if (opCode == OpCodes::TIMEOUT_OCCURED) {
      if (powerRetryCounter == 0) {
        powerRetryCounter++;
@ -118,6 +129,13 @@ void DualLaneAssemblyBase::handleModeReached() {
    // Ignore failures for now.
    pwrStateMachineWrapper();
  } else {
+    // For dual to single side transition, devices should be logically off, but the switch
+    // handling still needs to be done.
+    if (dualToSingleSideTransition) {
+      pwrStateMachine.start(targetMode, targetSubmode);
+      pwrStateMachineWrapper();
+      return;
+    }
    finishModeOp();
  }
 }
@ -229,6 +247,7 @@ void DualLaneAssemblyBase::finishModeOp() {
  pwrStateMachine.reset();
  powerRetryCounter = 0;
  tryingOtherSide = false;
+  dualToSingleSideTransition = false;
  dualModeErrorSwitch = true;
 }

--- a/mission/system/objects/DualLaneAssemblyBase.h
+++ b/mission/system/objects/DualLaneAssemblyBase.h
@ -31,6 +31,7 @@ class DualLaneAssemblyBase : public AssemblyBase, public ConfirmsFailuresIF {
  uint8_t powerRetryCounter = 0;
  bool tryingOtherSide = false;
  bool dualModeErrorSwitch = true;
+  bool dualToSingleSideTransition = false;
  duallane::Submodes defaultSubmode = duallane::Submodes::A_SIDE;

  enum RecoveryCustomStates {