173 lines
6.1 KiB
C
173 lines
6.1 KiB
C
#include "battery.h"
|
|
#define ADBMS_NO_LOGGING_DEFS // fix conflicting defines
|
|
#include "ADBMS_Driver.h"
|
|
#include "NTC.h"
|
|
#include "can.h"
|
|
#include "config_ADBMS6830.h"
|
|
#include "ts_state_machine.h"
|
|
#include <string.h>
|
|
#include <math.h>
|
|
#include "main.h"
|
|
|
|
#define SWO_LOG_PREFIX "[BATTERY] "
|
|
#include "swo_log.h"
|
|
|
|
#define MAX_ERRORS 4 // max number of errors in window before panic
|
|
#define MAX_ERRORS_WINDOW_SIZE 16 // size of the error window for error detection
|
|
|
|
#define MAX_TEMP 60 // max temperature in C
|
|
|
|
typeof(battery) battery = {};
|
|
|
|
static bool error_window[MAX_ERRORS_WINDOW_SIZE] = {};
|
|
static size_t error_window_index = 0;
|
|
static size_t error_count = 0;
|
|
|
|
static inline void update_error_window(bool error, int id) {
|
|
error_count -= error_window[error_window_index] ? 1 : 0;
|
|
error_count += error ? 1 : 0;
|
|
|
|
if (error_count >= MAX_ERRORS) {
|
|
can_send_error(TS_ERRORKIND_SLAVE_PANIC, id);
|
|
ts_sm_set_error_source(TS_ERROR_SOURCE_SLAVES, TS_ERRORKIND_SLAVE_PANIC, true);
|
|
} else {
|
|
ts_sm_set_error_source(TS_ERROR_SOURCE_SLAVES, TS_ERRORKIND_SLAVE_PANIC, false);
|
|
}
|
|
|
|
error_window[error_window_index] = error;
|
|
error_window_index += 1;
|
|
error_window_index %= MAX_ERRORS_WINDOW_SIZE;
|
|
}
|
|
|
|
HAL_StatusTypeDef battery_init(SPI_HandleTypeDef *hspi) {
|
|
// pull MSTR High for Microcontroller mode ADBMS6822
|
|
HAL_GPIO_WritePin(MSTR1_GPIO_Port, MSTR1_Pin, GPIO_PIN_SET);
|
|
auto const ret = AMS_Init(hspi);
|
|
if (ret.status != ADBMS_NO_ERROR) {
|
|
debug_log(LOG_LEVEL_ERROR, "Failed to initialize BMS: %s",
|
|
ADBMS_Status_ToString(ret.status));
|
|
if (ret.bms_id != -1) {
|
|
debug_log_cont(LOG_LEVEL_ERROR, " (on BMS ID: %hd)", ret.bms_id);
|
|
}
|
|
return HAL_ERROR;
|
|
}
|
|
|
|
// Initialize battery structure with default values
|
|
for (size_t i = 0; i < N_BMS; i++) {
|
|
battery.module[i].chip = &bms_data[i];
|
|
for (size_t j = 0; j < 10; j++) {
|
|
battery.module[i].cellTemps[j] = 0;
|
|
}
|
|
}
|
|
battery.pack.soc = 0.0f;
|
|
battery.pack.min_voltage = 0xFFFF;
|
|
battery.pack.max_voltage = 0;
|
|
battery.pack.min_temp = INT16_MAX;
|
|
battery.pack.max_temp = INT16_MIN;
|
|
|
|
debug_log(LOG_LEVEL_INFO, "Battery initialized successfully");
|
|
return HAL_OK;
|
|
}
|
|
|
|
[[gnu::optimize("no-math-errno")]]
|
|
HAL_StatusTypeDef battery_update() {
|
|
auto const ret = AMS_Idle_Loop();
|
|
if (ret.status != ADBMS_NO_ERROR) {
|
|
debug_log(LOG_LEVEL_ERROR, "Error while updating battery data: %s",
|
|
ADBMS_Status_ToString(ret.status));
|
|
if (ret.bms_id != -1) {
|
|
debug_log_cont(LOG_LEVEL_ERROR, " (on BMS ID: %hd)", ret.bms_id);
|
|
}
|
|
|
|
if (ret.status == ADBMS_OVERVOLT || ret.status == ADBMS_UNDERVOLT) {
|
|
if (ret.bms_id != -1 && ret.bms_id < N_BMS) {
|
|
const char* error_type = (ret.status == ADBMS_OVERVOLT) ? "overvoltage" : "undervoltage";
|
|
const uint32_t voltage_flags = (ret.status == ADBMS_OVERVOLT) ?
|
|
bms_data[ret.bms_id].overVoltage :
|
|
bms_data[ret.bms_id].underVoltage;
|
|
|
|
debug_log(LOG_LEVEL_ERROR, "Cell %s detected on module %d, affected cells: ",
|
|
error_type, ret.bms_id);
|
|
|
|
for (size_t cell = 0; cell < N_CELLS; cell++) {
|
|
if (voltage_flags & (1UL << cell)) {
|
|
debug_log_cont(LOG_LEVEL_ERROR, "%u (%d mV) ", cell, bms_data[ret.bms_id].cellVoltages[cell]);
|
|
}
|
|
}
|
|
|
|
if (!voltage_flags) {
|
|
debug_log_cont(LOG_LEVEL_ERROR, "none (something went wrong?)");
|
|
}
|
|
}
|
|
}
|
|
|
|
update_error_window(true, ret.bms_id);
|
|
return HAL_ERROR;
|
|
}
|
|
|
|
update_error_window(false, ret.bms_id);
|
|
|
|
battery.pack.min_voltage = 0xFFFF;
|
|
battery.pack.max_voltage = 0;
|
|
battery.pack.min_temp = INT16_MAX;
|
|
battery.pack.max_temp = INT16_MIN;
|
|
|
|
for (size_t i = 0; i < N_BMS; i++) {
|
|
// Initialize min/max indices for this module
|
|
battery.module[i].min_v_idx = 0;
|
|
battery.module[i].max_v_idx = 0;
|
|
battery.module[i].min_t_idx = 0;
|
|
battery.module[i].max_t_idx = 0;
|
|
|
|
// Track min/max voltages
|
|
for (size_t j = 0; j < N_CELLS; j++) {
|
|
if (bms_data[i].cellVoltages[j] < battery.pack.min_voltage) {
|
|
battery.pack.min_voltage = bms_data[i].cellVoltages[j];
|
|
}
|
|
if (bms_data[i].cellVoltages[j] > battery.pack.max_voltage) {
|
|
battery.pack.max_voltage = bms_data[i].cellVoltages[j];
|
|
}
|
|
|
|
// Update min/max voltage indices for this module
|
|
if (bms_data[i].cellVoltages[j] < bms_data[i].cellVoltages[battery.module[i].min_v_idx]) {
|
|
battery.module[i].min_v_idx = j;
|
|
}
|
|
if (bms_data[i].cellVoltages[j] > bms_data[i].cellVoltages[battery.module[i].max_v_idx]) {
|
|
battery.module[i].max_v_idx = j;
|
|
}
|
|
}
|
|
|
|
// Process temperature values
|
|
for (size_t j = 0; j < 10; j++) { //10 GPIOs
|
|
battery.module[i].cellTemps[j] = ntc_mv_to_celsius(bms_data[i].auxVoltages[j]);
|
|
|
|
|
|
// For new battery struct
|
|
if (battery.module[i].cellTemps[j] > battery.pack.max_temp) {
|
|
battery.pack.max_temp = battery.module[i].cellTemps[j];
|
|
}
|
|
if (battery.module[i].cellTemps[j] < battery.pack.min_temp) {
|
|
battery.pack.min_temp = battery.module[i].cellTemps[j];
|
|
}
|
|
|
|
// Update min/max temperature indices for this module
|
|
if (battery.module[i].cellTemps[j] < battery.module[i].cellTemps[battery.module[i].min_t_idx]) {
|
|
battery.module[i].min_t_idx = j;
|
|
}
|
|
if (battery.module[i].cellTemps[j] > battery.module[i].cellTemps[battery.module[i].max_t_idx]) {
|
|
battery.module[i].max_t_idx = j;
|
|
}
|
|
|
|
// Check for overtemperature condition
|
|
if (battery.module[i].cellTemps[j] > (MAX_TEMP * (uint16_t)(TEMP_CONV))) {
|
|
debug_log(LOG_LEVEL_ERROR, "Cell %u on BMS %u overtemp: %d0 mC", j, i, battery.module[i].cellTemps[j]);
|
|
can_send_error(TS_ERRORKIND_CELL_OVERTEMP, i);
|
|
ts_sm_set_error_source(TS_ERROR_SOURCE_SLAVES, TS_ERRORKIND_CELL_OVERTEMP, true);
|
|
} else {
|
|
ts_sm_set_error_source(TS_ERROR_SOURCE_SLAVES, TS_ERRORKIND_CELL_OVERTEMP, false);
|
|
}
|
|
}
|
|
}
|
|
|
|
return HAL_OK;
|
|
} |