Implement SoC estimation (UNTESTED!)

Core/Inc/SPI_Communication.h

@@ -25,7 +25,8 @@
 #define TOGGLE_STATUS_LED 0x06
 
 #define NUMBEROFSLAVES 6
-#define NUMBEROFCELLS 10
+#define N_CELLS_SERIES 10
+#define N_CELLS_PARALLEL 9
 #define NUMBEROFTEMPS 32
 
 #define SLAVE_TIMEOUT 200
@@ -33,7 +34,7 @@
 typedef struct {
 
   uint16_t slaveID;
-  uint16_t cellVoltages[NUMBEROFCELLS];
+  uint16_t cellVoltages[N_CELLS_SERIES];
   uint16_t cellTemps[NUMBEROFTEMPS];
   uint32_t timestamp;
   uint8_t error;

Core/Inc/SoC_Estimation.h

@@ -0,0 +1,15 @@
+#ifndef INC_SOC_ESTIMATION_H
+#define INC_SOC_ESTIMATION_H
+
+#include "SPI_Communication.h"
+
+#define SOCE_SHUNT_CURRENT_OFF_THRESH 20                  /* mA */
+#define CELL_VOLTAGE_CONVERSION_FACTOR 5.0f / 65535       /* V/quantum */
+#define BATTERY_CAPACITY (N_CELLS_PARALLEL * 2.5f * 3600) /* As */
+
+extern uint8_t current_soc;
+
+void estimate_soc();
+float calculate_soc_for_ocv(float ocv);
+
+#endif // INC_SOC_ESTIMATION_H

Core/Inc/util.h

@@ -0,0 +1,17 @@
+#ifndef INC_UTIL_H
+#define INC_UTIL_H
+
+#include <stdint.h>
+
+/**
+ * @brief Perform linear interpolation.
+ *
+ * @param n_points Size of source_x and source_y
+ * @param source_x x values for the interpolation source (sorted ascending)
+ * @param source_y y values corresponding to source_x
+ * @param target_x x value that a y value should be interpolated for
+ */
+float interp(uint32_t n_points, const float* source_x, const float* source_y,
+             float target_x);
+
+#endif // INC_UTIL_H

Core/Src/CAN_Communication.c

@@ -7,8 +7,9 @@
 
 #include "CAN_Communication.h"
 
-#include "stm32g4xx_hal_fdcan.h"
+#include "SoC_Estimation.h"
 
+#include "stm32g4xx_hal_fdcan.h"
 
 canFrame framebuffer[CANFRAMEBUFFERSIZE] = {0};
 uint8_t framebufferwritepointer;
@@ -181,7 +182,7 @@ void CAN_SendAbxStatus(FDCAN_HandleTypeDef* hcan) {
   buffer[0] = ctrltsstate.currentTSState | (1 << 7);
   buffer[1] = 160;
   buffer[2] = (uint8_t)(shuntvoltage1 / 2000);
-  buffer[3] = 240;
+  buffer[3] = current_soc;
   CAN_Transmit(hcan, AMS_STATUS_ID, buffer, 4);
 }
 

Core/Src/SPI_Communication.c

@@ -240,7 +240,7 @@ void InterSTMFrame(uint8_t targettsstate) {
                           (spirxbuf[n * 89 + 2] << 16) |
                           (spirxbuf[n * 89 + 3] << 8) | (spirxbuf[n * 89 + 4]);
 
-    for (int i = 0; i < NUMBEROFCELLS; i++) {
+    for (int i = 0; i < N_CELLS_SERIES; i++) {
       slaves[n].cellVoltages[i] =
           ((uint16_t)spirxbuf[n * 89 + 5 + 2 * i] << 8) |
           spirxbuf[n * 89 + 6 + 2 * i];

Core/Src/SoC_Estimation.c

@@ -0,0 +1,69 @@
+#include "SoC_Estimation.h"
+
+#include "CAN_Communication.h"
+#include "SPI_Communication.h"
+#include "util.h"
+
+#include <stdint.h>
+
+uint8_t current_soc = 0;
+
+static const float internal_resistance_curve_x[] = {2.0, 4.12};
+static const float internal_resistance_curve_y[] = {0.0528, 0.0294};
+#define INTERNAL_RESISTANCE_CURVE_PTS                                          \
+  (sizeof(internal_resistance_curve_x) / sizeof(*internal_resistance_curve_x))
+static const float soc_ocv_x[] = {2.1, 2.9,  3.2, 3.3,  3.4,
+                                  3.5, 3.68, 4.0, 4.15, 4.2};
+static const float soc_ocv_y[] = {0,     0.023, 0.06,  0.08,  0.119,
+                                  0.227, 0.541, 0.856, 0.985, 1.0};
+#define SOC_OCV_PTS (sizeof(soc_ocv_x) / sizeof(*soc_ocv_x))
+
+static int start_soc_known = 0;
+static float start_soc;
+static float start_as;
+static float min_cell_voltage;
+
+static void estimate_start_soc_off();
+static void estimate_start_soc_on();
+
+void estimate_soc() {
+  uint16_t min_v = 0xFFFF;
+  for (int slave = 0; slave < NUMBEROFSLAVES; slave++) {
+    for (int cell = 0; cell < N_CELLS_SERIES; cell++) {
+      uint16_t v = slaves[slave].cellVoltages[cell];
+      if (v < min_v) {
+        min_v = v;
+      }
+    }
+  }
+  min_cell_voltage = min_v * CELL_VOLTAGE_CONVERSION_FACTOR;
+
+  if (shuntcurrent < SOCE_SHUNT_CURRENT_OFF_THRESH) {
+    estimate_start_soc_off();
+  } else if (!start_soc_known) {
+    estimate_start_soc_on();
+  }
+
+  float soc = start_soc - (shuntampereseconds - start_as) / BATTERY_CAPACITY;
+  current_soc = soc * 255;
+}
+
+static void estimate_start_soc_on() {
+  float r_i = interp(INTERNAL_RESISTANCE_CURVE_PTS, internal_resistance_curve_x,
+                     internal_resistance_curve_y, min_cell_voltage);
+  float i = shuntcurrent / N_CELLS_PARALLEL;
+  float ocv = min_cell_voltage - i * r_i;
+  start_soc = calculate_soc_for_ocv(ocv);
+  start_as = shuntampereseconds;
+  start_soc_known = 1;
+}
+
+static void estimate_start_soc_off() {
+  start_soc = calculate_soc_for_ocv(min_cell_voltage);
+  start_as = shuntampereseconds;
+  start_soc_known = 1;
+}
+
+float calculate_soc_for_ocv(float ocv) {
+  return interp(SOC_OCV_PTS, soc_ocv_x, soc_ocv_y, ocv);
+}

Core/Src/util.c

@@ -0,0 +1,27 @@
+#include "util.h"
+
+#include <stdint.h>
+
+float interp(uint32_t n_points, const float* source_x, const float* source_y,
+             float target_x) {
+  uint32_t i;
+  for (i = 0; i < n_points; i++) {
+    if (source_x[i] > target_x) {
+      break;
+    }
+  }
+  if (i == 0) {
+    // target_x is smaller than the smallest value in source_x
+    i++;
+  }
+  if (i == n_points) {
+    // target_y is larger than the largest value in source_x
+    i--;
+  }
+  float x1 = source_x[i - 1];
+  float x2 = source_x[i];
+  float y1 = source_y[i - 1];
+  float y2 = source_y[i];
+  float slope = (y2 - y1) / (x2 - x1);
+  return y1 + slope * (target_x - x1);
+}