Merge remote-tracking branch 'origin/develop' into mueller/pus-15-tm-storage

2023-01-11 09:18:15 +01:00 · 2023-01-11 09:18:15 +01:00 · 25c59aa187
commit 25c59aa187
parent d14d7ae66d ce9fac6b71
35 changed files with 452 additions and 219 deletions
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@ -10,10 +10,27 @@ list yields a list of all related PRs for each release.

 # [unreleased]

+# [v1.19.0] 10.01.2023
+
+## Changed
+
+- 5V stack is now off by default
+
+## Fixed
+
+- PLOC SUPV: Minor adaptions and important bugfix for UART manager
+- Allow cloning and building the hosted OBSW version without proprietary libraries,
+  which also avoids the need to have a Gitea account.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/337
+
 ## Added

 - First version of ACS controller
  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/329
+- Allow commanding the 5V stack internally in software
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/334
+- Add automatic 5V stack commanding for all connected devices
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/335

 # [v1.18.0] 01.12.2022

--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -10,7 +10,7 @@
 cmake_minimum_required(VERSION 3.13)

 set(OBSW_VERSION_MAJOR_IF_GIT_FAILS 1)
-set(OBSW_VERSION_MINOR_IF_GIT_FAILS 18)
+set(OBSW_VERSION_MINOR_IF_GIT_FAILS 19)
 set(OBSW_VERSION_REVISION_IF_GIT_FAILS 0)

 # set(CMAKE_VERBOSE TRUE)
@ -141,7 +141,7 @@ set(OBSW_ADD_TMP_DEVICES
    ${INIT_VAL}
    CACHE STRING "Add TMP devices")
 set(OBSW_ADD_GOMSPACE_PCDU
-    1
+    ${INIT_VAL}
    CACHE STRING "Add GomSpace PCDU modules")
 set(OBSW_ADD_RW
    ${INIT_VAL}
@ -403,15 +403,14 @@ endif()
 add_subdirectory(thirdparty)

 if(EIVE_ADD_LINUX_FILES)
-  add_subdirectory(${LIB_ARCSEC_PATH})
+  if(TGT_BSP MATCHES "arm/q7s")
+    add_subdirectory(${LIB_GOMSPACE_PATH})
+    add_subdirectory(${LIB_ARCSEC_PATH})
+  endif()
  add_subdirectory(${LINUX_PATH})
 endif()
 add_subdirectory(${BSP_PATH})

-if(UNIX)
-  add_subdirectory(${LIB_GOMSPACE_PATH})
-endif()
-
 add_subdirectory(${COMMON_PATH})
 add_subdirectory(${DUMMY_PATH})

@ -490,7 +489,8 @@ target_link_libraries(${LIB_DUMMIES} PUBLIC ${LIB_FSFW_NAME} ${LIB_JSON_NAME})
 target_link_libraries(${OBSW_NAME} PRIVATE ${LIB_EIVE_MISSION} ${LIB_DUMMIES})

 if(TGT_BSP MATCHES "arm/q7s")
-  target_link_libraries(${LIB_EIVE_MISSION} PUBLIC ${LIB_GPS} ${LIB_ARCSEC})
+  target_link_libraries(${LIB_EIVE_MISSION} PUBLIC ${LIB_GPS} ${LIB_ARCSEC}
+                                                   ${LIB_GOMSPACE_CLIENTS})
 endif()

 target_link_libraries(${UNITTEST_NAME} PRIVATE Catch2 ${LIB_EIVE_MISSION}
@ -500,10 +500,6 @@ if(TGT_BSP MATCHES "arm/egse")
  target_link_libraries(${OBSW_NAME} PRIVATE ${LIB_ARCSEC})
 endif()

-if(UNIX)
-  target_link_libraries(${OBSW_NAME} PRIVATE ${LIB_GOMSPACE_CLIENTS})
-endif()
-
 if(EIVE_ADD_ETL_LIB)
  target_link_libraries(${LIB_EIVE_MISSION} PUBLIC ${LIB_ETL_TARGET})
 endif()
--- a/README.md
+++ b/README.md
@ -290,7 +290,7 @@ helper scripts as well.
 4. Run build command by double clicking the created target or by right clicking
   the project folder and selecting Build Project.

-# <a id="host-commands"></a> Useful and Common Commands (Host)
+# <a id="host-commands"></a> Useful and Common Commands

 ## Build generation

@ -319,14 +319,9 @@ cmake -DTGT_BSP=arm/q7s -DCMAKE_BUILD_TYPE=Release ..
 cmake --build . -j
 ```

-### Q7S Watchdog
-
-The watchdog will be built along side the primary OBSW binary.
-
-### Hosted
+### Hosted OBSW

 You can also use the FSFW OSAL `host` to build on Windows or for generic OSes.
-Note: Currently this is not supported.

 ```sh
 mkdir cmake-build-debug && cd cmake-build-debug
@ -334,6 +329,21 @@ cmake -DFSFW_OSAL=host -DCMAKE_BUILD_TYPE=Debug ..
 cmake --build . -j
 ```

+You can also use the `linux` OSAL:
+
+```sh
+mkdir cmake-build-debug && cd cmake-build-debug
+cmake -DFSFW_OSAL=linux -DCMAKE_BUILD_TYPE=Debug ..
+cmake --build . -j
+```
+
+Please note that some additional Linux setup might be necessary.
+You can find more information in the [Linux section of the FSFW example](https://egit.irs.uni-stuttgart.de/fsfw/fsfw-example-linux-mcu/src/branch/mueller/master/doc/README-linux.md#raising-message-queue-size-limit)
+
+### Q7S Watchdog
+
+The watchdog will be built along side the primary OBSW binary.
+
 ### Unittests

 To build the unittests, the corresponding target must be specified in the build command.
--- a/bsp_hosted/Dockerfile
+++ b/bsp_hosted/Dockerfile
@ -14,7 +14,7 @@ RUN set -ex; \
    rm -rf build-hosted; \
    mkdir build-hosted; \
    cd build-hosted; \
-    cmake -DCMAKE_BUILD_TYPE=Release -DOS_FSFW=linux ..;
+    cmake -DCMAKE_BUILD_TYPE=Release -DOSAL_FSFW=host ..;

 ENTRYPOINT ["cmake", "--build", "build-hosted"]
 CMD ["-j"]
--- a/bsp_q7s/core/CoreController.cpp
+++ b/bsp_q7s/core/CoreController.cpp
@ -1323,7 +1323,7 @@ void CoreController::performRebootFileHandling(bool recreateFile) {
 #if OBSW_VERBOSE_LEVEL >= 1
    sif::info << "CoreController::performRebootFileHandling: Recreating reboot file" << std::endl;
 #endif
-    rebootFile.enabled = true;
+    rebootFile.enabled = false;
    rebootFile.img00Cnt = 0;
    rebootFile.img01Cnt = 0;
    rebootFile.img10Cnt = 0;
--- a/bsp_q7s/core/ObjectFactory.cpp
+++ b/bsp_q7s/core/ObjectFactory.cpp
@ -54,6 +54,7 @@
 #include "linux/boardtest/LibgpiodTest.h"
 #endif
 #include <mission/devices/ImtqHandler.h>
+#include <mission/devices/PcduHandler.h>

 #include <sstream>

@ -80,7 +81,6 @@
 #include "mission/devices/HeaterHandler.h"
 #include "mission/devices/Max31865PT1000Handler.h"
 #include "mission/devices/P60DockHandler.h"
-#include "mission/devices/PCDUHandler.h"
 #include "mission/devices/PDU1Handler.h"
 #include "mission/devices/PDU2Handler.h"
 #include "mission/devices/PayloadPcduHandler.h"
@ -123,11 +123,12 @@ void Factory::setStaticFrameworkObjectIds() {
 void ObjectFactory::setStatics() { Factory::setStaticFrameworkObjectIds(); }

 void ObjectFactory::createTmpComponents() {
-  std::array<std::pair<object_id_t, address_t>, 5> tmpDevIds = {{
+  std::vector<std::pair<object_id_t, address_t>> tmpDevIds = {{
      {objects::TMP1075_HANDLER_TCS_0, addresses::TMP1075_TCS_0},
      {objects::TMP1075_HANDLER_TCS_1, addresses::TMP1075_TCS_1},
      {objects::TMP1075_HANDLER_PLPCDU_0, addresses::TMP1075_PLPCDU_0},
-      {objects::TMP1075_HANDLER_PLPCDU_1, addresses::TMP1075_PLPCDU_1},
+      // damaged
+      // {objects::TMP1075_HANDLER_PLPCDU_1, addresses::TMP1075_PLPCDU_1},
      {objects::TMP1075_HANDLER_IF_BOARD, addresses::TMP1075_IF_BOARD},
  }};
  std::vector<I2cCookie*> tmpDevCookies;
@ -204,7 +205,8 @@ void ObjectFactory::createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchI
 #endif
 }

-ReturnValue_t ObjectFactory::createRadSensorComponent(LinuxLibgpioIF* gpioComIF) {
+ReturnValue_t ObjectFactory::createRadSensorComponent(LinuxLibgpioIF* gpioComIF,
+                                                      Stack5VHandler& stackHandler) {
  using namespace gpio;
  if (gpioComIF == nullptr) {
    return returnvalue::FAILED;
@ -225,12 +227,8 @@ ReturnValue_t ObjectFactory::createRadSensorComponent(LinuxLibgpioIF* gpioComIF)
                    spi::DEFAULT_MAX_1227_MODE, spi::DEFAULT_MAX_1227_SPEED);
  spiCookieRadSensor->setMutexParams(MutexIF::TimeoutType::WAITING, spi::RAD_SENSOR_CS_TIMEOUT);
  auto radSensor = new RadiationSensorHandler(objects::RAD_SENSOR, objects::SPI_MAIN_COM_IF,
-                                              spiCookieRadSensor, gpioComIF);
+                                              spiCookieRadSensor, gpioComIF, stackHandler);
  static_cast<void>(radSensor);
-  // The radiation sensor ADC is powered by the 5V stack connector which should always be on
-  radSensor->setStartUpImmediately();
-  // It's a simple sensor, so just to to normal mode immediately
-  radSensor->setToGoToNormalModeImmediately();
 #if OBSW_DEBUG_RAD_SENSOR == 1
  radSensor->enablePeriodicDataPrint(true);
 #endif
@ -833,7 +831,8 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(LinuxLibgpioIF* gpioComIF,
 }

 void ObjectFactory::createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF,
-                                           PowerSwitchIF* pwrSwitcher) {
+                                           PowerSwitchIF* pwrSwitcher,
+                                           Stack5VHandler& stackHandler) {
  using namespace gpio;
  // Create all GPIO components first
  GpioCookie* plPcduGpios = new GpioCookie;
@ -879,10 +878,9 @@ void ObjectFactory::createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF*
      new SpiCookie(addresses::PLPCDU_ADC, gpioIds::PLPCDU_ADC_CS, plpcdu::MAX_ADC_REPLY_SIZE,
                    spi::DEFAULT_MAX_1227_MODE, spi::PL_PCDU_MAX_1227_SPEED);
  // Create device handler components
-  auto plPcduHandler = new PayloadPcduHandler(
-      objects::PLPCDU_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie, gpioComIF,
-      SdCardManager::instance(), pwrSwitcher, pcdu::Switches::PDU2_CH1_PL_PCDU_BATT_0_14V8,
-      pcdu::Switches::PDU2_CH6_PL_PCDU_BATT_1_14V8, false);
+  auto plPcduHandler =
+      new PayloadPcduHandler(objects::PLPCDU_HANDLER, objects::SPI_MAIN_COM_IF, spiCookie,
+                             gpioComIF, SdCardManager::instance(), stackHandler, false);
  spiCookie->setCallbackMode(PayloadPcduHandler::extConvAsTwoCallback, plPcduHandler);
 //  plPcduHandler->enablePeriodicPrintout(true, 5);
 //  static_cast<void>(plPcduHandler);
--- a/bsp_q7s/core/ObjectFactory.h
+++ b/bsp_q7s/core/ObjectFactory.h
@ -2,6 +2,7 @@
 #define BSP_Q7S_OBJECTFACTORY_H_

 #include <fsfw/returnvalues/returnvalue.h>
+#include <mission/system/objects/Stack5VHandler.h>
 #include <mission/tmtc/CcsdsIpCoreHandler.h>
 #include <mission/tmtc/CfdpTmFunnel.h>
 #include <mission/tmtc/PusTmFunnel.h>
@ -27,9 +28,9 @@ void createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF, SerialComIF** uar
                                   SpiComIF** spiRwComIF);
 void createPcduComponents(LinuxLibgpioIF* gpioComIF, PowerSwitchIF** pwrSwitcher);
 void createPlPcduComponents(LinuxLibgpioIF* gpioComIF, SpiComIF* spiComIF,
-                            PowerSwitchIF* pwrSwitcher);
+                            PowerSwitchIF* pwrSwitcher, Stack5VHandler& stackHandler);
 void createTmpComponents();
-ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF);
+ReturnValue_t createRadSensorComponent(LinuxLibgpioIF* gpioComIF, Stack5VHandler& handler);
 void createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialComIF* uartComIF,
                              PowerSwitchIF* pwrSwitcher);
 void createHeaterComponents(GpioIF* gpioIF, PowerSwitchIF* pwrSwitcher, HealthTableIF* healthTable);
--- a/bsp_q7s/fmObjectFactory.cpp
+++ b/bsp_q7s/fmObjectFactory.cpp
@ -34,8 +34,14 @@ void ObjectFactory::produce(void* args) {

  new CoreController(objects::CORE_CONTROLLER);
  createPcduComponents(gpioComIF, &pwrSwitcher);
-  createRadSensorComponent(gpioComIF);
+  auto* stackHandler = new Stack5VHandler(*pwrSwitcher);
+
+#if OBSW_ADD_RAD_SENSORS == 1
+  createRadSensorComponent(gpioComIF, *stackHandler);
+#endif
+#if OBSW_ADD_SUN_SENSORS == 1
  createSunSensorComponents(gpioComIF, spiMainComIF, pwrSwitcher, q7s::SPI_DEFAULT_DEV);
+#endif

 #if OBSW_ADD_ACS_BOARD == 1
  createAcsBoardComponents(gpioComIF, uartComIF, pwrSwitcher);
@ -45,7 +51,7 @@ void ObjectFactory::produce(void* args) {
  createTmpComponents();
 #endif
  createSolarArrayDeploymentComponents(*pwrSwitcher, *gpioComIF);
-  createPlPcduComponents(gpioComIF, spiMainComIF, pwrSwitcher);
+  createPlPcduComponents(gpioComIF, spiMainComIF, pwrSwitcher, *stackHandler);
 #if OBSW_ADD_SYRLINKS == 1
  createSyrlinksComponents(pwrSwitcher);
 #endif /* OBSW_ADD_SYRLINKS == 1 */
--- a/clone-submodules-no-privlibs.sh
+++ b/clone-submodules-no-privlibs.sh
@ -0,0 +1,3 @@
+#!/bin/bash
+
+git submodule update --init fsfw thirdparty/rapidcsv thirdparty/lwgps thirdparty/json
--- a/cmake/scripts/host/host-make-debug.sh
+++ b/cmake/scripts/host/host-make-debug.sh
@ -1,12 +1,14 @@
 #!/bin/bash
 cfg_script_name="cmake-build-cfg.py"
 init_dir=$(pwd)
+root_dir=""
 if [ -z "${EIVE_OBSW_ROOT}" ]; then
    counter=0
    while [ ${counter} -lt 5 ]
    do
        cd ..
        if [ -f ${cfg_script_name} ];then
+            root_dir=$(realpath "../..")
            break
        fi
        counter=$((counter=counter + 1))
@ -18,6 +20,7 @@ if [ -z "${EIVE_OBSW_ROOT}" ]; then
    fi
 else
    cfg_script_name="${EIVE_OBSW_ROOT}/cmake/scripts/${cfg_script_name}"
+    root_dir=${EIVE_OBSW_ROOT}
 fi

 build_generator="make"
@ -34,4 +37,5 @@ echo "Running command (without the leading +):"
 set -x # Print command 
 ${python} ${cfg_script_name} -o "${os_fsfw}" -g "${build_generator}" -b "debug" -l "${builddir}"
 # Use this if commands are added which should not be printed
-# set +x
+set +x
+cd ${root_dir}/${builddir}
--- a/cmake/scripts/host/host-make-release.sh
+++ b/cmake/scripts/host/host-make-release.sh
@ -7,6 +7,7 @@ if [ -z "${EIVE_OBSW_ROOT}" ]; then
    do
        cd ..
        if [ -f ${cfg_script_name} ];then
+            root_dir=$(realpath "../..")
            break
        fi
        counter=$((counter=counter + 1))
@ -20,7 +21,7 @@ else
    cfg_script_name="${EIVE_OBSW_ROOT}/cmake/scripts/${cfg_script_name}"
 fi

-build_generator="Unix Makefiles"
+build_generator="make"
 os_fsfw="host"
 builddir="cmake-build-release"
 if [ "${OS}" = "Windows_NT" ]; then
@ -34,4 +35,5 @@ echo "Running command (without the leading +):"
 set -x # Print command 
 ${python} ${cfg_script_name} -o "${os_fsfw}" -g "${build_generator}" -b "release" -l "${builddir}"
 # Use this if commands are added which should not be printed
-# set +x
+set +x
+cd ${root_dir}/${builddir}
--- a/linux/CMakeLists.txt
+++ b/linux/CMakeLists.txt
@ -1,4 +1,3 @@
-add_subdirectory(csp)
 add_subdirectory(utility)
 add_subdirectory(callbacks)
 add_subdirectory(boardtest)
@ -9,4 +8,9 @@ if(EIVE_ADD_LINUX_FSFWCONFIG)
  add_subdirectory(fsfwconfig)
 endif()

+# Dependency on proprietary library
+if(TGT_BSP MATCHES "arm/q7s")
+  add_subdirectory(csp)
+endif()
+
 target_sources(${OBSW_NAME} PUBLIC ObjectFactory.cpp scheduling.cpp)
--- a/linux/devices/CMakeLists.txt
+++ b/linux/devices/CMakeLists.txt
@ -7,4 +7,8 @@ target_sources(
                       ScexDleParser.cpp ScexHelper.cpp)

 add_subdirectory(ploc)
-add_subdirectory(startracker)
+
+# Dependency on proprietary library
+if(TGT_BSP MATCHES "arm/q7s")
+  add_subdirectory(startracker)
+endif()
--- a/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h
+++ b/linux/devices/devicedefinitions/PlocMPSoCDefinitions.h
@ -152,7 +152,7 @@ class TcBase : public ploc::SpTcBase, public MPSoCReturnValuesIF {
   *        sent and received packets.
   */
  TcBase(ploc::SpTcParams params, uint16_t apid, uint16_t sequenceCount)
-      : ploc::SpTcBase(params, apid, sequenceCount) {
+      : ploc::SpTcBase(params, apid, 0, sequenceCount) {
    spParams.setFullPayloadLen(INIT_LENGTH);
  }

--- a/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h
+++ b/linux/devices/devicedefinitions/PlocSupervisorDefinitions.h
@ -730,17 +730,13 @@ class FactoryReset : public TcBase {
      : TcBase(params, Apid::DATA_LOGGER,
               static_cast<uint8_t>(tc::DataLoggerServiceId::FACTORY_RESET), 0) {}

-  ReturnValue_t buildPacket(std::optional<uint8_t> op) {
-    if (op) {
-      setLenFromPayloadLen(1);
-    }
+  ReturnValue_t buildPacket(uint8_t op) {
+    setLenFromPayloadLen(1);
    auto res = checkSizeAndSerializeHeader();
    if (res != returnvalue::OK) {
      return res;
    }
-    if (op) {
-      payloadStart[0] = op.value();
-    }
+    payloadStart[0] = op;
    return calcAndSetCrc();
  }

--- a/linux/devices/ploc/PlocMPSoCHandler.cpp
+++ b/linux/devices/ploc/PlocMPSoCHandler.cpp
@ -184,12 +184,14 @@ void PlocMPSoCHandler::doShutDown() {
      powerState = PowerState::SHUTDOWN;
      break;
    case PowerState::OFF:
+      sequenceCount = 0;
      setMode(_MODE_POWER_DOWN);
      break;
    default:
      break;
  }
 #else
+  sequenceCount = 0;
  uartIsolatorSwitch.pullLow();
  setMode(_MODE_POWER_DOWN);
  powerState = PowerState::OFF;
@ -340,12 +342,13 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain
    }
  }

-  sequenceCount++;
  uint16_t recvSeqCnt = (*(start + 2) << 8 | *(start + 3)) & PACKET_SEQUENCE_COUNT_MASK;
  if (recvSeqCnt != sequenceCount) {
    triggerEvent(MPSOC_HANDLER_SEQUENCE_COUNT_MISMATCH, sequenceCount, recvSeqCnt);
    sequenceCount = recvSeqCnt;
  }
+  // This sequence count ping pong does not make any sense but it is how the MPSoC expects it.
+  sequenceCount++;
  return result;
 }

@ -403,11 +406,9 @@ void PlocMPSoCHandler::handleEvent(EventMessage* eventMessage) {
 ReturnValue_t PlocMPSoCHandler::prepareTcMemWrite(const uint8_t* commandData,
                                                  size_t commandDataLen) {
  ReturnValue_t result = returnvalue::OK;
-  sequenceCount++;
  mpsoc::TcMemWrite tcMemWrite(spParams, sequenceCount);
  result = tcMemWrite.buildPacket(commandData, commandDataLen);
  if (result != returnvalue::OK) {
-    sequenceCount--;
    return result;
  }
  finishTcPrep(tcMemWrite.getFullPacketLen());
@ -417,11 +418,9 @@ ReturnValue_t PlocMPSoCHandler::prepareTcMemWrite(const uint8_t* commandData,
 ReturnValue_t PlocMPSoCHandler::prepareTcMemRead(const uint8_t* commandData,
                                                 size_t commandDataLen) {
  ReturnValue_t result = returnvalue::OK;
-  sequenceCount++;
  mpsoc::TcMemRead tcMemRead(spParams, sequenceCount);
  result = tcMemRead.buildPacket(commandData, commandDataLen);
  if (result != returnvalue::OK) {
-    sequenceCount--;
    return result;
  }
  finishTcPrep(tcMemRead.getFullPacketLen());
@ -435,12 +434,10 @@ ReturnValue_t PlocMPSoCHandler::prepareTcFlashDelete(const uint8_t* commandData,
    return MPSoCReturnValuesIF::NAME_TOO_LONG;
  }
  ReturnValue_t result = returnvalue::OK;
-  sequenceCount++;
  mpsoc::TcFlashDelete tcFlashDelete(spParams, sequenceCount);
  result = tcFlashDelete.buildPacket(
      std::string(reinterpret_cast<const char*>(commandData), commandDataLen));
  if (result != returnvalue::OK) {
-    sequenceCount--;
    return result;
  }
  finishTcPrep(tcFlashDelete.getFullPacketLen());
@ -450,11 +447,9 @@ ReturnValue_t PlocMPSoCHandler::prepareTcFlashDelete(const uint8_t* commandData,
 ReturnValue_t PlocMPSoCHandler::prepareTcReplayStart(const uint8_t* commandData,
                                                     size_t commandDataLen) {
  ReturnValue_t result = returnvalue::OK;
-  sequenceCount++;
  mpsoc::TcReplayStart tcReplayStart(spParams, sequenceCount);
  result = tcReplayStart.buildPacket(commandData, commandDataLen);
  if (result != returnvalue::OK) {
-    sequenceCount--;
    return result;
  }
  finishTcPrep(tcReplayStart.getFullPacketLen());
@ -463,11 +458,9 @@ ReturnValue_t PlocMPSoCHandler::prepareTcReplayStart(const uint8_t* commandData,

 ReturnValue_t PlocMPSoCHandler::prepareTcReplayStop() {
  ReturnValue_t result = returnvalue::OK;
-  sequenceCount++;
  mpsoc::TcReplayStop tcReplayStop(spParams, sequenceCount);
  result = tcReplayStop.buildPacket();
  if (result != returnvalue::OK) {
-    sequenceCount--;
    return result;
  }
  finishTcPrep(tcReplayStop.getFullPacketLen());
@ -477,11 +470,9 @@ ReturnValue_t PlocMPSoCHandler::prepareTcReplayStop() {
 ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkPwrOn(const uint8_t* commandData,
                                                       size_t commandDataLen) {
  ReturnValue_t result = returnvalue::OK;
-  sequenceCount++;
  mpsoc::TcDownlinkPwrOn tcDownlinkPwrOn(spParams, sequenceCount);
  result = tcDownlinkPwrOn.buildPacket(commandData, commandDataLen);
  if (result != returnvalue::OK) {
-    sequenceCount--;
    return result;
  }
  finishTcPrep(tcDownlinkPwrOn.getFullPacketLen());
@ -490,11 +481,9 @@ ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkPwrOn(const uint8_t* commandDat

 ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkPwrOff() {
  ReturnValue_t result = returnvalue::OK;
-  sequenceCount++;
  mpsoc::TcDownlinkPwrOff tcDownlinkPwrOff(spParams, sequenceCount);
  result = tcDownlinkPwrOff.buildPacket();
  if (result != returnvalue::OK) {
-    sequenceCount--;
    return result;
  }
  finishTcPrep(tcDownlinkPwrOff.getFullPacketLen());
@ -504,11 +493,9 @@ ReturnValue_t PlocMPSoCHandler::prepareTcDownlinkPwrOff() {
 ReturnValue_t PlocMPSoCHandler::prepareTcReplayWriteSequence(const uint8_t* commandData,
                                                             size_t commandDataLen) {
  ReturnValue_t result = returnvalue::OK;
-  sequenceCount++;
  mpsoc::TcReplayWriteSeq tcReplayWriteSeq(spParams, sequenceCount);
  result = tcReplayWriteSeq.buildPacket(commandData, commandDataLen);
  if (result != returnvalue::OK) {
-    sequenceCount--;
    return result;
  }
  finishTcPrep(tcReplayWriteSeq.getFullPacketLen());
@ -517,11 +504,9 @@ ReturnValue_t PlocMPSoCHandler::prepareTcReplayWriteSequence(const uint8_t* comm

 ReturnValue_t PlocMPSoCHandler::prepareTcModeReplay() {
  ReturnValue_t result = returnvalue::OK;
-  sequenceCount++;
  mpsoc::TcModeReplay tcModeReplay(spParams, sequenceCount);
  result = tcModeReplay.buildPacket();
  if (result != returnvalue::OK) {
-    sequenceCount--;
    return result;
  }
  finishTcPrep(tcModeReplay.getFullPacketLen());
@ -530,11 +515,9 @@ ReturnValue_t PlocMPSoCHandler::prepareTcModeReplay() {

 ReturnValue_t PlocMPSoCHandler::prepareTcModeIdle() {
  ReturnValue_t result = returnvalue::OK;
-  sequenceCount++;
  mpsoc::TcModeIdle tcModeIdle(spParams, sequenceCount);
  result = tcModeIdle.buildPacket();
  if (result != returnvalue::OK) {
-    sequenceCount--;
    return result;
  }
  finishTcPrep(tcModeIdle.getFullPacketLen());
@ -544,11 +527,9 @@ ReturnValue_t PlocMPSoCHandler::prepareTcModeIdle() {
 ReturnValue_t PlocMPSoCHandler::prepareTcCamCmdSend(const uint8_t* commandData,
                                                    size_t commandDataLen) {
  ReturnValue_t result = returnvalue::OK;
-  sequenceCount++;
  mpsoc::TcCamcmdSend tcCamCmdSend(spParams, sequenceCount);
  result = tcCamCmdSend.buildPacket(commandData, commandDataLen);
  if (result != returnvalue::OK) {
-    sequenceCount--;
    return result;
  }
  finishTcPrep(tcCamCmdSend.getFullPacketLen());
@ -560,6 +541,7 @@ void PlocMPSoCHandler::finishTcPrep(size_t packetLen) {
  nextReplyId = mpsoc::ACK_REPORT;
  rawPacket = commandBuffer;
  rawPacketLen = packetLen;
+  sequenceCount++;
 }

 ReturnValue_t PlocMPSoCHandler::verifyPacket(const uint8_t* start, size_t foundLen) {
--- a/linux/devices/ploc/PlocMPSoCHandler.h
+++ b/linux/devices/ploc/PlocMPSoCHandler.h
@ -107,10 +107,7 @@ class PlocMPSoCHandler : public DeviceHandlerBase, public CommandsActionsIF {
  MessageQueueIF* eventQueue = nullptr;
  MessageQueueIF* commandActionHelperQueue = nullptr;

-  // Initiate the sequence count with the maximum value. It is incremented before
-  // a packet is sent, so the first value will be 0 accordingly using
-  // the wrap around of the counter.
-  SourceSequenceCounter sequenceCount = SourceSequenceCounter(ccsds::LIMIT_SEQUENCE_COUNT - 1);
+  SourceSequenceCounter sequenceCount = SourceSequenceCounter(0);

  uint8_t commandBuffer[mpsoc::MAX_COMMAND_SIZE];
  SpacePacketCreator creator;
--- a/linux/devices/ploc/PlocSupervisorHandler.cpp
+++ b/linux/devices/ploc/PlocSupervisorHandler.cpp
@ -136,29 +136,34 @@ ReturnValue_t PlocSupervisorHandler::executeAction(ActionId_t actionId,
 }

 void PlocSupervisorHandler::doStartUp() {
-  if (setTimeDuringStartup) {
-    if (startupState == StartupState::OFF) {
-      bootTimeout.resetTimer();
-      startupState = StartupState::BOOTING;
-    }
-    if (startupState == StartupState::BOOTING) {
-      if (bootTimeout.hasTimedOut()) {
-        uartIsolatorSwitch.pullHigh();
-        uartManager.start();
+  if (startupState == StartupState::OFF) {
+    bootTimeout.resetTimer();
+    startupState = StartupState::BOOTING;
+  }
+  if (startupState == StartupState::BOOTING) {
+    if (bootTimeout.hasTimedOut()) {
+      uartIsolatorSwitch.pullHigh();
+      uartManager.start();
+      if (SET_TIME_DURING_BOOT) {
        startupState = StartupState::SET_TIME;
+      } else {
+        startupState = StartupState::ON;
      }
    }
-    if (startupState == StartupState::ON) {
-      setMode(_MODE_TO_ON);
-    }
-  } else {
-    uartIsolatorSwitch.pullHigh();
+  }
+  if (startupState == StartupState::SET_TIME_EXECUTING) {
+    startupState = StartupState::ON;
+  }
+  if (startupState == StartupState::ON) {
    setMode(_MODE_TO_ON);
  }
 }

 void PlocSupervisorHandler::doShutDown() {
  setMode(_MODE_POWER_DOWN);
+  shutdownCmdSent = false;
+  packetInBuffer = false;
+  nextReplyId = supv::NONE;
  uartManager.stop();
  uartIsolatorSwitch.pullLow();
  startupState = StartupState::OFF;
@ -1467,11 +1472,10 @@ ReturnValue_t PlocSupervisorHandler::prepareReadGpioCmd(const uint8_t* commandDa
 ReturnValue_t PlocSupervisorHandler::prepareFactoryResetCmd(const uint8_t* commandData,
                                                            size_t len) {
  FactoryReset resetCmd(spParams);
-  std::optional<uint8_t> op;
-  if (len > 0) {
-    op = commandData[0];
+  if (len < 1) {
+    return HasActionsIF::INVALID_PARAMETERS;
  }
-  ReturnValue_t result = resetCmd.buildPacket(op);
+  ReturnValue_t result = resetCmd.buildPacket(commandData[0]);
  if (result != returnvalue::OK) {
    return result;
  }
@ -1897,9 +1901,8 @@ ReturnValue_t PlocSupervisorHandler::handleExecutionSuccessReport(ExecutionRepor
      break;
    }
    case supv::SET_TIME_REF: {
-      if (startupState == StartupState::SET_TIME_EXECUTING) {
-        startupState = StartupState::ON;
-      }
+      // We could only allow proper bootup when the time was set successfully, but
+      // this makes debugging difficult.
      break;
    }
    default:
--- a/linux/devices/ploc/PlocSupervisorHandler.h
+++ b/linux/devices/ploc/PlocSupervisorHandler.h
@ -97,11 +97,11 @@ class PlocSupervisorHandler : public DeviceHandlerBase {
  static const uint32_t COPY_ADC_TO_MRAM_TIMEOUT = 70000;
  // 60 s
  static const uint32_t MRAM_DUMP_TIMEOUT = 60000;
-  // 2 s
-  static const uint32_t BOOT_TIMEOUT = 2000;
+  // 4 s
+  static const uint32_t BOOT_TIMEOUT = 4000;
  enum class StartupState : uint8_t { OFF, BOOTING, SET_TIME, SET_TIME_EXECUTING, ON };

-  bool setTimeDuringStartup = true;
+  static constexpr bool SET_TIME_DURING_BOOT = true;

  StartupState startupState = StartupState::OFF;

@ -138,8 +138,6 @@ class PlocSupervisorHandler : public DeviceHandlerBase {
  uint32_t receivedMramDumpPackets = 0;
  /** Set to true as soon as a complete space packet is present in the spacePacketBuffer */
  bool packetInBuffer = false;
-  /** Points to the next free position in the space packet buffer */
-  uint16_t bufferTop = 0;

  /** This buffer is used to concatenate space packets received in two different read steps */
  uint8_t spacePacketBuffer[supv::MAX_PACKET_SIZE];
--- a/linux/devices/ploc/PlocSupvUartMan.cpp
+++ b/linux/devices/ploc/PlocSupvUartMan.cpp
@ -100,6 +100,7 @@ ReturnValue_t PlocSupvUartManager::performOperation(uint8_t operationCode) {
    state = InternalState::SLEEPING;
    lock->unlockMutex();
    semaphore->acquire();
+    putTaskToSleep = false;
    while (true) {
      if (putTaskToSleep) {
        performUartShutdown();
@ -295,11 +296,14 @@ void PlocSupvUartManager::stop() {
 }

 void PlocSupvUartManager::start() {
-  MutexGuard mg(lock);
-  if (state != InternalState::SLEEPING and state != InternalState::GO_TO_SLEEP) {
-    return;
+  {
+    MutexGuard mg(lock);
+    if (state != InternalState::SLEEPING and state != InternalState::GO_TO_SLEEP) {
+      return;
+    }
+    state = InternalState::DEFAULT;
  }
-  state = InternalState::DEFAULT;
+
  semaphore->release();
 }

--- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
+++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
@ -455,6 +455,8 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) {
  thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.4,
                        DeviceHandlerIF::SEND_READ);
  thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_0, length * 0.4, DeviceHandlerIF::GET_READ);
+  // damaged
+  /*
  thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4,
                        DeviceHandlerIF::PERFORM_OPERATION);
  thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4,
@ -464,6 +466,7 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) {
  thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4,
                        DeviceHandlerIF::SEND_READ);
  thisSequence->addSlot(objects::TMP1075_HANDLER_PLPCDU_1, length * 0.4, DeviceHandlerIF::GET_READ);
+  */
  thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.4,
                        DeviceHandlerIF::PERFORM_OPERATION);
  thisSequence->addSlot(objects::TMP1075_HANDLER_IF_BOARD, length * 0.4,
--- a/mission/controller/AcsController.cpp
+++ b/mission/controller/AcsController.cpp
@ -219,10 +219,9 @@ void AcsController::performDetumble() {
  {
    PoolReadGuard pg(&actuatorCmdData);
    if (pg.getReadResult() == returnvalue::OK) {
-      int32_t zeroVec[4] = {0, 0, 0, 0};
-      std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(double));
+      std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(double));
      actuatorCmdData.rwTargetTorque.setValid(false);
-      std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t));
+      std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t));
      actuatorCmdData.rwTargetSpeed.setValid(false);
      std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t));
      actuatorCmdData.mtqTargetDipole.setValid(true);
@ -468,7 +467,7 @@ void AcsController::copyMgmData() {
    PoolReadGuard pg(&sensorValues.mgm3Rm3100Set);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(mgmDataRaw.mgm3Rm3100.value, sensorValues.mgm3Rm3100Set.fieldStrengths.value,
-                  3 + sizeof(float));
+                  3 * sizeof(float));
      mgmDataRaw.mgm3Rm3100.setValid(sensorValues.mgm3Rm3100Set.fieldStrengths.isValid());
    }
  }
--- a/mission/controller/ThermalController.cpp
+++ b/mission/controller/ThermalController.cpp
@ -41,7 +41,8 @@ ThermalController::ThermalController(object_id_t objectId)
      tmp1075SetTcs0(objects::TMP1075_HANDLER_TCS_0),
      tmp1075SetTcs1(objects::TMP1075_HANDLER_TCS_1),
      tmp1075SetPlPcdu0(objects::TMP1075_HANDLER_PLPCDU_0),
-      tmp1075SetPlPcdu1(objects::TMP1075_HANDLER_PLPCDU_1),
+      // damaged
+      // tmp1075SetPlPcdu1(objects::TMP1075_HANDLER_PLPCDU_1),
      tmp1075SetIfBoard(objects::TMP1075_HANDLER_IF_BOARD),
      susSet0(objects::SUS_0_N_LOC_XFYFZM_PT_XF),
      susSet1(objects::SUS_1_N_LOC_XBYFZM_PT_XB),
@ -449,6 +450,8 @@ void ThermalController::copySensors() {
      }
    }
  }
+  // damaged
+  /*
  {
    PoolReadGuard pg(&tmp1075SetPlPcdu1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() == returnvalue::OK) {
@ -459,6 +462,7 @@ void ThermalController::copySensors() {
      }
    }
  }
+  */
  {
    PoolReadGuard pg(&tmp1075SetIfBoard, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
    if (pg.getReadResult() == returnvalue::OK) {
--- a/mission/controller/ThermalController.h
+++ b/mission/controller/ThermalController.h
@ -55,10 +55,12 @@ class ThermalController : public ExtendedControllerBase {
  MAX31865::Max31865Set max31865Set13;
  MAX31865::Max31865Set max31865Set14;
  MAX31865::Max31865Set max31865Set15;
+
  TMP1075::Tmp1075Dataset tmp1075SetTcs0;
  TMP1075::Tmp1075Dataset tmp1075SetTcs1;
  TMP1075::Tmp1075Dataset tmp1075SetPlPcdu0;
-  TMP1075::Tmp1075Dataset tmp1075SetPlPcdu1;
+  // damaged
+  // TMP1075::Tmp1075Dataset tmp1075SetPlPcdu1;
  TMP1075::Tmp1075Dataset tmp1075SetIfBoard;

  // SUS
--- a/mission/devices/CMakeLists.txt
+++ b/mission/devices/CMakeLists.txt
@ -3,7 +3,7 @@ target_sources(
  PRIVATE GomspaceDeviceHandler.cpp
          BpxBatteryHandler.cpp
          Tmp1075Handler.cpp
-          PCDUHandler.cpp
+          PcduHandler.cpp
          P60DockHandler.cpp
          PDU1Handler.cpp
          PDU2Handler.cpp
--- a/mission/devices/PayloadPcduHandler.cpp
+++ b/mission/devices/PayloadPcduHandler.cpp
@ -16,29 +16,37 @@

 PayloadPcduHandler::PayloadPcduHandler(object_id_t objectId, object_id_t comIF, CookieIF* cookie,
                                       GpioIF* gpioIF, SdCardMountedIF* sdcMan,
-                                       PowerSwitchIF* pwrSwitcher, power::Switch_t switchA,
-                                       power::Switch_t switchB, bool periodicPrintout)
+                                       Stack5VHandler& stackHandler, bool periodicPrintout)
    : DeviceHandlerBase(objectId, comIF, cookie),
      adcSet(this),
+      stackHandler(stackHandler),
      periodicPrintout(periodicPrintout),
      gpioIF(gpioIF),
-      sdcMan(sdcMan),
-      pwrStateMachine(switchA, switchB, pwrSwitcher) {}
+      sdcMan(sdcMan) {}

 void PayloadPcduHandler::doStartUp() {
-  if ((state != States::PL_PCDU_OFF) and (state != States::ON_TRANS_SSR)) {
+  if (state > States::STACK_5V_CORRECT) {
    // Config error
    sif::error << "PayloadPcduHandler::doStartUp: Invalid state" << std::endl;
  }
-  if (pwrStateMachine.getState() == power::States::IDLE) {
-    pwrStateMachine.start(MODE_ON, pwrSubmode);
-  }
  clearSetOnOffFlag = true;
-  auto opCode = pwrStateMachine.fsm();
-  if (opCode == power::OpCodes::TO_NOT_OFF_DONE or opCode == power::OpCodes::TIMEOUT_OCCURED) {
-    pwrStateMachine.reset();
+  if (state == States::PL_PCDU_OFF) {
+    state = States::STACK_5V_SWITCHING;
+  }
+  if (state == States::STACK_5V_SWITCHING) {
+    ReturnValue_t retval = stackHandler.deviceToOn(StackCommander::PL_PCDU, true);
+    if (retval == BUSY) {
+      return;
+    }
+    state = States::STACK_5V_PENDING;
+  }
+  if (state == States::STACK_5V_PENDING) {
+    if (stackHandler.isSwitchOn()) {
+      state = States::STACK_5V_CORRECT;
+    }
+  }
+  if (state == States::STACK_5V_CORRECT) {
    quickTransitionAlreadyCalled = false;
-    state = States::POWER_CHANNELS_ON;
    setMode(_MODE_TO_ON);
  }
 }
@ -48,27 +56,33 @@ void PayloadPcduHandler::doShutDown() {
    quickTransitionBackToOff(false, false);
    quickTransitionAlreadyCalled = true;
  }
-  if (pwrStateMachine.getState() == power::States::IDLE) {
-    pwrStateMachine.start(MODE_OFF, 0);
-  }
  if (clearSetOnOffFlag) {
    std::memset(adcSet.processed.value, 0, adcSet.processed.getSerializedSize());
    clearSetOnOffFlag = false;
  }
-
-  auto opCode = pwrStateMachine.fsm();
-  if (opCode == power::OpCodes::TO_OFF_DONE or opCode == power::OpCodes::TIMEOUT_OCCURED) {
-    pwrStateMachine.reset();
-    state = States::PL_PCDU_OFF;
-    // No need to set mode _MODE_POWER_DOWN, power switching was already handled
-    setMode(MODE_OFF);
+  ReturnValue_t retval = stackHandler.deviceToOff(StackCommander::PL_PCDU, true);
+  if (retval == BUSY) {
+    return;
  }
+  state = States::PL_PCDU_OFF;
+  // No need to set mode _MODE_POWER_DOWN, power switching was already handled
+  setMode(MODE_OFF);
 }

 void PayloadPcduHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
  if (getMode() == _MODE_TO_NORMAL) {
    stateMachineToNormal(modeFrom, subModeFrom);
    return;
+  } else if (getMode() == _MODE_TO_ON and modeFrom == MODE_NORMAL) {
+    gpioIF->pullLow(gpioIds::PLPCDU_ENB_HPA);
+    gpioIF->pullLow(gpioIds::PLPCDU_ENB_MPA);
+    gpioIF->pullLow(gpioIds::PLPCDU_ENB_TX);
+    gpioIF->pullLow(gpioIds::PLPCDU_ENB_X8);
+    gpioIF->pullLow(gpioIds::PLPCDU_ENB_DRO);
+    gpioIF->pullLow(gpioIds::PLPCDU_ENB_TX);
+    gpioIF->pullLow(gpioIds::PLPCDU_ENB_VBAT0);
+    gpioIF->pullLow(gpioIds::PLPCDU_ENB_VBAT1);
+    state = States::STACK_5V_CORRECT;
  }
  DeviceHandlerBase::doTransition(modeFrom, subModeFrom);
 }
@ -83,7 +97,7 @@ ReturnValue_t PayloadPcduHandler::stateMachineToNormal(Mode_t modeFrom, Submode_
      setMode(MODE_OFF);
      return returnvalue::FAILED;
    }
-    if (state == States::POWER_CHANNELS_ON) {
+    if (state == States::STACK_5V_CORRECT) {
 #if OBSW_VERBOSE_LEVEL >= 1
      sif::info << "Switching on SSR VBAT0 & VBAT1 GPIOs" << std::endl;
 #endif
@ -144,6 +158,10 @@ ReturnValue_t PayloadPcduHandler::stateMachineToNormal(Mode_t modeFrom, Submode_
    }
  };

+  // sif::debug << "DIFF MASK: " << (int)diffMask << std::endl;
+
+  // No handling for the SSRs: If those are pulled low, the ADC is off
+  // and normal mode does not really make sense anyway
  switchHandler(DRO_ON, gpioIds::PLPCDU_ENB_DRO, "DRO");
  switchHandler(X8_ON, gpioIds::PLPCDU_ENB_X8, "X8");
  switchHandler(TX_ON, gpioIds::PLPCDU_ENB_TX, "TX");
@ -358,7 +376,7 @@ void PayloadPcduHandler::quickTransitionBackToOff(bool startTransitionToOff, boo
  gpioIF->pullLow(gpioIds::PLPCDU_ENB_TX);
  gpioIF->pullLow(gpioIds::PLPCDU_ENB_VBAT0);
  gpioIF->pullLow(gpioIds::PLPCDU_ENB_VBAT1);
-  state = States::PL_PCDU_OFF;
+  state = States::STACK_5V_SWITCHING;
  adcState = AdcStates::OFF;
  if (startTransitionToOff) {
    startTransition(MODE_OFF, 0);
--- a/mission/devices/PayloadPcduHandler.h
+++ b/mission/devices/PayloadPcduHandler.h
@ -4,6 +4,7 @@
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
 #include <fsfw/globalfunctions/PeriodicOperationDivider.h>
 #include <fsfw/timemanager/Countdown.h>
+#include <mission/system/objects/Stack5VHandler.h>

 #include "events/subsystemIdRanges.h"
 #include "fsfw/FSFW.h"
@ -62,8 +63,7 @@ class PayloadPcduHandler : public DeviceHandlerBase {
  static constexpr Event I_HPA_OUT_OF_BOUNDS = event::makeEvent(SUBSYSTEM_ID, 11, severity::MEDIUM);

  PayloadPcduHandler(object_id_t objectId, object_id_t comIF, CookieIF* cookie, GpioIF* gpioIF,
-                     SdCardMountedIF* sdcMan, PowerSwitchIF* pwrSwitcher, power::Switch_t switchCh0,
-                     power::Switch_t switchCh1, bool periodicPrintout);
+                     SdCardMountedIF* sdcMan, Stack5VHandler& stackHandler, bool periodicPrintout);

  void setToGoToNormalModeImmediately(bool enable);
  void enablePeriodicPrintout(bool enable, uint8_t divider);
@ -78,7 +78,9 @@ class PayloadPcduHandler : public DeviceHandlerBase {
 private:
  enum class States : uint8_t {
    PL_PCDU_OFF,
-    POWER_CHANNELS_ON,
+    STACK_5V_SWITCHING,
+    STACK_5V_PENDING,
+    STACK_5V_CORRECT,
    // Solid State Relay, enable battery voltages VBAT0 and VBAT1. This will also switch on
    // the ADC
    ON_TRANS_SSR,
@ -108,6 +110,7 @@ class PayloadPcduHandler : public DeviceHandlerBase {

  bool goToNormalMode = false;
  plpcdu::PlPcduAdcSet adcSet;
+  Stack5VHandler& stackHandler;
  std::array<uint8_t, plpcdu::MAX_ADC_REPLY_SIZE> cmdBuf = {};
  // This variable is tied to DRO +6 V voltage. Voltages, currents are monitored and the experiment
  // is shut down immediately if there is a negative voltage.
@ -140,7 +143,6 @@ class PayloadPcduHandler : public DeviceHandlerBase {
  PoolEntry<float> processedValues =
      PoolEntry<float>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0});
  PoolEntry<float> tempC = PoolEntry<float>({0.0});
-  DualLanePowerStateMachine pwrStateMachine;

  void updateSwitchGpio(gpioId_t id, gpio::Levels level);

--- a/mission/devices/PcduHandler.cpp
+++ b/mission/devices/PcduHandler.cpp
@ -1,16 +1,16 @@
-#include "PCDUHandler.h"
-
 #include <OBSWConfig.h>
 #include <devices/powerSwitcherList.h>
 #include <fsfw/datapool/PoolReadGuard.h>
 #include <fsfw/housekeeping/HousekeepingSnapshot.h>
 #include <fsfw/ipc/MutexFactory.h>
 #include <fsfw/ipc/QueueFactory.h>
+#include <mission/devices/PcduHandler.h>
 #include <mission/devices/devicedefinitions/GomSpacePackets.h>

 PCDUHandler::PCDUHandler(object_id_t setObjectId, size_t cmdQueueSize)
    : SystemObject(setObjectId),
      poolManager(this, nullptr),
+      p60CoreHk(objects::P60DOCK_HANDLER),
      pdu1CoreHk(this),
      pdu2CoreHk(this),
      switcherSet(this),
@ -26,7 +26,27 @@ PCDUHandler::~PCDUHandler() {}
 ReturnValue_t PCDUHandler::performOperation(uint8_t counter) {
  if (counter == DeviceHandlerIF::PERFORM_OPERATION) {
    readCommandQueue();
-    return returnvalue::OK;
+  }
+  uint8_t switchState = 0;
+  {
+    PoolReadGuard pg(&p60CoreHk.outputEnables);
+    if (pg.getReadResult() == returnvalue::OK) {
+      switchState = p60CoreHk.outputEnables.value[10];
+    } else {
+      return returnvalue::OK;
+    }
+  }
+  {
+    PoolReadGuard pg(&switcherSet.p60Dock5VStack);
+    if (pg.getReadResult() == returnvalue::OK) {
+      if (switcherSet.p60Dock5VStack.value != switchState) {
+        triggerEvent(power::SWITCH_HAS_CHANGED, switchState, pcdu::Switches::P60_DOCK_5V_STACK);
+        MutexGuard mg(pwrMutex);
+        switchStates[pcdu::P60_DOCK_5V_STACK] = switchState;
+      }
+      switcherSet.p60Dock5VStack.setValid(true);
+      switcherSet.p60Dock5VStack.value = switchState;
+    }
  }
  return returnvalue::OK;
 }
@ -85,8 +105,10 @@ void PCDUHandler::initializeSwitchStates() {
    for (uint8_t idx = 0; idx < NUMBER_OF_SWITCHES; idx++) {
      if (idx < PDU::CHANNELS_LEN) {
        switchStates[idx] = INIT_SWITCHES_PDU1.at(idx);
-      } else {
+      } else if (idx < PDU::CHANNELS_LEN * 2) {
        switchStates[idx] = INIT_SWITCHES_PDU2.at(idx - PDU::CHANNELS_LEN);
+      } else {
+        switchStates[idx] = OFF;
      }
    }
  } catch (const std::out_of_range& err) {
@ -156,24 +178,25 @@ void PCDUHandler::updatePdu2SwitchStates() {
    for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) {
      switcherSet.pdu2Switches[idx] = pdu2CoreHk.outputEnables[idx];
    }
+    switcherSet.pdu2Switches.setValid(true);
    MutexGuard mg(pwrMutex);
-    checkAndUpdateSwitch(pdu, Switches::PDU2_CH0_Q7S, pdu2CoreHk.outputEnables[Channels::Q7S]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH0_Q7S, pdu2CoreHk.outputEnables[Channels::Q7S]);

-    checkAndUpdateSwitch(pdu, Switches::PDU2_CH1_PL_PCDU_BATT_0_14V8,
-                         pdu2CoreHk.outputEnables[Channels::PAYLOAD_PCDU_CH1]);
-    checkAndUpdateSwitch(pdu, Switches::PDU2_CH2_RW_5V, pdu2CoreHk.outputEnables[Channels::RW]);
-    checkAndUpdateSwitch(pdu, Switches::PDU2_CH3_TCS_BOARD_HEATER_IN_8V,
-                         pdu2CoreHk.outputEnables[Channels::TCS_HEATER_IN]);
-    checkAndUpdateSwitch(pdu, Switches::PDU2_CH4_SUS_REDUNDANT_3V3,
-                         pdu2CoreHk.outputEnables[Channels::SUS_REDUNDANT]);
-    checkAndUpdateSwitch(pdu, Switches::PDU2_CH5_DEPLOYMENT_MECHANISM_8V,
-                         pdu2CoreHk.outputEnables[Channels::DEPY_MECHANISM]);
-    checkAndUpdateSwitch(pdu, Switches::PDU2_CH6_PL_PCDU_BATT_1_14V8,
-                         pdu2CoreHk.outputEnables[Channels::PAYLOAD_PCDU_CH6]);
-    checkAndUpdateSwitch(pdu, Switches::PDU2_CH7_ACS_BOARD_SIDE_B_3V3,
-                         pdu2CoreHk.outputEnables[Channels::ACS_B_SIDE]);
-    checkAndUpdateSwitch(pdu, Switches::PDU2_CH8_PAYLOAD_CAMERA,
-                         pdu2CoreHk.outputEnables[Channels::PAYLOAD_CAMERA]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH1_PL_PCDU_BATT_0_14V8,
+                            pdu2CoreHk.outputEnables[Channels::PAYLOAD_PCDU_CH1]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH2_RW_5V, pdu2CoreHk.outputEnables[Channels::RW]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH3_TCS_BOARD_HEATER_IN_8V,
+                            pdu2CoreHk.outputEnables[Channels::TCS_HEATER_IN]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH4_SUS_REDUNDANT_3V3,
+                            pdu2CoreHk.outputEnables[Channels::SUS_REDUNDANT]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH5_DEPLOYMENT_MECHANISM_8V,
+                            pdu2CoreHk.outputEnables[Channels::DEPY_MECHANISM]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH6_PL_PCDU_BATT_1_14V8,
+                            pdu2CoreHk.outputEnables[Channels::PAYLOAD_PCDU_CH6]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH7_ACS_BOARD_SIDE_B_3V3,
+                            pdu2CoreHk.outputEnables[Channels::ACS_B_SIDE]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU2_CH8_PAYLOAD_CAMERA,
+                            pdu2CoreHk.outputEnables[Channels::PAYLOAD_CAMERA]);
    if (firstSwitchInfoPdu2) {
      firstSwitchInfoPdu2 = false;
    }
@ -192,24 +215,26 @@ void PCDUHandler::updatePdu1SwitchStates() {
    for (uint8_t idx = 0; idx < PDU::CHANNELS_LEN; idx++) {
      switcherSet.pdu1Switches[idx] = pdu1CoreHk.outputEnables[idx];
    }
+    switcherSet.pdu1Switches.setValid(true);
    MutexGuard mg(pwrMutex);
-    checkAndUpdateSwitch(pdu, Switches::PDU1_CH0_TCS_BOARD_3V3,
-                         pdu1CoreHk.outputEnables[Channels::TCS_BOARD_3V3]);
-    checkAndUpdateSwitch(pdu, Switches::PDU1_CH1_SYRLINKS_12V,
-                         pdu1CoreHk.outputEnables[Channels::SYRLINKS]);
-    checkAndUpdateSwitch(pdu, Switches::PDU1_CH2_STAR_TRACKER_5V,
-                         pdu1CoreHk.outputEnables[Channels::STR]);
-    checkAndUpdateSwitch(pdu, Switches::PDU1_CH3_MGT_5V, pdu1CoreHk.outputEnables[Channels::MGT]);
-    checkAndUpdateSwitch(pdu, Switches::PDU1_CH4_SUS_NOMINAL_3V3,
-                         pdu1CoreHk.outputEnables[Channels::SUS_NOMINAL]);
-    checkAndUpdateSwitch(pdu, Switches::PDU1_CH5_SOLAR_CELL_EXP_5V,
-                         pdu1CoreHk.outputEnables[Channels::SOL_CELL_EXPERIMENT]);
-    checkAndUpdateSwitch(pdu, Switches::PDU1_CH6_PLOC_12V,
-                         pdu1CoreHk.outputEnables[Channels::PLOC]);
-    checkAndUpdateSwitch(pdu, Switches::PDU1_CH7_ACS_A_SIDE_3V3,
-                         pdu1CoreHk.outputEnables[Channels::ACS_A_SIDE]);
-    checkAndUpdateSwitch(pdu, Switches::PDU1_CH8_UNOCCUPIED,
-                         pdu1CoreHk.outputEnables[Channels::UNUSED]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH0_TCS_BOARD_3V3,
+                            pdu1CoreHk.outputEnables[Channels::TCS_BOARD_3V3]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH1_SYRLINKS_12V,
+                            pdu1CoreHk.outputEnables[Channels::SYRLINKS]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH2_STAR_TRACKER_5V,
+                            pdu1CoreHk.outputEnables[Channels::STR]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH3_MGT_5V,
+                            pdu1CoreHk.outputEnables[Channels::MGT]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH4_SUS_NOMINAL_3V3,
+                            pdu1CoreHk.outputEnables[Channels::SUS_NOMINAL]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH5_SOLAR_CELL_EXP_5V,
+                            pdu1CoreHk.outputEnables[Channels::SOL_CELL_EXPERIMENT]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH6_PLOC_12V,
+                            pdu1CoreHk.outputEnables[Channels::PLOC]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH7_ACS_A_SIDE_3V3,
+                            pdu1CoreHk.outputEnables[Channels::ACS_A_SIDE]);
+    checkAndUpdatePduSwitch(pdu, Switches::PDU1_CH8_UNOCCUPIED,
+                            pdu1CoreHk.outputEnables[Channels::UNUSED]);
    if (firstSwitchInfoPdu1) {
      firstSwitchInfoPdu1 = false;
    }
@ -226,52 +251,52 @@ ReturnValue_t PCDUHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onO
  uint16_t memoryAddress = 0;
  size_t parameterValueSize = sizeof(uint8_t);
  uint8_t parameterValue = 0;
-  GomspaceDeviceHandler* pdu = nullptr;
+  GomspaceDeviceHandler* module = nullptr;

  switch (switchNr) {
    case pcdu::PDU1_CH0_TCS_BOARD_3V3: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_TCS_BOARD_3V3;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    case pcdu::PDU1_CH1_SYRLINKS_12V: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_SYRLINKS;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    case pcdu::PDU1_CH2_STAR_TRACKER_5V: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_STAR_TRACKER;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    case pcdu::PDU1_CH3_MGT_5V: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_MGT;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    case pcdu::PDU1_CH4_SUS_NOMINAL_3V3: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_SUS_NOMINAL;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    case pcdu::PDU1_CH5_SOLAR_CELL_EXP_5V: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_SOLAR_CELL_EXP;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    case pcdu::PDU1_CH6_PLOC_12V: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_PLOC;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    case pcdu::PDU1_CH7_ACS_A_SIDE_3V3: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_ACS_BOARD_SIDE_A;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    case pcdu::PDU1_CH8_UNOCCUPIED: {
      memoryAddress = PDU1::CONFIG_ADDRESS_OUT_EN_CHANNEL8;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU1_HANDLER);
      break;
    }
    // This is a dangerous command. Reject/Igore it for now
@ -283,42 +308,47 @@ ReturnValue_t PCDUHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onO
    }
    case pcdu::PDU2_CH1_PL_PCDU_BATT_0_14V8: {
      memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_PAYLOAD_PCDU_CH1;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
      break;
    }
    case pcdu::PDU2_CH2_RW_5V: {
      memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_RW;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
      break;
    }
    case pcdu::PDU2_CH3_TCS_BOARD_HEATER_IN_8V: {
      memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_TCS_BOARD_HEATER_IN;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
      break;
    }
    case pcdu::PDU2_CH4_SUS_REDUNDANT_3V3: {
      memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_SUS_REDUNDANT;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
      break;
    }
    case pcdu::PDU2_CH5_DEPLOYMENT_MECHANISM_8V: {
      memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_DEPLOYMENT_MECHANISM;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
      break;
    }
    case pcdu::PDU2_CH6_PL_PCDU_BATT_1_14V8: {
      memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_PAYLOAD_PCDU_CH6;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
      break;
    }
    case pcdu::PDU2_CH7_ACS_BOARD_SIDE_B_3V3: {
      memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_ACS_BOARD_SIDE_B;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
      break;
    }
    case pcdu::PDU2_CH8_PAYLOAD_CAMERA: {
      memoryAddress = PDU2::CONFIG_ADDRESS_OUT_EN_PAYLOAD_CAMERA;
-      pdu = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::PDU2_HANDLER);
+      break;
+    }
+    case pcdu::P60_DOCK_5V_STACK: {
+      memoryAddress = P60Dock::CONFIG_ADDRESS_OUT_EN_5V_STACK;
+      module = ObjectManager::instance()->get<GomspaceDeviceHandler>(objects::P60DOCK_HANDLER);
      break;
    }

@ -354,7 +384,7 @@ ReturnValue_t PCDUHandler::sendSwitchCommand(uint8_t switchNr, ReturnValue_t onO
  CommandMessage message;
  ActionMessage::setCommand(&message, GOMSPACE::PARAM_SET, storeAddress);

-  result = commandQueue->sendMessage(pdu->getCommandQueue(), &message, 0);
+  result = commandQueue->sendMessage(module->getCommandQueue(), &message, 0);
  if (result != returnvalue::OK) {
    sif::debug << "PCDUHandler::sendSwitchCommand: Failed to send message to PDU Handler"
               << std::endl;
@ -393,6 +423,7 @@ ReturnValue_t PCDUHandler::initializeLocalDataPool(localpool::DataPool& localDat
  using namespace pcdu;
  localDataPoolMap.emplace(PoolIds::PDU1_SWITCHES, &pdu1Switches);
  localDataPoolMap.emplace(PoolIds::PDU2_SWITCHES, &pdu2Switches);
+  localDataPoolMap.emplace(PoolIds::P60DOCK_SWITCHES, &p60Dock5VSwitch);
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(switcherSet.getSid(), false, 5.0));
  return returnvalue::OK;
@ -422,8 +453,8 @@ LocalPoolDataSetBase* PCDUHandler::getDataSetHandle(sid_t sid) {
  }
 }

-void PCDUHandler::checkAndUpdateSwitch(GOMSPACE::Pdu pdu, pcdu::Switches switchIdx,
-                                       uint8_t setValue) {
+void PCDUHandler::checkAndUpdatePduSwitch(GOMSPACE::Pdu pdu, pcdu::Switches switchIdx,
+                                          uint8_t setValue) {
  using namespace pcdu;
  if (switchStates[switchIdx] != setValue) {
 #if OBSW_INITIALIZE_SWITCHES == 1
--- a/mission/devices/PcduHandler.h
+++ b/mission/devices/PcduHandler.h
@ -56,6 +56,8 @@ class PCDUHandler : public PowerSwitchIF,
  /** Housekeeping manager. Handles updates of local pool variables. */
  LocalDataPoolManager poolManager;

+  P60Dock::CoreHkSet p60CoreHk;
+
  /** Hk table dataset of PDU1 */
  PDU1::Pdu1CoreHk pdu1CoreHk;
  /**
@ -71,6 +73,7 @@ class PCDUHandler : public PowerSwitchIF,
      PoolEntry<uint8_t>(pcdu::INIT_SWITCHES_PDU1.data(), pcdu::INIT_SWITCHES_PDU1.size());
  PoolEntry<uint8_t> pdu2Switches =
      PoolEntry<uint8_t>(pcdu::INIT_SWITCHES_PDU2.data(), pcdu::INIT_SWITCHES_PDU2.size());
+  PoolEntry<uint8_t> p60Dock5VSwitch = PoolEntry<uint8_t>();

  /** The timeStamp of the current pdu2HkTableDataset */
  CCSDSTime::CDS_short timeStampPdu2HkDataset;
@ -127,7 +130,7 @@ class PCDUHandler : public PowerSwitchIF,
   */
  void updateHkTableDataset(store_address_t storeId, LocalPoolDataSetBase* dataset,
                            CCSDSTime::CDS_short* datasetTimeStamp);
-  void checkAndUpdateSwitch(GOMSPACE::Pdu pdu, pcdu::Switches switchIdx, uint8_t setValue);
+  void checkAndUpdatePduSwitch(GOMSPACE::Pdu pdu, pcdu::Switches switchIdx, uint8_t setValue);
 };

 #endif /* MISSION_DEVICES_PCDUHANDLER_H_ */
--- a/mission/devices/RadiationSensorHandler.cpp
+++ b/mission/devices/RadiationSensorHandler.cpp
@ -2,11 +2,16 @@
 #include <devices/gpioIds.h>
 #include <fsfw/datapool/PoolReadGuard.h>
 #include <mission/devices/RadiationSensorHandler.h>
+#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
 #include <mission/devices/max1227.h>

 RadiationSensorHandler::RadiationSensorHandler(object_id_t objectId, object_id_t comIF,
-                                               CookieIF *comCookie, GpioIF *gpioIF)
-    : DeviceHandlerBase(objectId, comIF, comCookie), dataset(this), gpioIF(gpioIF) {
+                                               CookieIF *comCookie, GpioIF *gpioIF,
+                                               Stack5VHandler &stackHandler)
+    : DeviceHandlerBase(objectId, comIF, comCookie),
+      dataset(this),
+      gpioIF(gpioIF),
+      stackHandler(stackHandler) {
  if (comCookie == nullptr) {
    sif::error << "RadiationSensorHandler: Invalid com cookie" << std::endl;
  }
@ -15,18 +20,35 @@ RadiationSensorHandler::RadiationSensorHandler(object_id_t objectId, object_id_t
 RadiationSensorHandler::~RadiationSensorHandler() {}

 void RadiationSensorHandler::doStartUp() {
+  if (internalState == InternalState::OFF) {
+    ReturnValue_t retval = stackHandler.deviceToOn(StackCommander::RAD_SENSOR, true);
+    if (retval == BUSY) {
+      return;
+    }
+    internalState = InternalState::POWER_SWITCHING;
+  }
+  if (internalState == InternalState::POWER_SWITCHING) {
+    if (stackHandler.isSwitchOn()) {
+      internalState = InternalState::SETUP;
+    }
+  }
  if (internalState == InternalState::CONFIGURED) {
    if (goToNormalMode) {
      setMode(MODE_NORMAL);
-    }
-
-    else {
+    } else {
      setMode(_MODE_TO_ON);
    }
  }
 }

-void RadiationSensorHandler::doShutDown() { setMode(_MODE_POWER_DOWN); }
+void RadiationSensorHandler::doShutDown() {
+  ReturnValue_t retval = stackHandler.deviceToOff(StackCommander::RAD_SENSOR, true);
+  if (retval == BUSY) {
+    return;
+  }
+  internalState = InternalState::OFF;
+  setMode(_MODE_POWER_DOWN);
+}

 ReturnValue_t RadiationSensorHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
  switch (communicationStep) {
@ -73,9 +95,10 @@ ReturnValue_t RadiationSensorHandler::buildCommandFromCommand(DeviceCommandId_t
      ReturnValue_t result = gpioIF->pullHigh(gpioIds::ENABLE_RADFET);
      if (result != returnvalue::OK) {
 #if OBSW_VERBOSE_LEVEL >= 1
-        sif::warning << "RadiationSensorHandler::buildCommandFromCommand; Pulling RADFET Enale pin "
-                        "high failed"
-                     << std::endl;
+        sif::warning
+            << "RadiationSensorHandler::buildCommandFromCommand: Pulling RADFET Enable pin "
+               "high failed"
+            << std::endl;
 #endif
      }
      /* First the fifo will be reset here */
--- a/mission/devices/RadiationSensorHandler.h
+++ b/mission/devices/RadiationSensorHandler.h
@ -4,6 +4,7 @@
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
 #include <fsfw_hal/common/gpio/GpioIF.h>
 #include <mission/devices/devicedefinitions/RadSensorDefinitions.h>
+#include <mission/system/objects/Stack5VHandler.h>

 /**
 * @brief	This is the device handler class for radiation sensor on the OBC IF Board. The
@ -16,7 +17,7 @@
 class RadiationSensorHandler : public DeviceHandlerBase {
 public:
  RadiationSensorHandler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie,
-                         GpioIF *gpioIF);
+                         GpioIF *gpioIF, Stack5VHandler &handler);
  virtual ~RadiationSensorHandler();
  void setToGoToNormalModeImmediately();
  void enablePeriodicDataPrint(bool enable);
@ -39,16 +40,17 @@ class RadiationSensorHandler : public DeviceHandlerBase {
 private:
  enum class CommunicationStep { START_CONVERSION, READ_CONVERSIONS };

-  enum class InternalState { SETUP, CONFIGURED };
+  enum class InternalState { OFF, POWER_SWITCHING, SETUP, CONFIGURED };

  bool printPeriodicData = false;
  RAD_SENSOR::RadSensorDataset dataset;
  static const uint8_t MAX_CMD_LEN = RAD_SENSOR::READ_SIZE;
  GpioIF *gpioIF = nullptr;
+  Stack5VHandler &stackHandler;

  bool goToNormalMode = false;
  uint8_t cmdBuffer[MAX_CMD_LEN];
-  InternalState internalState = InternalState::SETUP;
+  InternalState internalState = InternalState::OFF;
  CommunicationStep communicationStep = CommunicationStep::START_CONVERSION;
 };

--- a/mission/devices/devicedefinitions/GomspaceDefinitions.h
+++ b/mission/devices/devicedefinitions/GomspaceDefinitions.h
@ -108,6 +108,8 @@ enum class SetIds : uint32_t { CORE = 1, AUX = 2, CONFIG = 3 };

 namespace P60Dock {

+static const uint16_t CONFIG_ADDRESS_OUT_EN_5V_STACK = 0x72;
+
 namespace pool {

 enum Ids : lp_id_t {
@ -711,7 +713,7 @@ class AuxHk : public StaticLocalDataSet<12> {

 namespace pcdu {

-enum PoolIds : uint32_t { PDU1_SWITCHES, PDU2_SWITCHES };
+enum PoolIds : uint32_t { PDU1_SWITCHES, PDU2_SWITCHES, P60DOCK_SWITCHES };

 /* Switches are uint8_t datatype and go from 0 to 255 */
 enum Switches : power::Switch_t {
@ -733,10 +735,11 @@ enum Switches : power::Switch_t {
  PDU2_CH5_DEPLOYMENT_MECHANISM_8V,
  PDU2_CH6_PL_PCDU_BATT_1_14V8,
  PDU2_CH7_ACS_BOARD_SIDE_B_3V3,
-  PDU2_CH8_PAYLOAD_CAMERA
-};
+  PDU2_CH8_PAYLOAD_CAMERA,

-static constexpr uint8_t NUMBER_OF_SWITCHES = 18;
+  P60_DOCK_5V_STACK,
+  NUMBER_OF_SWITCHES
+};

 static const uint8_t ON = 1;
 static const uint8_t OFF = 0;
@ -767,6 +770,7 @@ class SwitcherStates : public StaticLocalDataSet<NUMBER_OF_SWITCHES> {
      lp_vec_t<uint8_t, PDU::CHANNELS_LEN>(sid.objectId, PDU1_SWITCHES, this);
  lp_vec_t<uint8_t, PDU::CHANNELS_LEN> pdu2Switches =
      lp_vec_t<uint8_t, PDU::CHANNELS_LEN>(sid.objectId, PDU2_SWITCHES, this);
+  lp_var_t<uint8_t> p60Dock5VStack = lp_var_t<uint8_t>(sid.objectId, P60DOCK_SWITCHES, this);
 };

 }  // namespace pcdu
--- a/mission/system/objects/CMakeLists.txt
+++ b/mission/system/objects/CMakeLists.txt
@ -6,6 +6,7 @@ target_sources(
          ComSubsystem.cpp
          PayloadSubsystem.cpp
          AcsBoardAssembly.cpp
+          Stack5VHandler.cpp
          SusAssembly.cpp
          RwAssembly.cpp
          DualLanePowerStateMachine.cpp
--- a/mission/system/objects/Stack5VHandler.cpp
+++ b/mission/system/objects/Stack5VHandler.cpp
@ -0,0 +1,81 @@
+#include "Stack5VHandler.h"
+
+Stack5VHandler::Stack5VHandler(PowerSwitchIF& switcher) : switcher(switcher) {
+  stackLock = MutexFactory::instance()->createMutex();
+}
+
+ReturnValue_t Stack5VHandler::deviceToOn(StackCommander commander, bool updateStates) {
+  MutexGuard mg(stackLock);
+  if (updateStates) {
+    updateInternalStates();
+  }
+  if (handlerState == HandlerState::SWITCH_PENDING) {
+    return BUSY;
+  }
+  if (switchIsOn) {
+    if (commander == StackCommander::PL_PCDU) {
+      plPcduIsOn = true;
+    } else {
+      radSensorIsOn = true;
+    }
+    return returnvalue::OK;
+  }
+
+  handlerState = HandlerState::SWITCH_PENDING;
+  targetState = true;
+  return switcher.sendSwitchCommand(stackSwitch, PowerSwitchIF::SWITCH_ON);
+}
+
+ReturnValue_t Stack5VHandler::deviceToOff(StackCommander commander, bool updateStates) {
+  MutexGuard mg(stackLock);
+  if (updateStates) {
+    updateInternalStates();
+  }
+  // wait for our turn
+  if (handlerState == HandlerState::SWITCH_PENDING) {
+    return BUSY;
+  }
+  // If the switch is already off, we are done
+  if (not switchIsOn) {
+    if (commander == StackCommander::PL_PCDU) {
+      plPcduIsOn = false;
+    } else {
+      radSensorIsOn = false;
+    }
+    return returnvalue::OK;
+  }
+  // If one device is still on, do not turn off the switch
+  if ((commander == StackCommander::PL_PCDU and radSensorIsOn) or
+      (commander == StackCommander::RAD_SENSOR and plPcduIsOn)) {
+    return returnvalue::OK;
+  }
+  handlerState = HandlerState::SWITCH_PENDING;
+  targetState = false;
+  return switcher.sendSwitchCommand(stackSwitch, PowerSwitchIF::SWITCH_OFF);
+}
+
+bool Stack5VHandler::isSwitchOn() {
+  MutexGuard mg(stackLock);
+  return updateInternalStates();
+}
+
+void Stack5VHandler::update() {
+  MutexGuard mg(stackLock);
+  updateInternalStates();
+}
+
+bool Stack5VHandler::updateInternalStates() {
+  if (switcher.getSwitchState(stackSwitch) == PowerSwitchIF::SWITCH_ON) {
+    if (handlerState == HandlerState::SWITCH_PENDING and targetState) {
+      handlerState = HandlerState::IDLE;
+      switchIsOn = true;
+    }
+    return true;
+  } else if (handlerState == HandlerState::SWITCH_PENDING and not targetState) {
+    handlerState = HandlerState::IDLE;
+    switchIsOn = false;
+    radSensorIsOn = false;
+    plPcduIsOn = false;
+  }
+  return false;
+}
--- a/mission/system/objects/Stack5VHandler.h
+++ b/mission/system/objects/Stack5VHandler.h
@ -0,0 +1,35 @@
+#ifndef MISSION_SYSTEM_OBJECTS_STACK5VHANDLER_H_
+#define MISSION_SYSTEM_OBJECTS_STACK5VHANDLER_H_
+
+#include <fsfw/power/PowerSwitchIF.h>
+
+#include "mission/devices/devicedefinitions/GomspaceDefinitions.h"
+
+enum class StackCommander { RAD_SENSOR = 0, PL_PCDU = 1 };
+enum class HandlerState { SWITCH_PENDING, IDLE };
+
+class Stack5VHandler {
+ public:
+  static constexpr ReturnValue_t BUSY = returnvalue::makeCode(1, 0);
+  Stack5VHandler(PowerSwitchIF& switcher);
+
+  ReturnValue_t deviceToOn(StackCommander commander, bool updateStates);
+  ReturnValue_t deviceToOff(StackCommander commander, bool updateStates);
+
+  bool isSwitchOn();
+  void update();
+
+ private:
+  MutexIF* stackLock;
+  PowerSwitchIF& switcher;
+  bool switchIsOn = false;
+  bool targetState = false;
+  HandlerState handlerState = HandlerState::IDLE;
+  bool radSensorIsOn = false;
+  bool plPcduIsOn = false;
+  pcdu::Switches stackSwitch = pcdu::Switches::P60_DOCK_5V_STACK;
+
+  bool updateInternalStates();
+};
+
+#endif /* MISSION_SYSTEM_OBJECTS_STACK5VHANDLER_H_ */