Implement SoC estimation (UNTESTED!)
This commit is contained in:
parent
b1c21a981c
commit
1d70429708
|
@ -25,7 +25,8 @@
|
||||||
#define TOGGLE_STATUS_LED 0x06
|
#define TOGGLE_STATUS_LED 0x06
|
||||||
|
|
||||||
#define NUMBEROFSLAVES 6
|
#define NUMBEROFSLAVES 6
|
||||||
#define NUMBEROFCELLS 10
|
#define N_CELLS_SERIES 10
|
||||||
|
#define N_CELLS_PARALLEL 9
|
||||||
#define NUMBEROFTEMPS 32
|
#define NUMBEROFTEMPS 32
|
||||||
|
|
||||||
#define SLAVE_TIMEOUT 200
|
#define SLAVE_TIMEOUT 200
|
||||||
|
@ -33,7 +34,7 @@
|
||||||
typedef struct {
|
typedef struct {
|
||||||
|
|
||||||
uint16_t slaveID;
|
uint16_t slaveID;
|
||||||
uint16_t cellVoltages[NUMBEROFCELLS];
|
uint16_t cellVoltages[N_CELLS_SERIES];
|
||||||
uint16_t cellTemps[NUMBEROFTEMPS];
|
uint16_t cellTemps[NUMBEROFTEMPS];
|
||||||
uint32_t timestamp;
|
uint32_t timestamp;
|
||||||
uint8_t error;
|
uint8_t error;
|
||||||
|
|
|
@ -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
|
|
@ -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
|
|
@ -7,8 +7,9 @@
|
||||||
|
|
||||||
#include "CAN_Communication.h"
|
#include "CAN_Communication.h"
|
||||||
|
|
||||||
#include "stm32g4xx_hal_fdcan.h"
|
#include "SoC_Estimation.h"
|
||||||
|
|
||||||
|
#include "stm32g4xx_hal_fdcan.h"
|
||||||
|
|
||||||
canFrame framebuffer[CANFRAMEBUFFERSIZE] = {0};
|
canFrame framebuffer[CANFRAMEBUFFERSIZE] = {0};
|
||||||
uint8_t framebufferwritepointer;
|
uint8_t framebufferwritepointer;
|
||||||
|
@ -181,7 +182,7 @@ void CAN_SendAbxStatus(FDCAN_HandleTypeDef* hcan) {
|
||||||
buffer[0] = ctrltsstate.currentTSState | (1 << 7);
|
buffer[0] = ctrltsstate.currentTSState | (1 << 7);
|
||||||
buffer[1] = 160;
|
buffer[1] = 160;
|
||||||
buffer[2] = (uint8_t)(shuntvoltage1 / 2000);
|
buffer[2] = (uint8_t)(shuntvoltage1 / 2000);
|
||||||
buffer[3] = 240;
|
buffer[3] = current_soc;
|
||||||
CAN_Transmit(hcan, AMS_STATUS_ID, buffer, 4);
|
CAN_Transmit(hcan, AMS_STATUS_ID, buffer, 4);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -240,7 +240,7 @@ void InterSTMFrame(uint8_t targettsstate) {
|
||||||
(spirxbuf[n * 89 + 2] << 16) |
|
(spirxbuf[n * 89 + 2] << 16) |
|
||||||
(spirxbuf[n * 89 + 3] << 8) | (spirxbuf[n * 89 + 4]);
|
(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] =
|
slaves[n].cellVoltages[i] =
|
||||||
((uint16_t)spirxbuf[n * 89 + 5 + 2 * i] << 8) |
|
((uint16_t)spirxbuf[n * 89 + 5 + 2 * i] << 8) |
|
||||||
spirxbuf[n * 89 + 6 + 2 * i];
|
spirxbuf[n * 89 + 6 + 2 * i];
|
||||||
|
|
|
@ -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);
|
||||||
|
}
|
|
@ -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);
|
||||||
|
}
|
Loading…
Reference in New Issue