From a3d1dde4550f4d8ab22c5ea579639d5a211db87c Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Thu, 22 Dec 2022 14:13:41 +0100
Subject: [PATCH] this should do the job

---
 mission/devices/PayloadPcduHandler.cpp    | 36 +++++++++++++++++++----
 mission/devices/PayloadPcduHandler.h      |  4 ++-
 mission/system/objects/Stack5VHandler.cpp |  3 +-
 3 files changed, 36 insertions(+), 7 deletions(-)

diff --git a/mission/devices/PayloadPcduHandler.cpp b/mission/devices/PayloadPcduHandler.cpp
index 3409a483..e873e3e1 100644
--- a/mission/devices/PayloadPcduHandler.cpp
+++ b/mission/devices/PayloadPcduHandler.cpp
@@ -30,9 +30,23 @@ void PayloadPcduHandler::doStartUp() {
     sif::error << "PayloadPcduHandler::doStartUp: Invalid state" << std::endl;
   }
   clearSetOnOffFlag = true;
-  if (true) {
+  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);
+    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);
   }
 }
@@ -47,7 +61,19 @@ void PayloadPcduHandler::doShutDown() {
     clearSetOnOffFlag = false;
   }
 
-  if (true) {
+  if (state == States::STACK_5V_SWITCHING) {
+    ReturnValue_t retval = stackHandler.deviceToOff(StackCommander::PL_PCDU);
+    if (retval == BUSY) {
+      return;
+    }
+    state == States::STACK_5V_PENDING;
+  }
+  if (state == States::STACK_5V_PENDING) {
+    if (not stackHandler.isSwitchOn()) {
+      state == States::STACK_5V_CORRECT;
+    }
+  }
+  if (state == States::STACK_5V_CORRECT) {
     state = States::PL_PCDU_OFF;
     // No need to set mode _MODE_POWER_DOWN, power switching was already handled
     setMode(MODE_OFF);
@@ -72,7 +98,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
@@ -347,7 +373,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);
diff --git a/mission/devices/PayloadPcduHandler.h b/mission/devices/PayloadPcduHandler.h
index 899f73f8..0fd78887 100644
--- a/mission/devices/PayloadPcduHandler.h
+++ b/mission/devices/PayloadPcduHandler.h
@@ -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,
diff --git a/mission/system/objects/Stack5VHandler.cpp b/mission/system/objects/Stack5VHandler.cpp
index eb0af21f..e09f2a59 100644
--- a/mission/system/objects/Stack5VHandler.cpp
+++ b/mission/system/objects/Stack5VHandler.cpp
@@ -17,6 +17,7 @@ ReturnValue_t Stack5VHandler::deviceToOn(StackCommander commander) {
   if (handlerState == HandlerState::SWITCH_PENDING) {
     return BUSY;
   }
+  handlerState == HandlerState::SWITCH_PENDING;
   return switcher.sendSwitchCommand(stackSwitch, PowerSwitchIF::SWITCH_ON);
 }
 
@@ -37,7 +38,7 @@ ReturnValue_t Stack5VHandler::deviceToOff(StackCommander commander) {
       (commander == StackCommander::RAD_SENSOR and plPcduIsOn)) {
     return returnvalue::OK;
   }
-
+  handlerState == HandlerState::SWITCH_PENDING;
   return switcher.sendSwitchCommand(stackSwitch, PowerSwitchIF::SWITCH_OFF);
 }