71 lines
2.2 KiB
C
71 lines
2.2 KiB
C
#include "SoC_Estimation.h"
|
|
|
|
#include "CAN_Communication.h"
|
|
#include "Check_Shunt_Limits.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 < N_SLAVES; 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 (shunt_data.current < SOCE_SHUNT_CURRENT_OFF_THRESH) {
|
|
estimate_start_soc_off();
|
|
} else if (!start_soc_known) {
|
|
estimate_start_soc_on();
|
|
}
|
|
|
|
float soc =
|
|
start_soc - (shunt_data.ampere_seconds - 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 = ((float)shunt_data.current) / N_CELLS_PARALLEL;
|
|
float ocv = min_cell_voltage - i * r_i;
|
|
start_soc = calculate_soc_for_ocv(ocv);
|
|
start_as = shunt_data.ampere_seconds;
|
|
start_soc_known = 1;
|
|
}
|
|
|
|
static void estimate_start_soc_off() {
|
|
start_soc = calculate_soc_for_ocv(min_cell_voltage);
|
|
start_as = shunt_data.ampere_seconds;
|
|
start_soc_known = 1;
|
|
}
|
|
|
|
float calculate_soc_for_ocv(float ocv) {
|
|
return interp(SOC_OCV_PTS, soc_ocv_x, soc_ocv_y, ocv);
|
|
}
|