bms/Application/app/sox/soc.c

#include "soc.h"
#include "app/sox/measure.h"
#include "app/sox/measure_task.h"
#include "app/nv_storage.h"
#include "libs/logger.h"
#include "Least_Square.h"
#include "health.h"
#include "state.h"
#include "event_record.h"

#define LEAST_SQUARE 0

static soc_t _soc;
static uint8_t chargering = 0;
static u64     current_sample_ts = 0; //ms
static u32     force_full_ts = 0xFFFFFFFF; //s
static u32     force_empty_ts = 0xFFFFFFFF; //s
static float   soc_delta_time = 0;
static float   max_soc_delta_time = 0;
static float _charger_coefficient = 1.0f;
static float _discharger_coefficient = 1.0f;
static uint32_t charger_remain_time = 0;
static const float _discharger_gain[] = {1.0f/*>0度*/, 1.002f/*-2<t<=0*/, 1.005f/*-5<t<=-2*/, 1.008f/*-10<t<=-5*/, 1.02f/*-15<t<=-10*/, 1.04f/*-20<t<=-15*/};
#define MAX_TIME_FULL_TO_EMPTY (5 * 24 * 3600) //充满到欠压5天内达到，可以校准最小电量
#define MAX_TIME_EMPTY_TO_FULL (24 * 3600) //欠压到充满24小时内达到，可以校准最小电量
#define DEFALUT_MAX_COULOMB (MAX_HA * 3600.0f)
#define DEFALUT_MIN_COULOMB (25.0f * 3600.0f)
#define FULL_MAX_VOLTAGE_CHARGING (54000)//mV
#define FULL_MAX_VOLTAGE_STOP_CHARGING (53500)
#define AGINT_TEST_MAX_VOLTAGE_CHARGING (53000) //mV
#define FULL_MAX_VOLTAGE (54000) //mV
#define FULL_MIN_CURRENT (500.0f) //mA
static double start_charger_coulomb = 0.0f; //开始充电时候的容量
static void calibrate_soc_by_ocv(void);
static void _soc_clear(void);

#if LEAST_SQUARE==1
static void _least_square_timer_handler(shark_timer_t *timer);
static least_square_t discharger_vol_coef;
static least_square_t discharger_cell_coef;
static least_square_t discharger_capacity_coef;
static shark_timer_t least_square_timer = {.handler = _least_square_timer_handler};
static int least_square_time = 0;
static int least_square_started = 0;
#define LEAST_SQUARE_STEP_TIME 1000 * 5
#endif
void soc_init(void){
	set_log_level(MOD_SOC, L_debug);
	
	current_sample_ts = shark_get_mseconds();
	if (nv_restore_soc() != 0){
		soc_warning("SOC: nv storage is not inited, use default value!!\n");
		_soc_clear();
	}
	//如果最大容量和默认不一致，需要重新校准
	if (_soc.coulomb_max != DEFALUT_MAX_COULOMB) {
		_soc_clear();
		nv_save_all_soc();
	}
	if ((_soc.flags & SOC_FLAG_CALIBRATED) == 0){
		calibrate_soc_by_ocv();
		nv_save_soc();
	}else {
		if (_soc.capacity == 100) {
			force_full_ts = shark_get_seconds() + 1;
		}else if (_soc.capacity == 0) {
			force_empty_ts = shark_get_seconds() + 1;
		}
	}
	if (soc_get_version() != SOC_CURRENT_VERSION) {
		//DO SOMETHING, FOR SOC VERSION CHANGED
	}
	soc_log();
}

u8 soc_get_version(void) {
	return SOC_FLAG_TO_VER(_soc.flags);
}

void soc_set_version(u8 version) {
	if (version > 7) {
		return;
	}
	_soc.flags &= ~SOC_FLAG_VERSON_MASK;
	_soc.flags |= SOC_FLAG_VERSION(version);
}

static void _soc_clear(void){
	_soc.coulomb_min = 0;
	_soc.coulomb_max = DEFALUT_MAX_COULOMB; //30HA,这个值最总需要soh模块给
	_soc.flags = 0;
	_soc.charger_coulomb = 0;
	_soc.pre_charger_coulomb = 0;
	_soc.dischrger_coulomb = 0;
	_soc.pre_discharger_coulomb = 0;
	_soc.total_coulomb = 0;
}

void soc_clear_calibrate(int keep_cycle) {
	float total = _soc.total_coulomb;
	_soc_clear();
	if (keep_cycle) {
		_soc.total_coulomb = total;
	}
	nv_save_all_soc();
}

void soc_restore_by_iap(uint8_t flags, uint8_t capaticy){
	_soc.coulomb_min = 0;
	_soc.coulomb_max = DEFALUT_MAX_COULOMB; //30HA,这个值最总需要soh模块给
	_soc.flags = 0;
	_soc.charger_coulomb = 0;
	_soc.pre_charger_coulomb = 0;
	_soc.dischrger_coulomb = 0;
	_soc.pre_discharger_coulomb = 0;
	_soc.total_coulomb = 0;
	if (flags == 1) {
		_soc.flags |= SOC_FLAG_CALIBRATED;
	}
	_soc.capacity = capaticy;
	_soc.coulomb_now = (_soc.coulomb_max - _soc.coulomb_min) * _soc.capacity / 100.0f + _soc.coulomb_min;
	nv_save_all_soc();
}

static void soc_update_discharger_coeff(void){
	int low_temp = 0xFFFF;
	for (int i = 0; i < PACK_TEMPS_NUM-1; i++) {
		low_temp = MIN(low_temp, measure_value()->pack_temp[i]);
	}
	if (low_temp > 0) {
		_discharger_coefficient = _discharger_gain[0];
	}else {
		if (low_temp > -2) {
			_discharger_coefficient = _discharger_gain[1];
		}else if (low_temp > -5) {
			_discharger_coefficient = _discharger_gain[2];
		}else if (low_temp > -10) {
			_discharger_coefficient = _discharger_gain[3];
		}else if (low_temp > -15) {
			_discharger_coefficient = _discharger_gain[4];
		}else {
			_discharger_coefficient = _discharger_gain[5];
		}
		force_full_ts = 0xFFFFFFFF;
	}
	if (_soc.flags & SOC_FLAG_CALIBRATED) {
		float coff = 1.0f;
		if (_soc.capacity <= 20) {
			if (abs(measure_value()->load_current) >= CURRENT_BIGER) {
				coff = 1.06f;
			}else if (abs(measure_value()->load_current) >= CURRENT_MID) {
				coff = 1.05f;
			}else if (abs(measure_value()->load_current) >= CURRENT_NORMAL) {
				coff = 1.03f;
			}
		}else if (_soc.capacity <= 40) {
			if (abs(measure_value()->load_current) >= CURRENT_BIGER) {
				coff = 1.05f;
			}else if (abs(measure_value()->load_current) >= CURRENT_MID) {
				coff = 1.03f;
			}else if (abs(measure_value()->load_current) >= CURRENT_NORMAL) {
				coff = 1.02f;
			}
		}else if (_soc.capacity <= 60) {
			if (abs(measure_value()->load_current) >= CURRENT_BIGER) {
				coff = 1.03f;
			}else if (abs(measure_value()->load_current) >= CURRENT_MID) {
				coff = 1.02f;
			}else if (abs(measure_value()->load_current) >= CURRENT_NORMAL) {
				coff = 1.01f;
			}
		}
		if ((abs(measure_value()->load_current) > 10.0f) && (abs(measure_value()->load_current) < 500)) {
			coff = 1.05f;
		}
		_discharger_coefficient = _discharger_coefficient * coff;
	}
}

#if LEAST_SQUARE==1
static void start_least_square(int start){
	if (start && !least_square_started) {
		least_square_init(&discharger_vol_coef, 10);
		least_square_init(&discharger_cell_coef, 10);
		least_square_init(&discharger_capacity_coef, 10);
		least_square_time = 0;
		least_square_started = 1;
		shark_timer_post(&least_square_timer, LEAST_SQUARE_STEP_TIME);
	}else if (!start && least_square_started){
		least_square_time = 0;
		least_square_started = 0;
		shark_timer_cancel(&least_square_timer);
	}
}

static void _least_square_timer_handler(shark_timer_t *timer){
	if (least_square_put(&discharger_vol_coef, least_square_time, bms_state_get()->pack_voltage/1000.0f) == 1) {
		soc_error("voltage: A = %f, B = %f, v: %f\n", discharger_vol_coef.coeff.Ka, discharger_vol_coef.coeff.Cb, get_y_by_x(&discharger_vol_coef, least_square_time));
		int delta = get_x_by_y(&discharger_vol_coef, bms_health_pack_lower_voltage()/1000.0f) - get_x_by_y(&discharger_vol_coef, bms_state_get()->pack_voltage/1000.0f);
		soc_error("remain %d s to reach lower pack voltage\n", delta);
	}
	if (least_square_put(&discharger_cell_coef, least_square_time, bms_state_get()->cell_min_vol/1000.0f) == 1) {
		soc_error("cell: A = %f, B = %f, v: %f\n", discharger_cell_coef.coeff.Ka, discharger_cell_coef.coeff.Cb, get_y_by_x(&discharger_cell_coef, least_square_time));
		int delta = get_x_by_y(&discharger_cell_coef, bms_health_cell_lower_voltage()/1000.0f) - get_x_by_y(&discharger_cell_coef, bms_state_get()->cell_min_vol/1000.0f);
		soc_error("remain %d s to reach lower cell voltage\n", delta);
	}
	if (least_square_put(&discharger_capacity_coef, least_square_time, _soc.coulomb_now/3600.0f) == 1) {
		soc_error("capacity: A = %f, B = %f, c: %f\n", discharger_capacity_coef.coeff.Ka, discharger_capacity_coef.coeff.Cb, get_y_by_x(&discharger_capacity_coef, least_square_time));
		int delta = get_x_by_y(&discharger_capacity_coef, _soc.coulomb_min/3600.0f) - get_x_by_y(&discharger_capacity_coef, _soc.coulomb_now/3600.0f);
		soc_error("remain %d s to reach 0 min AH\n", delta);
	}
	least_square_time ++;
	shark_timer_post(&least_square_timer, LEAST_SQUARE_STEP_TIME);
}
#endif

#define TOHA(x) (float)(x/3600.0f)
void soc_log(void){
	soc_debug("C flags 0x%x\n", _soc.flags);
	soc_debug("C now: %.4f\n", TOHA(_soc.coulomb_now));
	soc_debug("C min: %.4f\n", TOHA(_soc.coulomb_min));
	soc_debug("C max: %.4f\n", TOHA(_soc.coulomb_max));
	soc_debug("C char: %.4f\n", TOHA(_soc.charger_coulomb));
	soc_debug("C dischar: %.4f\n", TOHA(_soc.dischrger_coulomb));
	soc_debug("C pre char: %.4f\n", TOHA(_soc.pre_discharger_coulomb));
	soc_debug("C pre dischar: %.4f\n", TOHA(_soc.pre_charger_coulomb));
	soc_debug("C tol: %.2f\n", _soc.total_coulomb);
	soc_debug("C energy: %f\n", _soc.energy);
	soc_debug("C delta time %f,%f, -- %d\n", max_soc_delta_time, soc_delta_time, force_full_ts);
	soc_debug("C discharger coefficient = %f\n", _discharger_coefficient);
	if (chargering){
		soc_debug("C remain %d\n", charger_remain_time);
	}
}

//初始上电或者nv出问题后，通过开路电压对soc做一次初略校准
static void calibrate_soc_by_ocv(void){
	uint16_t pack_vol = 0;
	for (int i = 0; i < CELLS_NUM; i++){
		pack_vol += measure_value()->cell_vol[i];
	}
	if (pack_vol <= (48000)){
		_soc.capacity = 0;
	}else if (pack_vol <= 49000){
		_soc.capacity = 5;
	}else if (pack_vol <= 50000){
		_soc.capacity = 10;
	}else if (pack_vol <= 51000){
		_soc.capacity = 30;
	}else if (pack_vol <= 52000){
		_soc.capacity = 50;
	}else if (pack_vol <= 53000){
		_soc.capacity = 60;
	}else {
		_soc.capacity = 80;
	}
	_soc.coulomb_now = (_soc.coulomb_max - _soc.coulomb_min) * _soc.capacity / 100.0f + _soc.coulomb_min;
	soc_warning("SOC: calibrate_soc_by_ocv -> capacity = %d, pack_voltage = %d\n", _soc.capacity, pack_vol);
}

static __inline__ float _delta_time(void){
	u32 delta = shark_get_mseconds() - current_sample_ts;
	current_sample_ts = shark_get_mseconds();
	soc_delta_time = (float)delta / (1000.0f);
	if (soc_delta_time > max_soc_delta_time){
		max_soc_delta_time = soc_delta_time;
	}
	return soc_delta_time; //秒
}

static __inline__ int can_modify_min_cap(void){
	if (force_full_ts == 0) {
		return 0;
	}
	if (shark_get_seconds() > force_full_ts){
		if ((shark_get_seconds() - force_full_ts) >= MAX_TIME_FULL_TO_EMPTY) {
			return 0;
		}else {
			return 1;
		}
	}
	return 0;
}

static __inline__ int can_modify_min_when_full(void){
	if (force_empty_ts == 0) {
		return 0;
	}
	if (shark_get_seconds() > force_empty_ts){
		if ((shark_get_seconds() - force_empty_ts) >= MAX_TIME_EMPTY_TO_FULL) {
			return 0;
		}else {
			return 1;
		}
	}
	return 0;
}

static bool is_force_full = false;
static u32  force_full_time = 0;
bool soc_is_force_full(void) {
	if (is_force_full) {
		if (shark_get_seconds() >= 5 + force_full_time) {
			is_force_full = false;
		}
	}
	return is_force_full;
}

#define min_cap_lfp 0.2f
static void _force_capacity_full(void){
	_soc.capacity = 100;
	is_force_full = true;
	force_full_time = shark_get_seconds();
	if (can_modify_min_when_full()) { //前面出现过电芯欠压, 当前容量没到最大容量
		double curr_real_cap = start_charger_coulomb + _soc.charger_coulomb;
		double curr_min_cap = 0.0f;
		if (curr_real_cap > _soc.coulomb_max) {
			curr_min_cap = 0.0f;
		}else {
			curr_min_cap = _soc.coulomb_max - curr_real_cap;
		}
		//don't trust if curr_min_cap big than before
		if (curr_min_cap < _soc.coulomb_min) {
			_soc.coulomb_min = _soc.coulomb_min * (1.0f - min_cap_lfp) + curr_min_cap * min_cap_lfp; //lowpass filter
			soc_warning("current real cap %f\n", _soc.coulomb_min);
			push_event(Min_Cap_For_Full, (u32)curr_real_cap);
		}
	}
	push_event(Charger_Full_cap, (u32)_soc.coulomb_now);
	_soc.coulomb_now = _soc.coulomb_max;//充满后，当前容量设置为最大容量

	force_full_ts = shark_get_seconds();
}

static int _soc_is_under_voltage(void) {
	return (bms_health()->powerdown_lower_voltage || bms_health()->sigle_cell_lower_voltage || 
		bms_health()->discharger_lower_voltage);
}

static int _is_normal_charging(void) {
	return (_soc.charger_coulomb >= (0.1f * 3600.0f));
}

static int _soc_update_by_ocv(uint8_t prev_charge_status){
	static int ocv_full_count = 0;
	//static int ocv_force_capaticy = 0;
	int changed = 0;
	if ((_soc.flags & SOC_FLAG_CALIBRATED) == 0){
		return 0;
	}
	if (!chargering){
		if (_soc.capacity && _soc_is_under_voltage()) {
			soc_warning("judge calib min col %d - %d\n", shark_get_seconds(), force_empty_ts);
			if (can_modify_min_cap()){
				if (health_is_low_current()) {
					_soc.coulomb_min = _soc.coulomb_now; //已经校准过了，而且电池在常温下进入powerdown，最小容量修正为当前容量
				}else if (health_is_mid_current()) {
					_soc.coulomb_min = _soc.coulomb_now * 1.0f;
				}else if (health_is_big_current()){
					_soc.coulomb_min = _soc.coulomb_now * 0.95f;
				}else {
					_soc.coulomb_min = _soc.coulomb_now * 0.9f;
				}
				_soc.coulomb_now = _soc.coulomb_min;
				push_event(Min_Cap_For_DisCharger, (u32)_soc.coulomb_now);
				soc_warning("calicablite coulomb_min %f\n", _soc.coulomb_min);
			}else {
				_soc.coulomb_now = _soc.coulomb_min;
			}
			force_empty_ts = shark_get_seconds();
			_soc.capacity = 0;
			return 1;
		}	
	}
	if (chargering || prev_charge_status) {
		if (chargering && (_soc.capacity != 100)) { 
			if (bms_health()->sigle_cell_over_voltage && _is_normal_charging()) { //单电芯过压强制充满
				_force_capacity_full();
				push_event(Charger_Full, bms_state_get()->pack_voltage);
				ocv_full_count = 0;
				changed = 1;
			}else if (bms_state_get()->pack_voltage >= (FULL_MAX_VOLTAGE_CHARGING) && _is_normal_charging()){
				if (ocv_full_count++ >= 100) { //连续100次(电流采集25(小于4A)或者5ms一次)电压和电流满足条件，强制充满
					_force_capacity_full();
					push_event(Charger_Full, 4);
					ocv_full_count = 0;
					changed = 1;
				}
			}else {
				ocv_full_count = 0;
			}
		} else if (!chargering && prev_charge_status && (_soc.capacity != 100)){
			if ((bms_state_get()->pack_voltage >= FULL_MAX_VOLTAGE_STOP_CHARGING) && _is_normal_charging()){//充电容量几乎接近最大容量
				_force_capacity_full();
				push_event(Charger_Full, 5);
				changed = 1;
			}
		}
	}
	return changed;
}

int soc_update_by_ocv(void){
	return _soc_update_by_ocv(0);
}


static void soc_calibrate(uint8_t prev_charge_status){
	if (!(_soc.flags & SOC_FLAG_CALIBRATED)){
		if (chargering){//用ocv进行严格校准
			if (_soc.capacity != 100){
				if (!bms_work_is_normal()) {
					if ((bms_state_get()->pack_voltage >= AGINT_TEST_MAX_VOLTAGE_CHARGING)){
						_force_capacity_full();
						push_event(Charger_Full, 12);
					}else if (bms_health()->sigle_cell_over_voltage) {
						_force_capacity_full();
						push_event(Charger_Full, 13);
					}
				}else {
					if ((bms_state_get()->pack_voltage >= FULL_MAX_VOLTAGE_CHARGING) && _is_normal_charging()){
						_force_capacity_full();
						push_event(Charger_Full, 10);
					}else if (bms_health()->sigle_cell_over_voltage) {
						_force_capacity_full();
						push_event(Charger_Full, 1);
					}
				}
			}
		}else if (prev_charge_status){
			if((_soc.capacity != 100) && ((bms_state_get()->pack_voltage >= FULL_MAX_VOLTAGE_STOP_CHARGING) || bms_health()->sigle_cell_over_voltage)){
				soc_debug("calibrate Capacity to 100\n");
				_force_capacity_full();
				push_event(Charger_Full, 2);
			}
		}else {
			if (_soc.capacity && _soc_is_under_voltage()) {
				 _soc.coulomb_now = _soc.coulomb_min = 0;
				_soc.capacity = 0;
			}
		}
	}
}

static void soc_update_charger_remain_time(void){
	if (!chargering) {
		return;
	}
	float delta_c = _soc.coulomb_max - _soc.coulomb_now;
	float current = measure_value()->load_current / 1000.0f; //A
	uint32_t remain = delta_c / current / 60; //分钟
	if (charger_remain_time == 0){
		charger_remain_time = remain;
	}else if (remain < charger_remain_time){
		charger_remain_time = remain;
	}else { //如果充电时间变长，考虑是否快充满电流小于1A
		if (bms_state_get()->pack_voltage < 53000 && current > 1.5f) {
			charger_remain_time = remain;
		}
	}
	if (_soc.capacity == 100) {
		charger_remain_time = 0;
	}
}

uint32_t soc_get_cycle(void){
	return _soc.total_coulomb/MAX_HA/2;
}

uint8_t soc_get_soh(void){
	return (_soc.coulomb_max - _soc.coulomb_min)/_soc.coulomb_max * 100;
}

uint32_t soc_get_charger_remain_time(void){
	return charger_remain_time;
}

static void soc_update_by_current_and_time(float current_now, float delta_time, uint8_t prev_charge_status){
	double current = current_now / 1000.0f; //A
	double delta_q = current * delta_time; 
	uint8_t est_capaticy = _soc.capacity;
	int update_capticy = 0;

	if (!chargering) {
		soc_update_discharger_coeff();
		delta_q = delta_q * _discharger_coefficient;
	}
	
	double est_coulomb = _soc.coulomb_now + delta_q;//计算当前容量，充电加, 放电减

	if (est_coulomb < 0){
		est_coulomb = 0;
	}else if (est_coulomb > _soc.coulomb_max) {
		est_coulomb = _soc.coulomb_max;
	}
	if (est_coulomb >= _soc.coulomb_min) {
		est_capaticy = ((est_coulomb - _soc.coulomb_min)/(_soc.coulomb_max - _soc.coulomb_min) + 0.005f) * 100;//四舍五入
	}
	if (chargering){
		delta_q = delta_q * _charger_coefficient;
		_soc.charger_coulomb += abs(delta_q);
		if ((est_capaticy < 100) && (est_capaticy >= _soc.capacity)){ //充电，容量不能等于100，需要靠电压和充电电流来矫正到100
			update_capticy = 1;
		}
	}else {
		_soc.dischrger_coulomb += abs(delta_q);
		if (est_coulomb < _soc.coulomb_min) {
			_soc.coulomb_min = est_coulomb;
		}
		if ((est_capaticy > 0) && (est_capaticy <= _soc.capacity)) {  //放电，容量不能等于0，需要靠欠压或者PowerDown 矫正到0
			update_capticy = 1;
		}
	}
	if (update_capticy) {
		if (_soc.capacity != est_capaticy) {
			_soc.capacity = est_capaticy;
		}else {
			update_capticy = 0;
		}
	}
	_soc.coulomb_now = est_coulomb;
	//通过电压校准SOC，只能在电压范围的两端校准
	update_capticy |= _soc_update_by_ocv(prev_charge_status);
	soc_calibrate(prev_charge_status);

	//如果没有校准过，充电过程中，电量100%后，设置校准标志位
	if (chargering && (_soc.capacity == 100)){
		if ((_soc.flags & SOC_FLAG_CALIBRATED) == 0){
			_soc.flags |= SOC_FLAG_CALIBRATED;
			update_capticy = 1;
			soc_warning("calibrate OK, charging coulomb: %f\n", _soc.charger_coulomb);
		}
	}
	if (_soc.coulomb_now >= _soc.coulomb_min) {
		_soc.energy = bms_state_get()->pack_voltage/1000.f * (_soc.coulomb_now - _soc.coulomb_min);
	}
	if (update_capticy) {
		nv_save_soc();
	}

}

/*休眠bms功耗 + 电芯自放电 28天 3% (28天1AH)*/
void soc_update_for_deepsleep(float sleep_time){
	soc_update_by_current_and_time(-(0.50f + 1000.0f/(24.f * 28.f)), sleep_time, 0); //休眠功耗310uA(300uA + 10uA固定消耗)
	current_sample_ts = shark_get_mseconds(); //唤醒后复位采集时间,如果不采集会重复计算
}

void soc_update(void){
	uint8_t pre_chargering = chargering;
	if (!chargering && bms_state_get()->charging){
		_soc.pre_charger_coulomb = _soc.charger_coulomb;
		_soc.charger_coulomb = 0;//clear charing
		_soc.total_coulomb += _soc.pre_charger_coulomb / 3600.0f;
		chargering = 1;
		start_charger_coulomb = _soc.coulomb_now;
#if LEAST_SQUARE==1		
		start_least_square(0);
#endif
		soc_warning("changed to chargering, current = %d\n", measure_value()->load_current);
	}else if (chargering && !bms_state_get()->charging){
		_soc.pre_discharger_coulomb = _soc.dischrger_coulomb;
		_soc.dischrger_coulomb = 0; //clear discharger
		_soc.total_coulomb += _soc.pre_discharger_coulomb / 3600.0f;
		chargering = 0;
		charger_remain_time = 0;
		soc_warning("changed to dischargering, current = %d\n", measure_value()->load_current);
	}
#if LEAST_SQUARE==1	
	if(!chargering && abs(measure_value()->load_current) >= 5000){
		start_least_square(1);
	}
#endif
	soc_update_by_current_and_time(measure_value()->load_current, _delta_time(), pre_chargering);
	soc_update_charger_remain_time();
}

soc_t *get_soc(void){
	return &_soc;
}