bsp_q7s/core/ObjectFactory.cpp

@@ -709,10 +709,10 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, UartComI
                                      acsBoardHelper, gpioComIF);
   static_cast<void>(acsAss);
 
-#if OBSW_TEST_ACS_BAORD_ASS == 1
+#if OBSW_TEST_ACS_BOARD_ASS == 1
   CommandMessage msg;
   ModeMessage::setModeMessage(&msg, ModeMessage::CMD_MODE_COMMAND, DeviceHandlerIF::MODE_NORMAL,
-                              AcsBoardAssembly::A_SIDE);
+                              duallane::A_SIDE);
   ReturnValue_t result = MessageQueueSenderIF::sendMessage(acsAss->getCommandQueue(), &msg);
   if (result != HasReturnvaluesIF::RETURN_OK) {
     sif::warning << "Sending mode command failed" << std::endl;

fsfw

@@ -1 +1 @@
-Subproject commit 45f0d7fd453eafddbc8a364e6c61a90b5f577c85
+Subproject commit 3c53e2c259c43d2ebcc8fc3642fbb6bff84093c6

linux/devices/GPSHyperionLinuxController.cpp

@@ -30,10 +30,12 @@ ReturnValue_t GPSHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_
                                                            uint32_t *msToReachTheMode) {
   if (not modeCommanded) {
     if (mode == MODE_ON or mode == MODE_OFF) {
-      // 10 minutes time to reach fix
+      // 5h time to reach fix
-      *msToReachTheMode = 600000;
+      *msToReachTheMode = MAX_SECONDS_TO_REACH_FIX;
       maxTimeToReachFix.resetTimer();
       modeCommanded = true;
+    } else if (mode == MODE_NORMAL) {
+      return HasModesIF::INVALID_MODE_RETVAL;
     }
   }
   return HasReturnvaluesIF::RETURN_OK;

linux/devices/GPSHyperionLinuxController.h

@@ -21,7 +21,7 @@
  */
 class GPSHyperionLinuxController : public ExtendedControllerBase {
  public:
-  static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 600;
+  static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 60 * 60 * 5;
   GPSHyperionLinuxController(object_id_t objectId, object_id_t parentId,
                              bool debugHyperionGps = false);
   virtual ~GPSHyperionLinuxController();

linux/devices/startracker/StarTrackerHandler.cpp

@@ -672,7 +672,7 @@ ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t
         return INVALID_SUBMODE;
       }
     default:
-      return HasModesIF::INVALID_MODE;
+      return HasModesIF::INVALID_MODE_RETVAL;
   }
 }
 

linux/fsfwconfig/OBSWConfig.h.in

@@ -84,7 +84,7 @@ debugging. */
 #define OBSW_ADD_UART_TEST_CODE                     0
 
 #define OBSW_TEST_ACS                               0
-#define OBSW_TEST_ACS_BAORD_ASS                     0
+#define OBSW_TEST_ACS_BOARD_ASS                     0
 #define OBSW_DEBUG_ACS                              0
 #define OBSW_TEST_SUS                               0
 #define OBSW_DEBUG_SUS                              0

mission/controller/ThermalController.cpp

@@ -42,7 +42,7 @@ ReturnValue_t ThermalController::checkModeCommand(Mode_t mode, Submode_t submode
     return INVALID_SUBMODE;
   }
   if ((mode != MODE_OFF) && (mode != MODE_ON) && (mode != MODE_NORMAL)) {
-    return INVALID_MODE;
+    return INVALID_MODE_RETVAL;
   }
   return RETURN_OK;
 }

mission/devices/GomspaceDeviceHandler.cpp

@@ -1,5 +1,7 @@
-#include <mission/devices/GomspaceDeviceHandler.h>
+#include "GomspaceDeviceHandler.h"
-#include <mission/devices/devicedefinitions/GomSpacePackets.h>
+
+#include "devicedefinitions/GomSpacePackets.h"
+#include "devicedefinitions/powerDefinitions.h"
 
 GomspaceDeviceHandler::GomspaceDeviceHandler(object_id_t objectId, object_id_t comIF,
                                              CookieIF* comCookie, uint16_t maxConfigTableAddress,
@@ -189,6 +191,11 @@ ReturnValue_t GomspaceDeviceHandler::generateSetParamCommand(const uint8_t* comm
                                                              size_t commandDataLen) {
   ReturnValue_t result =
       setParamCacher.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::BIG);
+  // This breaks layering but I really don't want to accept this command..
+  if (setParamCacher.getAddress() == PDU2::CONFIG_ADDRESS_OUT_EN_Q7S) {
+    triggerEvent(power::SWITCHING_Q7S_DENIED, 0, 0);
+    return HasReturnvaluesIF::RETURN_FAILED;
+  }
   if (result != HasReturnvaluesIF::RETURN_OK) {
     sif::error << "GomspaceDeviceHandler: Failed to deserialize set parameter "
                   "message"

mission/devices/PCDUHandler.cpp

@@ -160,22 +160,24 @@ void PCDUHandler::updatePdu2SwitchStates() {
   PoolReadGuard rg(&pdu2HkTableDataset);
   if (rg.getReadResult() == RETURN_OK) {
     MutexGuard mg(pwrMutex);
-    switchStates[Switches::PDU2_CH0_Q7S] = pdu2HkTableDataset.outEnabledQ7S.value;
+    checkAndUpdateSwitch(Switches::PDU2_CH0_Q7S, pdu2HkTableDataset.outEnabledQ7S.value);
-    switchStates[Switches::PDU2_CH1_PL_PCDU_BATT_0_14V8] =
+
-        pdu2HkTableDataset.outEnabledPlPCDUCh1.value;
+    checkAndUpdateSwitch(Switches::PDU2_CH1_PL_PCDU_BATT_0_14V8,
-    switchStates[Switches::PDU2_CH2_RW_5V] = pdu2HkTableDataset.outEnabledReactionWheels.value;
+                         pdu2HkTableDataset.outEnabledPlPCDUCh1.value);
-    switchStates[Switches::PDU2_CH3_TCS_BOARD_HEATER_IN_8V] =
+    checkAndUpdateSwitch(Switches::PDU2_CH2_RW_5V,
-        pdu2HkTableDataset.outEnabledTCSBoardHeaterIn.value;
+                         pdu2HkTableDataset.outEnabledReactionWheels.value);
-    switchStates[Switches::PDU2_CH4_SUS_REDUNDANT_3V3] =
+    checkAndUpdateSwitch(Switches::PDU2_CH3_TCS_BOARD_HEATER_IN_8V,
-        pdu2HkTableDataset.outEnabledSUSRedundant.value;
+                         pdu2HkTableDataset.outEnabledTCSBoardHeaterIn.value);
-    switchStates[Switches::PDU2_CH5_DEPLOYMENT_MECHANISM_8V] =
+    checkAndUpdateSwitch(Switches::PDU2_CH4_SUS_REDUNDANT_3V3,
-        pdu2HkTableDataset.outEnabledDeplMechanism.value;
+                         pdu2HkTableDataset.outEnabledSUSRedundant.value);
-    switchStates[Switches::PDU2_CH6_PL_PCDU_BATT_1_14V8] =
+    checkAndUpdateSwitch(Switches::PDU2_CH5_DEPLOYMENT_MECHANISM_8V,
-        pdu2HkTableDataset.outEnabledPlPCDUCh6.value;
+                         pdu2HkTableDataset.outEnabledDeplMechanism.value);
-    switchStates[Switches::PDU2_CH7_ACS_BOARD_SIDE_B_3V3] =
+    checkAndUpdateSwitch(Switches::PDU2_CH6_PL_PCDU_BATT_1_14V8,
-        pdu2HkTableDataset.outEnabledAcsBoardSideB.value;
+                         pdu2HkTableDataset.outEnabledPlPCDUCh6.value);
-    switchStates[Switches::PDU2_CH8_PAYLOAD_CAMERA] =
+    checkAndUpdateSwitch(Switches::PDU2_CH7_ACS_BOARD_SIDE_B_3V3,
-        pdu2HkTableDataset.outEnabledPayloadCamera.value;
+                         pdu2HkTableDataset.outEnabledAcsBoardSideB.value);
+    checkAndUpdateSwitch(Switches::PDU2_CH8_PAYLOAD_CAMERA,
+                         pdu2HkTableDataset.outEnabledPayloadCamera.value);
   } else {
     sif::debug << "PCDUHandler::updatePdu2SwitchStates: Failed to read PDU2 Hk Dataset"
                << std::endl;
@@ -187,19 +189,22 @@ void PCDUHandler::updatePdu1SwitchStates() {
   PoolReadGuard rg(&pdu1HkTableDataset);
   if (rg.getReadResult() == RETURN_OK) {
     MutexGuard mg(pwrMutex);
-    switchStates[Switches::PDU1_CH0_TCS_BOARD_3V3] = pdu1HkTableDataset.outEnabledTCSBoard3V3.value;
+    checkAndUpdateSwitch(Switches::PDU1_CH0_TCS_BOARD_3V3,
-    switchStates[Switches::PDU1_CH1_SYRLINKS_12V] = pdu1HkTableDataset.outEnabledSyrlinks.value;
+                         pdu1HkTableDataset.outEnabledTCSBoard3V3.value);
-    switchStates[Switches::PDU1_CH2_STAR_TRACKER_5V] =
+    checkAndUpdateSwitch(Switches::PDU1_CH1_SYRLINKS_12V,
-        pdu1HkTableDataset.outEnabledStarTracker.value;
+                         pdu1HkTableDataset.outEnabledSyrlinks.value);
-    switchStates[Switches::PDU1_CH3_MGT_5V] = pdu1HkTableDataset.outEnabledMGT.value;
+    checkAndUpdateSwitch(Switches::PDU1_CH2_STAR_TRACKER_5V,
-    switchStates[Switches::PDU1_CH4_SUS_NOMINAL_3V3] =
+                         pdu1HkTableDataset.outEnabledStarTracker.value);
-        pdu1HkTableDataset.outEnabledSUSNominal.value;
+    checkAndUpdateSwitch(Switches::PDU1_CH3_MGT_5V, pdu1HkTableDataset.outEnabledMGT.value);
-    switchStates[Switches::PDU1_CH5_SOLAR_CELL_EXP_5V] =
+    checkAndUpdateSwitch(Switches::PDU1_CH4_SUS_NOMINAL_3V3,
-        pdu1HkTableDataset.outEnabledSolarCellExp.value;
+                         pdu1HkTableDataset.outEnabledSUSNominal.value);
-    switchStates[Switches::PDU1_CH6_PLOC_12V] = pdu1HkTableDataset.outEnabledPLOC.value;
+    checkAndUpdateSwitch(Switches::PDU1_CH5_SOLAR_CELL_EXP_5V,
-    switchStates[Switches::PDU1_CH7_ACS_A_SIDE_3V3] =
+                         pdu1HkTableDataset.outEnabledSolarCellExp.value);
-        pdu1HkTableDataset.outEnabledAcsBoardSideA.value;
+    checkAndUpdateSwitch(Switches::PDU1_CH6_PLOC_12V, pdu1HkTableDataset.outEnabledPLOC.value);
-    switchStates[Switches::PDU1_CH8_UNOCCUPIED] = pdu1HkTableDataset.outEnabledChannel8.value;
+    checkAndUpdateSwitch(Switches::PDU1_CH7_ACS_A_SIDE_3V3,
+                         pdu1HkTableDataset.outEnabledAcsBoardSideA.value);
+    checkAndUpdateSwitch(Switches::PDU1_CH8_UNOCCUPIED,
+                         pdu1HkTableDataset.outEnabledChannel8.value);
   } else {
     sif::debug << "PCDUHandler::updatePdu1SwitchStates: Failed to read dataset" << std::endl;
   }
@@ -261,11 +266,12 @@ void PCDUHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onOff) const
       pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
       break;
     }
-    // That does not really make sense but let's keep it here for completeness reasons..
+    // This is a dangerous command. Reject/Igore it for now
     case pcduSwitches::PDU2_CH0_Q7S: {
-      memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_Q7S;
+      return;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
+      // memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_Q7S;
-      break;
+      // pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
+      // break;
     }
     case pcduSwitches::PDU2_CH1_PL_PCDU_BATT_0_14V8: {
       memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_PAYLOAD_PCDU_CH1;
@@ -344,6 +350,9 @@ void PCDUHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onOff) const
   if (result != RETURN_OK) {
     sif::debug << "PCDUHandler::sendSwitchCommand: Failed to send message to PDU Handler"
                << std::endl;
+  } else {
+    // Can't use trigger event because of const function constraint, but this hack seems to work
+    this->forwardEvent(power::SWITCH_CMD_SENT, parameterValue, switchNr);
   }
 }
 
@@ -592,3 +601,10 @@ LocalPoolDataSetBase* PCDUHandler::getDataSetHandle(sid_t sid) {
     return nullptr;
   }
 }
+
+void PCDUHandler::checkAndUpdateSwitch(pcduSwitches::Switches switchIdx, uint8_t setValue) {
+  if (switchStates[switchIdx] != setValue) {
+    triggerEvent(power::SWITCH_HAS_CHANGED, setValue, switchIdx);
+  }
+  switchStates[switchIdx] = setValue;
+}

mission/devices/PCDUHandler.h

@@ -8,13 +8,16 @@
 #include <fsfw/tasks/ExecutableObjectIF.h>
 #include <fsfw/timemanager/CCSDSTime.h>
 #include <mission/devices/GomspaceDeviceHandler.h>
-#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
+
+#include "devicedefinitions/GomspaceDefinitions.h"
+#include "devicedefinitions/powerDefinitions.h"
 
 /**
  * @brief	The PCDUHandler provides a compact interface to handle all devices related to the
- * 			control of power. This is necessary because the fsfw manages all power
+ * 			control of power.
- * related functionalities via one power object. This includes for example the switch on and off of
+ * @details
- * devices.
+ * This is necessary because the FSFW manages all power related functionalities via one
+ * power object. This includes for example switching on and off of devices.
  */
 class PCDUHandler : public PowerSwitchIF,
                     public HasLocalDataPoolIF,
@@ -114,6 +117,7 @@ class PCDUHandler : public PowerSwitchIF,
    */
   void updateHkTableDataset(store_address_t storeId, LocalPoolDataSetBase* dataset,
                             CCSDSTime::CDS_short* datasetTimeStamp);
+  void checkAndUpdateSwitch(pcduSwitches::Switches switchIdx, uint8_t setValue);
 };
 
 #endif /* MISSION_DEVICES_PCDUHANDLER_H_ */

mission/devices/devicedefinitions/powerDefinitions.h

@@ -0,0 +1,18 @@
+#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_POWERDEFINITIONS_H_
+#define MISSION_DEVICES_DEVICEDEFINITIONS_POWERDEFINITIONS_H_
+
+#include <common/config/commonSubsystemIds.h>
+#include <fsfw/events/Event.h>
+
+namespace power {
+
+static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PCDU_HANDLER;
+//! [EXPORT] : [COMMENT] Indicated that a FSFW object requested setting a switch
+//! P1: 1 if on was requested, 0 for off | P2: Switch Index
+static constexpr Event SWITCH_CMD_SENT = event::makeEvent(SUBSYSTEM_ID, 0, severity::INFO);
+static constexpr Event SWITCH_HAS_CHANGED = event::makeEvent(SUBSYSTEM_ID, 1, severity::INFO);
+static constexpr Event SWITCHING_Q7S_DENIED = event::makeEvent(SUBSYSTEM_ID, 2, severity::MEDIUM);
+
+}  // namespace power
+
+#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_POWERDEFINITIONS_H_ */

mission/system/AcsBoardAssembly.cpp

@@ -6,7 +6,10 @@
 
 AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, object_id_t parentId,
                                    PowerSwitchIF* switcher, AcsBoardHelper helper, GpioIF* gpioIF)
-    : AssemblyBase(objectId, parentId), pwrSwitcher(switcher), helper(helper), gpioIF(gpioIF) {
+    : AssemblyBase(objectId, parentId),
+      pwrStateMachine(SWITCH_A, SWITCH_B, switcher, state),
+      helper(helper),
+      gpioIF(gpioIF) {
   if (switcher == nullptr) {
     sif::error << "AcsBoardAssembly::AcsBoardAssembly: Invalid Power Switcher "
                   "IF passed"
@@ -27,24 +30,54 @@ AcsBoardAssembly::AcsBoardAssembly(object_id_t objectId, object_id_t parentId,
   initModeTableEntry(helper.gpsId, entry);
 }
 
-void AcsBoardAssembly::handleChildrenTransition() {
+void AcsBoardAssembly::performChildOperation() {
-  if (state == States::SWITCHING_POWER) {
+  using namespace duallane;
-    powerStateMachine(targetMode, targetSubmode);
+  if (state == PwrStates::SWITCHING_POWER or state == PwrStates::CHECKING_POWER) {
-    if (state == States::MODE_COMMANDING) {
+    if (targetMode != MODE_OFF) {
-      AssemblyBase::handleChildrenTransition();
+      pwrStateMachineWrapper(targetMode, targetSubmode);
+    }
+    // This state is the indicator that the power state machine is done
+    if (state == PwrStates::MODE_COMMANDING) {
+      AssemblyBase::performChildOperation();
     }
   } else {
-    AssemblyBase::handleChildrenTransition();
+    AssemblyBase::performChildOperation();
+    // This state is the indicator that the mode state machine is done
+    if (state == PwrStates::SWITCHING_POWER) {
+      pwrStateMachineWrapper(targetMode, targetSubmode);
+    }
+  }
+}
+
+void AcsBoardAssembly::startTransition(Mode_t mode, Submode_t submode) {
+  using namespace duallane;
+  // If anything other than MODE_OFF is commanded, perform power state machine first
+  if (mode != MODE_OFF) {
+    if (state != PwrStates::IDLE) {
+      state = PwrStates::IDLE;
+    }
+    // Cache the target modes, required by power state machine
+    targetMode = mode;
+    targetSubmode = submode;
+    state = PwrStates::SWITCHING_POWER;
+    // Perform power state machine first, then start mode transition. The power state machine will
+    // start the transition after it has finished
+    pwrStateMachineWrapper(mode, submode);
+  } 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);
   }
 }
 
 ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) {
+  using namespace duallane;
   ReturnValue_t result = RETURN_OK;
   refreshHelperModes();
-  powerStateMachine(mode, submode);
+  pwrStateMachineWrapper(mode, submode);
-  if (state == States::MODE_COMMANDING) {
+  if (state == PwrStates::MODE_COMMANDING) {
     if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
-      handleNormalOrOnModeCmd(mode, submode);
+      result = handleNormalOrOnModeCmd(mode, submode);
     } else {
       modeTable[ModeTableIdx::GYRO_0_A].setMode(MODE_OFF);
       modeTable[ModeTableIdx::GYRO_0_A].setSubmode(SUBMODE_NONE);
@@ -72,8 +105,9 @@ ReturnValue_t AcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode)
 }
 
 ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
+  using namespace duallane;
   refreshHelperModes();
-  if (state == States::SWITCHING_POWER) {
+  if (state == PwrStates::SWITCHING_POWER) {
     // Wrong mode
     sif::error << "Wrong mode, currently switching power" << std::endl;
     return RETURN_OK;
@@ -81,14 +115,14 @@ ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_
   if (wantedSubmode == A_SIDE) {
     if ((helper.gyro0SideAMode != wantedMode and helper.gyro1SideAMode != wantedMode) or
         (helper.mgm0SideAMode != wantedMode and helper.mgm1SideAMode != wantedMode) or
-        helper.gpsMode != wantedMode) {
+        helper.gpsMode != MODE_ON) {
       return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
     }
     return RETURN_OK;
   } else if (wantedSubmode == B_SIDE) {
     if ((helper.gyro2SideBMode != wantedMode and helper.gyro3SideBMode != wantedMode) or
         (helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or
-        helper.gpsMode != wantedMode) {
+        helper.gpsMode != MODE_ON) {
       return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
     }
     return RETURN_OK;
@@ -97,7 +131,7 @@ ReturnValue_t AcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_
          helper.gyro2AdisIdSideB != wantedMode and helper.gyro3SideBMode != wantedMode) or
         (helper.mgm0SideAMode != wantedMode and helper.mgm1SideAMode != wantedMode and
          helper.mgm2SideBMode != wantedMode and helper.mgm3SideBMode != wantedMode) or
-        helper.gpsMode != wantedMode) {
+        helper.gpsMode != MODE_ON) {
       // Trigger event, but don't start any other transitions. This is the last fallback mode.
       if (dualModeErrorSwitch) {
         triggerEvent(NOT_ENOUGH_DEVICES_DUAL_MODE, 0, 0);
@@ -111,16 +145,16 @@ 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;
   auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, ModeTableIdx tableIdx) {
     if (mode == DeviceHandlerIF::MODE_NORMAL) {
       if (isUseable(objectId, devMode)) {
-        if (mode != MODE_OFF) {
+        if (devMode == MODE_OFF or devMode == HasModesIF::UNDEFINED_MODE) {
-          modeTable[tableIdx].setMode(mode);
+          modeTable[tableIdx].setMode(MODE_ON);
           modeTable[tableIdx].setSubmode(SUBMODE_NONE);
         } else {
-          result = NEED_SECOND_STEP;
+          modeTable[tableIdx].setMode(mode);
-          modeTable[tableIdx].setMode(MODE_ON);
           modeTable[tableIdx].setSubmode(SUBMODE_NONE);
         }
       }
@@ -131,15 +165,20 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
       }
     }
   };
+  if (this->mode == MODE_OFF and mode == DeviceHandlerIF::MODE_NORMAL) {
+    if (internalState != STATE_SECOND_STEP) {
+      result = NEED_SECOND_STEP;
+    }
+  }
   switch (submode) {
     case (A_SIDE): {
       cmdSeq(helper.gyro0AdisIdSideA, helper.gyro0SideAMode, ModeTableIdx::GYRO_0_A);
       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);
-      cmdSeq(helper.gpsId, helper.gpsMode, ModeTableIdx::GPS);
+      modeTable[ModeTableIdx::GPS].setMode(MODE_ON);
-      ReturnValue_t result = gpioIF->pullLow(gpioIds::GNSS_SELECT);
+      modeTable[ModeTableIdx::GPS].setSubmode(SUBMODE_NONE);
-      if (result != 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"
                    << std::endl;
@@ -161,8 +200,7 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
       cmdSeq(helper.mgm2Lis3IdSideB, helper.mgm2SideBMode, ModeTableIdx::MGM_2_B);
       cmdSeq(helper.mgm3Rm3100IdSideB, helper.mgm3SideBMode, ModeTableIdx::MGM_3_B);
       cmdSeq(helper.gpsId, helper.gpsMode, ModeTableIdx::GPS);
-      gpioIF->pullHigh(gpioIds::GNSS_SELECT);
+      if (gpioIF->pullHigh(gpioIds::GNSS_SELECT) != HasReturnvaluesIF::RETURN_OK) {
-      if (result != HasReturnvaluesIF::RETURN_OK) {
 #if OBSW_VERBOSE_LEVEL >= 1
         sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select high"
                    << std::endl;
@@ -188,12 +226,13 @@ 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);
+      ReturnValue_t status = RETURN_OK;
       if (defaultSubmode == Submodes::A_SIDE) {
-        result = gpioIF->pullLow(gpioIds::GNSS_SELECT);
+        status = gpioIF->pullLow(gpioIds::GNSS_SELECT);
       } else {
-        result = gpioIF->pullHigh(gpioIds::GNSS_SELECT);
+        status = gpioIF->pullHigh(gpioIds::GNSS_SELECT);
       }
-      if (result != HasReturnvaluesIF::RETURN_OK) {
+      if (status != HasReturnvaluesIF::RETURN_OK) {
 #if OBSW_VERBOSE_LEVEL >= 1
         sif::error << "AcsBoardAssembly::handleNormalOrOnModeCmd: Could not pull GNSS select to"
                       "default side for dual mode"
@@ -209,89 +248,154 @@ ReturnValue_t AcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t s
   return result;
 }
 
-void AcsBoardAssembly::powerStateMachine(Mode_t mode, Submode_t submode) {
+ReturnValue_t AcsBoardAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
-  ReturnValue_t switchStateA = RETURN_OK;
+  using namespace duallane;
-  ReturnValue_t switchStateB = RETURN_OK;
+  if (submode != A_SIDE and submode != B_SIDE and submode != DUAL_MODE) {
-  if (state == States::IDLE or state == States::SWITCHING_POWER) {
+    return HasReturnvaluesIF::RETURN_FAILED;
-    switchStateA = pwrSwitcher->getSwitchState(SWITCH_A);
+  }
-    switchStateB = pwrSwitcher->getSwitchState(SWITCH_B);
+  return HasReturnvaluesIF::RETURN_OK;
+}
+
+bool AcsBoardAssembly::isUseable(object_id_t object, Mode_t mode) {
+  if (healthHelper.healthTable->isFaulty(object)) {
+    return false;
+  }
+
+  // Check if device is already in target mode
+  if (childrenMap[object].mode == mode) {
+    return true;
+  }
+
+  if (healthHelper.healthTable->isCommandable(object)) {
+    return true;
+  }
+  return false;
+}
+
+void AcsBoardAssembly::handleModeReached() {
+  using namespace duallane;
+  if (targetMode == MODE_OFF) {
+    if (state != PwrStates::IDLE) {
+      state = PwrStates::IDLE;
+    }
+    state = PwrStates::SWITCHING_POWER;
+    // Now we can switch off the power. After that, the AssemblyBase::handleModeReached function
+    // will be called
+    pwrStateMachineWrapper(targetMode, targetSubmode);
   } else {
-    return;
+    finishModeOp();
   }
-  if (mode == MODE_OFF) {
-    if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_OFF) {
-      state = States::MODE_COMMANDING;
-      return;
 }
+
+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 {
-    switch (submode) {
+    // Not sure when this would happen. This flag is reset if the mode was reached. If it
-      case (A_SIDE): {
+    // was not reached, the transition failure handler should be called.
-        if (switchStateA == PowerSwitchIF::SWITCH_ON and
+    sif::error << "AcsBoardAssembly::handleChildrenLostMode: Wrong handler called" << std::endl;
-            switchStateB == PowerSwitchIF::SWITCH_OFF) {
+    triggerEvent(TRANSITION_OTHER_SIDE_FAILED, mode, targetSubmode);
-          state = States::MODE_COMMANDING;
+    startTransition(mode, Submodes::DUAL_MODE);
-          return;
-        }
-        break;
-      }
-      case (B_SIDE): {
-        if (switchStateA == PowerSwitchIF::SWITCH_OFF and
-            switchStateB == PowerSwitchIF::SWITCH_ON) {
-          state = States::MODE_COMMANDING;
-          return;
-        }
-        break;
-      }
-      case (DUAL_MODE): {
-        if (switchStateA == PowerSwitchIF::SWITCH_ON and switchStateB == PowerSwitchIF::SWITCH_ON) {
-          state = States::MODE_COMMANDING;
-          return;
   }
 }
+
+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 (state == States::IDLE) {
+  // If this is not the case, start transition to other side. If it is the case, start
-    if (mode == MODE_OFF) {
+  // transition to dual mode.
-      if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
+  if (not tryingOtherSide) {
-        pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON);
+    triggerEvent(CANT_KEEP_MODE, mode, submode);
-      }
+    startTransition(mode, nextSubmode);
-      if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
+    tryingOtherSide = true;
-        pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
-      }
   } else {
-      switch (submode) {
+    triggerEvent(TRANSITION_OTHER_SIDE_FAILED, mode, targetSubmode);
-        case (A_SIDE): {
+    startTransition(mode, Submodes::DUAL_MODE);
-          if (switchStateA != PowerSwitchIF::SWITCH_ON) {
-            pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON);
-          }
-          if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
-            pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
-          }
-          break;
-        }
-        case (B_SIDE): {
-          if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
-            pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_OFF);
-          }
-          if (switchStateB != PowerSwitchIF::SWITCH_ON) {
-            pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
-          }
-          break;
-        }
-        case (DUAL_MODE): {
-          if (switchStateA != PowerSwitchIF::SWITCH_ON) {
-            pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON);
-          }
-          if (switchStateB != PowerSwitchIF::SWITCH_ON) {
-            pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_ON);
-          }
-          break;
   }
 }
+
+void AcsBoardAssembly::setPreferredSide(duallane::Submodes submode) {
+  using namespace duallane;
+  if (submode != Submodes::A_SIDE and submode != Submodes::B_SIDE) {
+    return;
   }
-    state = States::SWITCHING_POWER;
+  this->defaultSubmode = submode;
 }
-  if (state == States::SWITCHING_POWER) {
+
-    // TODO: Could check for a timeout (temporal or cycles) here and resend command
+void AcsBoardAssembly::selectGpsInDualMode(duallane::Submodes side) {
+  using namespace duallane;
+  if (submode != Submodes::DUAL_MODE) {
+    return;
+  }
+  ReturnValue_t result = RETURN_OK;
+  if (side == Submodes::A_SIDE) {
+    result = gpioIF->pullLow(gpioIds::GNSS_SELECT);
+  } else {
+    result = gpioIF->pullHigh(gpioIds::GNSS_SELECT);
+  }
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+#if OBSW_VERBOSE_LEVEL >= 1
+    sif::error << "AcsBoardAssembly::switchGpsInDualMode: Switching GPS failed" << std::endl;
+#endif
+  }
+}
+
+void AcsBoardAssembly::refreshHelperModes() {
+  try {
+    helper.gyro0SideAMode = childrenMap.at(helper.gyro0AdisIdSideA).mode;
+    helper.gyro1SideAMode = childrenMap.at(helper.gyro1L3gIdSideA).mode;
+    helper.gyro2SideBMode = childrenMap.at(helper.gyro2AdisIdSideB).mode;
+    helper.gyro3SideBMode = childrenMap.at(helper.gyro2AdisIdSideB).mode;
+    helper.mgm0SideAMode = childrenMap.at(helper.mgm0Lis3IdSideA).mode;
+    helper.mgm1SideAMode = childrenMap.at(helper.mgm1Rm3100IdSideA).mode;
+    helper.mgm2SideBMode = childrenMap.at(helper.mgm2Lis3IdSideB).mode;
+    helper.mgm3SideBMode = childrenMap.at(helper.mgm3Rm3100IdSideB).mode;
+    helper.gpsMode = childrenMap.at(helper.gpsId).mode;
+  } catch (const std::out_of_range& e) {
+    sif::error << "AcsBoardAssembly::refreshHelperModes: Invalid map: " << e.what() << std::endl;
+  }
+}
+
+void AcsBoardAssembly::initModeTableEntry(object_id_t id, ModeListEntry& entry) {
+  entry.setObject(id);
+  entry.setMode(MODE_OFF);
+  entry.setSubmode(SUBMODE_NONE);
+  entry.setInheritSubmode(false);
+  modeTable.insert(entry);
+}
+
+void AcsBoardAssembly::finishModeOp() {
+  using namespace duallane;
+  AssemblyBase::handleModeReached();
+  state = PwrStates::IDLE;
+  tryingOtherSide = false;
+  dualModeErrorSwitch = true;
+}
+
+void AcsBoardAssembly::pwrStateMachineWrapper(Mode_t mode, Submode_t submode) {
+  using namespace duallane;
+  OpCodes opCode = pwrStateMachine.powerStateMachine(mode, submode);
+  if (opCode == OpCodes::NONE) {
+    return;
+  } else if (opCode == OpCodes::FINISH_OP) {
+    finishModeOp();
+  } else if (opCode == OpCodes::START_TRANSITION) {
+    AssemblyBase::startTransition(mode, submode);
   }
 }
 
@@ -334,116 +438,3 @@ ReturnValue_t AcsBoardAssembly::initialize() {
   }
   return AssemblyBase::initialize();
 }
-
-ReturnValue_t AcsBoardAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
-  if (submode != A_SIDE and submode != B_SIDE and submode != DUAL_MODE) {
-    return HasReturnvaluesIF::RETURN_FAILED;
-  }
-  return HasReturnvaluesIF::RETURN_OK;
-}
-
-bool AcsBoardAssembly::isUseable(object_id_t object, Mode_t mode) {
-  if (healthHelper.healthTable->isFaulty(object)) {
-    return false;
-  }
-
-  // Check if device is already in target mode
-  if (childrenMap[object].mode == mode) {
-    return true;
-  }
-
-  if (healthHelper.healthTable->isCommandable(object)) {
-    return true;
-  }
-  return false;
-}
-
-void AcsBoardAssembly::handleModeReached() {
-  AssemblyBase::handleModeReached();
-  state = States::IDLE;
-  tryingOtherSide = false;
-  dualModeErrorSwitch = true;
-}
-
-void AcsBoardAssembly::handleChildrenLostMode(ReturnValue_t result) {
-  // 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) {
-  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(Submodes submode) {
-  if (submode != Submodes::A_SIDE and submode != Submodes::B_SIDE) {
-    return;
-  }
-  this->defaultSubmode = submode;
-}
-
-void AcsBoardAssembly::selectGpsInDualMode(Submodes side) {
-  if (submode != Submodes::DUAL_MODE) {
-    return;
-  }
-  ReturnValue_t result = RETURN_OK;
-  if (side == Submodes::A_SIDE) {
-    result = gpioIF->pullLow(gpioIds::GNSS_SELECT);
-  } else {
-    result = gpioIF->pullHigh(gpioIds::GNSS_SELECT);
-  }
-  if (result != HasReturnvaluesIF::RETURN_OK) {
-#if OBSW_VERBOSE_LEVEL >= 1
-    sif::error << "AcsBoardAssembly::switchGpsInDualMode: Switching GPS failed" << std::endl;
-#endif
-  }
-}
-
-void AcsBoardAssembly::refreshHelperModes() {
-  helper.gyro0SideAMode = childrenMap[helper.gyro0AdisIdSideA].mode;
-  helper.gyro1SideAMode = childrenMap[helper.gyro1L3gIdSideA].mode;
-  helper.gyro2SideBMode = childrenMap[helper.gyro2AdisIdSideB].mode;
-  helper.gyro3SideBMode = childrenMap[helper.gyro2AdisIdSideB].mode;
-  helper.mgm0SideAMode = childrenMap[helper.mgm0Lis3IdSideA].mode;
-  helper.mgm1SideAMode = childrenMap[helper.mgm1Rm3100IdSideA].mode;
-  helper.mgm2SideBMode = childrenMap[helper.mgm2Lis3IdSideB].mode;
-  helper.mgm3SideBMode = childrenMap[helper.mgm3Rm3100IdSideB].mode;
-}
-
-void AcsBoardAssembly::initModeTableEntry(object_id_t id, ModeListEntry& entry) {
-  entry.setObject(id);
-  entry.setMode(MODE_OFF);
-  entry.setSubmode(SUBMODE_NONE);
-  entry.setInheritSubmode(false);
-  modeTable.insert(entry);
-}

mission/system/AcsBoardAssembly.h

@@ -6,6 +6,8 @@
 #include <fsfw/devicehandlers/AssemblyBase.h>
 #include <fsfw/objectmanager/frameworkObjects.h>
 
+#include "DualLanePowerStateMachine.h"
+
 struct AcsBoardHelper {
   AcsBoardHelper(object_id_t mgm0Id, object_id_t mgm1Id, object_id_t mgm2Id, object_id_t mgm3Id,
                  object_id_t gyro0Id, object_id_t gyro1Id, object_id_t gyro2Id, object_id_t gyro3Id,
@@ -58,6 +60,17 @@ enum ModeTableIdx : uint8_t {
 class PowerSwitchIF;
 class GpioIF;
 
+/**
+ * @brief  Assembly class which manages redundant ACS board sides
+ * @details
+ * This class takes care of ensuring that enough devices on the ACS board are available at all
+ * times. It does so by doing autonomous transitions to the redundant side or activating both sides
+ * if not enough devices are available.
+ *
+ * This class also takes care of switching on the A side and/or B side power lanes. Normally,
+ * doing this task would be performed by the device handlers, but this is not possible for the
+ * ACS board where multiple sensors share the same power supply.
+ */
 class AcsBoardAssembly : public AssemblyBase {
  public:
   static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_BOARD_ASS;
@@ -67,19 +80,17 @@ class AcsBoardAssembly : public AssemblyBase {
       event::makeEvent(SUBSYSTEM_ID, 1, severity::HIGH);
   static constexpr uint8_t NUMBER_DEVICES_MODE_TABLE = 9;
 
-  enum Submodes : Submode_t { A_SIDE = 0, B_SIDE = 1, DUAL_MODE = 2 };
-
   AcsBoardAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher,
                    AcsBoardHelper helper, GpioIF* gpioIF);
 
-  void setPreferredSide(Submodes submode);
+  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.
    * @param side
    */
-  void selectGpsInDualMode(Submodes side);
+  void selectGpsInDualMode(duallane::Submodes side);
 
  private:
   static constexpr pcduSwitches::Switches SWITCH_A =
@@ -87,13 +98,13 @@ class AcsBoardAssembly : public AssemblyBase {
   static constexpr pcduSwitches::Switches SWITCH_B =
       pcduSwitches::Switches::PDU2_CH7_ACS_BOARD_SIDE_B_3V3;
 
-  enum class States { IDLE, SWITCHING_POWER, MODE_COMMANDING } state = States::IDLE;
+  // This helper object complete encapsulates power switching
-
+  DualLanePowerStateMachine pwrStateMachine;
-  PowerSwitchIF* pwrSwitcher = nullptr;
   bool tryingOtherSide = false;
   AcsBoardHelper helper;
   GpioIF* gpioIF = nullptr;
-  Submodes defaultSubmode = Submodes::A_SIDE;
+  duallane::PwrStates state = duallane::PwrStates::IDLE;
+  duallane::Submodes defaultSubmode = duallane::Submodes::A_SIDE;
   bool dualModeErrorSwitch = true;
   FixedArrayList<ModeListEntry, NUMBER_DEVICES_MODE_TABLE> modeTable;
 
@@ -103,7 +114,8 @@ class AcsBoardAssembly : public AssemblyBase {
   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 handleChildrenTransition() override;
+  void performChildOperation() override;
+  void startTransition(Mode_t mode, Submode_t submode) override;
   void handleModeReached() override;
   void handleModeTransitionFailed(ReturnValue_t result) override;
   void handleChildrenLostMode(ReturnValue_t result) override;
@@ -116,9 +128,16 @@ class AcsBoardAssembly : public AssemblyBase {
    */
   bool isUseable(object_id_t object, Mode_t mode);
   ReturnValue_t handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode);
-  void powerStateMachine(Mode_t mode, Submode_t submode);
   void initModeTableEntry(object_id_t id, ModeListEntry& entry);
   void refreshHelperModes();
+  void finishModeOp();
+  /**
+   * Thin wrapper function which is required because the helper class
+   * can not access protected member functions.
+   * @param mode
+   * @param submode
+   */
+  void pwrStateMachineWrapper(Mode_t mode, Submode_t submode);
 };
 
 #endif /* MISSION_SYSTEM_ACSBOARDASSEMBLY_H_ */

mission/system/CMakeLists.txt

@@ -6,4 +6,5 @@ target_sources(${LIB_EIVE_MISSION} PRIVATE
     EiveSystem.cpp
     ComSubsystem.cpp
     TcsSubsystem.cpp
+    DualLanePowerStateMachine.cpp
 )

mission/system/DualLanePowerStateMachine.cpp

@@ -0,0 +1,98 @@
+#include "DualLanePowerStateMachine.h"
+
+#include <fsfw/devicehandlers/AssemblyBase.h>
+#include <fsfw/power/PowerSwitchIF.h>
+
+DualLanePowerStateMachine::DualLanePowerStateMachine(pcduSwitches::Switches switchA,
+                                                     pcduSwitches::Switches switchB,
+                                                     PowerSwitchIF* pwrSwitcher,
+                                                     duallane::PwrStates& state)
+    : SWITCH_A(switchA), SWITCH_B(switchB), state(state), pwrSwitcher(pwrSwitcher) {}
+
+duallane::OpCodes DualLanePowerStateMachine::powerStateMachine(Mode_t mode, Submode_t submode) {
+  using namespace duallane;
+  ReturnValue_t switchStateA = RETURN_OK;
+  ReturnValue_t switchStateB = RETURN_OK;
+  if (state == PwrStates::IDLE or state == PwrStates::SWITCHING_POWER or
+      state == PwrStates::CHECKING_POWER) {
+    switchStateA = pwrSwitcher->getSwitchState(SWITCH_A);
+    switchStateB = pwrSwitcher->getSwitchState(SWITCH_B);
+  } else {
+    return OpCodes::NONE;
+  }
+  if (mode == HasModesIF::MODE_OFF) {
+    if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_OFF) {
+      return OpCodes::FINISH_OP;
+    }
+  } else {
+    switch (submode) {
+      case (A_SIDE): {
+        if (switchStateA == PowerSwitchIF::SWITCH_ON and
+            switchStateB == PowerSwitchIF::SWITCH_OFF) {
+          state = PwrStates::MODE_COMMANDING;
+          return OpCodes::START_TRANSITION;
+        }
+        break;
+      }
+      case (B_SIDE): {
+        if (switchStateA == PowerSwitchIF::SWITCH_OFF and
+            switchStateB == PowerSwitchIF::SWITCH_ON) {
+          state = PwrStates::MODE_COMMANDING;
+          return OpCodes::START_TRANSITION;
+        }
+        break;
+      }
+      case (DUAL_MODE): {
+        if (switchStateA == PowerSwitchIF::SWITCH_ON and switchStateB == PowerSwitchIF::SWITCH_ON) {
+          state = PwrStates::MODE_COMMANDING;
+          return OpCodes::START_TRANSITION;
+        }
+      }
+    }
+  }
+  if (state == PwrStates::SWITCHING_POWER) {
+    if (mode == HasModesIF::MODE_OFF) {
+      if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
+        pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON);
+      }
+      if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
+        pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
+      }
+    } else {
+      switch (submode) {
+        case (A_SIDE): {
+          if (switchStateA != PowerSwitchIF::SWITCH_ON) {
+            pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON);
+          }
+          if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
+            pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
+          }
+          break;
+        }
+        case (B_SIDE): {
+          if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
+            pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_OFF);
+          }
+          if (switchStateB != PowerSwitchIF::SWITCH_ON) {
+            pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
+          }
+          break;
+        }
+        case (DUAL_MODE): {
+          if (switchStateA != PowerSwitchIF::SWITCH_ON) {
+            pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON);
+          }
+          if (switchStateB != PowerSwitchIF::SWITCH_ON) {
+            pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_ON);
+          }
+          break;
+        }
+      }
+    }
+    state = PwrStates::CHECKING_POWER;
+  }
+  if (state == PwrStates::CHECKING_POWER) {
+    // TODO: Could check for a timeout (temporal or cycles) here and resend command
+  }
+  return OpCodes::NONE;
+}

mission/system/DualLanePowerStateMachine.h

@@ -0,0 +1,25 @@
+#ifndef MISSION_SYSTEM_DUALLANEPOWERSTATEMACHINE_H_
+#define MISSION_SYSTEM_DUALLANEPOWERSTATEMACHINE_H_
+
+#include <devices/powerSwitcherList.h>
+#include <fsfw/modes/HasModesIF.h>
+
+#include "definitions.h"
+
+class AssemblyBase;
+class PowerSwitchIF;
+
+class DualLanePowerStateMachine : public HasReturnvaluesIF {
+ public:
+  DualLanePowerStateMachine(pcduSwitches::Switches switchA, pcduSwitches::Switches switchB,
+                            PowerSwitchIF* pwrSwitcher, duallane::PwrStates& state);
+  duallane::OpCodes powerStateMachine(Mode_t mode, Submode_t submode);
+  const pcduSwitches::Switches SWITCH_A;
+  const pcduSwitches::Switches SWITCH_B;
+
+ private:
+  duallane::PwrStates& state;
+  PowerSwitchIF* pwrSwitcher = nullptr;
+};
+
+#endif /* MISSION_SYSTEM_DUALLANEPOWERSTATEMACHINE_H_ */

mission/system/definitions.h

@@ -0,0 +1,14 @@
+#ifndef MISSION_SYSTEM_DEFINITIONS_H_
+#define MISSION_SYSTEM_DEFINITIONS_H_
+
+#include <fsfw/modes/ModeMessage.h>
+
+namespace duallane {
+
+enum class PwrStates { IDLE, SWITCHING_POWER, CHECKING_POWER, MODE_COMMANDING };
+enum class OpCodes { NONE, FINISH_OP, START_TRANSITION };
+enum Submodes : Submode_t { A_SIDE = 0, B_SIDE = 1, DUAL_MODE = 2 };
+
+}  // namespace duallane
+
+#endif /* MISSION_SYSTEM_DEFINITIONS_H_ */