Implementation of SoC Prediction

untested. Maybe revert to Jaspers Version

Core/Inc/shunt_monitoring.h

@@ -28,4 +28,6 @@ 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,5 +49,6 @@ 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

@@ -5,7 +5,14 @@
 
 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)
+
+
 void soc_init();
-void soc_update();
+void soc_update(int32_t shunt_current);
+void soe_update();
+void soap_update();
 
 #endif // INC_SOC_ESTIMATION_H

Core/Src/main.c

@@ -50,6 +50,7 @@
 /* Private variables ---------------------------------------------------------*/
 ADC_HandleTypeDef hadc2;
 
+
 CAN_HandleTypeDef hcan;
 
 UART_HandleTypeDef huart1;
@@ -91,7 +92,7 @@ static void loop_delay() {
  */
 int main(void) {
   /* USER CODE BEGIN 1 */
-
+  uint8_t soc_init_complete = 0;
   /* USER CODE END 1 */
 
   /* MCU Configuration--------------------------------------------------------*/
@@ -121,7 +122,6 @@ 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,10 +138,17 @@ int main(void) {
     slaves_check();
     shunt_check();
     ts_sm_update();
-    soc_update();
+    if(soc_init_complete){
+      soc_update(shunt_getcurrent());
+    }
+    else
+    {
+      soc_init();
+      soc_init_complete = 1;
+    }
     can_send_status();
-
     loop_delay();
+
   }
   /* USER CODE END 3 */
 }

Core/Src/shunt_monitoring.c

@@ -70,3 +70,8 @@ void shunt_handle_can_msg(uint16_t id, const uint8_t *data) {
     break;
   }
 }
+
+int32_t shunt_getcurrent()
+{
+  return shunt_data.current;
+}

Core/Src/slave_monitoring.c

@@ -136,3 +136,14 @@ 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,14 +1,103 @@
 #include "soc_estimation.h"
 
 #include <stdint.h>
+#include "slave_monitoring.h"
+#include "stm32f3xx_hal.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};
+
+//---------------------------------------------------------------------------------------------------------------
+
+
+
+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() {
-  current_soc = 0;
-  // TODO
+  float minvoltage = ((float)slaves_get_minimum_voltage())/1000;
+  current_floatsoc = soc_approxsocbyocv(minvoltage);
+  batterycapacity = BATTERYCAPACITYAs*current_floatsoc;
+  current_soc = (uint8_t)(current_floatsoc*100);
 }
 
-void soc_update() {
+/**
+ * @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;
+    }
+  }
+  
+  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;
+  
+
+}