started SUS assembly

fixes for PCDU code and switcher list handling

mission/system/SusAssembly.cpp

@@ -0,0 +1,101 @@
+#include "SusAssembly.h"
+
+#include <devices/powerSwitcherList.h>
+#include <fsfw/power/PowerSwitchIF.h>
+#include <fsfw/serviceinterface.h>
+
+SusAssembly::SusAssembly(object_id_t objectId, object_id_t parentId, PowerSwitchIF* pwrSwitcher,
+                         SusAssHelper helper)
+    : AssemblyBase(objectId, parentId), helper(helper), pwrSwitcher(pwrSwitcher) {}
+
+ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
+  ReturnValue_t result = RETURN_OK;
+  refreshHelperModes();
+  return RETURN_OK;
+}
+
+ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
+  return RETURN_OK;
+}
+
+ReturnValue_t SusAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
+  if (submode != NOMINAL and submode != REDUNDANT and submode != DUAL_MODE) {
+    return HasReturnvaluesIF::RETURN_FAILED;
+  }
+  return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t SusAssembly::initialize() {
+  ReturnValue_t result = RETURN_OK;
+  for (const auto& id : helper.susIds) {
+    result = registerChild(id);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+      return result;
+    }
+  }
+  return result;
+}
+
+bool SusAssembly::isUseable(object_id_t object, Mode_t mode) {
+  if (healthHelper.healthTable->isFaulty(object)) {
+    return false;
+  }
+
+  // Check if device is already in target mode
+  if (childrenMap[object].mode == mode) {
+    return true;
+  }
+
+  if (healthHelper.healthTable->isCommandable(object)) {
+    return true;
+  }
+  return false;
+}
+
+void SusAssembly::powerStateMachine(Mode_t mode, Submode_t submode) {
+  ReturnValue_t switchStateNom = RETURN_OK;
+  ReturnValue_t switchStateRed = RETURN_OK;
+  if (state == States::IDLE or state == States::SWITCHING_POWER) {
+    switchStateNom = pwrSwitcher->getSwitchState(pcduSwitches::PDU1_CH4_SUS_NOMINAL_3V3);
+    switchStateRed = pwrSwitcher->getSwitchState(pcduSwitches::PDU2_CH4_SUS_REDUNDANT_3V3);
+    if (mode == MODE_OFF) {
+      if (switchStateNom == PowerSwitchIF::SWITCH_OFF and
+          switchStateRed == PowerSwitchIF::SWITCH_OFF) {
+        state = States::MODE_COMMANDING;
+        return;
+      }
+    } else {
+      switch (submode) {
+        case (NOMINAL): {
+          if (switchStateNom == PowerSwitchIF::SWITCH_ON and
+              switchStateRed == PowerSwitchIF::SWITCH_OFF) {
+            state = States::MODE_COMMANDING;
+            return;
+          }
+          break;
+        }
+        case (REDUNDANT): {
+          if (switchStateNom == PowerSwitchIF::SWITCH_OFF and
+              switchStateRed == PowerSwitchIF::SWITCH_ON) {
+            state = States::MODE_COMMANDING;
+            return;
+          }
+          break;
+        }
+        case (DUAL_MODE): {
+          if (switchStateNom == PowerSwitchIF::SWITCH_ON and
+              switchStateRed == PowerSwitchIF::SWITCH_ON) {
+            state = States::MODE_COMMANDING;
+            return;
+          }
+        }
+      }
+    }
+  }
+}
+
+void SusAssembly::refreshHelperModes() {
+  for (uint8_t idx = 0; idx < helper.susModes.size(); idx++) {
+    helper.susModes[idx] = childrenMap[helper.susIds[idx]].mode;
+  }
+}