Basic SoC estimation

Core/Inc/soc_estimation.h

@@ -3,9 +3,16 @@
 
 #include <stdint.h>
 
-extern uint8_t current_soc;
+extern float current_soc;
 
 void soc_init();
 void soc_update();
 
+typedef struct {
+  uint16_t ocv;
+  float soc;
+} ocv_soc_pair_t;
+extern ocv_soc_pair_t OCV_SOC_PAIRS[];
+float soc_for_ocv(uint16_t ocv);
+
 #endif // INC_SOC_ESTIMATION_H

Core/Src/can.c

@@ -8,6 +8,7 @@
 
 #include "can-halal.h"
 
+#include <math.h>
 #include <stdint.h>
 
 void can_init(CAN_HandleTypeDef *handle) {
@@ -22,7 +23,7 @@ void can_init(CAN_HandleTypeDef *handle) {
 HAL_StatusTypeDef can_send_status() {
   uint8_t data[6];
   data[0] = ts_state.current_state | (sdc_closed << 7);
-  data[1] = current_soc;
+  data[1] = roundf(current_soc);
   ftcan_marshal_unsigned(&data[2], min_voltage, 2);
   ftcan_marshal_signed(&data[4], max_temp, 2);
   return ftcan_transmit(CAN_ID_AMS_STATUS, data, sizeof(data));

Core/Src/soc_estimation.c

@@ -1,14 +1,91 @@
 #include "soc_estimation.h"
 
+#include "shunt_monitoring.h"
+#include "slave_monitoring.h"
+
+#include "stm32f3xx_hal.h"
+
+#include <stddef.h>
 #include <stdint.h>
 
-uint8_t current_soc;
+#define SOC_ESTIMATION_NO_CURRENT_THRESH 200  // mA
+#define SOC_ESTIMATION_NO_CURRENT_TIME 100000 // ms
+#define SOC_ESTIMATION_BATTERY_CAPACITY 19528 // mAh
+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},
+    {40100, 83.79f}, {40200, 90.26f}, {40400, 94.58f}, {41000, 98.89f},
+    {42000, 100.00f}};
+
+float current_soc;
+
+int current_was_flowing;
+uint32_t last_current_time;
+uint32_t first_current_time;
+float soc_before_current;
+int32_t ah_before_current;
 
 void soc_init() {
   current_soc = 0;
-  // TODO
+  last_current_time = 0;
+  current_was_flowing = 1;
 }
 
 void soc_update() {
-  // TODO
+  uint32_t now = HAL_GetTick();
+  if (shunt_data.current >= SOC_ESTIMATION_NO_CURRENT_THRESH) {
+    last_current_time = now;
+    if (!current_was_flowing) {
+      first_current_time = now;
+      soc_before_current = current_soc;
+      ah_before_current = shunt_data.current_counter;
+    }
+    current_was_flowing = 1;
+  } else {
+    current_was_flowing = 0;
+  }
+
+  if (now - last_current_time >= SOC_ESTIMATION_NO_CURRENT_TIME ||
+      last_current_time == 0) {
+    // Assume we're measuring OCV if there's been no current for a while (or
+    // we've just turned on the battery).
+    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;
+    current_soc =
+        soc_before_current + (float)ah_delta / SOC_ESTIMATION_BATTERY_CAPACITY;
+  }
+}
+
+float soc_for_ocv(uint16_t ocv) {
+  size_t i = 0;
+  size_t array_length = sizeof(OCV_SOC_PAIRS) / sizeof(*OCV_SOC_PAIRS);
+  // Find the index of the first element with OCV greater than the target OCV
+  while (i < array_length && OCV_SOC_PAIRS[i].ocv <= ocv) {
+    i++;
+  }
+
+  // If the target OCV is lower than the smallest OCV in the array, return the
+  // first SOC value
+  if (i == 0) {
+    return OCV_SOC_PAIRS[0].soc;
+  }
+
+  // If the target OCV is higher than the largest OCV in the array, return the
+  // last SOC value
+  if (i == array_length) {
+    return OCV_SOC_PAIRS[array_length - 1].soc;
+  }
+
+  // Perform linear interpolation
+  uint16_t ocv1 = OCV_SOC_PAIRS[i - 1].ocv;
+  uint16_t ocv2 = OCV_SOC_PAIRS[i].ocv;
+  float soc1 = OCV_SOC_PAIRS[i - 1].soc;
+  float soc2 = OCV_SOC_PAIRS[i].soc;
+
+  float slope = (soc2 - soc1) / (ocv2 - ocv1);
+  float interpolated_soc = soc1 + slope * (ocv - ocv1);
+
+  return interpolated_soc;
 }