93 lines
2.6 KiB
C
93 lines
2.6 KiB
C
#include "soc_estimation.h"
|
|
#include <stdint.h>
|
|
|
|
#define SOC_ESTIMATION_NO_CURRENT_THRESH 1000 // mA
|
|
#define SOC_ESTIMATION_NO_CURRENT_TIME 100000 // ms
|
|
#define SOC_ESTIMATION_BATTERY_CAPACITY 28800 // mAs
|
|
|
|
#define MIN_CELL_VOLTAGE 3000
|
|
#define MAX_CELL_VOLTAGE 4130
|
|
|
|
|
|
// https://www.desmos.com/calculator/mm22vmxl2x
|
|
ocv_soc_pair_t OCV_SOC_PAIRS[] = {
|
|
{3000, 0.00f}, {3350, 10.00f}, {3450, 20.00f},
|
|
{3500, 30.00f}, {3530, 40.00f}, {3570, 50.00f},
|
|
{3600, 60.00f}, {3630, 70.00f}, {3700, 80.00f},
|
|
{3800, 90.00f}, {4130, 100.00f}
|
|
};
|
|
|
|
float current_soc;
|
|
|
|
int current_was_flowing;
|
|
uint32_t last_current_time;
|
|
float soc_before_current;
|
|
float mAs_before_current;
|
|
|
|
void soc_init() {
|
|
current_soc = 0;
|
|
last_current_time = 0;
|
|
current_was_flowing = 1;
|
|
}
|
|
|
|
void soc_update() {
|
|
uint32_t now = HAL_GetTick();
|
|
if (CURRENT_MEASUREMENT >= SOC_ESTIMATION_NO_CURRENT_THRESH) {
|
|
last_current_time = now;
|
|
if (!current_was_flowing) {
|
|
soc_before_current = current_soc;
|
|
mAs_before_current = CURRENT_MEASUREMENT;
|
|
}
|
|
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).
|
|
uint8_t id = 0;
|
|
uint16_t min_voltage = 0;
|
|
sm_check_battery_temperature(&id, &min_voltage);
|
|
current_soc = soc_for_ocv(min_voltage);
|
|
} else {
|
|
// Otherwise, use the current counter to update SoC
|
|
float as_delta = CURRENT_MEASUREMENT - mAs_before_current;
|
|
float soc_delta = as_delta / SOC_ESTIMATION_BATTERY_CAPACITY * 100;
|
|
current_soc = soc_before_current + soc_delta;
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|