bms/Application/app/sox/soft_current_protect.c

#include <string.h>
#include "bsp/shark_bsp.h"
#include "libs/shark_libs.h"
#include "libs/circle_buffer.h"
#include "libs/logger.h"
#define MAX_SIZE 50
typedef struct {
	c_buffer_t   current;
	c_buffer_t   delta_ts;
	u32  ts_sum;
	u64  last_ts;
	float i2t_sum;
}fifo_i2_time_t;

static fifo_i2_time_t _fifo_i2;
static u8 _buffer[MAX_SIZE * 2];

void soft_current_init(void){
	_fifo_i2.ts_sum = 0;
	_fifo_i2.last_ts = 0;
	_fifo_i2.i2t_sum = 0.0f;
	circle_buffer_init(&_fifo_i2.current, _buffer, MAX_SIZE);
	circle_reset(&_fifo_i2.current);
	circle_buffer_init(&_fifo_i2.delta_ts, _buffer + MAX_SIZE, MAX_SIZE);
	circle_reset(&_fifo_i2.delta_ts);
}


int soft_current_push(float current_ma){
	u8 current = (int)((abs(current_ma)))/1000;
	if (current <= MAX_SOFT_CURRENT) {
		current = 0; //СÓÚ45A£¬²»¼ÆÈë
	}
	if (_fifo_i2.last_ts == 0){
		circle_put_one_data(&_fifo_i2.current, current);
		circle_put_one_data(&_fifo_i2.delta_ts, 15);
	}else {
		u8 delta_ts = shark_get_mseconds() - _fifo_i2.last_ts;
		float ceff = (current * current)/(MAX_I2T);
		ceff = ceff * ceff;
		_fifo_i2.i2t_sum += (current * current * delta_ts)/1000.0f * ceff;
		_fifo_i2.ts_sum += delta_ts;
		while ((circle_get_write_space(&_fifo_i2.current) <= 0) || (_fifo_i2.ts_sum > 1000)){
			u8 pre_current = 0, pre_ts = 0;
			circle_get_one_data(&_fifo_i2.current, &pre_current);
			circle_get_one_data(&_fifo_i2.delta_ts, &pre_ts);
			ceff = (pre_current * pre_current)/(MAX_I2T);
			ceff = ceff * ceff;
			_fifo_i2.i2t_sum -= (pre_current * pre_current * pre_ts)/1000.0f * ceff;
			_fifo_i2.ts_sum -= pre_ts;
		}
		circle_put_one_data(&_fifo_i2.current, current);
		circle_put_one_data(&_fifo_i2.delta_ts,delta_ts);
	}
	_fifo_i2.last_ts = shark_get_mseconds();
	return (_fifo_i2.i2t_sum >= MAX_I2T); //ÕÛËãµ½1s
}