Integrate current

The current counter on the shunt can't be activated for some reason.

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;
 
@@ -28,6 +29,4 @@ void shunt_check();
 
 void shunt_handle_can_msg(uint16_t id, const uint8_t *data);
 
-int32_t shunt_getcurrent();
-
 #endif // INC_SHUNT_MONITORING_H

Core/Inc/slave_monitoring.h

@@ -49,6 +49,5 @@ void slaves_check();
 void slaves_handle_panic(const uint8_t *data);
 void slaves_handle_status(const uint8_t *data);
 void slaves_handle_log(const uint8_t *data);
-uint16_t slaves_get_minimum_voltage();
 
 #endif // INC_SLAVE_MONITORING_H

Core/Inc/soc_estimation.h

@@ -3,16 +3,16 @@
 
 #include <stdint.h>
 
-extern uint8_t current_soc;
-
-
-#define N_MODELPARAMETERS 11
-#define BATTERYCAPACITYAs (21000.0*3600) //TODO Check if value is correct Cap in Ah * 3600 (Convert to As)
-
+extern float current_soc;
 
 void soc_init();
-void soc_update(int32_t shunt_current);
-void soe_update();
-void soap_update();
+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/main.c

@@ -50,7 +50,6 @@
 /* Private variables ---------------------------------------------------------*/
 ADC_HandleTypeDef hadc2;
 
-
 CAN_HandleTypeDef hcan;
 
 UART_HandleTypeDef huart1;
@@ -92,7 +91,7 @@ static void loop_delay() {
  */
 int main(void) {
   /* USER CODE BEGIN 1 */
-  uint8_t soc_init_complete = 0;
+
   /* USER CODE END 1 */
 
   /* MCU Configuration--------------------------------------------------------*/
@@ -122,6 +121,7 @@ int main(void) {
   slaves_init();
   shunt_init();
   ts_sm_init();
+  soc_init();
   HAL_GPIO_WritePin(AMS_NERROR_GPIO_Port, AMS_NERROR_Pin, GPIO_PIN_SET);
   /* USER CODE END 2 */
 
@@ -138,17 +138,10 @@ int main(void) {
     slaves_check();
     shunt_check();
     ts_sm_update();
-    if(soc_init_complete){
-      soc_update(shunt_getcurrent());
-    }
-    else
-    {
-      soc_init();
-      soc_init_complete = 1;
-    }
+    soc_update();
     can_send_status();
-    loop_delay();
 
+    loop_delay();
   }
   /* USER CODE END 3 */
 }

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,15 +70,13 @@ 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;
     break;
   }
 }
-
-int32_t shunt_getcurrent()
-{
-  return shunt_data.current;
-}

Core/Src/slave_monitoring.c

@@ -136,14 +136,3 @@ void slaves_handle_status(const uint8_t *data) {
 void slaves_handle_log(const uint8_t *data) {
   // TODO
 }
-
-uint16_t slaves_get_minimum_voltage()
-{
-  uint16_t minvoltage = 50000;
-  for(uint8_t idx = 0; idx < N_SLAVES;idx++){
-    if(slaves->min_voltage < minvoltage){
-      min_voltage = slaves->min_voltage;
-    }
-  }
-  return minvoltage;
-}

Core/Src/soc_estimation.c

@@ -1,103 +1,91 @@
 #include "soc_estimation.h"
 
-#include <stdint.h>
+#include "shunt_monitoring.h"
 #include "slave_monitoring.h"
+
 #include "stm32f3xx_hal.h"
 
+#include <stddef.h>
+#include <stdint.h>
 
-//------------------------------------Battery RC and OCV-SoC Parameters-----------------------------------------
-//@Note Parameters were obtained by EIS Measurements at the start of the season
-//If the errror with this values is to large, consider retesting some cells
-const float SOC[N_MODELPARAMETERS]={0,0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1};
-const float R0[N_MODELPARAMETERS]={0.0089,0.0087,0.0090,0.0087,0.0087,0.0087,0.0088,0.0088,0.0087,0.0088,0.0089};
-const float R1[N_MODELPARAMETERS]={0.0164,0.0063,0.0050,0.0055,0.0051,0.0052,0.0057,0.0048,0.0059,0.0055,0.0061};
-const float C1[N_MODELPARAMETERS]={2.5694,0.2649,0.2876,0.2594,0.2415,0.2360,0.2946,0.2558,0.2818,0.2605,0.2763};
-const float OCV_Data[N_MODELPARAMETERS]={2.762504,3.326231,3.460875,3.57681,3.655326,3.738444,3.835977,3.925841,4.032575,4.078275,4.191449};
+#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},
+    {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;
+float mAs_before_current;
 
-
-float soc_approxparameterbysoc(float,float*, uint8_t);
-float soc_approxsocbyocv(float);
-uint8_t current_soc;
-float current_floatsoc;
-
-float batterycapacity;
-
-
-/**
- * @brief This Function initializes the SoC Prediction
- * @note Because SoC is initalized using the OCV-Curve of the Cell, it is necessary to obtain a valid value
- * for the lowest cell voltage before calling this function
- */
 void soc_init() {
-  float minvoltage = ((float)slaves_get_minimum_voltage())/1000;
-  current_floatsoc = soc_approxsocbyocv(minvoltage);
-  batterycapacity = BATTERYCAPACITYAs*current_floatsoc;
-  current_soc = (uint8_t)(current_floatsoc*100);
+  current_soc = 0;
+  last_current_time = 0;
+  current_was_flowing = 1;
 }
 
-/**
- * @brief Update Function for the State of Charge. Call this Function every time the shunt sends a new current
- * @note The SoC Prediction works using a Coulomb Counter to track the SoC. Alternativly and maybe more elegant
- * would be to track the SoC using the integrated current counter of the shunt.
- * @param shunt_current 
- */
-void soc_update(int32_t shunt_current) {
-  // TODO
-  static uint32_t lasttick = 0;
-
-  if(lasttick != 0)
-  {
-    uint32_t dt = HAL_GetTick() - lasttick;
-    batterycapacity += batterycapacity + ((float) dt*shunt_current)/1000;
-    current_floatsoc = batterycapacity/BATTERYCAPACITYAs;
-    current_soc = (uint8_t) (current_floatsoc*100);
-  }
-  
-  lasttick=HAL_GetTick();
-
-}
-
-void soe_update()
-{
-  //TODO
-}
-void soap_update()
-{
-  //TODO
-}
-
-float soc_approxparameterbysoc(float soc,float* lut, uint8_t lutlen)
-{
-  //TODO
-  return 0;
-}
-
-float soc_approxsocbyocv(float ocv)
-{
-
-  if(ocv < OCV_Data[0])
-    return 0;
-
-  if(ocv > OCV_Data[N_MODELPARAMETERS])
-    return 1;
-  //Iterate through OCV Lookup
-  uint8_t ocvindex = 0;
-  for(uint8_t i = 0; i < (N_MODELPARAMETERS-1);i++)
-  {
-    if((OCV_Data[i] <= ocv) && (OCV_Data[i+1] > ocv))
-    {
-      ocvindex = i;
+void soc_update() {
+  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;
+      mAs_before_current = shunt_data.current_counter;
     }
+    current_was_flowing = 1;
+  } else {
+    current_was_flowing = 0;
   }
-  
-  float m = (ocv-OCV_Data[ocvindex])/(OCV_Data[ocvindex+1]-OCV_Data[ocvindex]);
-  
-  float soc = (SOC[ocvindex+1] - SOC[ocvindex])*m + SOC[ocvindex];
 
-  return soc;
-  
+  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
+    float as_delta = shunt_data.current_counter - mAs_before_current;
+    current_soc =
+        soc_before_current + as_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;
+}