bms/Application/app/sox/health.c

#include "bsp/gpio.h"
#include "bsp/ml5238.h"
#include "libs/logger.h"
#include "state.h"
#include "iostate.h"
#include "measure.h"
#include "measure_task.h"
#include "health.h"
#include "Least_Square.h"

#if 0
#define MIN_VOLTAGE_FOR_DISCHARGER (2.2f * CELLS_NUM * 1000) //允许能放电的最小电压
#define MIN_VOLTAGE_FOR_RECOVERY_DISCHARGER (2.3f * CELLS_NUM * 1000) //恢复放电的最小电压
#define MIN_VOLTAGE_FOR_POWER_DOWN (2.1f * CELLS_NUM* 1000)
#define SIGLE_CELL_LOWER_DISCHARGER_VOLTAGE (1820) //最小允许的电芯放电电压 1.8v, 考虑到采样的误差取 1.82
#endif

static int8_t charger_normal_low_temp[PACK_TEMPS_NUM] = {0,0,0,-5}; //正常的充电最低温度
static int8_t charger_normal_high_temp[PACK_TEMPS_NUM] = {50,50,50,75}; //正常的充电最高温度
static int8_t charger_lower_low_temp[PACK_TEMPS_NUM] = {-1,-1,-1,-6}; //需要停止充电的最低温度
static int8_t charger_higher_high_temp[PACK_TEMPS_NUM] = {55,55,55,85}; //需要停止充电的最高温度

static int8_t discharger_normal_low_temp[PACK_TEMPS_NUM] = {-20,-20,-20,-25};//正常的放电最低温度
static int8_t discharger_normal_high_temp[PACK_TEMPS_NUM] = {55,55,55,75};//正常的放电最高温度
static int8_t discharger_lower_low_temp[PACK_TEMPS_NUM] = {-25,-25,-25,-30}; //需要停止放电的最低温度
static int8_t discharger_higher_high_temp[PACK_TEMPS_NUM] = {60,60,60,85};//需要停止放电的最高温度
static int8_t work_lower_temp[PACK_TEMPS_NUM - 1] = {0,0,0}; //pcb温度不用判断
static int8_t work_lower_temp_recovry[PACK_TEMPS_NUM - 1] = {5,5,5}; //pcb温度不用判断

/*定义低温和正常温度下的电池保护参数, [0]低温参数, [1]常温参数 */
/*能提供动力的最小电压*/
static float min_discharger_power_vol[] = {32000, 38000}; //允许能提供动力的最小电压
static float min_discharger_power_recovery_vol[] = {34000, 40000}; //恢复能提供动力的最小电压
static float min_discharger_power_cell_vol[] = {2100, 2600}; //允许能提供动力的最小电芯电压
static float min_discharger_power_recovery_cell_vol[] = {2200, 2700}; //恢复能提供动力的最小电芯电压

/*能提供大电的最小电压*/
static float min_discharger_vol[] = {30000, 36000};//允许能放电的最小电压
static float min_discharger_recovery_vol[] = {32000, 40000};//恢复放电的最小电压
static float min_discharger_cell_vol[] = {1900, 2500};//允许能放电的最小电芯电压
static float min_discharger_cell_recovery_vol[] = {2000, 2600};//恢复放电的最小电芯电压

/*电池PowerDown的最小电压 */
static float min_discharger_pdown_vol[] = {28000, 34000}; //power down的最小电压
static float min_discharger_pdown_cell_vol[] = {1900, 2200};                        //power down的最小电芯电压

#define MAX_TRY_FOR_AUX_SHORT 10

/* health 模块，只检测状态，不做任何控制,如果有异常情况，控制中心会统一处理  */
static void check_ml5238_state(int event);
static void load_detect_handler(shark_timer_t *timer);
static void clear_short_current_handler(shark_timer_t *timer);
static void charger_detect_handler(shark_timer_t *timer);
static void _aux_lock_timer_handler(shark_timer_t *t);
static void _aux_unlock_timer_handler(shark_timer_t *t);
void soft_current_init(void);
int soft_current_push(float current_ma);
static bms_health_t _health;
static debounce_timer_t _load_detect_timer = {.max_count = 100, .interval = 10, ._timer.handler = load_detect_handler};
static debounce_timer_t _charger_detect_timer = {.max_count = 500, .interval = 10, ._timer.handler = charger_detect_handler};
static shark_timer_t _clear_short_current_timer = {.handler = clear_short_current_handler};
static error_counts_t error_counts;
static u16 discharger_lower_cell_voltage = 0;
static u16 discharger_lower_voltage = 0;

void health_init(void){
	/* 5238如果有异常情况，比如短路，负载移除，通过这个handler上报 */
	ml5238_register_notify_handler(check_ml5238_state);
	soft_current_init();
	set_log_level(MOD_HEALTH, L_debug);
	for (int i = 0; i < CELLS_NUM; i++){
		_health.internal_resistance[i] = 1;//毫欧,暂时用一个固定数据，后期需要计算R0=(U2-U1)/(I1-I2) - R1(R1为电路上的等效电阻+采样电阻)
	}
	_health.is_work_temp_normal = 1;
}

void health_log(void){
	health_debug("soft short:%d\n", error_counts.soft_current_short);
	health_debug("hard short:%d\n", error_counts.hard_current_short);
	health_debug("work temp: %d\n", _health.is_work_temp_normal);
	health_debug("aux_short: %d, %d\n", error_counts.aux_short, error_counts.aux_real_short);
	health_debug("lower voltage: %d, %d, %d, %d\n", discharger_lower_cell_voltage, discharger_lower_voltage, error_counts.cell_under_voltage, error_counts.pack_under_voltage);
	health_debug("uart error %d, %d, %d\n", error_counts.uart_crc_error, error_counts.uart_len_error, error_counts.uart_dir_error);
	health_debug("Temp abnormal: %d,%d,%d,%d\n", error_counts.discharger_high_temp, error_counts.charger_high_temp, error_counts.discharger_lower_temp, error_counts.charger_lower_temp);
}

bms_health_t *bms_health(){
	return &_health;
}

void health_add_uart_error(uint32_t c, uint32_t l, uint32_t d) {
	error_counts.uart_crc_error += c;
	error_counts.uart_len_error += l;
	error_counts.uart_dir_error += d;
}

uint32_t bms_health_pack_lower_voltage(void){
	return min_discharger_vol[_health.is_work_temp_normal];
}

uint32_t bms_health_cell_lower_voltage(void){
	return min_discharger_cell_vol[_health.is_work_temp_normal];
}


static void clear_short_current_handler(shark_timer_t *timer){
	_health.load_current_short = 0; //负载移除，clear load current short
	health_warning("clear load current short\n");
}

static void load_detect_handler(shark_timer_t *timer){
	if (ml5238_is_load_disconnect()){
		_load_detect_timer.count ++;
	}else {
		_load_detect_timer.count = 0;
	}
	if (_load_detect_timer.count >= _load_detect_timer.max_count) {
		ml5238_enable_load_detect(0);
		_load_detect_timer.count = 0;
		shark_timer_post(&_clear_short_current_timer, 60 * 1000); //负载移除1分钟后，清除current short flags, can open discharger again
		health_warning("load disconnect\n");
	}else {
		shark_timer_post(&_load_detect_timer._timer, _load_detect_timer.interval);
	}
}

static void charger_detect_handler(shark_timer_t *timer){
	if (!io_state()->charger_detect || !bms_state_get()->charging) {
		_charger_detect_timer.count ++;
	}else {
		_charger_detect_timer.count = 0;
	}
	if (_charger_detect_timer.count >= _charger_detect_timer.max_count){
		_health.charger_over_current = 0;
		_charger_detect_timer.count = 0;
		health_warning("clear charger over current\n");
	}else {
		shark_timer_post(&_charger_detect_timer._timer, _charger_detect_timer.interval);
	}
}

static void check_ml5238_state(int event){
	health_warning("ml5238 event=0x%x\n", event);
	if (event == ML5238_Event_Charger_Over_Current){
		shark_timer_post(&_charger_detect_timer._timer, _charger_detect_timer.interval);
	}else if (event == ML5238_Event_Short_Current) { //ml5238触发短路保护,充放电mos全部关闭
		_health.load_current_short = 1;
		error_counts.hard_current_short ++;
		ml5238_enable_load_detect(1); //打开负载检测
		shark_timer_post(&_load_detect_timer._timer, _load_detect_timer.interval);
	}else if (event == ML5238_Event_Load_Disconnect) {
		shark_timer_post(&_load_detect_timer._timer, _load_detect_timer.interval);
	}
}

static void debug_health(void){
	uint32_t *value = (uint32_t *)&_health;
	if (*value != 0){
		//health_error("health value = 0x%x\n", *value);
	}
}

/* 检测电流情况，看是否过流等 */
static debounce_t _charger_over_current = {
	.count = 0,
	.max_count = 70
};

/* 55 - 100A, 14 - I x I / 750 */
void check_current_state(void){
	float current = measure_value()->load_current;
	if (bms_state_get()->charging) {
		//_discharger_over_current.count = 0;
		if (!_health.charger_over_current) {
			if (current > MAX_CURRENT_FOR_CHARGER) {
				_charger_over_current.count ++;
			}else {
				_charger_over_current.count = 0;
			}
			if (_charger_over_current.count >= _charger_over_current.max_count){
				_health.charger_over_current = 1;
				_charger_over_current.count = 0;
				health_warning("charger over current\n");
				shark_timer_post(&_charger_detect_timer._timer, _charger_detect_timer.interval);
			}
		}
	}else{
		_charger_over_current.count = 0;
		if (!_health.load_current_short){
			if (soft_current_push(current)) {
				_health.load_current_short = 1;
				error_counts.soft_current_short ++;
				//_discharger_over_current.count = 0;
				ml5238_enable_load_detect(1); //打开负载检测
				shark_timer_post(&_load_detect_timer._timer, _load_detect_timer.interval);
				soft_current_init();
			}		
		}
	}	
}

/* 检测pack电压，cell电压，pack电压过低触发powerdown*/
static debounce_t _discharger_lower_voltage = {.count = 0, .max_count = 20, .init_count = 0};
static debounce_t _power_down_voltage = {.count = 0, .max_count = 20, .init_count = 0};
static debounce_t _sigle_cell_discharger_lower_vol = {.count = 0, .max_count = 100, .init_count = 0};
static debounce_t _sigle_cell_charger_max_vol = {.count = 0, .max_count = 20, .init_count = 0};
static debounce_t _shut_discharger_lower_voltage = {.count = 0, .max_count = 20,.init_count = 0};
static debounce_t _shut_discharger_cell_lower_voltage = {.count = 0, .max_count = 400,.init_count = 0};

static int judge_debounce(int input, debounce_t *d){
	if (input) {
		d->count ++;
		if (d->count >= d->max_count){
			d->count = 0;
			return 1;
		}
		return 0;
	}else {
		d->count = d->init_count;
		return 0;
	}
}

static int _can_powerdown(void){
	if (io_state()->charger_detect_irq || bms_state_get()->charging || !bms_work_is_normal()){
		return 0;
	}
	if ((bms_state_get()->pack_voltage <= min_discharger_pdown_vol[_health.is_work_temp_normal] ||
			bms_state_get()->cell_min_vol <= min_discharger_pdown_cell_vol[_health.is_work_temp_normal])){
		return 1;
	}
	return 0;
}


void check_voltage_state(void) {
	if (bms_state_get()->charging){ //check sigle cell's voltage for charger
		_health.discharger_shutpower_voltage = 0;
		_health.sigle_cell_lower_voltage = 0;
		_health.discharger_lower_voltage = 0;
		_health.discharger_cell_shutpower_voltage = 0;
		if ((bms_state_get()->cell_max_vol>= SIGLE_CELL_MAX_CHARGER_VOLTAGE)){
			if (judge_debounce(!_health.sigle_cell_over_voltage, &_sigle_cell_charger_max_vol)){
				_health.sigle_cell_over_voltage = 1;
				sys_debug("sigle cell %d\n", bms_state_get()->cell_max_vol);
			}
		}else if ((bms_state_get()->cell_max_vol < SIGLE_CELL_MAX_CHARGER_VOLTAGE)){
			if (judge_debounce(_health.sigle_cell_over_voltage, &_sigle_cell_charger_max_vol)){
				_health.sigle_cell_over_voltage = 0;
			}
		}
		_health.charger_over_voltage = _health.sigle_cell_over_voltage;
	}else{ 
		//check sigle cell's voltage for discharger
		_health.charger_over_voltage = _health.sigle_cell_over_voltage = 0;
		
		if ((bms_state_get()->cell_min_vol <= min_discharger_cell_vol[_health.is_work_temp_normal])){
			if (judge_debounce(!_health.sigle_cell_lower_voltage, &_sigle_cell_discharger_lower_vol)){
				_health.sigle_cell_lower_voltage = 1;
				error_counts.cell_under_voltage++;
				discharger_lower_cell_voltage = bms_state_get()->cell_min_vol;
			}
		}else if ((bms_state_get()->cell_min_vol >= min_discharger_cell_recovery_vol[_health.is_work_temp_normal])){
			if (judge_debounce(_health.sigle_cell_lower_voltage, &_sigle_cell_discharger_lower_vol)){
				_health.sigle_cell_lower_voltage = 0;
			}
		}
		//check sigle pack's voltage for discharger
		if (bms_state_get()->pack_voltage <= min_discharger_vol[_health.is_work_temp_normal]){
			if (judge_debounce(!_health.discharger_lower_voltage, &_discharger_lower_voltage)){
				_health.discharger_lower_voltage = 1;
				error_counts.pack_under_voltage++;
				discharger_lower_voltage = bms_state_get()->pack_voltage;
			}
		}else if (bms_state_get()->pack_voltage >= min_discharger_recovery_vol[_health.is_work_temp_normal]){
			if (judge_debounce(_health.discharger_lower_voltage, &_discharger_lower_voltage)){
				_health.discharger_lower_voltage = 0;
			}
		}
		//check for shutdown power
		if ((bms_state_get()->cell_min_vol <= min_discharger_power_cell_vol[_health.is_work_temp_normal])){
			if (judge_debounce(!_health.discharger_cell_shutpower_voltage, &_shut_discharger_cell_lower_voltage)){
				_health.discharger_cell_shutpower_voltage = 1;
				discharger_lower_cell_voltage = bms_state_get()->cell_min_vol;
			}
		}else if ((bms_state_get()->cell_min_vol >= min_discharger_power_recovery_cell_vol[_health.is_work_temp_normal])){
			if (judge_debounce(_health.discharger_cell_shutpower_voltage, &_shut_discharger_cell_lower_voltage)){
				_health.discharger_cell_shutpower_voltage = 0;
			}
		}
		if ((bms_state_get()->pack_voltage <= min_discharger_power_vol[_health.is_work_temp_normal])){
			if (judge_debounce(!_health.discharger_shutpower_voltage, &_shut_discharger_lower_voltage)){
				_health.discharger_shutpower_voltage = 1;
				discharger_lower_voltage = bms_state_get()->pack_voltage;
			}
		}else if ((bms_state_get()->pack_voltage >= min_discharger_power_recovery_vol[_health.is_work_temp_normal])){
			if (judge_debounce(_health.discharger_shutpower_voltage, &_shut_discharger_lower_voltage)){
				_health.discharger_shutpower_voltage = 0;
			}
		}
	}	

	/* check for power down */
	if (_can_powerdown()){
		if (judge_debounce(!_health.powerdown_lower_voltage, &_power_down_voltage)) {
			/*
			 * no need to clear powerdown(bms is shutdown), when charger insert, 
			 * system will power on with powerdown_lower_voltage cleared
			*/			
			_health.powerdown_lower_voltage = 1;
		}
	}
	debug_health();
}


/* 检测温度情况，看是否过高温，或者过低温 */

static debounce_t _charger_over_temp_count      = {.count = 0, .max_count = 8, .init_count = 0};
static debounce_t _charger_lower_temp_count     = {.count = 0, .max_count = 8, .init_count = 0};
static debounce_t _charger_normal_temp_count    = {.count = 0, .max_count = 8, .init_count = 0};
static debounce_t _discharger_over_temp_count   = {.count = 0, .max_count = 8, .init_count = 0};
static debounce_t _discharger_lower_temp_count  = {.count = 0, .max_count = 8, .init_count = 0};
static debounce_t _discharger_normal_temp_count = {.count = 0, .max_count = 8, .init_count = 0};
static debounce_t _work_lower_temp_count = 		{.count = 0, .max_count = 8, .init_count = 0};

static int _is_over_temp(int8_t *temps, int size){
	int count = 0;
	for (int i = 0; i < size; i++){
		if (measure_value()->pack_temp[i] >= temps[i]){
			count ++;
		}
	}
	return count;
}

static int _is_low_temp(int8_t *temps, int size){
	int count = 0;
	for (int i = 0; i < size; i++){
		if (measure_value()->pack_temp[i] < temps[i]){
			count ++;
		}
	}
	return count;
}

static uint8_t small_power_detect_count = 0;
static shark_timer_t _aux_lock_timer = {.handler = _aux_lock_timer_handler};
static shark_timer_t _aux_unlock_timer = {.handler = _aux_unlock_timer_handler};

u32 _aux_unlock_delay(float voltage){
	float aux_current = voltage / SMALL_CURRENT_R;
	return aux_current * 10 * 1000; //ms
}

static void _aux_lock_timer_handler(shark_timer_t *t){
	AUX_VOL_OPEN(1);
	if (++small_power_detect_count >= MAX_TRY_FOR_AUX_SHORT){
		//端口电压小于阈值，判断为小电流短路
		int short_voltage = get_small_current_voltage()/1000;
		int pack_voltage = bms_state_get()->pack_voltage/1000;
		if (short_voltage >= AUX_SHORT_DIFF_VOLTAGE) {
			_health.small_current_short = 1;
			error_counts.aux_short ++;
			AUX_VOL_OPEN(0);
			small_power_detect_count = 0;
			u32 delay_time = _aux_unlock_delay(short_voltage);
			if (short_voltage >= (pack_voltage - AUX_SHORT_REAL_DIFF_VOLTAGE)){ //real short
				error_counts.aux_real_short ++;
				_health.small_current_real_short = 1;
				delay_time = 30 * 1000;
			}
			shark_timer_post( &_aux_lock_timer, delay_time); //30s后再次尝试打开
			shark_timer_cancel(&_aux_unlock_timer);			
			health_debug("aux short, v:%d, and retry after %ds\n", short_voltage, delay_time/1000);
		}
	}else {
		health_debug("open aux[re-enable], %lld\n", shark_get_mseconds());
		shark_timer_post( &_aux_unlock_timer, 200);
	}
}

static void _aux_unlock_timer_handler(shark_timer_t *t){
	if (!io_state()->aux_lock_detect){
		health_debug("unlock aux detect\n");
		small_power_detect_count = 0;
		_health.small_current_short = 0;
		_health.small_current_real_short = 0;
		AUX_VOL_OPEN(1);
	}
}


void health_stop_aux_detect(void){
	shark_timer_cancel(&_aux_unlock_timer);
	shark_timer_cancel(&_aux_lock_timer);
	_health.small_current_short = 0;
	_health.small_current_real_short = 0;
}

void health_process_aux_lock(void){
	if (io_state()->aux_lock_detect) {
		if (AUX_VOL_IS_OPEN()){
			AUX_VOL_OPEN(0);
			health_debug("close aux[locked], %lld\n", shark_get_mseconds());
			shark_timer_post( &_aux_lock_timer, 1);
			shark_timer_cancel(&_aux_unlock_timer);
		}
	}else {
		if (AUX_VOL_IS_OPEN()) {
			shark_timer_post( &_aux_unlock_timer, 500);
			shark_timer_cancel(&_aux_lock_timer);
		}
	}
}


void check_temp_state(void){
	if (!_health.over_temp_deny_charger){
		if (_is_over_temp(charger_higher_high_temp, sizeof(charger_higher_high_temp))) {//超过允许的最高温度
			debounce_inc(_charger_over_temp_count);
		}else {
			debounce_reset(_charger_over_temp_count);
		}
		if (debounce_reach_max(_charger_over_temp_count)){
			_health.over_temp_deny_charger = 1;
			error_counts.charger_high_temp ++;
			debounce_reset(_charger_over_temp_count);
		}			
	}
	if (!_health.lower_temp_deny_charger){
		if (_is_low_temp(charger_lower_low_temp, sizeof(charger_lower_low_temp))) {//低于允许的最低温度
			debounce_inc(_charger_lower_temp_count);
		}else {
			debounce_reset(_charger_lower_temp_count);
		}
		if (debounce_reach_max(_charger_lower_temp_count)) {
			_health.lower_temp_deny_charger = 1;
			error_counts.charger_lower_temp ++;
			debounce_reset(_charger_lower_temp_count);
		}
	}
	if (_health.lower_temp_deny_charger || _health.over_temp_deny_charger) {
		if (!_is_over_temp(charger_normal_high_temp, sizeof(charger_normal_high_temp)) && !_is_low_temp(charger_normal_low_temp, sizeof(charger_normal_low_temp))){
			debounce_inc(_charger_normal_temp_count);
		}else {
			debounce_reset(_charger_normal_temp_count);
		}
		if (debounce_reach_max(_charger_normal_temp_count)){
			_health.over_temp_deny_charger = 0;
			_health.lower_temp_deny_charger = 0;
			debounce_reset(_charger_normal_temp_count);
		}	
	}

	if (!_health.over_temp_deny_discharger){
		if (_is_over_temp(discharger_higher_high_temp, sizeof(discharger_higher_high_temp))) {//超过允许的最高温度
			debounce_inc(_discharger_over_temp_count);
		}else {
			debounce_reset(_discharger_over_temp_count);
		}
		if (debounce_reach_max(_discharger_over_temp_count)){
			_health.over_temp_deny_discharger = 1;
			error_counts.discharger_high_temp ++;
			debounce_reset(_discharger_over_temp_count);
		}			
	}
	
	if (!_health.lower_temp_deny_discharger){
		if (_is_low_temp(discharger_lower_low_temp, sizeof(discharger_lower_low_temp))) {//低于允许的最低温度
			debounce_inc(_discharger_lower_temp_count);
		}else {
			debounce_reset(_discharger_lower_temp_count);
		}
		if (debounce_reach_max(_discharger_lower_temp_count)) {
			_health.lower_temp_deny_discharger = 1;
			error_counts.discharger_lower_temp ++;
			debounce_reset(_discharger_lower_temp_count);
		}
	}
	if (_health.lower_temp_deny_discharger || _health.over_temp_deny_discharger) {
		if (!_is_over_temp(discharger_normal_high_temp, sizeof(discharger_lower_low_temp)) && !_is_low_temp(discharger_normal_low_temp, sizeof(discharger_normal_low_temp))){
			debounce_inc(_discharger_normal_temp_count);
		}else {
			debounce_reset(_discharger_normal_temp_count);
		}
		if (debounce_reach_max(_discharger_normal_temp_count)){
			_health.over_temp_deny_discharger = 0;
			_health.lower_temp_deny_discharger = 0;
			debounce_reset(_discharger_normal_temp_count);
		}	
	}
	if (!_health.is_work_temp_normal){
		/* 3个电芯温度都正常才算正常 */
		if (_is_over_temp(work_lower_temp_recovry, sizeof(work_lower_temp_recovry)) == sizeof(work_lower_temp_recovry)){
			debounce_inc(_work_lower_temp_count);
			if (debounce_reach_max(_work_lower_temp_count)){
				_health.is_work_temp_normal = 1;
				debounce_reset(_work_lower_temp_count);
			}
		}else {
			debounce_reset(_work_lower_temp_count);
		}
	}else {
		if (_is_low_temp(work_lower_temp, sizeof(work_lower_temp))){
			debounce_inc(_work_lower_temp_count);
			if (debounce_reach_max(_work_lower_temp_count)){
				_health.is_work_temp_normal = 0;
				debounce_reset(_work_lower_temp_count);
			}
		}else {
			debounce_reset(_work_lower_temp_count);
		}
	}
	if (bms_state_get()->charging){
		_health.discharger_over_temp = 0;
		_health.discharger_lower_temp = 0;		
		_health.charger_over_temp = _health.over_temp_deny_charger;
		_health.charger_lower_temp = _health.lower_temp_deny_charger;

	}else {
		_health.charger_over_temp = 0;
		_health.charger_lower_temp = 0;
		_health.discharger_over_temp = _health.over_temp_deny_discharger;
		_health.discharger_lower_temp = _health.lower_temp_deny_discharger;
	}
	debug_health();
}