squashed EIVE software

2026-04-15 13:25:48 +02:00
1 parent eced94f335
commit 996aa72fb4
@@ -0,0 +1,111 @@
+#include "DualLanePowerStateMachine.h"
+
+#include <fsfw/devicehandlers/AssemblyBase.h>
+#include <fsfw/power/PowerSwitchIF.h>
+
+DualLanePowerStateMachine::DualLanePowerStateMachine(power::Switch_t switchA,
+                                                     power::Switch_t switchB,
+                                                     PowerSwitchIF* pwrSwitcher,
+                                                     dur_millis_t checkTimeout)
+    : PowerStateMachineBase(pwrSwitcher, checkTimeout), SWITCH_A(switchA), SWITCH_B(switchB) {}
+
+power::OpCodes DualLanePowerStateMachine::fsm() {
+  using namespace duallane;
+  ReturnValue_t switchStateA = returnvalue::OK;
+  ReturnValue_t switchStateB = returnvalue::OK;
+  if (state == power::States::IDLE or state == power::States::MODE_COMMANDING) {
+    return opResult;
+  }
+  if (state == power::States::SWITCHING_POWER or state == power::States::CHECKING_POWER) {
+    switchStateA = pwrSwitcher->getSwitchState(SWITCH_A);
+    switchStateB = pwrSwitcher->getSwitchState(SWITCH_B);
+  } else {
+    return opResult;
+  }
+  if (targetMode == HasModesIF::MODE_OFF) {
+    if (switchStateA == PowerSwitchIF::SWITCH_OFF and switchStateB == PowerSwitchIF::SWITCH_OFF) {
+      state = power::States::IDLE;
+      opResult = power::OpCodes::TO_OFF_DONE;
+      return opResult;
+    }
+  } else {
+    switch (targetSubmode) {
+      case (A_SIDE): {
+        if (switchStateA == PowerSwitchIF::SWITCH_ON and
+            switchStateB == PowerSwitchIF::SWITCH_OFF) {
+          state = power::States::MODE_COMMANDING;
+          opResult = power::OpCodes::TO_NOT_OFF_DONE;
+          return opResult;
+        }
+        break;
+      }
+      case (B_SIDE): {
+        if (switchStateA == PowerSwitchIF::SWITCH_OFF and
+            switchStateB == PowerSwitchIF::SWITCH_ON) {
+          state = power::States::MODE_COMMANDING;
+          opResult = power::OpCodes::TO_NOT_OFF_DONE;
+          return opResult;
+        }
+        break;
+      }
+      case (DUAL_MODE): {
+        if (switchStateA == PowerSwitchIF::SWITCH_ON and switchStateB == PowerSwitchIF::SWITCH_ON) {
+          state = power::States::MODE_COMMANDING;
+          opResult = power::OpCodes::TO_NOT_OFF_DONE;
+          return opResult;
+        }
+      }
+    }
+  }
+  if (state == power::States::SWITCHING_POWER) {
+    if (targetMode == HasModesIF::MODE_OFF) {
+      if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
+        pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_OFF);
+      }
+      if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
+        pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
+      }
+      checkTimeout.resetTimer();
+    } else {
+      switch (targetSubmode) {
+        case (A_SIDE): {
+          if (switchStateA != PowerSwitchIF::SWITCH_ON) {
+            pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON);
+          }
+          if (switchStateB != PowerSwitchIF::SWITCH_OFF) {
+            pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_OFF);
+          }
+          checkTimeout.resetTimer();
+          break;
+        }
+        case (B_SIDE): {
+          if (switchStateA != PowerSwitchIF::SWITCH_OFF) {
+            pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_OFF);
+          }
+          if (switchStateB != PowerSwitchIF::SWITCH_ON) {
+            pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_ON);
+          }
+          checkTimeout.resetTimer();
+          break;
+        }
+        case (DUAL_MODE): {
+          if (switchStateA != PowerSwitchIF::SWITCH_ON) {
+            pwrSwitcher->sendSwitchCommand(SWITCH_A, PowerSwitchIF::SWITCH_ON);
+          }
+          if (switchStateB != PowerSwitchIF::SWITCH_ON) {
+            pwrSwitcher->sendSwitchCommand(SWITCH_B, PowerSwitchIF::SWITCH_ON);
+          }
+          checkTimeout.resetTimer();
+          break;
+        }
+      }
+    }
+    state = power::States::CHECKING_POWER;
+  }
+  if (state == power::States::CHECKING_POWER) {
+    if (checkTimeout.hasTimedOut()) {
+      return power::OpCodes::TIMEOUT_OCCURED;
+    }
+  }
+  return opResult;
+}