custom FDIR for tmp devices

mission/system/tcs/TcsBoardAssembly.cpp

@@ -0,0 +1,179 @@
+#include "TcsBoardAssembly.h"
+
+#include <fsfw/devicehandlers/DeviceHandlerIF.h>
+#include <fsfw/ipc/QueueFactory.h>
+
+TcsBoardAssembly::TcsBoardAssembly(object_id_t objectId, PowerSwitchIF* pwrSwitcher,
+                                   power::Switch_t theSwitch, TcsBoardHelper helper,
+                                   std::atomic_bool& tcsShortlyUnavailable)
+    : SharedPowerAssemblyBase(objectId, pwrSwitcher, theSwitch, 16),
+      helper(helper),
+      tcsShortlyUnavailable(tcsShortlyUnavailable) {
+  ModeListEntry entry;
+  for (uint8_t idx = 0; idx < NUMBER_RTDS; idx++) {
+    entry.setObject(helper.rtdInfos[idx].first);
+    entry.setMode(MODE_OFF);
+    entry.setSubmode(SUBMODE_NONE);
+    modeTable.insert(entry);
+  }
+}
+
+ReturnValue_t TcsBoardAssembly::commandChildren(Mode_t mode, Submode_t submode) {
+  ReturnValue_t result = returnvalue::OK;
+  // Initialize the mode table to ensure all devices are in a defined state
+  for (uint8_t idx = 0; idx < NUMBER_RTDS; idx++) {
+    modeTable[idx].setMode(MODE_OFF);
+    modeTable[idx].setSubmode(SUBMODE_NONE);
+  }
+  if (recoveryState == RecoveryState::RECOVERY_IDLE) {
+    result = checkAndHandleHealthStates(mode, submode);
+    if (result == NEED_TO_CHANGE_HEALTH) {
+      return returnvalue::OK;
+    }
+  }
+  if (recoveryState != RecoveryState::RECOVERY_STARTED) {
+    if (mode == DeviceHandlerIF::MODE_NORMAL or mode == MODE_ON) {
+      result = handleNormalOrOnModeCmd(mode, submode);
+    }
+  } else {
+    tcsShortlyUnavailable = true;
+  }
+  HybridIterator<ModeListEntry> tableIter(modeTable.begin(), modeTable.end());
+  executeTable(tableIter);
+  return result;
+}
+
+ReturnValue_t TcsBoardAssembly::checkChildrenStateOn(Mode_t wantedMode, Submode_t wantedSubmode) {
+  int devsInWrongMode = 0;
+  try {
+    for (uint8_t idx = 0; idx < NUMBER_RTDS; idx++) {
+      if (childrenMap.at(helper.rtdInfos[idx].first).mode != wantedMode) {
+        devsInWrongMode++;
+      }
+    }
+  } catch (const std::out_of_range& e) {
+    sif::error << "TcsBoardAssembly: Invalid children map: " << e.what() << std::endl;
+  }
+  if (devsInWrongMode == NUMBER_RTDS) {
+    if (warningSwitch) {
+      sif::warning << "TcsBoardAssembly::checkChildrenStateOn: All devices in wrong mode"
+                   << std::endl;
+      warningSwitch = false;
+    }
+    return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE;
+  }
+  // TODO: Can't really do something other than power cycling if devices in wrong mode.
+  // Might attempt one power-cycle. In any case, trigger an event
+  if (devsInWrongMode > 0) {
+    if (warningSwitch) {
+      sif::warning << "TcsBoardAssembly::checkChildrenStateOn: " << devsInWrongMode << " devices in"
+                   << " wrong mode" << std::endl;
+      warningSwitch = false;
+    }
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t TcsBoardAssembly::isModeCombinationValid(Mode_t mode, Submode_t submode) {
+  if (mode == MODE_ON or mode == MODE_OFF or mode == DeviceHandlerIF::MODE_NORMAL) {
+    return returnvalue::OK;
+  }
+  return HasModesIF::INVALID_MODE;
+}
+
+ReturnValue_t TcsBoardAssembly::handleNormalOrOnModeCmd(Mode_t mode, Submode_t submode) {
+  ReturnValue_t result = returnvalue::OK;
+  bool needsSecondStep = false;
+  Mode_t devMode = 0;
+  object_id_t objId = 0;
+  try {
+    for (uint8_t idx = 0; idx < NUMBER_RTDS; idx++) {
+      devMode = childrenMap.at(helper.rtdInfos[idx].first).mode;
+      objId = helper.rtdInfos[idx].first;
+      if (mode == devMode) {
+        modeTable[idx].setMode(mode);
+      } else if (mode == DeviceHandlerIF::MODE_NORMAL) {
+        if (isUseable(objId, devMode)) {
+          if (devMode == MODE_ON) {
+            modeTable[idx].setMode(mode);
+            modeTable[idx].setSubmode(SUBMODE_NONE);
+          } else {
+            modeTable[idx].setMode(MODE_ON);
+            modeTable[idx].setSubmode(SUBMODE_NONE);
+            if (internalState != STATE_SECOND_STEP) {
+              needsSecondStep = true;
+            }
+          }
+        }
+      } else if (mode == MODE_ON) {
+        if (isUseable(objId, devMode)) {
+          modeTable[idx].setMode(MODE_ON);
+          modeTable[idx].setSubmode(SUBMODE_NONE);
+        }
+      }
+    }
+  } catch (const std::out_of_range& e) {
+    sif::error << "TcsBoardAssembly: Invalid children map: " << e.what() << std::endl;
+  }
+  if (needsSecondStep) {
+    result = NEED_SECOND_STEP;
+  }
+  return result;
+}
+
+bool TcsBoardAssembly::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 TcsBoardAssembly::handleChildrenLostMode(ReturnValue_t result) {
+  triggerEvent(CHILDREN_LOST_MODE, result);
+  startTransition(mode, submode);
+}
+
+ReturnValue_t TcsBoardAssembly::checkAndHandleHealthStates(Mode_t deviceMode,
+                                                           Submode_t deviceSubmode) {
+  ReturnValue_t status = returnvalue::OK;
+  for (const auto& dev : helper.rtdInfos) {
+    HealthState health = healthHelper.healthTable->getHealth(dev.first);
+    if (health == HealthState::HEALTHY) {
+      return returnvalue::OK;
+    }
+  }
+
+  for (const auto& dev : helper.rtdInfos) {
+    HealthState health = healthHelper.healthTable->getHealth(dev.first);
+    if (health == FAULTY or health == PERMANENT_FAULTY) {
+      status = NEED_TO_CHANGE_HEALTH;
+    } else if (health == EXTERNAL_CONTROL) {
+      modeHelper.setForced(true);
+    }
+  }
+  return status;
+}
+
+bool TcsBoardAssembly::checkAndHandleRecovery() {
+  bool recoveryPending = SharedPowerAssemblyBase::checkAndHandleRecovery();
+  tcsShortlyUnavailable = recoveryPending;
+  return recoveryPending;
+}
+
+void TcsBoardAssembly::handleModeTransitionFailed(ReturnValue_t result) {
+  if (targetMode == MODE_OFF) {
+    AssemblyBase::handleModeTransitionFailed(result);
+  } else {
+    // To avoid transitioning back to off
+    triggerEvent(MODE_TRANSITION_FAILED, result);
+  }
+}