mission/controller/PowerController.cpp

@@ -43,6 +43,12 @@ ReturnValue_t PowerController::getParameter(uint8_t domainId, uint8_t parameterI
         case 0x1:
           parameterWrapper->set(batteryMaximumCapacity);
           break;
+        case 0x2:
+          parameterWrapper->set(coulombCounterVoltageUpperThreshold);
+          break;
+        case 0x3:
+          parameterWrapper->set(maxAllowedTimeDiff);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }
@@ -119,7 +125,16 @@ void PowerController::calculateStateOfCharge() {
   ReturnValue_t result = updateEpsData();
   if (result != returnvalue::OK) {
     triggerEvent(power::DATASET_READ_FAILED);
-    sif::info << "Power Controller::Reading of Datasets has failed" << std::endl;
+    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);
+      }
+    }
     return;
   }
 
@@ -127,7 +142,22 @@ void PowerController::calculateStateOfCharge() {
   iBat = p60CoreHk.batteryCurrent.value + bpxBatteryHk.heaterCurrent.value +
          bpxBatteryHk.dischargeCurrent.value;
 
-  calculateOpenCircuitVoltageCharge();
+  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);
+      }
+    }
+    return;
+  }
   calculateCoulombCounterCharge();
 
   // commit to dataset
@@ -145,38 +175,35 @@ void PowerController::calculateStateOfCharge() {
   oldTime = now;
 }
 
-void PowerController::calculateOpenCircuitVoltageCharge() {
+ReturnValue_t 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) {
+  ReturnValue_t result = lookUpTableOcvIdxFinder(vBatCorrected, lookUpTableIdx);
-    lookUpTableIdx--;
+  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;
 }
 
-void PowerController::calculateCoulombCounterCharge() {
+ReturnValue_t PowerController::calculateCoulombCounterCharge() {
   if ((pwrCtrlCoreHk.coulombCounterCharge.value == 0) or
       (p60CoreHk.batteryVoltage.value > coulombCounterVoltageUpperThreshold and
        pwrCtrlCoreHk.coulombCounterCharge.value >= coulombCounterChargeUpperThreshold)) {
     coulombCounterCharge = openCircuitVoltageCharge;
+    return returnvalue::OK;
   }
 
   else {
     double timeDiff = timevalOperations::toDouble(now - oldTime);
-    coulombCounterCharge = pwrCtrlCoreHk.coulombCounterCharge.value + iBat * timeDiff;
+    if (timeDiff < maxAllowedTimeDiff) {
+      coulombCounterCharge = pwrCtrlCoreHk.coulombCounterCharge.value + iBat * timeDiff;
+      return returnvalue::OK;
+    }
   }
+  return returnvalue::FAILED;
 }
 
 ReturnValue_t PowerController::updateEpsData() {
@@ -216,3 +243,19 @@ float PowerController::charge2stateOfCharge(float capacity) {
 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) {
+  if (voltage >= lookUpTableOcv[1][100]) {
+    triggerEvent(power::VOLTAGE_OUT_OF_BOUNDS, 0);
+    sif::error << "Power Controller::Voltage is too high" << std::endl;
+    return returnvalue::FAILED;
+  } else if (voltage <= lookUpTableOcv[1][0]) {
+    triggerEvent(power::VOLTAGE_OUT_OF_BOUNDS, 1);
+    sif::error << "Power Controller::Voltage is too low" << std::endl;
+    return returnvalue::FAILED;
+  }
+  while (lookUpTableOcv[1][idx] > voltage) {
+    idx--;
+  }
+  return returnvalue::OK;
+}

mission/controller/PowerController.h

@@ -43,16 +43,18 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
   void performControlOperation() override;
 
   void calculateStateOfCharge();
-  void calculateOpenCircuitVoltageCharge();
+  ReturnValue_t calculateOpenCircuitVoltageCharge();
-  void calculateCoulombCounterCharge();
+  ReturnValue_t calculateCoulombCounterCharge();
   ReturnValue_t updateEpsData();
   float charge2stateOfCharge(float capacity);
+  ReturnValue_t lookUpTableOcvIdxFinder(float voltage, uint8_t& idx);
   float linearInterpolation(float x, float x0, float x1, float y0, float y1);
 
   // Parameters
   float batteryInternalResistance = 70.0 / 2.0 / 1000.0;  // [Ohm]
   float batteryMaximumCapacity = 2.6 * 2;                 // [Ah]
   float coulombCounterVoltageUpperThreshold = 16.2e3;     // [mV]
+  double maxAllowedTimeDiff = 0.5;                        // [s]
 
   // OCV Look-up-Table
   float lookUpTableOcv[2][100] = {
@@ -100,6 +102,9 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
   float coulombCounterCharge = 0;                  // [mC]
   float coulombCounterChargeUpperThreshold = 0.0;  // [mC]
 
+  static constexpr float INVALID_TOTAL_BATTERY_CURRENT = 0;
+  static constexpr float INVALID_SOC = -1;
+
   // HK Datasets for Calculation
   BpxBatteryHk bpxBatteryHk = BpxBatteryHk(objects::BPX_BATT_HANDLER);
   P60Dock::CoreHkSet p60CoreHk = P60Dock::CoreHkSet(objects::P60DOCK_HANDLER);