bsp_q7s/core/ObjectFactory.cpp

@@ -6,6 +6,8 @@
 #include <linux/obc/PdecHandler.h>
 #include <linux/obc/Ptme.h>
 #include <linux/obc/PtmeConfig.h>
+#include <mission/system/AcsBoardFdir.h>
+#include <mission/system/SusAssembly.h>
 
 #include "OBSWConfig.h"
 #include "bsp_q7s/boardtest/Q7STestTask.h"
@@ -580,6 +582,7 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI
                                     Levels::LOW);
   gpioCookieAcsBoard->addGpio(gpioIds::GNSS_SELECT, gpio);
   gpioComIF->addGpios(gpioCookieAcsBoard);
+  AcsBoardFdir* fdir = nullptr;
 
 #if OBSW_ADD_ACS_HANDLERS == 1
   std::string spiDev = q7s::SPI_DEFAULT_DEV;
@@ -588,6 +591,8 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI
                     MGMLIS3MDL::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED);
   auto mgmLis3Handler = new MgmLIS3MDLHandler(objects::MGM_0_LIS3_HANDLER, objects::SPI_COM_IF,
                                               spiCookie, spi::LIS3_TRANSITION_DELAY);
+  fdir = new AcsBoardFdir(objects::MGM_0_LIS3_HANDLER);
+  mgmLis3Handler->setCustomFdir(fdir);
   static_cast<void>(mgmLis3Handler);
 #if OBSW_TEST_ACS == 1
   mgmLis3Handler->setStartUpImmediately();
@@ -602,6 +607,9 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI
                     RM3100::MAX_BUFFER_SIZE, spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED);
   auto mgmRm3100Handler = new MgmRM3100Handler(objects::MGM_1_RM3100_HANDLER, objects::SPI_COM_IF,
                                                spiCookie, spi::RM3100_TRANSITION_DELAY);
+  fdir = new AcsBoardFdir(objects::MGM_1_RM3100_HANDLER);
+  mgmRm3100Handler->setCustomFdir(fdir);
+  mgmRm3100Handler->setParent(objects::ACS_BOARD_ASS);
   static_cast<void>(mgmRm3100Handler);
 #if OBSW_TEST_ACS == 1
   mgmRm3100Handler->setStartUpImmediately();
@@ -616,6 +624,9 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI
                     MGMLIS3MDL::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED);
   mgmLis3Handler = new MgmLIS3MDLHandler(objects::MGM_2_LIS3_HANDLER, objects::SPI_COM_IF,
                                          spiCookie, spi::LIS3_TRANSITION_DELAY);
+  fdir = new AcsBoardFdir(objects::MGM_2_LIS3_HANDLER);
+  mgmLis3Handler->setCustomFdir(fdir);
+  mgmLis3Handler->setParent(objects::ACS_BOARD_ASS);
   static_cast<void>(mgmLis3Handler);
 #if OBSW_TEST_ACS == 1
   mgmLis3Handler->setStartUpImmediately();
@@ -629,6 +640,9 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI
                     RM3100::MAX_BUFFER_SIZE, spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED);
   mgmRm3100Handler = new MgmRM3100Handler(objects::MGM_3_RM3100_HANDLER, objects::SPI_COM_IF,
                                           spiCookie, spi::RM3100_TRANSITION_DELAY);
+  fdir = new AcsBoardFdir(objects::MGM_3_RM3100_HANDLER);
+  mgmRm3100Handler->setCustomFdir(fdir);
+  mgmRm3100Handler->setParent(objects::ACS_BOARD_ASS);
 #if OBSW_TEST_ACS == 1
   mgmRm3100Handler->setStartUpImmediately();
   mgmRm3100Handler->setToGoToNormalMode(true);
@@ -644,6 +658,9 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI
                             spi::DEFAULT_ADIS16507_SPEED);
   auto adisHandler = new GyroADIS1650XHandler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_COM_IF,
                                               spiCookie, ADIS1650X::Type::ADIS16505);
+  fdir = new AcsBoardFdir(objects::GYRO_0_ADIS_HANDLER);
+  adisHandler->setCustomFdir(fdir);
+  adisHandler->setParent(objects::ACS_BOARD_ASS);
   static_cast<void>(adisHandler);
 #if OBSW_TEST_ACS == 1
   adisHandler->setStartUpImmediately();
@@ -659,6 +676,9 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI
                     spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED);
   auto gyroL3gHandler = new GyroHandlerL3GD20H(objects::GYRO_1_L3G_HANDLER, objects::SPI_COM_IF,
                                                spiCookie, spi::L3G_TRANSITION_DELAY);
+  fdir = new AcsBoardFdir(objects::GYRO_1_L3G_HANDLER);
+  gyroL3gHandler->setCustomFdir(fdir);
+  gyroL3gHandler->setParent(objects::ACS_BOARD_ASS);
   static_cast<void>(gyroL3gHandler);
 #if OBSW_TEST_ACS == 1
   gyroL3gHandler->setStartUpImmediately();
@@ -674,6 +694,9 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI
                             spi::DEFAULT_ADIS16507_SPEED);
   adisHandler = new GyroADIS1650XHandler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_COM_IF,
                                          spiCookie, ADIS1650X::Type::ADIS16505);
+  fdir = new AcsBoardFdir(objects::GYRO_2_ADIS_HANDLER);
+  adisHandler->setCustomFdir(fdir);
+  adisHandler->setParent(objects::ACS_BOARD_ASS);
 #if OBSW_TEST_ACS == 1
   adisHandler->setStartUpImmediately();
   adisHandler->setToGoToNormalModeImmediately();
@@ -684,6 +707,9 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI
                     spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED);
   gyroL3gHandler = new GyroHandlerL3GD20H(objects::GYRO_3_L3G_HANDLER, objects::SPI_COM_IF,
                                           spiCookie, spi::L3G_TRANSITION_DELAY);
+  fdir = new AcsBoardFdir(objects::GYRO_3_L3G_HANDLER);
+  gyroL3gHandler->setCustomFdir(fdir);
+  gyroL3gHandler->setParent(objects::ACS_BOARD_ASS);
 #if OBSW_TEST_ACS == 1
   gyroL3gHandler->setStartUpImmediately();
   gyroL3gHandler->setToGoToNormalMode(true);
@@ -713,7 +739,14 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI
   auto acsAss = new AcsBoardAssembly(objects::ACS_BOARD_ASS, objects::NO_OBJECT, pwrSwitcher,
                                      acsBoardHelper, gpioComIF);
   static_cast<void>(acsAss);
-
+  std::array<object_id_t, 12> susIds = {objects::SUS_0, objects::SUS_1,  objects::SUS_2,
+                                        objects::SUS_3, objects::SUS_4,  objects::SUS_5,
+                                        objects::SUS_6, objects::SUS_7,  objects::SUS_8,
+                                        objects::SUS_9, objects::SUS_10, objects::SUS_11};
+  SusAssHelper susAssHelper = SusAssHelper(susIds);
+  auto susAss =
+      new SusAssembly(objects::SUS_BOARD_ASS, objects::NO_OBJECT, pwrSwitcher, susAssHelper);
+  static_cast<void>(susAss);
 #if OBSW_TEST_ACS_BOARD_ASS == 1
   CommandMessage msg;
   ModeMessage::setModeMessage(&msg, ModeMessage::CMD_MODE_COMMAND, DeviceHandlerIF::MODE_NORMAL,

common/config/commonObjects.h

@@ -93,7 +93,8 @@ enum commonObjects: uint32_t {
     PTME_CONFIG = 44330004,
 
     // 0x73 ('s') for assemblies and system/subsystem components
-    ACS_BOARD_ASS = 0x73000001
+    ACS_BOARD_ASS = 0x73000001,
+    SUS_BOARD_ASS = 0x73000002
 };
 }
 

fsfw

@@ -1 +1 @@
-Subproject commit fec5f83f4f2459facee25939e0292115f89a6d73
+Subproject commit ddc1cdb1f5d1ee6f532c04b0419e24f8f40566cf

linux/devices/GPSHyperionLinuxController.cpp

@@ -114,12 +114,14 @@ void GPSHyperionLinuxController::readGpsDataFromGpsd() {
   gps = myGpsmm.read();
   if (gps == nullptr) {
     sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed" << std::endl;
+    return;
   }
   PoolReadGuard pg(&gpsSet);
   if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_VERBOSE_LEVEL >= 1
     sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading dataset failed" << std::endl;
 #endif
+    return;
   }
 
   // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix

mission/system/AcsBoardAssembly.cpp

@@ -7,7 +7,8 @@
 AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, object_id_t parentId,
                                    PowerSwitchIF* switcher, AcsBoardHelper helper, GpioIF* gpioIF)
     : DualLaneAssemblyBase(objectId, parentId, switcher, SWITCH_A, SWITCH_B,
-                           POWER_STATE_MACHINE_TIMEOUT),
+                           POWER_STATE_MACHINE_TIMEOUT, SIDE_SWITCH_TRANSITION_NOT_ALLOWED,
+                           TRANSITION_OTHER_SIDE_FAILED),
       helper(helper),
       gpioIF(gpioIF) {
   if (switcher == nullptr) {
@@ -34,27 +35,29 @@ ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode)
   using namespace duallane;
   ReturnValue_t result = RETURN_OK;
   refreshHelperModes();
-  if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
+  // Initialize the mode table to ensure all devices are in a defined state
-    result = handleNormalOrOnModeCmd(mode, submode);
+  modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF);
-  } else {
+  modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE);
-    modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF);
+  modeTable[ModeTableIdx::GYRO_1_A].setMode(MODE_OFF);
-    modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE);
+  modeTable[ModeTableIdx::GYRO_1_A].setSubmode(SUBMODE_NONE);
-    modeTable[ModeTableIdx::GYRO_1_A].setMode(MODE_OFF);
+  modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF);
-    modeTable[ModeTableIdx::GYRO_1_A].setSubmode(SUBMODE_NONE);
+  modeTable[ModeTableIdx::GYRO_2_B].setSubmode(SUBMODE_NONE);
-    modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF);
+  modeTable[ModeTableIdx::GYRO_3_B].setMode(MODE_OFF);
-    modeTable[ModeTableIdx::GYRO_2_B].setSubmode(SUBMODE_NONE);
+  modeTable[ModeTableIdx::GYRO_3_B].setSubmode(SUBMODE_NONE);
-    modeTable[ModeTableIdx::GYRO_3_B].setMode(MODE_OFF);
+  modeTable[ModeTableIdx::MGM_0_A].setMode(MODE_OFF);
-    modeTable[ModeTableIdx::GYRO_3_B].setSubmode(SUBMODE_NONE);
+  modeTable[ModeTableIdx::MGM_0_A].setSubmode(SUBMODE_NONE);
-    modeTable[ModeTableIdx::MGM_0_A].setMode(MODE_OFF);
+  modeTable[ModeTableIdx::MGM_1_A].setMode(MODE_OFF);
-    modeTable[ModeTableIdx::MGM_0_A].setSubmode(SUBMODE_NONE);
+  modeTable[ModeTableIdx::MGM_1_A].setSubmode(SUBMODE_NONE);
-    modeTable[ModeTableIdx::MGM_1_A].setMode(MODE_OFF);
+  modeTable[ModeTableIdx::MGM_2_B].setMode(MODE_OFF);
-    modeTable[ModeTableIdx::MGM_1_A].setSubmode(SUBMODE_NONE);
+  modeTable[ModeTableIdx::MGM_2_B].setSubmode(SUBMODE_NONE);
-    modeTable[ModeTableIdx::MGM_2_B].setMode(MODE_OFF);
+  modeTable[ModeTableIdx::MGM_3_B].setMode(MODE_OFF);
-    modeTable[ModeTableIdx::MGM_2_B].setSubmode(SUBMODE_NONE);
+  modeTable[ModeTableIdx::MGM_3_B].setSubmode(SUBMODE_NONE);
-    modeTable[ModeTableIdx::MGM_3_B].setMode(MODE_OFF);
+  modeTable[ModeTableIdx::GPS].setMode(MODE_OFF);
-    modeTable[ModeTableIdx::MGM_3_B].setSubmode(SUBMODE_NONE);
+  modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
-    modeTable[ModeTableIdx::GPS].setMode(MODE_OFF);
+  if (recoveryState != RecoveryState::RECOVERY_STARTED) {
-    modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
+    if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
+      result = handleNormalOrOnModeCmd(mode, submode);
+    }
   }
   HybridIterator<ModeListEntry> tableIter(modeTable.begin(), modeTable.end());
   executeTable(tableIter);
@@ -99,16 +102,22 @@ ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_
 ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
   using namespace duallane;
   ReturnValue_t result = RETURN_OK;
+  bool needsSecondStep = false;
   auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, ModeTableIdx tableIdx) {
-    if (mode == DeviceHandlerIF::MODE_NORMAL) {
+    if (mode == devMode) {
+      modeTable[tableIdx].setMode(mode);
+      modeTable[tableIdx].setSubmode(submode);
+    } else if (mode == DeviceHandlerIF::MODE_NORMAL) {
       if (isUseable(objectId, devMode)) {
         if (devMode == MODE_ON) {
           modeTable[tableIdx].setMode(mode);
           modeTable[tableIdx].setSubmode(SUBMODE_NONE);
-
         } else {
           modeTable[tableIdx].setMode(MODE_ON);
           modeTable[tableIdx].setSubmode(SUBMODE_NONE);
+          if (internalState != STATE_SECOND_STEP) {
+            needsSecondStep = true;
+          }
         }
       }
     } else if (mode == MODE_ON) {
@@ -118,11 +127,7 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
       }
     }
   };
-  if (this->mode == MODE_OFF and mode == DeviceHandlerIF::MODE_NORMAL) {
+  bool gpsUsable = isUseable(helper.gpsId, helper.gpsMode);
-    if (internalState != STATE_SECOND_STEP) {
-      result = NEED_SECOND_STEP;
-    }
-  }
   switch (submode) {
     case (A_SIDE): {
       modeTable[ModeTableIdx::GYRO_2_B].setMode(MODE_OFF);
@@ -137,15 +142,15 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
       cmdSeq(helper.gyro1L3gIdSideA, helper.gyro1SideAMode, ModeTableIdx::GYRO_1_A);
       cmdSeq(helper.mgm0Lis3IdSideA, helper.mgm0SideAMode, ModeTableIdx::MGM_0_A);
       cmdSeq(helper.mgm1Rm3100IdSideA, helper.mgm1SideAMode, ModeTableIdx::MGM_1_A);
-      modeTable[ModeTableIdx::GPS].setMode(MODE_ON);
+      if (gpsUsable) {
-      modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
+        if (gpioIF->pullLow(gpioIds::GNSS_SELECT) != HasReturnvaluesIF::RETURN_OK) {
-      if (gpioIF->pullLow(gpioIds::GNSS_SELECT) != HasReturnvaluesIF::RETURN_OK) {
 #if OBSW_VERBOSE_LEVEL >= 1
-        sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select low"
+          sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select low"
-                   << std::endl;
+                     << std::endl;
 #endif
+        }
       }
-      return result;
+      break;
     }
     case (B_SIDE): {
       modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF);
@@ -160,15 +165,15 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
       cmdSeq(helper.gyro3L3gIdSideB, helper.gyro3SideBMode, ModeTableIdx::GYRO_3_B);
       cmdSeq(helper.mgm2Lis3IdSideB, helper.mgm2SideBMode, ModeTableIdx::MGM_2_B);
       cmdSeq(helper.mgm3Rm3100IdSideB, helper.mgm3SideBMode, ModeTableIdx::MGM_3_B);
-      modeTable[ModeTableIdx::GPS].setMode(MODE_ON);
+      if (gpsUsable) {
-      modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
+        if (gpioIF->pullHigh(gpioIds::GNSS_SELECT) != HasReturnvaluesIF::RETURN_OK) {
-      if (gpioIF->pullHigh(gpioIds::GNSS_SELECT) != HasReturnvaluesIF::RETURN_OK) {
 #if OBSW_VERBOSE_LEVEL >= 1
-        sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select high"
+          sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select high"
-                   << std::endl;
+                     << std::endl;
 #endif
+        }
       }
-      return result;
+      break;
     }
     case (DUAL_MODE): {
       cmdSeq(helper.gpsId, helper.gpsMode, ModeTableIdx::GPS);
@@ -180,80 +185,37 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
       cmdSeq(helper.gyro3L3gIdSideB, helper.gyro3SideBMode, ModeTableIdx::GYRO_3_B);
       cmdSeq(helper.mgm2Lis3IdSideB, helper.mgm2SideBMode, ModeTableIdx::MGM_2_B);
       cmdSeq(helper.mgm3Rm3100IdSideB, helper.mgm3SideBMode, ModeTableIdx::MGM_3_B);
-      modeTable[ModeTableIdx::GPS].setMode(MODE_ON);
-      modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
       ReturnValue_t status = RETURN_OK;
-      if (defaultSubmode == Submodes::A_SIDE) {
+      if (gpsUsable) {
-        status = gpioIF->pullLow(gpioIds::GNSS_SELECT);
+        if (defaultSubmode == Submodes::A_SIDE) {
-      } else {
+          status = gpioIF->pullLow(gpioIds::GNSS_SELECT);
-        status = gpioIF->pullHigh(gpioIds::GNSS_SELECT);
+        } else {
-      }
+          status = gpioIF->pullHigh(gpioIds::GNSS_SELECT);
-      if (status != HasReturnvaluesIF::RETURN_OK) {
+        }
+        if (status != HasReturnvaluesIF::RETURN_OK) {
 #if OBSW_VERBOSE_LEVEL >= 1
-        sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select to"
+          sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select to"
-                      "default side for dual mode"
+                        "default side for dual mode"
-                   << std::endl;
+                     << std::endl;
 #endif
+        }
       }
-      return result;
+      break;
     }
     default: {
       sif::error << "AcsBoardAssembly::handleNormalModeCmd: Unknown submode" << std::endl;
     }
   }
+  if (gpsUsable) {
+    modeTable[ModeTableIdx::GPS].setMode(MODE_ON);
+    modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
+  }
+  if (needsSecondStep) {
+    result = NEED_SECOND_STEP;
+  }
   return result;
 }
 
-void AcsBoardAssembly::handleChildrenLostMode(ReturnValue_t result) {
-  using namespace duallane;
-  // Some ACS board components are required for Safe-Mode. It would be good if the software
-  // transitions from A side to B side and from B side to dual mode autonomously
-  // to ensure that that enough sensors are available without an operators intervention.
-  // Therefore, the lost mode handler was overwritten to start these transitions
-  Submode_t nextSubmode = Submodes::A_SIDE;
-  if (submode == Submodes::A_SIDE) {
-    nextSubmode = Submodes::B_SIDE;
-  }
-  if (not tryingOtherSide) {
-    triggerEvent(CANT_KEEP_MODE, mode, submode);
-    startTransition(mode, nextSubmode);
-    tryingOtherSide = true;
-  } else {
-    // Not sure when this would happen. This flag is reset if the mode was reached. If it
-    // was not reached, the transition failure handler should be called.
-    sif::error << "AcsBoardAssembly::handleChildrenLostMode: Wrong handler called" << std::endl;
-    triggerEvent(TRANSITION_OTHER_SIDE_FAILED, mode, targetSubmode);
-    startTransition(mode, Submodes::DUAL_MODE);
-  }
-}
-
-void AcsBoardAssembly::handleModeTransitionFailed(ReturnValue_t result) {
-  using namespace duallane;
-  Submode_t nextSubmode = Submodes::A_SIDE;
-  if (submode == Submodes::A_SIDE) {
-    nextSubmode = Submodes::B_SIDE;
-  }
-  // Check whether the transition was started because the mode could not be kept (not commanded).
-  // If this is not the case, start transition to other side. If it is the case, start
-  // transition to dual mode.
-  if (not tryingOtherSide) {
-    triggerEvent(CANT_KEEP_MODE, mode, submode);
-    startTransition(mode, nextSubmode);
-    tryingOtherSide = true;
-  } else {
-    triggerEvent(TRANSITION_OTHER_SIDE_FAILED, mode, targetSubmode);
-    startTransition(mode, Submodes::DUAL_MODE);
-  }
-}
-
-void AcsBoardAssembly::setPreferredSide(duallane::Submodes submode) {
-  using namespace duallane;
-  if (submode != Submodes::A_SIDE and submode != Submodes::B_SIDE) {
-    return;
-  }
-  this->defaultSubmode = submode;
-}
-
 void AcsBoardAssembly::selectGpsInDualMode(duallane::Submodes side) {
   using namespace duallane;
   if (submode != Submodes::DUAL_MODE) {
@@ -288,15 +250,6 @@ void AcsBoardAssembly::refreshHelperModes() {
   }
 }
 
-void AcsBoardAssembly::finishModeOp() {
-  using namespace duallane;
-  AssemblyBase::handleModeReached();
-  pwrStateMachine.reset();
-  powerRetryCounter = 0;
-  tryingOtherSide = false;
-  dualModeErrorSwitch = true;
-}
-
 ReturnValue_t AcsBoardAssembly::initialize() {
   ReturnValue_t result = registerChild(helper.gyro0AdisIdSideA);
   if (result != HasReturnvaluesIF::RETURN_OK) {

mission/system/AcsBoardAssembly.h

@@ -77,6 +77,7 @@ class AcsBoardAssembly : public DualLaneAssemblyBase {
   // TRANSITION_OTHER_SIDE_FAILED_ID
   // NOT_ENOUGH_DEVICES_DUAL_MODE_ID
   // POWER_STATE_MACHINE_TIMEOUT_ID
+  // SIDE_SWITCH_TRANSITION_NOT_ALLOWED_ID
   static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_BOARD_ASS;
   static constexpr Event TRANSITION_OTHER_SIDE_FAILED =
       event::makeEvent(SUBSYSTEM_ID, 0, severity::HIGH);
@@ -84,14 +85,17 @@ class AcsBoardAssembly : public DualLaneAssemblyBase {
       event::makeEvent(SUBSYSTEM_ID, 1, severity::HIGH);
   static constexpr Event POWER_STATE_MACHINE_TIMEOUT =
       event::makeEvent(SUBSYSTEM_ID, 2, severity::MEDIUM);
+  //! [EXPORT] : [COMMENT] Not implemented, would increase already high complexity. Operator
+  //! should instead command the assembly off first and then command the assembly on into the
+  //! desired mode/submode combination
+  static constexpr Event SIDE_SWITCH_TRANSITION_NOT_ALLOWED =
+      event::makeEvent(SUBSYSTEM_ID, 3, severity::LOW);
 
   static constexpr uint8_t NUMBER_DEVICES_MODE_TABLE = 9;
 
   AcsBoardAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher,
                    AcsBoardHelper helper, GpioIF* gpioIF);
 
-  void setPreferredSide(duallane::Submodes submode);
-
   /**
    * In dual mode, the A side or the B side GPS device can be used, but not both.
    * This function can be used to switch the used GPS device.
@@ -106,12 +110,10 @@ class AcsBoardAssembly : public DualLaneAssemblyBase {
       pcduSwitches::Switches::PDU2_CH7_ACS_BOARD_SIDE_B_3V3;
 
   bool tryingOtherSide = false;
+  bool dualModeErrorSwitch = true;
   AcsBoardHelper helper;
   GpioIF* gpioIF = nullptr;
 
-  // duallane::PwrStates state = duallane::PwrStates::IDLE;
-  duallane::Submodes defaultSubmode = duallane::Submodes::A_SIDE;
-  bool dualModeErrorSwitch = true;
   FixedArrayList<ModeListEntry, NUMBER_DEVICES_MODE_TABLE> modeTable;
 
   ReturnValue_t initialize() override;
@@ -120,12 +122,8 @@ class AcsBoardAssembly : public DualLaneAssemblyBase {
   ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override;
   ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
 
-  void handleModeTransitionFailed(ReturnValue_t result) override;
-  void handleChildrenLostMode(ReturnValue_t result) override;
-
   ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode);
   void refreshHelperModes();
-  void finishModeOp();
 };
 
 #endif /* MISSION_SYSTEM_ACSBOARDASSEMBLY_H_ */

mission/system/AcsBoardFdir.cpp

@@ -0,0 +1,6 @@
+#include "AcsBoardFdir.h"
+
+#include <common/config/commonObjects.h>
+
+AcsBoardFdir::AcsBoardFdir(object_id_t sensorId)
+    : DeviceHandlerFailureIsolation(sensorId, objects::ACS_BOARD_ASS) {}

mission/system/AcsBoardFdir.h

@@ -0,0 +1,11 @@
+#ifndef MISSION_SYSTEM_ACSBOARDFDIR_H_
+#define MISSION_SYSTEM_ACSBOARDFDIR_H_
+
+#include <fsfw/devicehandlers/DeviceHandlerFailureIsolation.h>
+
+class AcsBoardFdir : public DeviceHandlerFailureIsolation {
+ public:
+  AcsBoardFdir(object_id_t sensorId);
+};
+
+#endif /* MISSION_SYSTEM_ACSBOARDFDIR_H_ */

mission/system/CMakeLists.txt

@@ -8,4 +8,5 @@ target_sources(${LIB_EIVE_MISSION} PRIVATE
     TcsSubsystem.cpp
     DualLanePowerStateMachine.cpp
     DualLaneAssemblyBase.cpp
+    AcsBoardFdir.cpp
 )

mission/system/DualLaneAssemblyBase.cpp

@@ -1,12 +1,20 @@
 #include "DualLaneAssemblyBase.h"
 
+#include <fsfw/ipc/QueueFactory.h>
+
 DualLaneAssemblyBase::DualLaneAssemblyBase(object_id_t objectId, object_id_t parentId,
                                            PowerSwitchIF* pwrSwitcher,
                                            pcduSwitches::Switches switch1,
-                                           pcduSwitches::Switches switch2, Event pwrTimeoutEvent)
+                                           pcduSwitches::Switches switch2, Event pwrTimeoutEvent,
+                                           Event sideSwitchNotAllowedEvent,
+                                           Event transitionOtherSideFailedEvent)
     : AssemblyBase(objectId, parentId),
       pwrStateMachine(switch1, switch2, pwrSwitcher),
-      pwrTimeoutEvent(pwrTimeoutEvent) {}
+      pwrTimeoutEvent(pwrTimeoutEvent),
+      sideSwitchNotAllowedEvent(sideSwitchNotAllowedEvent),
+      transitionOtherSideFailedEvent(transitionOtherSideFailedEvent) {
+  eventQueue = QueueFactory::instance()->createMessageQueue(10);
+}
 
 void DualLaneAssemblyBase::performChildOperation() {
   using namespace duallane;
@@ -22,21 +30,17 @@ void DualLaneAssemblyBase::performChildOperation() {
 }
 
 void DualLaneAssemblyBase::startTransition(Mode_t mode, Submode_t submode) {
+  // doStartTransition(mode, submode);
   using namespace duallane;
   pwrStateMachine.reset();
-  // If anything other than MODE_OFF is commanded, perform power state machine first
   if (mode != MODE_OFF) {
+    // 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);
     // Cache these for later after the power state machine has finished
     targetMode = mode;
     targetSubmode = submode;
-    // Perform power state machine first, then start mode transition. The power state machine will
-    // start the transition after it has finished
-    pwrStateMachineWrapper();
   } else {
-    // Command the devices to off first before switching off the power. The handleModeReached
-    // custom implementation will take care of starting the power state machine.
     AssemblyBase::startTransition(mode, submode);
   }
 }
@@ -60,25 +64,27 @@ bool DualLaneAssemblyBase::isUseable(object_id_t object, Mode_t mode) {
 ReturnValue_t DualLaneAssemblyBase::pwrStateMachineWrapper() {
   using namespace duallane;
   OpCodes opCode = pwrStateMachine.powerStateMachine();
-  if (opCode == OpCodes::NONE) {
+  if (customRecoveryStates == RecoveryCustomStates::IDLE) {
-    return RETURN_OK;
+    if (opCode == OpCodes::NONE) {
-  } else if (opCode == OpCodes::FINISH_OP) {
+      return RETURN_OK;
-    // Will be called for transitions to MODE_OFF, where everything is done after power switching
+    } else if (opCode == OpCodes::TO_OFF_DONE) {
-    finishModeOp();
+      // Will be called for transitions to MODE_OFF, where everything is done after power switching
-  } else if (opCode == OpCodes::START_TRANSITION) {
+      finishModeOp();
-    // Will be called for transitions from MODE_OFF to anything else, where the mode still has
+    } else if (opCode == OpCodes::TO_NOT_OFF_DONE) {
-    // to be commanded after power switching
+      // Will be called for transitions from MODE_OFF to anything else, where the mode still has
-    AssemblyBase::startTransition(targetMode, targetSubmode);
+      // to be commanded after power switching
-  } else if (opCode == OpCodes::TIMEOUT_OCCURED) {
+      AssemblyBase::startTransition(targetMode, targetSubmode);
-    if (powerRetryCounter == 0) {
+    } else if (opCode == OpCodes::TIMEOUT_OCCURED) {
-      powerRetryCounter++;
+      if (powerRetryCounter == 0) {
-      pwrStateMachine.reset();
+        powerRetryCounter++;
-    } else {
+        pwrStateMachine.reset();
+      } else {
 #if OBSW_VERBOSE_LEVEL >= 1
-      sif::warning << "Timeout occured in power state machine" << std::endl;
+        sif::warning << "Timeout occured in power state machine" << std::endl;
 #endif
-      triggerEvent(pwrTimeoutEvent, 0, 0);
+        triggerEvent(pwrTimeoutEvent, 0, 0);
-      return RETURN_FAILED;
+        return RETURN_FAILED;
+      }
     }
   }
   return RETURN_OK;
@@ -89,6 +95,13 @@ ReturnValue_t DualLaneAssemblyBase::isModeCombinationValid(Mode_t mode, Submode_
   if (submode != A_SIDE and submode != B_SIDE and submode != DUAL_MODE) {
     return HasReturnvaluesIF::RETURN_FAILED;
   }
+  if (sideSwitchTransition(mode, submode)) {
+    // I could implement this but this would increase the already high complexity. This is not
+    // necessary. The operator should can send a command to switch the assembly off first and
+    // then send a command to turn on the other side, either to ON or to NORMAL
+    triggerEvent(SIDE_SWITCH_TRANSITION_NOT_ALLOWED_ID, 0, 0);
+    return TRANS_NOT_ALLOWED;
+  }
   return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -103,3 +116,121 @@ void DualLaneAssemblyBase::handleModeReached() {
     finishModeOp();
   }
 }
+
+MessageQueueId_t DualLaneAssemblyBase::getEventReceptionQueue() { return eventQueue->getId(); }
+
+void DualLaneAssemblyBase::handleChildrenLostMode(ReturnValue_t result) {
+  using namespace duallane;
+  // Some ACS board components are required for Safe-Mode. It would be good if the software
+  // transitions from A side to B side and from B side to dual mode autonomously
+  // to ensure that that enough sensors are available without an operators intervention.
+  // Therefore, the lost mode handler was overwritten to start these transitions
+  Submode_t nextSubmode = Submodes::A_SIDE;
+  if (submode == Submodes::A_SIDE) {
+    nextSubmode = Submodes::B_SIDE;
+  }
+  if (not tryingOtherSide) {
+    triggerEvent(CANT_KEEP_MODE, mode, submode);
+    startTransition(mode, nextSubmode);
+    tryingOtherSide = true;
+  } else {
+    // Not sure when this would happen. This flag is reset if the mode was reached. If it
+    // was not reached, the transition failure handler should be called.
+    sif::error << "DualLaneAssemblyBase::handleChildrenLostMode: Wrong handler called" << std::endl;
+    triggerEvent(transitionOtherSideFailedEvent, mode, targetSubmode);
+    startTransition(mode, Submodes::DUAL_MODE);
+  }
+}
+
+void DualLaneAssemblyBase::handleModeTransitionFailed(ReturnValue_t result) {
+  using namespace duallane;
+  Submode_t nextSubmode = Submodes::A_SIDE;
+  if (submode == Submodes::A_SIDE) {
+    nextSubmode = Submodes::B_SIDE;
+  }
+  // Check whether the transition was started because the mode could not be kept (not commanded).
+  // If this is not the case, start transition to other side. If it is the case, start
+  // transition to dual mode.
+  if (not tryingOtherSide) {
+    triggerEvent(CANT_KEEP_MODE, mode, submode);
+    startTransition(mode, nextSubmode);
+    tryingOtherSide = true;
+  } else {
+    triggerEvent(transitionOtherSideFailedEvent, mode, targetSubmode);
+    startTransition(mode, Submodes::DUAL_MODE);
+  }
+}
+
+bool DualLaneAssemblyBase::checkAndHandleRecovery() {
+  using namespace duallane;
+  OpCodes opCode = OpCodes::NONE;
+  if (recoveryState == RECOVERY_IDLE) {
+    return AssemblyBase::checkAndHandleRecovery();
+  }
+  if (customRecoveryStates == IDLE) {
+    pwrStateMachine.start(MODE_OFF, 0);
+    customRecoveryStates = RecoveryCustomStates::POWER_SWITCHING_OFF;
+  }
+  if (customRecoveryStates == POWER_SWITCHING_OFF) {
+    opCode = pwrStateMachine.powerStateMachine();
+    if (opCode == OpCodes::TO_OFF_DONE or opCode == OpCodes::TIMEOUT_OCCURED) {
+      customRecoveryStates = RecoveryCustomStates::POWER_SWITCHING_ON;
+      pwrStateMachine.start(targetMode, targetSubmode);
+    }
+  }
+  if (customRecoveryStates == POWER_SWITCHING_ON) {
+    opCode = pwrStateMachine.powerStateMachine();
+    if (opCode == OpCodes::TO_NOT_OFF_DONE or opCode == OpCodes::TIMEOUT_OCCURED) {
+      customRecoveryStates = RecoveryCustomStates::DONE;
+    }
+  }
+  if (customRecoveryStates == DONE) {
+    bool pendingRecovery = AssemblyBase::checkAndHandleRecovery();
+    if (not pendingRecovery) {
+      pwrStateMachine.reset();
+      customRecoveryStates = RecoveryCustomStates::IDLE;
+    }
+    // For a recovery on one side, only do the recovery once
+    for (auto& child : childrenMap) {
+      if (healthHelper.healthTable->getHealth(child.first) == HasHealthIF::NEEDS_RECOVERY) {
+        sendHealthCommand(child.second.commandQueue, HEALTHY);
+        child.second.healthChanged = false;
+      }
+    }
+    return pendingRecovery;
+  }
+  return true;
+}
+
+bool DualLaneAssemblyBase::sideSwitchTransition(Mode_t mode, Submode_t submode) {
+  using namespace duallane;
+  if (this->mode == MODE_OFF) {
+    return false;
+  }
+  if (this->mode == MODE_ON or this->mode == DeviceHandlerIF::MODE_NORMAL) {
+    if (this->submode == Submodes::A_SIDE and submode == Submodes::B_SIDE) {
+      return true;
+    } else if (this->submode == Submodes::B_SIDE and submode == Submodes::A_SIDE) {
+      return true;
+    }
+    return false;
+  }
+  return false;
+}
+
+void DualLaneAssemblyBase::finishModeOp() {
+  using namespace duallane;
+  AssemblyBase::handleModeReached();
+  pwrStateMachine.reset();
+  powerRetryCounter = 0;
+  tryingOtherSide = false;
+  dualModeErrorSwitch = true;
+}
+
+void DualLaneAssemblyBase::setPreferredSide(duallane::Submodes submode) {
+  using namespace duallane;
+  if (submode != Submodes::A_SIDE and submode != Submodes::B_SIDE) {
+    return;
+  }
+  this->defaultSubmode = submode;
+}

mission/system/DualLaneAssemblyBase.h

@@ -11,21 +11,37 @@
  * power lanes and are required for the majority of satellite modes. Therefore, there is a lot
  * of common code, for example the power switching.
  */
-class DualLaneAssemblyBase : public AssemblyBase {
+class DualLaneAssemblyBase : public AssemblyBase, public ConfirmsFailuresIF {
  public:
-  static constexpr uint8_t TRANSITION_OTHER_SIDE_FAILED_ID = 0;
+  static constexpr UniqueEventId_t TRANSITION_OTHER_SIDE_FAILED_ID = 0;
-  static constexpr uint8_t NOT_ENOUGH_DEVICES_DUAL_MODE_ID = 1;
+  static constexpr UniqueEventId_t NOT_ENOUGH_DEVICES_DUAL_MODE_ID = 1;
-  static constexpr uint8_t POWER_STATE_MACHINE_TIMEOUT_ID = 2;
+  static constexpr UniqueEventId_t POWER_STATE_MACHINE_TIMEOUT_ID = 2;
+  static constexpr UniqueEventId_t SIDE_SWITCH_TRANSITION_NOT_ALLOWED_ID = 3;
 
   DualLaneAssemblyBase(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher,
                        pcduSwitches::Switches switch1, pcduSwitches::Switches switch2,
-                       Event pwrSwitchTimeoutEvent);
+                       Event pwrSwitchTimeoutEvent, Event sideSwitchNotAllowedEvent,
+                       Event transitionOtherSideFailedEvent);
 
  protected:
   // This helper object complete encapsulates power switching
   DualLanePowerStateMachine pwrStateMachine;
   Event pwrTimeoutEvent;
+  Event sideSwitchNotAllowedEvent;
+  Event transitionOtherSideFailedEvent;
   uint8_t powerRetryCounter = 0;
+  bool tryingOtherSide = false;
+  bool dualModeErrorSwitch = true;
+  duallane::Submodes defaultSubmode = duallane::Submodes::A_SIDE;
+
+  enum RecoveryCustomStates {
+    IDLE,
+    POWER_SWITCHING_OFF,
+    POWER_SWITCHING_ON,
+    DONE
+  } customRecoveryStates = RecoveryCustomStates::IDLE;
+
+  MessageQueueIF* eventQueue = nullptr;
 
   /**
    * Check whether it makes sense to send mode commands to the device
@@ -42,16 +58,29 @@ class DualLaneAssemblyBase : public AssemblyBase {
    */
   virtual ReturnValue_t pwrStateMachineWrapper();
   virtual ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
+  /**
+   * Custom recovery implementation to ensure that the power lines are commanded off for a
+   * recovery.
+   * @return
+   */
+  virtual bool checkAndHandleRecovery() override;
+  void setPreferredSide(duallane::Submodes submode);
   virtual void performChildOperation() override;
   virtual void startTransition(Mode_t mode, Submode_t submode) override;
+  virtual void handleChildrenLostMode(ReturnValue_t result) override;
+  virtual void handleModeTransitionFailed(ReturnValue_t result) override;
   virtual void handleModeReached();
 
+  MessageQueueId_t getEventReceptionQueue() override;
+
+  bool sideSwitchTransition(Mode_t mode, Submode_t submode);
+
   /**
    * Implemented by user. Will be called if a full mode operation has finished.
    * This includes both the regular mode state machine operations and the power state machine
    * operations
    */
-  virtual void finishModeOp() = 0;
+  virtual void finishModeOp();
 
   template <size_t MAX_SIZE>
   void initModeTableEntry(object_id_t id, ModeListEntry& entry,

mission/system/DualLanePowerStateMachine.cpp

@@ -14,6 +14,7 @@ void DualLanePowerStateMachine::setCheckTimeout(dur_millis_t timeout) {
 }
 
 void DualLanePowerStateMachine::start(Mode_t mode, Submode_t submode) {
+  reset();
   checkTimeout.resetTimer();
   targetMode = mode;
   targetSubmode = submode;
@@ -34,18 +35,19 @@ duallane::OpCodes DualLanePowerStateMachine::powerStateMachine() {
   ReturnValue_t switchStateA = RETURN_OK;
   ReturnValue_t switchStateB = RETURN_OK;
   if (state == PwrStates::IDLE or state == PwrStates::MODE_COMMANDING) {
-    return duallane::OpCodes::NONE;
+    return opResult;
   }
   if (state == PwrStates::SWITCHING_POWER or state == PwrStates::CHECKING_POWER) {
     switchStateA = pwrSwitcher->getSwitchState(SWITCH_A);
     switchStateB = pwrSwitcher->getSwitchState(SWITCH_B);
   } else {
-    return OpCodes::NONE;
+    return opResult;
   }
   if (targetMode == HasModesIF::MODE_OFF) {
     if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_OFF) {
       state = PwrStates::IDLE;
-      return OpCodes::FINISH_OP;
+      opResult = OpCodes::TO_OFF_DONE;
+      return opResult;
     }
   } else {
     switch (targetSubmode) {
@@ -53,7 +55,8 @@ duallane::OpCodes DualLanePowerStateMachine::powerStateMachine() {
         if (switchStateA == PowerSwitchIF::SWITCH_ON and
             switchStateB == PowerSwitchIF::SWITCH_OFF) {
           state = PwrStates::MODE_COMMANDING;
-          return OpCodes::START_TRANSITION;
+          opResult = OpCodes::TO_NOT_OFF_DONE;
+          return opResult;
         }
         break;
       }
@@ -61,14 +64,16 @@ duallane::OpCodes DualLanePowerStateMachine::powerStateMachine() {
         if (switchStateA == PowerSwitchIF::SWITCH_OFF and
             switchStateB == PowerSwitchIF::SWITCH_ON) {
           state = PwrStates::MODE_COMMANDING;
-          return OpCodes::START_TRANSITION;
+          opResult = OpCodes::TO_NOT_OFF_DONE;
+          return opResult;
         }
         break;
       }
       case (DUAL_MODE): {
         if (switchStateA == PowerSwitchIF::SWITCH_ON and switchStateB == PowerSwitchIF::SWITCH_ON) {
           state = PwrStates::MODE_COMMANDING;
-          return OpCodes::START_TRANSITION;
+          opResult = OpCodes::TO_NOT_OFF_DONE;
+          return opResult;
         }
       }
     }
@@ -123,11 +128,12 @@ duallane::OpCodes DualLanePowerStateMachine::powerStateMachine() {
       return OpCodes::TIMEOUT_OCCURED;
     }
   }
-  return OpCodes::NONE;
+  return opResult;
 }
 
 void DualLanePowerStateMachine::reset() {
   state = duallane::PwrStates::IDLE;
+  opResult = duallane::OpCodes::NONE;
   targetMode = HasModesIF::MODE_OFF;
   targetSubmode = 0;
   checkTimeout.resetTimer();

mission/system/DualLanePowerStateMachine.h

@@ -23,6 +23,7 @@ class DualLanePowerStateMachine : public HasReturnvaluesIF {
   const pcduSwitches::Switches SWITCH_B;
 
  private:
+  duallane::OpCodes opResult = duallane::OpCodes::NONE;
   duallane::PwrStates state = duallane::PwrStates::IDLE;
   PowerSwitchIF* pwrSwitcher = nullptr;
   Mode_t targetMode = HasModesIF::MODE_OFF;

mission/system/SusAssembly.cpp

@@ -7,7 +7,8 @@
 SusAssembly::SusAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher,
                          SusAssHelper helper)
     : DualLaneAssemblyBase(objectId, parentId, pwrSwitcher, SWITCH_NOM, SWITCH_RED,
-                           POWER_STATE_MACHINE_TIMEOUT),
+                           POWER_STATE_MACHINE_TIMEOUT, SIDE_SWITCH_TRANSITION_NOT_ALLOWED,
+                           TRANSITION_OTHER_SIDE_FAILED),
       helper(helper),
       pwrSwitcher(pwrSwitcher) {
   ModeListEntry entry;
@@ -19,16 +20,15 @@ SusAssembly::SusAssembly(object_id_t objectId, object_id_t parentId, PowerSwitch
 ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
   ReturnValue_t result = RETURN_OK;
   refreshHelperModes();
-  powerStateMachine(mode, submode);
+  // Initialize the mode table to ensure all devices are in a defined state
-  if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
+  for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
-    if (state == States::MODE_COMMANDING) {
+    modeTable[idx].setMode(MODE_OFF);
+    modeTable[idx].setSubmode(SUBMODE_NONE);
+  }
+  if (recoveryState != RecoveryState::RECOVERY_STARTED) {
+    if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
       handleNormalOrOnModeCmd(mode, submode);
     }
-  } else {
-    for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
-      modeTable[idx].setMode(MODE_OFF);
-      modeTable[idx].setSubmode(SUBMODE_NONE);
-    }
   }
 
   HybridIterator<ModeListEntry> tableIter(modeTable.begin(), modeTable.end());
@@ -39,20 +39,26 @@ ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
 ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
   using namespace duallane;
   ReturnValue_t result = RETURN_OK;
-  auto cmdSeq = [&](object_id_t objectId, uint8_t tableIdx) {
+  bool needsSecondStep = false;
-    if (mode == DeviceHandlerIF::MODE_NORMAL) {
+  auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, uint8_t tableIdx) {
-      if (isUseable(objectId, mode)) {
+    if (mode == devMode) {
-        if (helper.susModes[tableIdx] != MODE_OFF) {
+      modeTable[tableIdx].setMode(mode);
+      modeTable[tableIdx].setSubmode(submode);
+    } else if (mode == DeviceHandlerIF::MODE_NORMAL) {
+      if (isUseable(objectId, devMode)) {
+        if (devMode == MODE_ON) {
           modeTable[tableIdx].setMode(mode);
           modeTable[tableIdx].setSubmode(SUBMODE_NONE);
         } else {
-          result = NEED_SECOND_STEP;
           modeTable[tableIdx].setMode(MODE_ON);
           modeTable[tableIdx].setSubmode(SUBMODE_NONE);
+          if (internalState != STATE_SECOND_STEP) {
+            needsSecondStep = true;
+          }
         }
       }
     } else if (mode == MODE_ON) {
-      if (isUseable(objectId, mode)) {
+      if (isUseable(objectId, devMode)) {
         modeTable[tableIdx].setMode(MODE_ON);
         modeTable[tableIdx].setSubmode(SUBMODE_NONE);
       }
@@ -61,30 +67,33 @@ ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submod
   switch (submode) {
     case (A_SIDE): {
       for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS_ONE_SIDE; idx++) {
-        cmdSeq(helper.susIds[idx], idx);
+        cmdSeq(helper.susIds[idx], helper.susModes[idx], idx);
         // Switch off devices on redundant side
         modeTable[idx + NUMBER_SUN_SENSORS_ONE_SIDE].setMode(MODE_OFF);
         modeTable[idx + NUMBER_SUN_SENSORS_ONE_SIDE].setSubmode(SUBMODE_NONE);
       }
-      return result;
+      break;
     }
     case (B_SIDE): {
       for (uint8_t idx = NUMBER_SUN_SENSORS_ONE_SIDE; idx < NUMBER_SUN_SENSORS; idx++) {
-        cmdSeq(helper.susIds[idx], idx);
+        cmdSeq(helper.susIds[idx], helper.susModes[idx], idx);
         // Switch devices on nominal side
         modeTable[idx - NUMBER_SUN_SENSORS_ONE_SIDE].setMode(MODE_OFF);
         modeTable[idx - NUMBER_SUN_SENSORS_ONE_SIDE].setSubmode(SUBMODE_NONE);
       }
-      return result;
+      break;
     }
     case (DUAL_MODE): {
       for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
-        cmdSeq(helper.susIds[idx], idx);
+        cmdSeq(helper.susIds[idx], helper.susModes[idx], idx);
       }
-      return result;
+      break;
     }
   }
-  return RETURN_OK;
+  if (needsSecondStep) {
+    result = NEED_SECOND_STEP;
+  }
+  return result;
 }
 
 ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {

mission/system/SusAssembly.h

@@ -20,13 +20,23 @@ class SusAssembly : DualLaneAssemblyBase {
   static constexpr uint8_t NUMBER_SUN_SENSORS_ONE_SIDE = 6;
   static constexpr uint8_t NUMBER_SUN_SENSORS = 12;
 
+  // Use these variables instead of magic numbers when generator was updated
+  // TRANSITION_OTHER_SIDE_FAILED_ID
+  // NOT_ENOUGH_DEVICES_DUAL_MODE_ID
+  // POWER_STATE_MACHINE_TIMEOUT_ID
+  // SIDE_SWITCH_TRANSITION_NOT_ALLOWED_ID
   static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SUS_BOARD_ASS;
   static constexpr Event TRANSITION_OTHER_SIDE_FAILED =
-      event::makeEvent(SUBSYSTEM_ID, TRANSITION_OTHER_SIDE_FAILED_ID, severity::HIGH);
+      event::makeEvent(SUBSYSTEM_ID, 0, severity::HIGH);
   static constexpr Event NOT_ENOUGH_DEVICES_DUAL_MODE =
-      event::makeEvent(SUBSYSTEM_ID, NOT_ENOUGH_DEVICES_DUAL_MODE_ID, severity::HIGH);
+      event::makeEvent(SUBSYSTEM_ID, 1, severity::HIGH);
   static constexpr Event POWER_STATE_MACHINE_TIMEOUT =
-      event::makeEvent(SUBSYSTEM_ID, POWER_STATE_MACHINE_TIMEOUT_ID, severity::MEDIUM);
+      event::makeEvent(SUBSYSTEM_ID, 2, severity::MEDIUM);
+  //! [EXPORT] : [COMMENT] Not implemented, would increase already high complexity. Operator
+  //! should instead command the assembly off first and then command the assembly on into the
+  //! desired mode/submode combination
+  static constexpr Event SIDE_SWITCH_TRANSITION_NOT_ALLOWED =
+      event::makeEvent(SUBSYSTEM_ID, 3, severity::LOW);
 
   SusAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher,
               SusAssHelper helper);
@@ -41,15 +51,14 @@ class SusAssembly : DualLaneAssemblyBase {
 
   SusAssHelper helper;
   PowerSwitchIF* pwrSwitcher = nullptr;
-
+  bool tryingOtherSide = false;
+  bool dualModeErrorSwitch = true;
   ReturnValue_t initialize() override;
 
   // AssemblyBase overrides
   ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) override;
   ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) override;
-  ReturnValue_t isModeCombinationValid(Mode_t mode, Submode_t submode) override;
   void handleModeReached() override;
-  void handleModeTransitionFailed(ReturnValue_t result) override;
 
   /**
    * Check whether it makes sense to send mode commands to the device

mission/system/definitions.h

@@ -6,7 +6,7 @@
 namespace duallane {
 
 enum class PwrStates { IDLE, SWITCHING_POWER, CHECKING_POWER, MODE_COMMANDING };
-enum class OpCodes { NONE, FINISH_OP, START_TRANSITION, TIMEOUT_OCCURED };
+enum class OpCodes { NONE, TO_OFF_DONE, TO_NOT_OFF_DONE, TIMEOUT_OCCURED };
 enum Submodes : Submode_t { A_SIDE = 0, B_SIDE = 1, DUAL_MODE = 2 };
 
 }  // namespace duallane