Merge remote-tracking branch 'origin/main' into scex-bugfix

mission/controller/CMakeLists.txt

@@ -1,6 +1,7 @@
 if(TGT_BSP MATCHES "arm/q7s" OR TGT_BSP MATCHES "")
   target_sources(${LIB_EIVE_MISSION} PRIVATE ThermalController.cpp
-                                             AcsController.cpp)
+                                             AcsController.cpp
+                                             PowerController.cpp)
 endif()
 
 add_subdirectory(acs)

mission/controller/PowerController.cpp

@@ -0,0 +1,372 @@
+#include <mission/controller/PowerController.h>
+
+PowerController::PowerController(object_id_t objectId, bool enableHkSets)
+    : ExtendedControllerBase(objectId),
+      enableHkSets(enableHkSets),
+      parameterHelper(this),
+      pwrCtrlCoreHk(this),
+      enablePl(this) {}
+
+ReturnValue_t PowerController::initialize() {
+  ReturnValue_t result = parameterHelper.initialize();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  return ExtendedControllerBase::initialize();
+}
+
+ReturnValue_t PowerController::handleCommandMessage(CommandMessage *message) {
+  ReturnValue_t result = actionHelper.handleActionMessage(message);
+  if (result == returnvalue::OK) {
+    return result;
+  }
+  result = parameterHelper.handleParameterMessage(message);
+  if (result == returnvalue::OK) {
+    return result;
+  }
+  return result;
+}
+
+MessageQueueId_t PowerController::getCommandQueue() const { return commandQueue->getId(); }
+
+ReturnValue_t PowerController::getParameter(uint8_t domainId, uint8_t parameterId,
+                                            ParameterWrapper *parameterWrapper,
+                                            const ParameterWrapper *newValues,
+                                            uint16_t startAtIndex) {
+  switch (domainId) {
+    case 0x0:  // direct members
+      switch (parameterId) {
+        case 0x0:
+          parameterWrapper->set(batteryInternalResistance);
+          break;
+        case 0x1:
+          parameterWrapper->set(batteryMaximumCapacity);
+          break;
+        case 0x2: {
+          float oldCoulombCounterVoltageUpperThreshold = coulombCounterVoltageUpperThreshold;
+          ReturnValue_t result = newValues->getElement(&coulombCounterVoltageUpperThreshold);
+          if (result != returnvalue::OK) {
+            coulombCounterVoltageUpperThreshold = oldCoulombCounterVoltageUpperThreshold;
+            return result;
+          }
+          result = calculateCoulombCounterChargeUpperThreshold();
+          if (result != returnvalue::OK) {
+            coulombCounterVoltageUpperThreshold = oldCoulombCounterVoltageUpperThreshold;
+            return result;
+          }
+          parameterWrapper->set(coulombCounterVoltageUpperThreshold);
+          break;
+        }
+        case 0x3:
+          parameterWrapper->set(maxAllowedTimeDiff);
+          break;
+        case 0x4:
+          parameterWrapper->set(payloadOpLimitOn);
+          break;
+        case 0x5:
+          parameterWrapper->set(payloadOpLimitLow);
+          break;
+        case 0x6:
+          parameterWrapper->set(higherModesLimit);
+          break;
+        default:
+          return INVALID_IDENTIFIER_ID;
+      }
+      break;
+    default:
+      return INVALID_DOMAIN_ID;
+  };
+  return returnvalue::OK;
+}
+
+void PowerController::performControlOperation() {
+  switch (internalState) {
+    case InternalState::STARTUP: {
+      initialCountdown.resetTimer();
+      internalState = InternalState::INITIAL_DELAY;
+      return;
+    }
+    case InternalState::INITIAL_DELAY: {
+      if (initialCountdown.hasTimedOut()) {
+        internalState = InternalState::INIT;
+      }
+      return;
+    }
+    case InternalState::INIT: {
+      ReturnValue_t result = calculateCoulombCounterChargeUpperThreshold();
+      if (result == returnvalue::OK) {
+        internalState = InternalState::READY;
+      }
+      return;
+    }
+    case InternalState::READY: {
+      if (mode != MODE_OFF) {
+        calculateStateOfCharge();
+        if (mode == MODE_NORMAL) {
+          watchStateOfCharge();
+        } else {
+          PoolReadGuard pg(&enablePl);
+          if (pg.getReadResult() == returnvalue::OK) {
+            enablePl.setValidity(false, true);
+          }
+        }
+      }
+      break;
+    }
+    default:
+      break;
+  }
+}
+
+ReturnValue_t PowerController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+                                                       LocalDataPoolManager &poolManager) {
+  localDataPoolMap.emplace(pwrctrl::PoolIds::TOTAL_BATTERY_CURRENT, new PoolEntry<int16_t>({0}));
+  localDataPoolMap.emplace(pwrctrl::PoolIds::OPEN_CIRCUIT_VOLTAGE_CHARGE,
+                           new PoolEntry<float>({0.0}));
+  localDataPoolMap.emplace(pwrctrl::PoolIds::COULOMB_COUNTER_CHARGE, new PoolEntry<float>({0.0}));
+  poolManager.subscribeForRegularPeriodicPacket({pwrCtrlCoreHk.getSid(), enableHkSets, 60.0});
+  localDataPoolMap.emplace(pwrctrl::PoolIds::PAYLOAD_FLAG, new PoolEntry<uint8_t>({false}));
+  poolManager.subscribeForRegularPeriodicPacket({enablePl.getSid(), false, 60.0});
+  return returnvalue::OK;
+}
+
+LocalPoolDataSetBase *PowerController::getDataSetHandle(sid_t sid) {
+  switch (sid.ownerSetId) {
+    case pwrctrl::CORE_HK:
+      return &pwrCtrlCoreHk;
+    case pwrctrl::ENABLE_PL:
+      return &enablePl;
+    default:
+      return nullptr;
+  }
+  return nullptr;
+}
+
+ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode,
+                                                uint32_t *msToReachTheMode) {
+  if (mode == MODE_OFF or mode == MODE_ON or mode == MODE_NORMAL) {
+    if (submode == SUBMODE_NONE) {
+      return returnvalue::OK;
+    } else {
+      return INVALID_SUBMODE;
+    }
+  }
+  return INVALID_MODE;
+}
+
+void PowerController::calculateStateOfCharge() {
+  // get time
+  Clock::getClock_timeval(&now);
+
+  // update EPS HK values
+  ReturnValue_t result = updateEpsData();
+  if (result != returnvalue::OK) {
+    triggerEvent(power::DATASET_READ_FAILED);
+    sif::error << "Power Controller::Reading of Datasets has failed" << std::endl;
+    {
+      PoolReadGuard pg(&pwrCtrlCoreHk);
+      if (pg.getReadResult() == returnvalue::OK) {
+        pwrCtrlCoreHk.totalBatteryCurrent.value = INVALID_TOTAL_BATTERY_CURRENT;
+        pwrCtrlCoreHk.openCircuitVoltageCharge.value = INVALID_SOC;
+        pwrCtrlCoreHk.coulombCounterCharge.value = INVALID_SOC;
+        pwrCtrlCoreHk.setValidity(false, true);
+      }
+    }
+    // store time for next run
+    oldTime = now;
+    return;
+  }
+
+  // calculate total battery current
+  iBat = p60CoreHk.batteryCurrent.value + bpxBatteryHk.dischargeCurrent.value;
+
+  result = calculateOpenCircuitVoltageCharge();
+  if (result != returnvalue::OK) {
+    // notifying events have already been triggered
+    {
+      PoolReadGuard pg(&pwrCtrlCoreHk);
+      if (pg.getReadResult() == returnvalue::OK) {
+        pwrCtrlCoreHk.totalBatteryCurrent.value = iBat;
+        pwrCtrlCoreHk.totalBatteryCurrent.setValid(true);
+        pwrCtrlCoreHk.openCircuitVoltageCharge.value = INVALID_SOC;
+        pwrCtrlCoreHk.openCircuitVoltageCharge.setValid(false);
+        pwrCtrlCoreHk.coulombCounterCharge.value = INVALID_SOC;
+        pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
+      }
+    }
+    // store time for next run
+    oldTime = now;
+    return;
+  }
+
+  result = calculateCoulombCounterCharge();
+  if (result != returnvalue::OK) {
+    // notifying events have already been triggered
+    {
+      PoolReadGuard pg(&pwrCtrlCoreHk);
+      if (pg.getReadResult() == returnvalue::OK) {
+        pwrCtrlCoreHk.totalBatteryCurrent.value = iBat;
+        pwrCtrlCoreHk.totalBatteryCurrent.setValid(true);
+        pwrCtrlCoreHk.openCircuitVoltageCharge.value =
+            charge2stateOfCharge(openCircuitVoltageCharge, false);
+        pwrCtrlCoreHk.openCircuitVoltageCharge.setValid(true);
+        pwrCtrlCoreHk.coulombCounterCharge.value = INVALID_SOC;
+        pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
+      }
+    }
+    // store time for next run
+    oldTime = now;
+    return;
+  }
+
+  // commit to dataset
+  {
+    PoolReadGuard pg(&pwrCtrlCoreHk);
+    if (pg.getReadResult() == returnvalue::OK) {
+      pwrCtrlCoreHk.totalBatteryCurrent.value = iBat;
+      pwrCtrlCoreHk.openCircuitVoltageCharge.value =
+          charge2stateOfCharge(openCircuitVoltageCharge, false);
+      pwrCtrlCoreHk.coulombCounterCharge.value = charge2stateOfCharge(coulombCounterCharge, true);
+      pwrCtrlCoreHk.setValidity(true, true);
+    }
+  }
+  // store time for next run
+  oldTime = now;
+}
+
+void PowerController::watchStateOfCharge() {
+  if (pwrCtrlCoreHk.coulombCounterCharge.isValid()) {
+    if (pwrCtrlCoreHk.coulombCounterCharge.value < payloadOpLimitOn) {
+      PoolReadGuard pg(&enablePl);
+      if (pg.getReadResult() == returnvalue::OK) {
+        enablePl.plUseAllowed.value = false;
+        enablePl.setValidity(true, true);
+      }
+    } else {
+      PoolReadGuard pg(&enablePl);
+      if (pg.getReadResult() == returnvalue::OK) {
+        enablePl.plUseAllowed.value = true;
+        enablePl.setValidity(true, true);
+      }
+    }
+    if (not pwrLvlLowFlag and pwrCtrlCoreHk.coulombCounterCharge.value < payloadOpLimitLow) {
+      triggerEvent(power::POWER_LEVEL_LOW);
+      pwrLvlLowFlag = true;
+    } else if (pwrLvlLowFlag and pwrCtrlCoreHk.coulombCounterCharge.value > payloadOpLimitLow) {
+      pwrLvlLowFlag = false;
+    }
+    if (not pwrLvlCriticalFlag and pwrCtrlCoreHk.coulombCounterCharge.value < higherModesLimit) {
+      triggerEvent(power::POWER_LEVEL_CRITICAL);
+      pwrLvlCriticalFlag = true;
+    } else if (pwrLvlCriticalFlag and pwrCtrlCoreHk.coulombCounterCharge.value > higherModesLimit) {
+      pwrLvlCriticalFlag = false;
+    }
+  } else {
+    PoolReadGuard pg(&enablePl);
+    if (pg.getReadResult() == returnvalue::OK) {
+      enablePl.plUseAllowed.value = false;
+      enablePl.setValidity(true, true);
+    }
+  }
+}
+
+ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() {
+  float vBatCorrected =
+      (bpxBatteryHk.battVoltage.value - iBat * batteryInternalResistance) * CONVERT_FROM_MILLI;
+  uint8_t lookUpTableIdx = LOOK_UP_TABLE_MAX_IDX;
+  ReturnValue_t result = lookUpTableOcvIdxFinder(vBatCorrected, lookUpTableIdx, false);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  openCircuitVoltageCharge = linearInterpolation(
+      vBatCorrected, lookUpTableOcv[1][lookUpTableIdx], lookUpTableOcv[1][lookUpTableIdx + 1],
+      lookUpTableOcv[0][lookUpTableIdx], lookUpTableOcv[0][lookUpTableIdx + 1]);
+  return returnvalue::OK;
+}
+
+ReturnValue_t PowerController::calculateCoulombCounterCharge() {
+  double timeDiff = timevalOperations::toDouble(now - oldTime);
+  if (timeDiff > maxAllowedTimeDiff) {
+    // should not be a permanent state so no spam protection required
+    triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast<uint32_t>(timeDiff * 10));
+    sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDiff
+               << std::endl;
+    return returnvalue::FAILED;
+  }
+  if (not pwrCtrlCoreHk.coulombCounterCharge.isValid()) {
+    coulombCounterCharge = openCircuitVoltageCharge;
+  } else {
+    coulombCounterCharge =
+        coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDiff * SECONDS_TO_HOURS;
+    if (coulombCounterCharge >= coulombCounterChargeUpperThreshold) {
+      coulombCounterCharge = coulombCounterChargeUpperThreshold;
+    }
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t PowerController::updateEpsData() {
+  std::vector<ReturnValue_t> results;
+  {
+    PoolReadGuard pgBat(&bpxBatteryHk);
+    results.push_back(pgBat.getReadResult());
+  }
+  {
+    PoolReadGuard pgP60(&p60CoreHk);
+    results.push_back(pgP60.getReadResult());
+  }
+  for (const auto &result : results) {
+    if (result != returnvalue::OK) {
+      return result;
+    }
+  }
+  return returnvalue::OK;
+}
+
+float PowerController::charge2stateOfCharge(float capacity, bool coulombCounter) {
+  if (coulombCounter) {
+    return capacity / coulombCounterChargeUpperThreshold;
+  }
+  return capacity / batteryMaximumCapacity;
+}
+
+float PowerController::linearInterpolation(float x, float x0, float x1, float y0, float y1) {
+  return y0 + (x - x0) * (y1 - y0) / (x1 - x0);
+}
+
+ReturnValue_t PowerController::lookUpTableOcvIdxFinder(float voltage, uint8_t &idx, bool paramCmd) {
+  if (voltage >= lookUpTableOcv[1][99]) {
+    if (not voltageOutOfBoundsFlag and not paramCmd) {
+      triggerEvent(power::VOLTAGE_OUT_OF_BOUNDS, 0, static_cast<uint32_t>(voltage * 10));
+      voltageOutOfBoundsFlag = true;
+    }
+    sif::error << "Power Controller::Voltage is too high: " << voltage << std::endl;
+    return returnvalue::FAILED;
+  } else if (voltage <= lookUpTableOcv[1][0]) {
+    if (not voltageOutOfBoundsFlag and not paramCmd) {
+      triggerEvent(power::VOLTAGE_OUT_OF_BOUNDS, 1, static_cast<uint32_t>(voltage * 10));
+      voltageOutOfBoundsFlag = true;
+    }
+    sif::error << "Power Controller::Voltage is too low: " << voltage << std::endl;
+    return returnvalue::FAILED;
+  }
+  voltageOutOfBoundsFlag = false;
+  while (lookUpTableOcv[1][idx] > voltage) {
+    idx--;
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t PowerController::calculateCoulombCounterChargeUpperThreshold() {
+  uint8_t lookUpTableIdx = LOOK_UP_TABLE_MAX_IDX;
+  ReturnValue_t result =
+      lookUpTableOcvIdxFinder(coulombCounterVoltageUpperThreshold, lookUpTableIdx, true);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  coulombCounterChargeUpperThreshold =
+      linearInterpolation(coulombCounterVoltageUpperThreshold, lookUpTableOcv[1][lookUpTableIdx],
+                          lookUpTableOcv[1][lookUpTableIdx + 1], lookUpTableOcv[0][lookUpTableIdx],
+                          lookUpTableOcv[0][lookUpTableIdx + 1]);
+  return returnvalue::OK;
+}

mission/controller/PowerController.h

@@ -0,0 +1,131 @@
+#ifndef MISSION_CONTROLLER_POWERCONTROLLER_H_
+#define MISSION_CONTROLLER_POWERCONTROLLER_H_
+
+#include <eive/objects.h>
+#include <fsfw/controller/ExtendedControllerBase.h>
+#include <fsfw/datapool/PoolReadGuard.h>
+#include <fsfw/parameters/ParameterHelper.h>
+#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
+#include <mission/controller/controllerdefinitions/PowerCtrlDefinitions.h>
+#include <mission/power/bpxBattDefs.h>
+#include <mission/power/gsDefs.h>
+
+#include <cmath>
+
+class PowerController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
+ public:
+  static constexpr dur_millis_t INIT_DELAY = 500;
+
+  PowerController(object_id_t objectId, bool enableHkSets);
+
+  MessageQueueId_t getCommandQueue() const;
+  ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId,
+                             ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues,
+                             uint16_t startAtIndex) override;
+
+ private:
+  bool enableHkSets = false;
+  ParameterHelper parameterHelper;
+
+  enum class InternalState { STARTUP, INITIAL_DELAY, INIT, READY };
+  InternalState internalState = InternalState::STARTUP;
+
+  // Initial delay to make sure all pool variables have been initialized their owners
+  Countdown initialCountdown = Countdown(INIT_DELAY);
+
+  ReturnValue_t initialize() override;
+  ReturnValue_t handleCommandMessage(CommandMessage* message) override;
+  ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
+                                        LocalDataPoolManager& poolManager) override;
+  LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
+  ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
+                                 uint32_t* msToReachTheMode) override;
+  void performControlOperation() override;
+
+  void calculateStateOfCharge();
+  void watchStateOfCharge();
+  ReturnValue_t calculateOpenCircuitVoltageCharge();
+  ReturnValue_t calculateCoulombCounterCharge();
+  ReturnValue_t updateEpsData();
+  float charge2stateOfCharge(float capacity, bool coulombCounter);
+  ReturnValue_t lookUpTableOcvIdxFinder(float voltage, uint8_t& idx, bool paramCmd);
+  float linearInterpolation(float x, float x0, float x1, float y0, float y1);
+  ReturnValue_t calculateCoulombCounterChargeUpperThreshold();
+
+  // Parameters
+  float batteryInternalResistance = 70.0 / 2.0 / 1000.0;  // [Ohm]
+  float batteryMaximumCapacity = 2.6 * 2;                 // [Ah]
+  float coulombCounterVoltageUpperThreshold = 16.2;       // [V]
+  double maxAllowedTimeDiff = 1.5;                        // [s]
+  float payloadOpLimitOn = 0.90;                          // [%]
+  float payloadOpLimitLow = 0.75;                         // [%]
+  float higherModesLimit = 0.6;                           // [%]
+
+  // OCV Look-up-Table {[Ah],[V]}
+  static constexpr uint8_t LOOK_UP_TABLE_MAX_IDX = 99;
+  float lookUpTableOcv[2][100] = {
+      {0.00000000e+00, 3.16227766e-04, 4.52809661e-04, 6.48382625e-04, 9.28425483e-04,
+       1.32942162e-03, 1.90361194e-03, 2.72580074e-03, 3.90310099e-03, 5.58888885e-03,
+       8.00278514e-03, 1.14592671e-02, 1.64086377e-02, 2.34956903e-02, 3.36437110e-02,
+       4.81747620e-02, 6.89819174e-02, 9.87758887e-02, 1.41438170e-01, 2.02526713e-01,
+       2.90000000e-01, 3.00000000e-01, 3.62820513e-01, 4.25641026e-01, 4.88461538e-01,
+       5.51282051e-01, 6.14102564e-01, 6.76923077e-01, 7.39743590e-01, 8.02564103e-01,
+       8.65384615e-01, 9.28205128e-01, 9.91025641e-01, 1.05384615e+00, 1.11666667e+00,
+       1.17948718e+00, 1.24230769e+00, 1.30512821e+00, 1.36794872e+00, 1.43076923e+00,
+       1.49358974e+00, 1.55641026e+00, 1.61923077e+00, 1.68205128e+00, 1.74487179e+00,
+       1.80769231e+00, 1.87051282e+00, 1.93333333e+00, 1.99615385e+00, 2.05897436e+00,
+       2.12179487e+00, 2.18461538e+00, 2.24743590e+00, 2.31025641e+00, 2.37307692e+00,
+       2.43589744e+00, 2.49871795e+00, 2.56153846e+00, 2.62435897e+00, 2.68717949e+00,
+       2.75000000e+00, 2.81282051e+00, 2.87564103e+00, 2.93846154e+00, 3.00128205e+00,
+       3.06410256e+00, 3.12692308e+00, 3.18974359e+00, 3.25256410e+00, 3.31538462e+00,
+       3.37820513e+00, 3.44102564e+00, 3.50384615e+00, 3.56666667e+00, 3.62948718e+00,
+       3.69230769e+00, 3.75512821e+00, 3.81794872e+00, 3.88076923e+00, 3.94358974e+00,
+       4.00641026e+00, 4.06923077e+00, 4.13205128e+00, 4.19487179e+00, 4.25769231e+00,
+       4.32051282e+00, 4.38333333e+00, 4.44615385e+00, 4.50897436e+00, 4.57179487e+00,
+       4.63461538e+00, 4.69743590e+00, 4.76025641e+00, 4.82307692e+00, 4.88589744e+00,
+       4.94871795e+00, 5.01153846e+00, 5.07435897e+00, 5.13717949e+00, 5.20000000e+00},
+      {12.52033533, 12.58720948, 12.61609309, 12.65612591, 12.67105282, 12.69242681, 12.72303245,
+       12.76685696, 12.80313768, 12.83600741, 12.8830739,  12.94720576, 13.00112629, 13.07833563,
+       13.17486308, 13.27128842, 13.37713879, 13.49275604, 13.60395193, 13.68708863, 13.75196335,
+       13.7582376,  13.79298643, 13.82885799, 13.87028849, 13.91585718, 13.96701874, 14.02343574,
+       14.07665641, 14.12626342, 14.1675095,  14.20582917, 14.23342159, 14.25724476, 14.27264301,
+       14.28922389, 14.30898535, 14.32750837, 14.34358057, 14.35965277, 14.37698366, 14.3943261,
+       14.41079196, 14.42679817, 14.44261008, 14.45771025, 14.47281042, 14.48751461, 14.50193089,
+       14.5164887,  14.53193477, 14.54738084, 14.56341235, 14.58054578, 14.59799552, 14.61632769,
+       14.63716465, 14.66935073, 14.70511347, 14.74315094, 14.77251031, 14.80005585, 14.8315427,
+       14.86078285, 14.89444687, 14.93495892, 14.97114013, 15.01055751, 15.0538516,  15.09698825,
+       15.14850029, 15.18947994, 15.24249483, 15.28521713, 15.335695,   15.37950723, 15.43241224,
+       15.48082213, 15.53314287, 15.58907248, 15.64030253, 15.68385331, 15.74149122, 15.80051882,
+       15.84959348, 15.90443241, 15.95743724, 16.01283068, 16.07629253, 16.13470801, 16.1890518,
+       16.24200781, 16.30521118, 16.37368429, 16.43661267, 16.49604875, 16.56223813, 16.62741412,
+       16.67249918, 16.74926904}};
+
+  // Variables
+  timeval now;
+  timeval oldTime;
+  int16_t iBat = 0;                                    // [mA]
+  float openCircuitVoltageCharge = 0.0;                // [Ah]
+  float coulombCounterCharge = 0.0;                    // [Ah]
+  float coulombCounterChargeUpperThreshold = 0.0;      // [Ah]
+  float oldCoulombCounterVoltageUpperThreshold = 0.0;  // [V]
+
+  static constexpr float CONVERT_FROM_MILLI = 1e-3;
+  static constexpr float SECONDS_TO_HOURS = 1. / (60. * 60.);
+
+  static constexpr int16_t INVALID_TOTAL_BATTERY_CURRENT = 0;
+  static constexpr float INVALID_SOC = -1;
+
+  bool pwrLvlLowFlag = false;
+  bool pwrLvlCriticalFlag = false;
+  bool voltageOutOfBoundsFlag = false;
+
+  // HK Datasets for Calculation
+  BpxBatteryHk bpxBatteryHk = BpxBatteryHk(objects::BPX_BATT_HANDLER);
+  P60Dock::CoreHkSet p60CoreHk = P60Dock::CoreHkSet(objects::P60DOCK_HANDLER);
+  // Output Dataset
+  pwrctrl::CoreHk pwrCtrlCoreHk;
+  // Dataset for PL Flag
+  pwrctrl::EnablePl enablePl;
+};
+
+#endif /* MISSION_CONTROLLER_POWERCONTROLLER_H_ */

mission/controller/controllerdefinitions/PowerCtrlDefinitions.h

@@ -0,0 +1,51 @@
+#ifndef MISSION_CONTROLLER_CONTROLLERDEFINITIONS_POWERCTRLDEFINITIONS_H_
+#define MISSION_CONTROLLER_CONTROLLERDEFINITIONS_POWERCTRLDEFINITIONS_H_
+
+#include <fsfw/datapool/PoolReadGuard.h>
+#include <fsfw/datapoollocal/StaticLocalDataSet.h>
+#include <fsfw/datapoollocal/localPoolDefinitions.h>
+#include <mission/power/defs.h>
+
+#include <cstdint>
+
+namespace pwrctrl {
+
+enum SetIds : uint32_t { CORE_HK, ENABLE_PL };
+
+enum PoolIds : lp_id_t {
+  TOTAL_BATTERY_CURRENT,
+  OPEN_CIRCUIT_VOLTAGE_CHARGE,
+  COULOMB_COUNTER_CHARGE,
+  PAYLOAD_FLAG
+};
+
+static constexpr uint8_t CORE_HK_ENTRIES = 3;
+static constexpr uint8_t ENABLE_PL_ENTRIES = 1;
+
+class CoreHk : public StaticLocalDataSet<CORE_HK_ENTRIES> {
+ public:
+  CoreHk(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CORE_HK) {}
+
+  lp_var_t<int16_t> totalBatteryCurrent =
+      lp_var_t<int16_t>(sid.objectId, TOTAL_BATTERY_CURRENT, this);
+  lp_var_t<float> openCircuitVoltageCharge =
+      lp_var_t<float>(sid.objectId, OPEN_CIRCUIT_VOLTAGE_CHARGE, this);
+  lp_var_t<float> coulombCounterCharge =
+      lp_var_t<float>(sid.objectId, COULOMB_COUNTER_CHARGE, this);
+
+ private:
+};
+
+class EnablePl : public StaticLocalDataSet<ENABLE_PL_ENTRIES> {
+ public:
+  EnablePl(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, ENABLE_PL) {}
+  EnablePl(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, ENABLE_PL)) {}
+
+  lp_var_t<uint8_t> plUseAllowed = lp_var_t<uint8_t>(sid.objectId, PAYLOAD_FLAG, this);
+
+ private:
+};
+
+}  // namespace pwrctrl
+
+#endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_POWERCTRLDEFINITIONS_H_ */

mission/payload/PayloadPcduHandler.cpp

@@ -564,6 +564,19 @@ bool PayloadPcduHandler::checkCurrent(float val, float upperBound, Event event)
   return true;
 }
 
+ReturnValue_t PayloadPcduHandler::checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode,
+                                                   uint32_t* msToReachTheMode) {
+  if (commandedMode != MODE_OFF) {
+    PoolReadGuard pg(&enablePl);
+    if (pg.getReadResult() == returnvalue::OK) {
+      if (enablePl.plUseAllowed.isValid() and not enablePl.plUseAllowed.value) {
+        return NON_OP_STATE_OF_CHARGE;
+      }
+    }
+  }
+  return DeviceHandlerBase::checkModeCommand(commandedMode, commandedSubmode, msToReachTheMode);
+}
+
 ReturnValue_t PayloadPcduHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
   using namespace plpcdu;
   if (mode == MODE_NORMAL) {

mission/payload/PayloadPcduHandler.h

@@ -4,6 +4,7 @@
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
 #include <fsfw/globalfunctions/PeriodicOperationDivider.h>
 #include <fsfw/timemanager/Countdown.h>
+#include <mission/controller/controllerdefinitions/PowerCtrlDefinitions.h>
 #include <mission/payload/payloadPcduDefinitions.h>
 #include <mission/system/objects/Stack5VHandler.h>
 
@@ -170,6 +171,10 @@ class PayloadPcduHandler : public DeviceHandlerBase {
   ReturnValue_t handleDoubleParamUpdate(std::string key, ParameterWrapper* parameterWrapper,
                                         const ParameterWrapper* newValues);
   LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
+
+  pwrctrl::EnablePl enablePl = pwrctrl::EnablePl(objects::POWER_CONTROLLER);
+  ReturnValue_t checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode,
+                                 uint32_t* msToReachTheMode) override;
 };
 
 #endif /* LINUX_DEVICES_PLPCDUHANDLER_H_ */

mission/pollingSeqTables.cpp

@@ -133,6 +133,7 @@ ReturnValue_t pst::pstGompaceCan(FixedTimeslotTaskIF *thisSequence) {
   thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.5, DeviceHandlerIF::GET_READ);
   thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.5, DeviceHandlerIF::GET_READ);
   thisSequence->addSlot(objects::ACU_HANDLER, length * 0.5, DeviceHandlerIF::GET_READ);
+
   if (thisSequence->checkSequence() != returnvalue::OK) {
     sif::error << "GomSpace PST initialization failed" << std::endl;
     return returnvalue::FAILED;
@@ -602,5 +603,8 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
                         DeviceHandlerIF::SEND_READ);
   thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
                         DeviceHandlerIF::GET_READ);
+
+  thisSequence->addSlot(objects::POWER_CONTROLLER, length * config::spiSched::SCHED_BLOCK_10_PERIOD,
+                        0);
   return returnvalue::OK;
 }

mission/power/defs.h

@@ -47,6 +47,23 @@ static constexpr Event SWITCHING_Q7S_DENIED = event::makeEvent(SUBSYSTEM_ID, 2,
 
 static constexpr Event FDIR_REACTION_IGNORED = event::makeEvent(SUBSYSTEM_ID, 3, severity::MEDIUM);
 
+//! [EXPORT] : [COMMENT] The dataset read for the inputs of the Power Controller has failed.
+static constexpr Event DATASET_READ_FAILED = event::makeEvent(SUBSYSTEM_ID, 4, severity::INFO);
+//! [EXPORT] : [COMMENT] The battery voltage read is out of the bounds in which it is supposed to
+//! be.
+//! P1: 1 too high, 0 too low
+//! P2: voltage in V * 10
+static constexpr Event VOLTAGE_OUT_OF_BOUNDS = event::makeEvent(SUBSYSTEM_ID, 5, severity::HIGH);
+//! [EXPORT] : [COMMENT] Time difference for Coulomb Counter was too large.
+//! P1: time in s * 10
+static constexpr Event TIMEDELTA_OUT_OF_BOUNDS = event::makeEvent(SUBSYSTEM_ID, 6, severity::LOW);
+//! [EXPORT] : [COMMENT] The State of Charge is below the limit for payload use. Setting Payload to
+//! faulty.
+static constexpr Event POWER_LEVEL_LOW = event::makeEvent(SUBSYSTEM_ID, 7, severity::HIGH);
+//! [EXPORT] : [COMMENT] The State of Charge is below the limit for higher modes. Setting Reaction
+//! Wheels to faulty.
+static constexpr Event POWER_LEVEL_CRITICAL = event::makeEvent(SUBSYSTEM_ID, 8, severity::HIGH);
+
 enum class States { IDLE, SWITCHING_POWER, CHECKING_POWER, MODE_COMMANDING };
 enum class OpCodes { NONE, TO_OFF_DONE, TO_NOT_OFF_DONE, TIMEOUT_OCCURED };
 

mission/system/EiveSystem.cpp

@@ -81,6 +81,55 @@ ReturnValue_t EiveSystem::initialize() {
     return result;
   }
 
+  auto* plSs = ObjectManager::instance()->get<HasModesIF>(objects::PL_SUBSYSTEM);
+  if (plSs == nullptr) {
+    return ObjectManager::CHILD_INIT_FAILED;
+  }
+  plSsQueueId = plSs->getCommandQueue();
+
+  auto* plPcdu = ObjectManager::instance()->get<HasHealthIF>(objects::PLPCDU_HANDLER);
+  if (plPcdu == nullptr) {
+    return ObjectManager::CHILD_INIT_FAILED;
+  }
+  plPcduQueueId = plPcdu->getCommandQueue();
+
+  auto* plocMpsoc = ObjectManager::instance()->get<HasHealthIF>(objects::PLOC_MPSOC_HANDLER);
+  if (plocMpsoc == nullptr) {
+    return ObjectManager::CHILD_INIT_FAILED;
+  }
+  plocMpsocQueueId = plocMpsoc->getCommandQueue();
+
+  auto* plocSupervisor =
+      ObjectManager::instance()->get<HasHealthIF>(objects::PLOC_SUPERVISOR_HANDLER);
+  if (plocSupervisor == nullptr) {
+    return ObjectManager::CHILD_INIT_FAILED;
+  }
+  plocSupervisorQueueId = plocSupervisor->getCommandQueue();
+
+  auto* camera = ObjectManager::instance()->get<HasHealthIF>(objects::CAM_SWITCHER);
+  if (camera == nullptr) {
+    return ObjectManager::CHILD_INIT_FAILED;
+  }
+  cameraQueueId = camera->getCommandQueue();
+
+  auto* scex = ObjectManager::instance()->get<HasHealthIF>(objects::SCEX);
+  if (scex == nullptr) {
+    return ObjectManager::CHILD_INIT_FAILED;
+  }
+  scexQueueId = scex->getCommandQueue();
+
+  auto* radSensor = ObjectManager::instance()->get<HasHealthIF>(objects::RAD_SENSOR);
+  if (radSensor == nullptr) {
+    return ObjectManager::CHILD_INIT_FAILED;
+  }
+  radSensorQueueId = radSensor->getCommandQueue();
+
+  auto* str = ObjectManager::instance()->get<HasHealthIF>(objects::STAR_TRACKER);
+  if (str == nullptr) {
+    return ObjectManager::CHILD_INIT_FAILED;
+  }
+  strQueueId = str->getCommandQueue();
+
   auto* bpxDest = ObjectManager::instance()->get<HasActionsIF>(objects::BPX_BATT_HANDLER);
   if (bpxDest == nullptr) {
     return ObjectManager::CHILD_INIT_FAILED;
@@ -120,6 +169,8 @@ ReturnValue_t EiveSystem::initialize() {
   manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::OBC_OVERHEATING));
   manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::MGT_OVERHEATING));
   manager->subscribeToEvent(eventQueue->getId(), event::getEventId(pdec::INVALID_TC_FRAME));
+  manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_LOW));
+  manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_CRITICAL));
   return Subsystem::initialize();
 }
 
@@ -151,6 +202,20 @@ void EiveSystem::handleEventMessages() {
             commandSelfToSafe();
             break;
           }
+          case power::POWER_LEVEL_LOW: {
+            forceOffPayload();
+            break;
+          }
+          case power::POWER_LEVEL_CRITICAL:
+            CommandMessage msg;
+            HealthMessage::setHealthMessage(&msg, HealthMessage::HEALTH_SET, HasHealthIF::FAULTY);
+            ReturnValue_t result = MessageQueueSenderIF::sendMessage(
+                strQueueId, &msg, MessageQueueIF::NO_QUEUE, false);
+            if (result != returnvalue::OK) {
+              sif::error << "EIVE System: Sending FAULTY command to STR Assembly failed"
+                         << std::endl;
+            }
+            break;
         }
         break;
       default:
@@ -336,6 +401,42 @@ void EiveSystem::pdecRecoveryLogic() {
   }
 }
 
+void EiveSystem::forceOffPayload() {
+  CommandMessage msg;
+  // set PL to faulty
+  HealthMessage::setHealthMessage(&msg, HealthMessage::HEALTH_SET, HasHealthIF::FAULTY);
+
+  ReturnValue_t result = commandQueue->sendMessage(plPcduQueueId, &msg);
+  if (result != returnvalue::OK) {
+    sif::error << "EIVE System: Sending FAULTY command to PL PCDU failed" << std::endl;
+  }
+
+  result = commandQueue->sendMessage(plocMpsocQueueId, &msg);
+  if (result != returnvalue::OK) {
+    sif::error << "EIVE System: Sending FAULTY command to PLOC MPSOC failed" << std::endl;
+  }
+
+  result = commandQueue->sendMessage(plocSupervisorQueueId, &msg);
+  if (result != returnvalue::OK) {
+    sif::error << "EIVE System: Sending FAULTY command to PLOC SUPERVISOR failed" << std::endl;
+  }
+
+  result = commandQueue->sendMessage(cameraQueueId, &msg);
+  if (result != returnvalue::OK) {
+    sif::error << "EIVE System: Sending FAULTY command to PL CAM failed" << std::endl;
+  }
+
+  result = commandQueue->sendMessage(scexQueueId, &msg);
+  if (result != returnvalue::OK) {
+    sif::error << "EIVE System: Sending FAULTY command to SCEX failed" << std::endl;
+  }
+
+  result = commandQueue->sendMessage(radSensorQueueId, &msg);
+  if (result != returnvalue::OK) {
+    sif::error << "EIVE System: Sending FAULTY command to RAD SENSOR failed" << std::endl;
+  }
+}
+
 void EiveSystem::commonI2cRecoverySequenceFinish() {
   alreadyTriedI2cRecovery = true;
   performI2cReboot = false;

mission/system/EiveSystem.h

@@ -49,6 +49,15 @@ class EiveSystem : public Subsystem, public HasActionsIF {
   PowerSwitchIF* powerSwitcher = nullptr;
   std::atomic_uint16_t& i2cErrors;
 
+  MessageQueueId_t plSsQueueId = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t plPcduQueueId = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t plocMpsocQueueId = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t plocSupervisorQueueId = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t cameraQueueId = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t scexQueueId = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t radSensorQueueId = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t strQueueId = MessageQueueIF::NO_QUEUE;
+
   MessageQueueId_t pdecHandlerQueueId = MessageQueueIF::NO_QUEUE;
 
   MessageQueueId_t bpxBattQueueId = MessageQueueIF::NO_QUEUE;
@@ -68,6 +77,8 @@ class EiveSystem : public Subsystem, public HasActionsIF {
   ReturnValue_t sendFullRebootCommand();
   ReturnValue_t sendSelfRebootCommand();
 
+  void forceOffPayload();
+
   void pdecRecoveryLogic();
 
   void i2cRecoveryLogic();

mission/system/objects/CamSwitcher.cpp

@@ -1,6 +1,6 @@
 #include "CamSwitcher.h"
 
-CamSwitcher::CamSwitcher(object_id_t objectId, PowerSwitchIF &pwrSwitcher,
+CamSwitcher::CamSwitcher(object_id_t objectId, PowerSwitchIF& pwrSwitcher,
                          power::Switch_t pwrSwitch)
     : PowerSwitcherComponent(objectId, &pwrSwitcher, pwrSwitch) {}
 void CamSwitcher::performFaultyOperation() {
@@ -8,3 +8,17 @@ void CamSwitcher::performFaultyOperation() {
     switcher.turnOff();
   }
 }
+
+ReturnValue_t CamSwitcher::checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode,
+                                            uint32_t* msToReachTheMode) {
+  if (commandedMode != MODE_OFF) {
+    PoolReadGuard pg(&enablePl);
+    if (pg.getReadResult() == returnvalue::OK) {
+      if (enablePl.plUseAllowed.isValid() and not enablePl.plUseAllowed.value) {
+        return TRANS_NOT_ALLOWED;
+      }
+    }
+  }
+  return PowerSwitcherComponent::checkModeCommand(commandedMode, commandedSubmode,
+                                                  msToReachTheMode);
+}

mission/system/objects/CamSwitcher.h

@@ -1,13 +1,19 @@
 #ifndef MISSION_SYSTEM_OBJECTS_CAMSWITCHER_H_
 #define MISSION_SYSTEM_OBJECTS_CAMSWITCHER_H_
 
+#include <common/config/eive/objects.h>
 #include <fsfw/power/PowerSwitcherComponent.h>
+#include <mission/controller/controllerdefinitions/PowerCtrlDefinitions.h>
 
 class CamSwitcher : public PowerSwitcherComponent {
  public:
-  CamSwitcher(object_id_t objectId, PowerSwitchIF &pwrSwitcher, power::Switch_t pwrSwitch);
+  CamSwitcher(object_id_t objectId, PowerSwitchIF& pwrSwitcher, power::Switch_t pwrSwitch);
 
  private:
+  pwrctrl::EnablePl enablePl = pwrctrl::EnablePl(objects::POWER_CONTROLLER);
+  ReturnValue_t checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode,
+                                 uint32_t* msToReachTheMode) override;
+
   void performFaultyOperation() override;
 };
 

mission/system/power/CMakeLists.txt

@@ -1 +1 @@
-target_sources(${LIB_EIVE_MISSION} PRIVATE GomspacePowerFdir.cpp)
+target_sources(${LIB_EIVE_MISSION} PRIVATE epsModeTree.cpp EpsSubsystem.cpp GomspacePowerFdir.cpp)

mission/system/power/EpsSubsystem.cpp

@@ -0,0 +1,27 @@
+#include <mission/system/power/EpsSubsystem.h>
+
+#include "fsfw/devicehandlers/DeviceHandlerIF.h"
+
+EpsSubsystem::EpsSubsystem(object_id_t objectId, uint32_t maxNumberOfSequences,
+                           uint32_t maxNumberOfTables)
+    : Subsystem(objectId, maxNumberOfSequences, maxNumberOfTables) {}
+
+void EpsSubsystem::announceMode(bool recursive) {
+  const char* modeStr = "UNKNOWN";
+  switch (mode) {
+    case (HasModesIF::MODE_OFF): {
+      modeStr = "OFF";
+      break;
+    }
+    case (HasModesIF::MODE_ON): {
+      modeStr = "ON";
+      break;
+    }
+    case (DeviceHandlerIF::MODE_NORMAL): {
+      modeStr = "NORMAL";
+      break;
+    }
+  }
+  sif::info << "EPS subsystem is now in " << modeStr << " mode" << std::endl;
+  return Subsystem::announceMode(recursive);
+}

mission/system/power/EpsSubsystem.h

@@ -0,0 +1,13 @@
+#ifndef MISSION_SYSTEM_OBJECTS_EPSSUBSYSTEM_H_
+#define MISSION_SYSTEM_OBJECTS_EPSSUBSYSTEM_H_
+#include <fsfw/subsystem/Subsystem.h>
+
+class EpsSubsystem : public Subsystem {
+ public:
+  EpsSubsystem(object_id_t objectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables);
+
+ private:
+  void announceMode(bool recursive) override;
+};
+
+#endif /* MISSION_SYSTEM_OBJECTS_EPSSUBSYSTEM_H_ */

mission/system/power/epsModeTree.cpp

@@ -0,0 +1,104 @@
+#include <mission/system/power/epsModeTree.h>
+
+#include "eive/objects.h"
+#include "fsfw/devicehandlers/DeviceHandlerIF.h"
+#include "fsfw/subsystem/Subsystem.h"
+#include "mission/system/treeUtil.h"
+
+EpsSubsystem satsystem::eps::EPS_SUBSYSTEM(objects::EPS_SUBSYSTEM, 12, 24);
+
+namespace {
+// Alias for checker function
+const auto check = subsystem::checkInsert;
+void buildOffSequence(Subsystem& ss, ModeListEntry& eh);
+void buildNormalSequence(Subsystem& ss, ModeListEntry& eh);
+}  // namespace
+
+static const auto OFF = HasModesIF::MODE_OFF;
+static const auto NML = DeviceHandlerIF::MODE_NORMAL;
+
+auto EPS_SEQUENCE_OFF = std::make_pair(OFF, FixedArrayList<ModeListEntry, 3>());
+auto EPS_TABLE_OFF_TGT = std::make_pair((OFF << 24) | 1, FixedArrayList<ModeListEntry, 2>());
+auto EPS_TABLE_OFF_TRANS_0 = std::make_pair((OFF << 24) | 2, FixedArrayList<ModeListEntry, 2>());
+
+auto EPS_SEQUENCE_NORMAL = std::make_pair(NML, FixedArrayList<ModeListEntry, 3>());
+auto EPS_TABLE_NORMAL_TGT = std::make_pair((NML << 24) | 1, FixedArrayList<ModeListEntry, 2>());
+auto EPS_TABLE_NORMAL_TRANS_0 = std::make_pair((NML << 24) | 2, FixedArrayList<ModeListEntry, 7>());
+
+Subsystem& satsystem::eps::init() {
+  ModeListEntry entry;
+  buildOffSequence(EPS_SUBSYSTEM, entry);
+  buildNormalSequence(EPS_SUBSYSTEM, entry);
+  EPS_SUBSYSTEM.setInitialMode(NML);
+  return EPS_SUBSYSTEM;
+}
+
+namespace {
+
+void buildOffSequence(Subsystem& ss, ModeListEntry& eh) {
+  std::string context = "satsystem::eps::buildOffSequence";
+  auto ctxc = context.c_str();
+  // Insert Helper Table
+  auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table) {
+    eh.setObject(obj);
+    eh.setMode(mode);
+    eh.setSubmode(submode);
+    check(table.insert(eh), ctxc);
+  };
+  // Insert Helper Sequence
+  auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
+                 bool checkSuccess) {
+    eh.setTableId(tableId);
+    eh.setWaitSeconds(waitSeconds);
+    eh.setCheckSuccess(checkSuccess);
+    check(sequence.insert(eh), ctxc);
+  };
+
+  // OFF target table is empty
+  check(ss.addTable(TableEntry(EPS_TABLE_OFF_TGT.first, &EPS_TABLE_OFF_TGT.second)), ctxc);
+
+  // Transition 0
+  iht(objects::POWER_CONTROLLER, OFF, 0, EPS_TABLE_OFF_TRANS_0.second);
+  check(ss.addTable(TableEntry(EPS_TABLE_OFF_TRANS_0.first, &EPS_TABLE_OFF_TRANS_0.second)), ctxc);
+
+  ihs(EPS_SEQUENCE_OFF.second, EPS_TABLE_OFF_TGT.first, 0, false);
+  ihs(EPS_SEQUENCE_OFF.second, EPS_TABLE_OFF_TRANS_0.first, 0, false);
+  check(ss.addSequence(SequenceEntry(EPS_SEQUENCE_OFF.first, &EPS_SEQUENCE_OFF.second,
+                                     EPS_SEQUENCE_OFF.first)),
+        ctxc);
+}
+
+void buildNormalSequence(Subsystem& ss, ModeListEntry& eh) {
+  std::string context = "satsystem::tcs::buildNormalSequence";
+  auto ctxc = context.c_str();
+  // Insert Helper Table
+  auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table) {
+    eh.setObject(obj);
+    eh.setMode(mode);
+    eh.setSubmode(submode);
+    check(table.insert(eh), ctxc);
+  };
+  // Insert Helper Sequence
+  auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
+                 bool checkSuccess) {
+    eh.setTableId(tableId);
+    eh.setWaitSeconds(waitSeconds);
+    eh.setCheckSuccess(checkSuccess);
+    check(sequence.insert(eh), ctxc);
+  };
+
+  // Normal table target table is empty
+  check(ss.addTable(TableEntry(EPS_TABLE_NORMAL_TGT.first, &EPS_TABLE_NORMAL_TGT.second)), ctxc);
+
+  // Transition 0
+  iht(objects::POWER_CONTROLLER, NML, 0, EPS_TABLE_NORMAL_TRANS_0.second);
+  check(ss.addTable(TableEntry(EPS_TABLE_NORMAL_TRANS_0.first, &EPS_TABLE_NORMAL_TRANS_0.second)),
+        ctxc);
+
+  ihs(EPS_SEQUENCE_NORMAL.second, EPS_TABLE_NORMAL_TGT.first, 0, false);
+  ihs(EPS_SEQUENCE_NORMAL.second, EPS_TABLE_NORMAL_TRANS_0.first, 0, false);
+  check(ss.addSequence(SequenceEntry(EPS_SEQUENCE_NORMAL.first, &EPS_SEQUENCE_NORMAL.second,
+                                     EPS_SEQUENCE_NORMAL.first)),
+        ctxc);
+}
+}  // namespace

mission/system/power/epsModeTree.h

@@ -0,0 +1,15 @@
+#ifndef MISSION_SYSTEM_TREE_EPSMODETREE_H_
+#define MISSION_SYSTEM_TREE_EPSMODETREE_H_
+
+#include <mission/system/power/EpsSubsystem.h>
+
+namespace satsystem {
+namespace eps {
+
+extern EpsSubsystem EPS_SUBSYSTEM;
+Subsystem& init();
+
+}  // namespace eps
+}  // namespace satsystem
+
+#endif /* MISSION_SYSTEM_TREE_EPSMODETREE_H_ */

mission/system/systemTree.cpp

@@ -11,6 +11,7 @@
 #include "eive/objects.h"
 #include "mission/com/defs.h"
 #include "mission/system/acs/acsModeTree.h"
+#include "mission/system/power/epsModeTree.h"
 #include "mission/system/tcs/tcsModeTree.h"
 #include "mission/system/tree/payloadModeTree.h"
 #include "treeUtil.h"
@@ -40,6 +41,8 @@ void satsystem::init(bool commandPlPcdu1) {
   tcsSubsystem.connectModeTreeParent(EIVE_SYSTEM);
   auto& comSubsystem = com::init();
   comSubsystem.connectModeTreeParent(EIVE_SYSTEM);
+  auto& epsSubsystem = eps::init();
+  epsSubsystem.connectModeTreeParent(EIVE_SYSTEM);
   ModeListEntry entry;
   buildBootSequence(EIVE_SYSTEM, entry);
   buildSafeSequence(EIVE_SYSTEM, entry);
@@ -141,9 +144,11 @@ void buildBootSequence(Subsystem& ss, ModeListEntry& eh) {
   iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TGT.second);
   iht(objects::COM_SUBSYSTEM, com::RX_ONLY, 0, EIVE_TABLE_BOOT_TGT.second);
   iht(objects::TCS_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TGT.second);
+  iht(objects::EPS_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TGT.second);
   check(ss.addTable(TableEntry(EIVE_TABLE_BOOT_TGT.first, &EIVE_TABLE_BOOT_TGT.second)), ctxc);
 
-  // Build SAFE transition 0.
+  // Build BOOT transition 0.
+  iht(objects::EPS_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TRANS_0.second);
   iht(objects::TCS_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TRANS_0.second);
   iht(objects::COM_SUBSYSTEM, com::RX_ONLY, 0, EIVE_TABLE_BOOT_TRANS_0.second);
   iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TRANS_0.second);
@@ -151,7 +156,7 @@ void buildBootSequence(Subsystem& ss, ModeListEntry& eh) {
   check(ss.addTable(TableEntry(EIVE_TABLE_BOOT_TRANS_0.first, &EIVE_TABLE_BOOT_TRANS_0.second)),
         ctxc);
 
-  // Build Safe sequence
+  // Build BOOT sequence
   ihs(EIVE_SEQUENCE_BOOT.second, EIVE_TABLE_BOOT_TGT.first, 0, false);
   ihs(EIVE_SEQUENCE_BOOT.second, EIVE_TABLE_BOOT_TRANS_0.first, 0, false);
   check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_BOOT.first, &EIVE_SEQUENCE_BOOT.second,
@@ -187,13 +192,14 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
   check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TGT.first, &EIVE_TABLE_SAFE_TGT.second)), ctxc);
 
   // Build SAFE transition 0.
+  iht(objects::EPS_SUBSYSTEM, NML, 0, EIVE_TABLE_SAFE_TRANS_0.second);
   iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_SAFE_TRANS_0.second);
   iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TRANS_0.second);
   iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TRANS_0.second, true);
   check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_0.first, &EIVE_TABLE_SAFE_TRANS_0.second)),
         ctxc);
 
-  // Build Safe sequence
+  // Build SAFE sequence
   ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TGT.first, 0, false);
   ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TRANS_0.first, 0, false);
   check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_SAFE.first, &EIVE_SEQUENCE_SAFE.second,
@@ -224,6 +230,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
   check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TGT.first, &EIVE_TABLE_IDLE_TGT.second)), ctxc);
 
   // Build IDLE transition 0
+  iht(objects::EPS_SUBSYSTEM, NML, 0, EIVE_TABLE_IDLE_TRANS_0.second);
   iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_IDLE_TRANS_0.second);
   iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TRANS_0.second);
   check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TRANS_0.first, &EIVE_TABLE_IDLE_TRANS_0.second)),
@@ -261,6 +268,7 @@ void buildPtgNadirSequence(Subsystem& ss, ModeListEntry& eh) {
         ctxc);
 
   // Build PTG_NADIR transition 0
+  iht(objects::EPS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_NADIR_TRANS_0.second);
   iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_NADIR_TRANS_0.second);
   iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_NADIR, 0, EIVE_TABLE_PTG_NADIR_TRANS_0.second);
   check(ss.addTable(
@@ -299,6 +307,7 @@ void buildPtgTargetSequence(Subsystem& ss, ModeListEntry& eh) {
         ctxc);
 
   // Build PTG_TARGET transition 0
+  iht(objects::EPS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_TARGET_TRANS_0.second);
   iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_TARGET_TRANS_0.second);
   iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET, 0, EIVE_TABLE_PTG_TARGET_TRANS_0.second);
   check(ss.addTable(
@@ -338,6 +347,7 @@ void buildPtgTargetGsSequence(Subsystem& ss, ModeListEntry& eh) {
         ctxc);
 
   // Build PTG_TARGET_GS transition 0
+  iht(objects::EPS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second);
   iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second);
   iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET_GS, 0,
       EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second);
@@ -379,6 +389,7 @@ void buildPtgInertialSequence(Subsystem& ss, ModeListEntry& eh) {
         ctxc);
 
   // Build PTG_INERTIAL transition 0
+  iht(objects::EPS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_INERTIAL_TRANS_0.second);
   iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_INERTIAL_TRANS_0.second);
   iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_INERTIAL, 0,
       EIVE_TABLE_PTG_INERTIAL_TRANS_0.second);