mission/controller/PowerController.cpp

@@ -1,11 +1,11 @@
-#include <fsfw/datapool/PoolReadGuard.h>
+
 #include <mission/controller/PowerController.h>
 
 PowerController::PowerController(object_id_t objectId, bool enableHkSets)
     : ExtendedControllerBase(objectId),
       enableHkSets(enableHkSets),
       parameterHelper(this),
-      stateOfChargeData(this) {}
+      pwrCtrlCoreHk(this) {}
 
 ReturnValue_t PowerController::initialize() {
   ReturnValue_t result = parameterHelper.initialize();
@@ -40,6 +40,9 @@ ReturnValue_t PowerController::getParameter(uint8_t domainId, uint8_t parameterI
         case 0x0:
           parameterWrapper->set(batteryInternalResistance);
           break;
+        case 0x1:
+          parameterWrapper->set(batteryMaximumCapacity);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }
@@ -78,13 +81,18 @@ void PowerController::performControlOperation() {
 
 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});
   return returnvalue::OK;
 }
 
 LocalPoolDataSetBase *PowerController::getDataSetHandle(sid_t sid) {
   switch (sid.ownerSetId) {
-    case pwrctrl::STATE_OF_CHARGE_DATA:
+    case pwrctrl::CORE_HK:
-      return &stateOfChargeData;
+      return &pwrCtrlCoreHk;
     default:
       return nullptr;
   }
@@ -105,53 +113,72 @@ ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode,
 
 void PowerController::calculateStateOfCharge() {
   // get time
-  timeval now;
   Clock::getClock_timeval(&now);
 
-  // update EPS hk values
+  // update EPS HK values
   ReturnValue_t result = updateEpsData();
+  if (result != returnvalue::OK) {
+    triggerEvent(power::DATASET_READ_FAILED);
+    sif::info << "Power Controller::Reading of Datasets has failed" << std::endl;
+    return;
+  }
 
-  float iBat = p60CoreHk.batteryCurrent.value + bpxBatteryHk.heaterCurrent.value +
+  // calculate total battery current
+  iBat = p60CoreHk.batteryCurrent.value + bpxBatteryHk.heaterCurrent.value +
          bpxBatteryHk.dischargeCurrent.value;
 
-  // Open Circuit Voltage Charge
+  calculateOpenCircuitVoltageCharge();
-  // ToDo: battery heater and battery discharge currents missing
+  calculateCoulombCounterCharge();
-  float vBatCorrected = p60CoreHk.batteryVoltage.value - iBat * batteryInternalResistance;
-  uint8_t lookUpTableIdx;
-  for (lookUpTableIdx = 24; lookUpTableIdx > 0; lookUpTableIdx--) {
-    if (lookUpTableOcv[1][lookUpTableIdx] < vBatCorrected) {
-      break;
-    }
-  }
-  // ToDo: check indexing
-  float openCircuitVoltageCharge =
-      lookUpTableOcv[0][lookUpTableIdx] +
-      (vBatCorrected - lookUpTableOcv[1][lookUpTableIdx]) *
-          (lookUpTableOcv[1][lookUpTableIdx + 1] - lookUpTableOcv[1][lookUpTableIdx]) /
-          (lookUpTableOcv[0][lookUpTableIdx + 1] - lookUpTableOcv[0][lookUpTableIdx]);
 
-  // Coulomb Counter
+  // commit to dataset
-  float coulombCounterCharge = 0;
-  if (stateOfChargeData.coulombCounterCharge.value == 0)
-    coulombCounterCharge = openCircuitVoltageCharge;
-  else {
-    double timeDiff = timevalOperations::toDouble(now - oldTime);
-    coulombCounterCharge = stateOfChargeData.coulombCounterCharge.value + iBat * timeDiff;
-  }
-
-  // commit TM
   {
-    PoolReadGuard pg(&stateOfChargeData);
+    PoolReadGuard pg(&pwrCtrlCoreHk);
-    stateOfChargeData.openCircuitVoltageCharge.value =
+    if (pg.getReadResult() == returnvalue::OK) {
-        charge2stateOfCharge(openCircuitVoltageCharge);
+      pwrCtrlCoreHk.totalBatteryCurrent.value = iBat;
-    stateOfChargeData.coulombCounterCharge.value = charge2stateOfCharge(coulombCounterCharge);
+      pwrCtrlCoreHk.openCircuitVoltageCharge.value = charge2stateOfCharge(openCircuitVoltageCharge);
-    stateOfChargeData.setValidity(true, true);
+      pwrCtrlCoreHk.coulombCounterCharge.value = charge2stateOfCharge(coulombCounterCharge);
+      pwrCtrlCoreHk.setValidity(true, true);
+    }
   }
 
   // store time for next run
   oldTime = now;
 }
 
+void PowerController::calculateOpenCircuitVoltageCharge() {
+  float vBatCorrected = p60CoreHk.batteryVoltage.value - iBat * batteryInternalResistance;
+  if (vBatCorrected >= lookUpTableOcv[1][100]) {
+    triggerEvent(power::VOLTAGE_OUT_OF_BOUNDS, 0);
+    sif::error << "Power Controller::Voltage is too high" << std::endl;
+    return;
+  } else if (vBatCorrected <= lookUpTableOcv[1][0]) {
+    triggerEvent(power::VOLTAGE_OUT_OF_BOUNDS, 1);
+    sif::error << "Power Controller::Voltage is too low" << std::endl;
+    return;
+  }
+  uint8_t lookUpTableIdx = 99;
+  while (lookUpTableOcv[1][lookUpTableIdx] > vBatCorrected) {
+    lookUpTableIdx--;
+  }
+
+  openCircuitVoltageCharge = linearInterpolation(
+      vBatCorrected, lookUpTableOcv[1][lookUpTableIdx], lookUpTableOcv[1][lookUpTableIdx + 1],
+      lookUpTableOcv[0][lookUpTableIdx], lookUpTableOcv[0][lookUpTableIdx + 1]);
+}
+
+void PowerController::calculateCoulombCounterCharge() {
+  if ((pwrCtrlCoreHk.coulombCounterCharge.value == 0) or
+      (p60CoreHk.batteryVoltage.value > coulombCounterVoltageUpperThreshold and
+    		  pwrCtrlCoreHk.coulombCounterCharge.value >= )) {
+    coulombCounterCharge = openCircuitVoltageCharge;
+  }
+
+  else {
+    double timeDiff = timevalOperations::toDouble(now - oldTime);
+    coulombCounterCharge = pwrCtrlCoreHk.coulombCounterCharge.value + iBat * timeDiff;
+  }
+}
+
 ReturnValue_t PowerController::updateEpsData() {
   std::vector<ReturnValue_t> results;
   {
@@ -185,3 +212,7 @@ ReturnValue_t PowerController::updateEpsData() {
 float PowerController::charge2stateOfCharge(float capacity) {
   return capacity / batteryMaximumCapacity;
 }
+
+float PowerController::linearInterpolation(float x, float x0, float x1, float y0, float y1) {
+  return y0 + (x - x0) * (y1 - y0) / (x1 - x0);
+}

mission/controller/PowerController.h

@@ -43,15 +43,19 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
   void performControlOperation() override;
 
   void calculateStateOfCharge();
+  void calculateOpenCircuitVoltageCharge();
+  void calculateCoulombCounterCharge();
   ReturnValue_t updateEpsData();
   float charge2stateOfCharge(float capacity);
+  float linearInterpolation(float x, float x0, float x1, float y0, float y1);
 
   // Parameters
-  float batteryInternalResistance = 70 / 2 / 1000;  // [Ohm]
+  float batteryInternalResistance = 70.0 / 2.0 / 1000.0;  // [Ohm]
   float batteryMaximumCapacity = 2.6 * 2;                 // [Ah]
+  float coulombCounterVoltageUpperThreshold = 16.2e3;     // [mV]
 
   // OCV Look-up-Table
-  float lookUpTableOcv[2][25] = {
+  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,
@@ -89,7 +93,12 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
        16.67249918, 16.74926904}};
 
   // Variables
+  timeval now;
   timeval oldTime;
+  int16_t iBat = 0.0;                              // [mA]
+  float openCircuitVoltageCharge = 0.0;            // [mC]
+  float coulombCounterCharge = 0;                  // [mC]
+  float coulombCounterChargeUpperThreshold = 0.0;  // [mC]
 
   // HK Datasets for Calculation
   BpxBatteryHk bpxBatteryHk = BpxBatteryHk(objects::BPX_BATT_HANDLER);
@@ -98,7 +107,7 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
   PDU2::Pdu2CoreHk pdu2CoreHk = PDU2::Pdu2CoreHk(objects::PDU2_HANDLER);
   ACU::CoreHk acuCoreHk = ACU::CoreHk(objects::ACU_HANDLER);
   // Output Dataset
-  pwrctrl::StateOfChargedData stateOfChargeData;
+  pwrctrl::CoreHk pwrCtrlCoreHk;
 };
 
 #endif /* MISSION_CONTROLLER_POWERCONTROLLER_H_ */

mission/controller/controllerdefinitions/PowerCtrlDefinitions.h

@@ -1,27 +1,31 @@
 #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 { STATE_OF_CHARGE_DATA };
+enum SetIds : uint32_t { CORE_HK };
 
 enum PoolIds : lp_id_t {
+  TOTAL_BATTERY_CURRENT,
   OPEN_CIRCUIT_VOLTAGE_CHARGE,
   COULOMB_COUNTER_CHARGE,
 };
 
-static constexpr uint8_t STATE_OF_CHARGE_ENTRIES = 2;
+static constexpr uint8_t CORE_HK_ENTRIES = 3;
 
-class StateOfChargedData : public StaticLocalDataSet<STATE_OF_CHARGE_ENTRIES> {
+class CoreHk : public StaticLocalDataSet<CORE_HK_ENTRIES> {
  public:
-  StateOfChargedData(HasLocalDataPoolIF* hkOwner)
+  CoreHk(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CORE_HK) {}
-      : StaticLocalDataSet(hkOwner, STATE_OF_CHARGE_DATA) {}
 
+  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 =

mission/power/defs.h

@@ -47,6 +47,13 @@ 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
+static constexpr Event VOLTAGE_OUT_OF_BOUNDS = event::makeEvent(SUBSYSTEM_ID, 5, 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 };