From 6a77635bfb9dd095e5e8d10dbec931ed8d25039b Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Wed, 27 Sep 2023 16:36:09 +0200
Subject: [PATCH] soc calculation fixes

---
 mission/controller/PowerController.cpp | 17 ++++++++++-------
 mission/controller/PowerController.h   |  2 +-
 2 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/mission/controller/PowerController.cpp b/mission/controller/PowerController.cpp
index 12ed5216..1eb3abaf 100644
--- a/mission/controller/PowerController.cpp
+++ b/mission/controller/PowerController.cpp
@@ -162,8 +162,7 @@ void PowerController::calculateStateOfCharge() {
   }
 
   // calculate total battery current
-  iBat = p60CoreHk.batteryCurrent.value + bpxBatteryHk.heaterCurrent.value +
-         bpxBatteryHk.dischargeCurrent.value;
+  iBat = p60CoreHk.batteryCurrent.value + bpxBatteryHk.dischargeCurrent.value;
 
   result = calculateOpenCircuitVoltageCharge();
   if (result != returnvalue::OK) {
@@ -193,7 +192,7 @@ void PowerController::calculateStateOfCharge() {
         pwrCtrlCoreHk.totalBatteryCurrent.value = iBat;
         pwrCtrlCoreHk.totalBatteryCurrent.setValid(true);
         pwrCtrlCoreHk.openCircuitVoltageCharge.value =
-            charge2stateOfCharge(openCircuitVoltageCharge);
+            charge2stateOfCharge(openCircuitVoltageCharge, false);
         pwrCtrlCoreHk.openCircuitVoltageCharge.setValid(true);
         pwrCtrlCoreHk.coulombCounterCharge.value = INVALID_SOC;
         pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
@@ -209,8 +208,9 @@ void PowerController::calculateStateOfCharge() {
     PoolReadGuard pg(&pwrCtrlCoreHk);
     if (pg.getReadResult() == returnvalue::OK) {
       pwrCtrlCoreHk.totalBatteryCurrent.value = iBat;
-      pwrCtrlCoreHk.openCircuitVoltageCharge.value = charge2stateOfCharge(openCircuitVoltageCharge);
-      pwrCtrlCoreHk.coulombCounterCharge.value = charge2stateOfCharge(coulombCounterCharge);
+      pwrCtrlCoreHk.openCircuitVoltageCharge.value =
+          charge2stateOfCharge(openCircuitVoltageCharge, false);
+      pwrCtrlCoreHk.coulombCounterCharge.value = charge2stateOfCharge(coulombCounterCharge, true);
       pwrCtrlCoreHk.setValidity(true, true);
     }
   }
@@ -286,8 +286,11 @@ ReturnValue_t PowerController::updateEpsData() {
   return returnvalue::OK;
 }
 
-float PowerController::charge2stateOfCharge(float capacity) {
-  return capacity / coulombCounterChargeUpperThreshold;
+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) {
diff --git a/mission/controller/PowerController.h b/mission/controller/PowerController.h
index e7031ad8..81522023 100644
--- a/mission/controller/PowerController.h
+++ b/mission/controller/PowerController.h
@@ -47,7 +47,7 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
   ReturnValue_t calculateOpenCircuitVoltageCharge();
   ReturnValue_t calculateCoulombCounterCharge();
   ReturnValue_t updateEpsData();
-  float charge2stateOfCharge(float capacity);
+  float charge2stateOfCharge(float capacity, bool coulombCounter);
   ReturnValue_t lookUpTableOcvIdxFinder(float voltage, uint8_t& idx);
   float linearInterpolation(float x, float x0, float x1, float y0, float y1);
   ReturnValue_t calculateCoulombCounterChargeUpperThreshold();