From 40ee5ddade994b8e475dc2e51f63c7d1e63c219c Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 31 Jul 2023 13:14:04 +0200
Subject: [PATCH] what if everyone was a returnvalue

---
 mission/controller/PowerController.cpp | 59 +++++++++++++++++++-------
 mission/controller/PowerController.h   |  1 +
 mission/power/defs.h                   |  3 ++
 3 files changed, 48 insertions(+), 15 deletions(-)

diff --git a/mission/controller/PowerController.cpp b/mission/controller/PowerController.cpp
index 289e70b8..21e3ad3d 100644
--- a/mission/controller/PowerController.cpp
+++ b/mission/controller/PowerController.cpp
@@ -158,7 +158,26 @@ void PowerController::calculateStateOfCharge() {
     }
     return;
   }
-  calculateCoulombCounterCharge();
+
+  result = calculateCoulombCounterCharge();
+  // store time for next run
+  oldTime = now;
+  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);
+        pwrCtrlCoreHk.openCircuitVoltageCharge.setValid(true);
+        pwrCtrlCoreHk.coulombCounterCharge.value = INVALID_SOC;
+        pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
+      }
+    }
+    return;
+  }
 
   // commit to dataset
   {
@@ -170,9 +189,6 @@ void PowerController::calculateStateOfCharge() {
       pwrCtrlCoreHk.setValidity(true, true);
     }
   }
-
-  // store time for next run
-  oldTime = now;
 }
 
 ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() {
@@ -189,21 +205,20 @@ ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() {
 }
 
 ReturnValue_t PowerController::calculateCoulombCounterCharge() {
-  if ((pwrCtrlCoreHk.coulombCounterCharge.value == 0) or
+  double timeDiff = timevalOperations::toDouble(now - oldTime);
+  if (timeDiff > maxAllowedTimeDiff) {
+    triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS);
+    sif::error << "Power Controller::Time delta too large for Coulomb Counter" << std::endl;
+    return returnvalue::FAILED;
+  }
+  if ((not pwrCtrlCoreHk.coulombCounterCharge.isValid()) or
       (p60CoreHk.batteryVoltage.value > coulombCounterVoltageUpperThreshold and
        pwrCtrlCoreHk.coulombCounterCharge.value >= coulombCounterChargeUpperThreshold)) {
     coulombCounterCharge = openCircuitVoltageCharge;
-    return returnvalue::OK;
+  } else {
+    coulombCounterCharge = pwrCtrlCoreHk.coulombCounterCharge.value + iBat * timeDiff;
   }
-
-  else {
-    double timeDiff = timevalOperations::toDouble(now - oldTime);
-    if (timeDiff < maxAllowedTimeDiff) {
-      coulombCounterCharge = pwrCtrlCoreHk.coulombCounterCharge.value + iBat * timeDiff;
-      return returnvalue::OK;
-    }
-  }
-  return returnvalue::FAILED;
+  return returnvalue::OK;
 }
 
 ReturnValue_t PowerController::updateEpsData() {
@@ -259,3 +274,17 @@ ReturnValue_t PowerController::lookUpTableOcvIdxFinder(float voltage, uint8_t &i
   }
   return returnvalue::OK;
 }
+
+ReturnValue_t PowerController::calculateCoulombCounterChargeUpperThreshold() {
+  uint8_t lookUpTableIdx = 99;
+  ReturnValue_t result =
+      lookUpTableOcvIdxFinder(coulombCounterVoltageUpperThreshold, lookUpTableIdx);
+  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;
+}
diff --git a/mission/controller/PowerController.h b/mission/controller/PowerController.h
index 971bc0af..0f15363b 100644
--- a/mission/controller/PowerController.h
+++ b/mission/controller/PowerController.h
@@ -49,6 +49,7 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
   float charge2stateOfCharge(float capacity);
   ReturnValue_t lookUpTableOcvIdxFinder(float voltage, uint8_t& idx);
   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]
diff --git a/mission/power/defs.h b/mission/power/defs.h
index 682be614..87fc3f08 100644
--- a/mission/power/defs.h
+++ b/mission/power/defs.h
@@ -53,6 +53,9 @@ static constexpr Event DATASET_READ_FAILED = event::makeEvent(SUBSYSTEM_ID, 4, s
 //! be.
 //! P1: 1 too high, 0 too low
 static constexpr Event VOLTAGE_OUT_OF_BOUNDS = event::makeEvent(SUBSYSTEM_ID, 5, severity::HIGH);
+//! [EXPORT] : [COMMENT] Time difference for Coulomb Counter was too large.
+static constexpr Event TIMEDELTA_OUT_OF_BOUNDS =
+    event::makeEvent(SUBSYSTEM_ID, 6, severity::MEDIUM);
 
 enum class States { IDLE, SWITCHING_POWER, CHECKING_POWER, MODE_COMMANDING };
 enum class OpCodes { NONE, TO_OFF_DONE, TO_NOT_OFF_DONE, TIMEOUT_OCCURED };