system folder restructuring

mission/system/objects/SusAssembly.cpp

@@ -0,0 +1,155 @@
+#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)
+    : DualLaneAssemblyBase(objectId, parentId, pwrSwitcher, SWITCH_NOM, SWITCH_RED,
+                           POWER_STATE_MACHINE_TIMEOUT, SIDE_SWITCH_TRANSITION_NOT_ALLOWED,
+                           TRANSITION_OTHER_SIDE_FAILED),
+      helper(helper),
+      pwrSwitcher(pwrSwitcher) {
+  ModeListEntry entry;
+  for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
+    initModeTableEntry(helper.susIds[idx], entry, modeTable);
+  }
+}
+
+ReturnValue_t SusAssembly::commandChildren(Mode_t mode, Submode_t submode) {
+  ReturnValue_t result = RETURN_OK;
+  refreshHelperModes();
+  // Initialize the mode table to ensure all devices are in a defined state
+  for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
+    modeTable[idx].setMode(MODE_OFF);
+    modeTable[idx].setSubmode(SUBMODE_NONE);
+  }
+  if (recoveryState != RecoveryState::RECOVERY_STARTED) {
+    if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
+      result = handleNormalOrOnModeCmd(mode, submode);
+    }
+  }
+
+  HybridIterator<ModeListEntry> tableIter(modeTable.begin(), modeTable.end());
+  executeTable(tableIter);
+  return result;
+}
+
+ReturnValue_t SusAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
+  using namespace duallane;
+  ReturnValue_t result = RETURN_OK;
+  bool needsSecondStep = false;
+  auto cmdSeq = [&](object_id_t objectId, Mode_t devMode, uint8_t tableIdx) {
+    if (mode == devMode) {
+      modeTable[tableIdx].setMode(mode);
+    } else if (mode == DeviceHandlerIF::MODE_NORMAL) {
+      if (isUseable(objectId, devMode)) {
+        if (devMode == MODE_ON) {
+          modeTable[tableIdx].setMode(mode);
+          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
+        } else {
+          modeTable[tableIdx].setMode(MODE_ON);
+          modeTable[tableIdx].setSubmode(SUBMODE_NONE);
+          if (internalState != STATE_SECOND_STEP) {
+            needsSecondStep = true;
+          }
+        }
+      }
+    } else if (mode == MODE_ON) {
+      if (isUseable(objectId, devMode)) {
+        modeTable[tableIdx].setMode(MODE_ON);
+        modeTable[tableIdx].setSubmode(SUBMODE_NONE);
+      }
+    }
+  };
+  switch (submode) {
+    case (A_SIDE): {
+      for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS_ONE_SIDE; idx++) {
+        cmdSeq(helper.susIds[idx], helper.susModes[idx], idx);
+        // Switch off devices on redundant side
+        modeTable[idx + NUMBER_SUN_SENSORS_ONE_SIDE].setMode(MODE_OFF);
+        modeTable[idx + NUMBER_SUN_SENSORS_ONE_SIDE].setSubmode(SUBMODE_NONE);
+      }
+      break;
+    }
+    case (B_SIDE): {
+      for (uint8_t idx = NUMBER_SUN_SENSORS_ONE_SIDE; idx < NUMBER_SUN_SENSORS; idx++) {
+        cmdSeq(helper.susIds[idx], helper.susModes[idx], idx);
+        // Switch devices on nominal side
+        modeTable[idx - NUMBER_SUN_SENSORS_ONE_SIDE].setMode(MODE_OFF);
+        modeTable[idx - NUMBER_SUN_SENSORS_ONE_SIDE].setSubmode(SUBMODE_NONE);
+      }
+      break;
+    }
+    case (DUAL_MODE): {
+      for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS; idx++) {
+        cmdSeq(helper.susIds[idx], helper.susModes[idx], idx);
+      }
+      break;
+    }
+  }
+  if (needsSecondStep) {
+    result = NEED_SECOND_STEP;
+  }
+  return result;
+}
+
+ReturnValue_t SusAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
+  using namespace duallane;
+  refreshHelperModes();
+  if (wantedSubmode == A_SIDE) {
+    for (uint8_t idx = 0; idx < NUMBER_SUN_SENSORS_ONE_SIDE; idx++) {
+      if (helper.susModes[idx] != wantedMode) {
+        return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
+      }
+    }
+    return RETURN_OK;
+  } else if (wantedSubmode == B_SIDE) {
+    for (uint8_t idx = NUMBER_SUN_SENSORS_ONE_SIDE; idx < NUMBER_SUN_SENSORS; idx++) {
+      if (helper.susModes[idx] != wantedMode) {
+        return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
+      }
+    }
+    return RETURN_OK;
+  } else {
+    // Trigger event if devices are faulty? This is the last fallback mode, returning
+    // a failure here would trigger a transition to MODE_OFF unless handleModeTransitionFailed
+    // is overriden
+    return RETURN_OK;
+  }
+  return 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 AssemblyBase::initialize();
+}
+
+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::refreshHelperModes() {
+  for (uint8_t idx = 0; idx < helper.susModes.size(); idx++) {
+    helper.susModes[idx] = childrenMap[helper.susIds[idx]].mode;
+  }
+}