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);

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,

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);
 }