diff --git a/Core/Inc/SPI_Communication.h b/Core/Inc/SPI_Communication.h index 3c9cd36..3b99d7c 100644 --- a/Core/Inc/SPI_Communication.h +++ b/Core/Inc/SPI_Communication.h @@ -25,7 +25,8 @@ #define TOGGLE_STATUS_LED 0x06 #define NUMBEROFSLAVES 6 -#define NUMBEROFCELLS 10 +#define N_CELLS_SERIES 10 +#define N_CELLS_PARALLEL 9 #define NUMBEROFTEMPS 32 #define SLAVE_TIMEOUT 200 @@ -33,7 +34,7 @@ typedef struct { uint16_t slaveID; - uint16_t cellVoltages[NUMBEROFCELLS]; + uint16_t cellVoltages[N_CELLS_SERIES]; uint16_t cellTemps[NUMBEROFTEMPS]; uint32_t timestamp; uint8_t error; diff --git a/Core/Inc/SoC_Estimation.h b/Core/Inc/SoC_Estimation.h new file mode 100644 index 0000000..47623cc --- /dev/null +++ b/Core/Inc/SoC_Estimation.h @@ -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 diff --git a/Core/Inc/util.h b/Core/Inc/util.h new file mode 100644 index 0000000..8e451da --- /dev/null +++ b/Core/Inc/util.h @@ -0,0 +1,17 @@ +#ifndef INC_UTIL_H +#define INC_UTIL_H + +#include + +/** + * @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 diff --git a/Core/Src/CAN_Communication.c b/Core/Src/CAN_Communication.c index 2cd3749..8df3318 100644 --- a/Core/Src/CAN_Communication.c +++ b/Core/Src/CAN_Communication.c @@ -7,8 +7,9 @@ #include "CAN_Communication.h" -#include "stm32g4xx_hal_fdcan.h" +#include "SoC_Estimation.h" +#include "stm32g4xx_hal_fdcan.h" canFrame framebuffer[CANFRAMEBUFFERSIZE] = {0}; uint8_t framebufferwritepointer; @@ -181,7 +182,7 @@ void CAN_SendAbxStatus(FDCAN_HandleTypeDef* hcan) { buffer[0] = ctrltsstate.currentTSState | (1 << 7); buffer[1] = 160; buffer[2] = (uint8_t)(shuntvoltage1 / 2000); - buffer[3] = 240; + buffer[3] = current_soc; CAN_Transmit(hcan, AMS_STATUS_ID, buffer, 4); } diff --git a/Core/Src/SPI_Communication.c b/Core/Src/SPI_Communication.c index f0e9049..add21aa 100644 --- a/Core/Src/SPI_Communication.c +++ b/Core/Src/SPI_Communication.c @@ -240,7 +240,7 @@ void InterSTMFrame(uint8_t targettsstate) { (spirxbuf[n * 89 + 2] << 16) | (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] = ((uint16_t)spirxbuf[n * 89 + 5 + 2 * i] << 8) | spirxbuf[n * 89 + 6 + 2 * i]; diff --git a/Core/Src/SoC_Estimation.c b/Core/Src/SoC_Estimation.c new file mode 100644 index 0000000..3515d1b --- /dev/null +++ b/Core/Src/SoC_Estimation.c @@ -0,0 +1,69 @@ +#include "SoC_Estimation.h" + +#include "CAN_Communication.h" +#include "SPI_Communication.h" +#include "util.h" + +#include + +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); +} diff --git a/Core/Src/util.c b/Core/Src/util.c new file mode 100644 index 0000000..919096e --- /dev/null +++ b/Core/Src/util.c @@ -0,0 +1,27 @@ +#include "util.h" + +#include + +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); +}