From fca45014e45e571fa4b1f412fd7ecf204227b041 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 10 Jan 2023 11:13:34 +0100
Subject: [PATCH] fixes for switchig algorithm

---
 mission/devices/PayloadPcduHandler.cpp     |  4 +--
 mission/devices/RadiationSensorHandler.cpp |  4 +--
 mission/system/objects/Stack5VHandler.cpp  | 37 +++++++++++++++-------
 mission/system/objects/Stack5VHandler.h    |  7 ++--
 4 files changed, 35 insertions(+), 17 deletions(-)

diff --git a/mission/devices/PayloadPcduHandler.cpp b/mission/devices/PayloadPcduHandler.cpp
index bcba7a6c..908a4c3e 100644
--- a/mission/devices/PayloadPcduHandler.cpp
+++ b/mission/devices/PayloadPcduHandler.cpp
@@ -34,7 +34,7 @@ void PayloadPcduHandler::doStartUp() {
     state = States::STACK_5V_SWITCHING;
   }
   if (state == States::STACK_5V_SWITCHING) {
-    ReturnValue_t retval = stackHandler.deviceToOn(StackCommander::PL_PCDU);
+    ReturnValue_t retval = stackHandler.deviceToOn(StackCommander::PL_PCDU, true);
     if (retval == BUSY) {
       return;
     }
@@ -62,7 +62,7 @@ void PayloadPcduHandler::doShutDown() {
   }
 
   if (state == States::STACK_5V_SWITCHING) {
-    ReturnValue_t retval = stackHandler.deviceToOff(StackCommander::PL_PCDU);
+    ReturnValue_t retval = stackHandler.deviceToOff(StackCommander::PL_PCDU, true);
     if (retval == BUSY) {
       return;
     }
diff --git a/mission/devices/RadiationSensorHandler.cpp b/mission/devices/RadiationSensorHandler.cpp
index 9e24b771..0520daec 100644
--- a/mission/devices/RadiationSensorHandler.cpp
+++ b/mission/devices/RadiationSensorHandler.cpp
@@ -21,7 +21,7 @@ RadiationSensorHandler::~RadiationSensorHandler() {}
 
 void RadiationSensorHandler::doStartUp() {
   if (internalState == InternalState::OFF) {
-    ReturnValue_t retval = stackHandler.deviceToOn(StackCommander::RAD_SENSOR);
+    ReturnValue_t retval = stackHandler.deviceToOn(StackCommander::RAD_SENSOR, true);
     if (retval == BUSY) {
       return;
     }
@@ -42,7 +42,7 @@ void RadiationSensorHandler::doStartUp() {
 }
 
 void RadiationSensorHandler::doShutDown() {
-  ReturnValue_t retval = stackHandler.deviceToOff(StackCommander::RAD_SENSOR);
+  ReturnValue_t retval = stackHandler.deviceToOff(StackCommander::RAD_SENSOR, true);
   if (retval == BUSY) {
     return;
   }
diff --git a/mission/system/objects/Stack5VHandler.cpp b/mission/system/objects/Stack5VHandler.cpp
index d970d0f1..457ca7b5 100644
--- a/mission/system/objects/Stack5VHandler.cpp
+++ b/mission/system/objects/Stack5VHandler.cpp
@@ -4,8 +4,11 @@ Stack5VHandler::Stack5VHandler(PowerSwitchIF& switcher) : switcher(switcher) {
   stackLock = MutexFactory::instance()->createMutex();
 }
 
-ReturnValue_t Stack5VHandler::deviceToOn(StackCommander commander) {
+ReturnValue_t Stack5VHandler::deviceToOn(StackCommander commander, bool updateStates) {
   MutexGuard mg(stackLock);
+  if (updateStates) {
+    updateInternalStates();
+  }
   if (switchIsOn) {
     if (commander == StackCommander::PL_PCDU) {
       plPcduIsOn = true;
@@ -21,8 +24,11 @@ ReturnValue_t Stack5VHandler::deviceToOn(StackCommander commander) {
   return switcher.sendSwitchCommand(stackSwitch, PowerSwitchIF::SWITCH_ON);
 }
 
-ReturnValue_t Stack5VHandler::deviceToOff(StackCommander commander) {
+ReturnValue_t Stack5VHandler::deviceToOff(StackCommander commander, bool updateStates) {
   MutexGuard mg(stackLock);
+  if (updateStates) {
+    updateInternalStates();
+  }
   if (not switchIsOn) {
     if (commander == StackCommander::PL_PCDU) {
       plPcduIsOn = false;
@@ -44,6 +50,15 @@ ReturnValue_t Stack5VHandler::deviceToOff(StackCommander commander) {
 
 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 (not switchIsOn) {
       if (handlerState == HandlerState::SWITCH_PENDING) {
@@ -52,15 +67,15 @@ bool Stack5VHandler::isSwitchOn() {
       switchIsOn = true;
     }
     return true;
-  } else {
-    if (switchIsOn) {
-      if (handlerState == HandlerState::SWITCH_PENDING) {
-        handlerState = HandlerState::IDLE;
-      }
-      switchIsOn = false;
-    }
-    radSensorIsOn = false;
-    plPcduIsOn = false;
   }
+  if (switchIsOn) {
+    if (handlerState == HandlerState::SWITCH_PENDING) {
+      handlerState = HandlerState::IDLE;
+    }
+    switchIsOn = false;
+  }
+  radSensorIsOn = false;
+  plPcduIsOn = false;
+
   return false;
 }
diff --git a/mission/system/objects/Stack5VHandler.h b/mission/system/objects/Stack5VHandler.h
index 3a703b86..36dfc907 100644
--- a/mission/system/objects/Stack5VHandler.h
+++ b/mission/system/objects/Stack5VHandler.h
@@ -13,10 +13,11 @@ class Stack5VHandler {
   static constexpr ReturnValue_t BUSY = returnvalue::makeCode(1, 0);
   Stack5VHandler(PowerSwitchIF& switcher);
 
-  ReturnValue_t deviceToOn(StackCommander commander);
-  ReturnValue_t deviceToOff(StackCommander commander);
+  ReturnValue_t deviceToOn(StackCommander commander, bool updateStates);
+  ReturnValue_t deviceToOff(StackCommander commander, bool updateStates);
 
   bool isSwitchOn();
+  void update();
 
  private:
   MutexIF* stackLock;
@@ -26,6 +27,8 @@ class Stack5VHandler {
   bool radSensorIsOn = false;
   bool plPcduIsOn = false;
   pcdu::Switches stackSwitch = pcdu::Switches::P60_DOCK_5V_STACK;
+
+  bool updateInternalStates();
 };
 
 #endif /* MISSION_SYSTEM_OBJECTS_STACK5VHANDLER_H_ */