From 208d84e2a5a68c6b68b11b6ebd43f7141bcb967c Mon Sep 17 00:00:00 2001
From: "Jasper v. Blanckenburg" <j.blanckenburg@fasttube.de>
Date: Sun, 25 Jun 2023 16:41:29 +0200
Subject: [PATCH] Integrate current

The current counter on the shunt can't be activated for some reason.
---
 Core/Inc/shunt_monitoring.h | 11 ++++++-----
 Core/Src/shunt_monitoring.c | 12 +++++++++++-
 Core/Src/soc_estimation.c   | 14 +++++++-------
 3 files changed, 24 insertions(+), 13 deletions(-)

diff --git a/Core/Inc/shunt_monitoring.h b/Core/Inc/shunt_monitoring.h
index 67001c1..5c96a2a 100644
--- a/Core/Inc/shunt_monitoring.h
+++ b/Core/Inc/shunt_monitoring.h
@@ -10,16 +10,17 @@
 #define SHUNT_THRESH_OVERTEMP 1000      // 1/10 °C
 
 typedef struct {
-  int32_t current;
-  int32_t voltage_bat;
-  int32_t voltage_veh;
-  int32_t voltage3;
+  int32_t current;     // mA
+  int32_t voltage_bat; // mV
+  int32_t voltage_veh; // mV
+  int32_t voltage3;    // mV
   int32_t busbartemp;
   int32_t power;
   int32_t energy;
-  int32_t current_counter;
+  float current_counter; // mAs
 
   uint32_t last_message;
+  uint32_t last_current_message;
 } ShuntData;
 extern ShuntData shunt_data;
 
diff --git a/Core/Src/shunt_monitoring.c b/Core/Src/shunt_monitoring.c
index 8a92045..8fcd540 100644
--- a/Core/Src/shunt_monitoring.c
+++ b/Core/Src/shunt_monitoring.c
@@ -20,6 +20,7 @@ void shunt_init() {
   shunt_data.energy = 0;
   shunt_data.current_counter = 0;
   shunt_data.last_message = 0;
+  shunt_data.last_current_message = 0;
 }
 
 void shunt_check() {
@@ -46,6 +47,12 @@ void shunt_handle_can_msg(uint16_t id, const uint8_t *data) {
   switch (id) {
   case CAN_ID_SHUNT_CURRENT:
     shunt_data.current = result;
+    if (shunt_data.last_current_message > 0) {
+      uint32_t now = HAL_GetTick();
+      float dt = (now - shunt_data.last_current_message) * 0.001f;
+      shunt_data.current_counter += shunt_data.current * dt;
+    }
+    shunt_data.last_current_message = HAL_GetTick();
     break;
   case CAN_ID_SHUNT_VOLTAGE1:
     shunt_data.voltage_bat = result;
@@ -63,7 +70,10 @@ void shunt_handle_can_msg(uint16_t id, const uint8_t *data) {
     shunt_data.power = result;
     break;
   case CAN_ID_SHUNT_CURRENT_COUNTER:
-    shunt_data.current_counter = result;
+    // TODO: Use this when we get the shunt to emit current counter data (the
+    // shunt apparently emits As, not mAs)
+
+    // shunt_data.current_counter = result * 1000;
     break;
   case CAN_ID_SHUNT_ENERGY_COUNTER:
     shunt_data.energy = result;
diff --git a/Core/Src/soc_estimation.c b/Core/Src/soc_estimation.c
index 70924b2..1bcb9f5 100644
--- a/Core/Src/soc_estimation.c
+++ b/Core/Src/soc_estimation.c
@@ -8,9 +8,9 @@
 #include <stddef.h>
 #include <stdint.h>
 
-#define SOC_ESTIMATION_NO_CURRENT_THRESH 200  // mA
-#define SOC_ESTIMATION_NO_CURRENT_TIME 100000 // ms
-#define SOC_ESTIMATION_BATTERY_CAPACITY 19528 // mAh
+#define SOC_ESTIMATION_NO_CURRENT_THRESH 200     // mA
+#define SOC_ESTIMATION_NO_CURRENT_TIME 100000    // ms
+#define SOC_ESTIMATION_BATTERY_CAPACITY 70300800 // mAs
 ocv_soc_pair_t OCV_SOC_PAIRS[] = {
     {25000, 0.00f},  {29900, 3.97f},  {32300, 9.36f},  {33200, 12.60f},
     {33500, 13.68f}, {34100, 20.15f}, {35300, 32.01f}, {38400, 66.53f},
@@ -23,7 +23,7 @@ int current_was_flowing;
 uint32_t last_current_time;
 uint32_t first_current_time;
 float soc_before_current;
-int32_t ah_before_current;
+float mAs_before_current;
 
 void soc_init() {
   current_soc = 0;
@@ -38,7 +38,7 @@ void soc_update() {
     if (!current_was_flowing) {
       first_current_time = now;
       soc_before_current = current_soc;
-      ah_before_current = shunt_data.current_counter;
+      mAs_before_current = shunt_data.current_counter;
     }
     current_was_flowing = 1;
   } else {
@@ -52,9 +52,9 @@ void soc_update() {
     current_soc = soc_for_ocv(min_voltage);
   } else {
     // Otherwise, use the current counter to update SoC
-    int32_t ah_delta = shunt_data.current_counter - ah_before_current;
+    float as_delta = shunt_data.current_counter - mAs_before_current;
     current_soc =
-        soc_before_current + (float)ah_delta / SOC_ESTIMATION_BATTERY_CAPACITY;
+        soc_before_current + as_delta / SOC_ESTIMATION_BATTERY_CAPACITY;
   }
 }