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"

#define LEAST_SQUARE 0

static soc_t _soc;
static uint8_t chargering = 0;
static u64     time_ms = 0;
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 uint8_t is_force_full = 0;
static uint8_t is_force_empty = 0;
uint32_t charger_remain_time = 0;

#define DEFALUT_MAX_COULOMB (MAX_HA * 3600.0f)
#define DEFALUT_MIN_COULOMB (25.0f * 3600.0f)
static void calibrate_soc_by_ocv(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;
#define LEAST_SQUARE_STEP_TIME 1000
#endif
void soc_init(void){
	set_log_level(MOD_SOC, L_debug);
	
	time_ms = shark_get_mseconds();
	if (nv_restore_soc() != 0){
		soc_warning("SOC: nv storage is not inited, use default value!!\n");
		_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 ((_soc.flags & SOC_FLAG_CALIBRATED) == 0){
		calibrate_soc_by_ocv();
		nv_save_soc();
	}
	soc_log();
}

#if LEAST_SQUARE==1
static void start_least_square(void){
	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;
	shark_timer_post(&least_square_timer, LEAST_SQUARE_STEP_TIME);
}

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.Cb, discharger_vol_coef.Ka, get_y_by_x(&discharger_vol_coef, least_square_time));
	}
	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.Cb, discharger_cell_coef.Ka, get_y_by_x(&discharger_cell_coef, least_square_time));
	}
	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.Cb, discharger_capacity_coef.Ka, get_y_by_x(&discharger_capacity_coef, least_square_time));
	}
	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\n", max_soc_delta_time, soc_delta_time);
	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 < (2700 * CELLS_NUM)){
		_soc.capacity = 0;
	}else if (pack_vol < (2950 * CELLS_NUM)){
		_soc.capacity = 5;
	}else if (pack_vol < (3200 * CELLS_NUM)){
		_soc.capacity = 15;
	}else if (pack_vol < (3400 * CELLS_NUM)){
		_soc.capacity = 25;
	}else if (pack_vol < (3500 * CELLS_NUM)){
		_soc.capacity = 85;
	}else if (pack_vol < (3550 * CELLS_NUM)){
		_soc.capacity = 95;
	}else {
		_soc.capacity = 100;
	}
	_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() - time_ms;
	time_ms = 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; //秒
}

int soc_update_by_ocv(void){
	
	int changed = 0;
	if (_soc.flags & SOC_FLAG_CALIBRATED){
		if (!chargering){
			if (bms_health()->is_work_temp_normal) {
				if (!is_force_empty && (bms_health()->powerdown_lower_voltage || bms_health()->sigle_cell_lower_voltage || bms_health()->discharger_lower_voltage)) {
					_soc.coulomb_min = _soc.coulomb_now; //已经校准过了，而且电池在常温下进入powerdown，最小容量修正为当前容量
					_soc.capacity = 0;
					is_force_empty = 1;
					changed = 1;
					soc_warning("current coulomb %f\n", _soc.coulomb_now);
				}
			}
		}
		if (chargering && !is_force_full){
			if (bms_state_get()->pack_voltage >= (53500) && (measure_value()->load_current <= 500.0f)){
				_soc.capacity = 100;
				is_force_full = 1;
				changed = 1;
			}
		}
	}
	return changed;
}


static void soc_calibrate(uint8_t prev_charge_status){
	static int cali_full_count = 0;
	if (!(_soc.flags & SOC_FLAG_CALIBRATED)){
		if (chargering){//用ocv进行严格校准
			if (!is_force_full && (measure_value()->load_current <= 500.0f) && (bms_state_get()->pack_voltage >= 53500)){				
				cali_full_count ++;
				if (cali_full_count == 10) {
					soc_debug("calibrate Capacity to 100, measure_value()->load_current %d\n", measure_value()->load_current);
					_soc.capacity = 100;
					is_force_full = 1;
				}
			}
		}else if (prev_charge_status){
			if(!is_force_full && (bms_state_get()->pack_voltage >= 53500)){
				soc_debug("calibrate Capacity to 100\n");
				_soc.capacity = 100;
				is_force_full = 1;
			}
		}
	}
}

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;
	}
	if (_soc.capacity == 100) {
		charger_remain_time = 0;
	}
}

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

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;
	if (chargering){
		delta_q = delta_q * _charger_coefficient;
		_soc.charger_coulomb += abs(delta_q);
	}else {
		delta_q = delta_q * _discharger_coefficient;
		_soc.dischrger_coulomb += abs(delta_q); //转为正数
	}
	_soc.coulomb_now = _soc.coulomb_now + delta_q; //充电加, 放电减

	if (_soc.coulomb_now < 0){
		_soc.coulomb_now = 0;
	}
	uint8_t old_cap = _soc.capacity;
	if ((_soc.coulomb_now - _soc.coulomb_min) >= 0){
		_soc.capacity = ((_soc.coulomb_now - _soc.coulomb_min)/(_soc.coulomb_max - _soc.coulomb_min) + 0.005f) * 100;//四舍五入
	}else {
		_soc.capacity = 0;
	}
	if (_soc.capacity > 100){
		_soc.capacity = 100;
	}
	if (chargering && (_soc.capacity == 100) && (!is_force_full)){
		_soc.capacity = 99;//充电的时候必须通过ocv才能把电量校准到100
	}else if (!chargering && (_soc.capacity == 0) && !is_force_empty){
		_soc.capacity = 1;
	}
	//通过电压校准SOC，只能在电压范围的两端校准
	soc_update_by_ocv();
	soc_calibrate(prev_charge_status);

	//如果没有校准过，充电过程中，电量100%后，设置校准标志位
	if (chargering && (_soc.capacity == 100)){
		if ((_soc.flags & SOC_FLAG_CALIBRATED) == 0){
			_soc.coulomb_now = _soc.coulomb_max;
			_soc.flags |= SOC_FLAG_CALIBRATED;
			nv_save_soc();
			soc_warning("calibrate OK, charging coulomb: %f\n", _soc.charger_coulomb);
		}else { //如果校准过，单电芯过压，100%的容量，设置最大容量为当前容量
			if (bms_health()->sigle_cell_over_voltage){
				if ((_soc.coulomb_now >= DEFALUT_MIN_COULOMB) && (_soc.coulomb_now <= DEFALUT_MAX_COULOMB)) {
					_soc.coulomb_max = _soc.coulomb_now;
					soc_warning("signal cell over vol, cap full, reset coul max to coul now: %f\n", _soc.coulomb_max);
				}
			}
		}
	}
	_soc.energy = bms_state_get()->pack_voltage/1000.f * (_soc.coulomb_now - _soc.coulomb_min);
	if (old_cap != _soc.capacity) {
		nv_save_soc();
	}

}

void soc_update_for_deepsleep(float sleep_time){
	soc_update_by_current_and_time(-1.0f, sleep_time, 0);
}

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;
		is_force_empty = 0;
		if (_soc.capacity < 100) {
			is_force_full = 0;
		}
		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;
		if (_soc.capacity < 100) {
			is_force_full = 0;
		}
		soc_warning("changed to dischargering, current = %d\n", measure_value()->load_current);
	}

	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;
}