continue stack 5v

mission/system/objects/Stack5VHandler.cpp

@@ -1,9 +1,39 @@
 #include "Stack5VHandler.h"
 
-Stack5VHandler::Stack5VHandler(PowerSwitchIF& switcher) : switcher(switcher) {}
+Stack5VHandler::Stack5VHandler(PowerSwitchIF& switcher) : switcher(switcher) {
+  stackLock = MutexFactory::instance()->createMutex();
+}
 
-ReturnValue_t Stack5VHandler::commandSwitchOn() { return returnvalue::OK; }
+ReturnValue_t Stack5VHandler::deviceToOn(StackCommander commander) {
+  MutexGuard mg(stackLock);
+  if (radSensorIsOn or plPcduIsOn) {
+    return returnvalue::OK;
+  }
+  if (commander == StackCommander::PL_PCDU) {
+    plPcduCommandedOn = true;
+  } else if (commander == StackCommander::RAD_SENSOR) {
+    radSensorCommandedOn = true;
+  }
+  return switcher.sendSwitchCommand(stackSwitch, PowerSwitchIF::SWITCH_ON);
+}
 
-ReturnValue_t Stack5VHandler::commandSwitchOff() { return returnvalue::OK; }
+ReturnValue_t Stack5VHandler::deviceToOff(StackCommander commander) {
+  MutexGuard mg(stackLock);
+  if (not radSensorIsOn and not plPcduIsOn and not plPcduCommandedOn and not radSensorCommandedOn) {
+    return returnvalue::OK;
+  }
+  if (commander == StackCommander::PL_PCDU) {
+    plPcduCommandedOn = true;
+  } else if (commander == StackCommander::RAD_SENSOR) {
+    radSensorCommandedOn = true;
+  }
+  return switcher.sendSwitchCommand(stackSwitch, PowerSwitchIF::SWITCH_OFF);
+}
 
-bool Stack5VHandler::isSwitchOn() { return false; }
+bool Stack5VHandler::isSwitchOn() {
+  MutexGuard mg(stackLock);
+  if (switcher.getSwitchState(stackSwitch) == PowerSwitchIF::SWITCH_ON) {
+    return true;
+  }
+  return false;
+}

mission/system/objects/Stack5VHandler.h

@@ -5,17 +5,22 @@
 
 #include "mission/devices/devicedefinitions/GomspaceDefinitions.h"
 
+enum class StackCommander { RAD_SENSOR = 0, PL_PCDU = 1 };
+
 class Stack5VHandler {
  public:
   Stack5VHandler(PowerSwitchIF& switcher);
 
-  ReturnValue_t commandSwitchOn();
+  ReturnValue_t deviceToOn(StackCommander commander);
-  ReturnValue_t commandSwitchOff();
+  ReturnValue_t deviceToOff(StackCommander commander);
 
   bool isSwitchOn();
 
  private:
+  MutexIF* stackLock;
   PowerSwitchIF& switcher;
+  bool radSensorCommandedOn = false;
+  bool plPcduCommandedOn = false;
   bool radSensorIsOn = false;
   bool plPcduIsOn = false;
   pcdu::Switches stackSwitch = pcdu::Switches::P60_DOCK_5V_STACK;