295 lines
10 KiB
C
295 lines
10 KiB
C
#include "kit_time.h"
|
||
#include "kit_data.h"
|
||
#include "kit_debug.h"
|
||
|
||
#include "iso_check.h"
|
||
#include "gpio_manager.h"
|
||
#include "eeprom_manager.h"
|
||
#include "signal_manager.h"
|
||
#include "hv_adc_manager.h"
|
||
#include "run_status.h"
|
||
|
||
#define ISO_SAMPLE_CNT (20u)
|
||
#define ISO_RES_BUF_CNT (3u) //sfj 原本6 暂改3
|
||
#define DIVIDER_RES_VALUE (5100u) // 档位kΩ 半桥电阻
|
||
#define ISO_STABLE_AD_RATE (3u)
|
||
#define ISO_SAMPLE_TIME_MAX (10000u) //sfj 7.25 原本60000 因为采样平均值通不过
|
||
#define ISO_SAMPLE_TIME_MIN (3000u)
|
||
|
||
typedef enum
|
||
{
|
||
kIsoStep_None = 0,
|
||
kIsoStep_TotalVolt,
|
||
kIsoStep_IsoVolt,
|
||
kIsoStep_PosVolt,
|
||
kIsoStep_NegVolt,
|
||
kIsoStep_End,
|
||
}IsoStep;
|
||
|
||
typedef struct
|
||
{
|
||
int32_t v0;
|
||
int32_t v1;
|
||
int32_t v2;
|
||
int32_t v3;
|
||
uint16_t iso_bat_rate;
|
||
uint8_t sample_pos[kIsoVolt_End];
|
||
uint8_t res_buf_pos;
|
||
|
||
IsoCtrl ctrl;
|
||
IsoCtrl bsu_ctrl;
|
||
IsoStep step;
|
||
uint16_t dly;
|
||
int32_t ad_buf[kIsoVolt_End][ISO_SAMPLE_CNT];
|
||
uint16_t pos_res_buf[ISO_SAMPLE_CNT];
|
||
uint16_t neg_res_buf[ISO_SAMPLE_CNT];
|
||
uint16_t data[kIsoData_End];
|
||
}IsoItem;
|
||
|
||
IsoItem iso_item;
|
||
|
||
#define ISO_S1_ON() drv_gpio_set_pin_status(kGpioType_IsoS1, kGpioStatus_High)
|
||
#define ISO_S1_OFF() drv_gpio_set_pin_status(kGpioType_IsoS1, kGpioStatus_Low)
|
||
#define ISO_S2_ON() drv_gpio_set_pin_status(kGpioType_IsoS2, kGpioStatus_High)
|
||
#define ISO_S2_OFF() drv_gpio_set_pin_status(kGpioType_IsoS2, kGpioStatus_Low)
|
||
#define ISO_S3_ON() drv_gpio_set_pin_status(kGpioType_IsoS3, kGpioStatus_High)
|
||
#define ISO_S3_OFF() drv_gpio_set_pin_status(kGpioType_IsoS3, kGpioStatus_Low)
|
||
|
||
uint16_t bms_get_iso_data(IsoData idx)
|
||
{
|
||
uint16_t tmp = 0;
|
||
if(idx < kIsoData_End)
|
||
{
|
||
tmp = iso_item.data[idx];
|
||
}
|
||
return tmp;
|
||
}
|
||
|
||
void bms_set_iso_ctrl(IsoCtrl ctrl)
|
||
{
|
||
if(ctrl < kIsoCtrl_End)
|
||
{
|
||
iso_item.ctrl = ctrl;
|
||
}
|
||
}
|
||
|
||
void bms_set_iso_volt_ad(IsoVolt idx, int32_t ad)
|
||
{
|
||
if(idx < kIsoVolt_End)
|
||
{
|
||
iso_item.ad_buf[idx][iso_item.sample_pos[idx]] = ad;
|
||
if(++iso_item.sample_pos[idx] >= ISO_SAMPLE_CNT)
|
||
{
|
||
iso_item.sample_pos[idx] = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
//aim_rate 是最大最小差值和最小值比例
|
||
static bool is_volt_ad_stable(int32_t *buf, uint16_t len, uint16_t aim_rate)
|
||
{
|
||
bool res = false;
|
||
int32_t i, max, min;
|
||
|
||
max = min = buf[0];
|
||
for(i = 0; i < len; i++)
|
||
{
|
||
if(max < buf[i])
|
||
{
|
||
max = buf[i];
|
||
}
|
||
else if(min > buf[i])
|
||
{
|
||
min = buf[i];
|
||
}
|
||
}
|
||
//KIT_DEBUG_PRINTF("iso ad %d %d %d\r\n", max, min, (max - min) * 10000 / min);
|
||
if((min > 0) && (((int64_t)(max - min) * 10000 / min) < aim_rate))
|
||
{
|
||
res = true;
|
||
}
|
||
return res;
|
||
}
|
||
|
||
/********************************绝缘情况分析********************************************
|
||
1.当绝缘无穷大时，理论上V0 = V1
|
||
2.当S2闭合时，正极处并上电阻导致分压减小，进而V2 > V1
|
||
3.当S3闭合时，负极处并上电阻导致分压减小，进而V3 < V1
|
||
****************************************************************************************/
|
||
void insu_sample_calc_insu_res(void)
|
||
{
|
||
uint16_t total_volt;
|
||
float tmp, v0v1, v0v2, v0v3, v1v2, v1v3;
|
||
|
||
if(iso_item.v1 > 10)
|
||
{
|
||
if (iso_item.v2 <= iso_item.v1)
|
||
{
|
||
KIT_DEBUG_PRINTF("iso err v1 %d v2 %d\r\n", iso_item.v1, iso_item.v2);
|
||
iso_item.v2 = iso_item.v1 + 1;
|
||
}
|
||
|
||
if (iso_item.v3 >= iso_item.v1)
|
||
{
|
||
KIT_DEBUG_PRINTF("iso err v1 %d v3 %d\r\n", iso_item.v1, iso_item.v3);
|
||
iso_item.v3 = iso_item.v1 - 1;
|
||
}
|
||
|
||
v0v1 = (float)iso_item.v0 * iso_item.v1;
|
||
v0v2 = (float)iso_item.v0 * iso_item.v2;
|
||
v0v3 = (float)iso_item.v0 * iso_item.v3;
|
||
v1v2 = (float)iso_item.v1 * iso_item.v2;
|
||
v1v3 = (float)iso_item.v1 * iso_item.v3;
|
||
total_volt = bms_get_high_volt(kHvType_Bat);
|
||
|
||
tmp = 4 * v0v1 - 2 * v0v2 - v1v2;
|
||
if(tmp > 0)
|
||
{
|
||
tmp = (v0v2 - v0v1) * 2 * DIVIDER_RES_VALUE / tmp;
|
||
//iso_item.data[kIsoData_PosRes] = tmp;
|
||
//iso_item.data[kIsoData_PosValue] = tmp * 10000 / total_volt;
|
||
iso_item.pos_res_buf[iso_item.res_buf_pos] = tmp;
|
||
}
|
||
else
|
||
{
|
||
KIT_DEBUG_PRINTF("iso pos neg\r\n");
|
||
}
|
||
|
||
tmp = 4 * v0v3 - 2 * v0v1 - v1v3;
|
||
if(tmp > 0)
|
||
{
|
||
tmp = (v0v1 - v0v3) * 2 * DIVIDER_RES_VALUE / tmp;
|
||
//iso_item.data[kIsoData_NegRes] = tmp;
|
||
//iso_item.data[kIsoData_NegValue] = tmp * 10000 / total_volt;
|
||
iso_item.neg_res_buf[iso_item.res_buf_pos] = tmp;
|
||
}
|
||
else
|
||
{
|
||
KIT_DEBUG_PRINTF("iso neg neg\r\n");
|
||
}
|
||
|
||
// iso_item.data[kIsoData_Res] = (iso_item.data[kIsoData_PosRes] > iso_item.data[kIsoData_NegRes]) ? iso_item.data[kIsoData_NegRes] : iso_item.data[kIsoData_PosRes];
|
||
// iso_item.data[kIsoData_Value] = (iso_item.data[kIsoData_PosValue] > iso_item.data[kIsoData_NegValue]) ? iso_item.data[kIsoData_NegValue] : iso_item.data[kIsoData_PosValue];
|
||
if(++iso_item.res_buf_pos >= ISO_RES_BUF_CNT)
|
||
{
|
||
iso_item.res_buf_pos = 0;
|
||
|
||
iso_item.data[kIsoData_PosRes] = kit_get_unsign_avg_filter_max_min(iso_item.pos_res_buf, ISO_RES_BUF_CNT);
|
||
iso_item.data[kIsoData_NegRes] = kit_get_unsign_avg_filter_max_min(iso_item.neg_res_buf, ISO_RES_BUF_CNT);
|
||
|
||
tmp = (uint32_t)iso_item.data[kIsoData_PosRes] * 10000 / total_volt;
|
||
iso_item.data[kIsoData_PosValue] = (tmp > UINT16_MAX) ? UINT16_MAX : tmp;
|
||
|
||
tmp = (uint32_t)iso_item.data[kIsoData_NegRes] * 10000 / total_volt;
|
||
iso_item.data[kIsoData_NegValue] = (tmp > UINT16_MAX) ? UINT16_MAX : tmp;
|
||
|
||
iso_item.data[kIsoData_Res] = (iso_item.data[kIsoData_PosRes] > iso_item.data[kIsoData_NegRes]) ? iso_item.data[kIsoData_NegRes] : iso_item.data[kIsoData_PosRes];
|
||
iso_item.data[kIsoData_Value] = (iso_item.data[kIsoData_PosValue] > iso_item.data[kIsoData_NegValue]) ? iso_item.data[kIsoData_NegValue] : iso_item.data[kIsoData_PosValue];
|
||
|
||
KIT_DEBUG_PRINTF("iso %d %d %d %d\r\n", iso_item.v0, iso_item.v1, iso_item.v2, iso_item.v3);
|
||
KIT_DEBUG_PRINTF("iso pos %d neg %d\r\n", iso_item.data[kIsoData_PosRes], iso_item.data[kIsoData_NegRes]);
|
||
}
|
||
}
|
||
}
|
||
|
||
void bms_poll_iso(uint32_t base_time)
|
||
{
|
||
bool is_iso_enable = true;
|
||
iso_item.dly += base_time;
|
||
if((iso_item.ctrl == kIsoCtrl_Disable) || (iso_item.bsu_ctrl == kIsoCtrl_Disable)
|
||
|| (iso_item.ctrl == kIsoCtrl_SelfCheck && bms_get_circuit_status(kCircuitType_Main) == kCircuitStatus_On))
|
||
{
|
||
iso_item.dly = 0;
|
||
is_iso_enable = false;
|
||
iso_item.step = kIsoStep_None;
|
||
kit_set_buf(iso_item.data, kIsoData_End * 2, 0xFF);
|
||
}
|
||
bms_set_signal(kSignalIdx_BsuCtrlIso, (SignalStatus)is_iso_enable);
|
||
switch(iso_item.step)
|
||
{
|
||
case kIsoStep_None:
|
||
ISO_S1_OFF();
|
||
ISO_S2_OFF();
|
||
ISO_S3_OFF();
|
||
if(is_iso_enable == true)
|
||
{
|
||
iso_item.dly = 0;
|
||
iso_item.step = kIsoStep_TotalVolt;
|
||
KIT_DEBUG_PRINTF("iso tv \r\n");
|
||
}
|
||
break;
|
||
case kIsoStep_TotalVolt:
|
||
if ((is_volt_ad_stable(iso_item.ad_buf[kIsoVolt_TotalVolt], ISO_SAMPLE_CNT, ISO_STABLE_AD_RATE) == true) || (iso_item.dly >= ISO_SAMPLE_TIME_MAX))
|
||
{
|
||
KIT_DEBUG_PRINTF("iso tv %d\r\n", iso_item.dly);
|
||
ISO_S1_ON();
|
||
iso_item.dly = 0;
|
||
iso_item.step = kIsoStep_IsoVolt;
|
||
iso_item.v0 = (int64_t)kit_get_int32_avg_filter_max_min(iso_item.ad_buf[kIsoVolt_TotalVolt], ISO_SAMPLE_CNT) * iso_item.iso_bat_rate / 1000;
|
||
}
|
||
break;
|
||
case kIsoStep_IsoVolt:
|
||
if (((is_volt_ad_stable(iso_item.ad_buf[kIsoVolt_Other], ISO_SAMPLE_CNT, ISO_STABLE_AD_RATE) == true) || (iso_item.dly >= ISO_SAMPLE_TIME_MAX))
|
||
&& (iso_item.dly >= ISO_SAMPLE_TIME_MIN))
|
||
{
|
||
KIT_DEBUG_PRINTF("iso iv %d\r\n", iso_item.dly);
|
||
ISO_S2_ON();
|
||
iso_item.dly = 0;
|
||
iso_item.step = kIsoStep_PosVolt;
|
||
iso_item.v1 = kit_get_int32_avg_filter_max_min(iso_item.ad_buf[kIsoVolt_Other], ISO_SAMPLE_CNT);
|
||
}
|
||
break;
|
||
case kIsoStep_PosVolt:
|
||
if (((is_volt_ad_stable(iso_item.ad_buf[kIsoVolt_Other], ISO_SAMPLE_CNT, ISO_STABLE_AD_RATE) == true) || (iso_item.dly >= ISO_SAMPLE_TIME_MAX))
|
||
&& (iso_item.dly >= ISO_SAMPLE_TIME_MIN))
|
||
{
|
||
KIT_DEBUG_PRINTF("iso pv %d\r\n", iso_item.dly);
|
||
ISO_S2_OFF();
|
||
ISO_S3_ON();
|
||
iso_item.dly = 0;
|
||
iso_item.step = kIsoStep_NegVolt;
|
||
iso_item.v2 = kit_get_int32_avg_filter_max_min(iso_item.ad_buf[kIsoVolt_Other], ISO_SAMPLE_CNT);
|
||
}
|
||
break;
|
||
|
||
case kIsoStep_NegVolt:
|
||
if (((is_volt_ad_stable(iso_item.ad_buf[kIsoVolt_Other], ISO_SAMPLE_CNT, ISO_STABLE_AD_RATE) == true) || (iso_item.dly >= ISO_SAMPLE_TIME_MAX))
|
||
&& (iso_item.dly >= ISO_SAMPLE_TIME_MIN))
|
||
{
|
||
KIT_DEBUG_PRINTF("iso nv %d\r\n", iso_item.dly);
|
||
ISO_S3_OFF();
|
||
iso_item.dly = 0;
|
||
iso_item.v3 = kit_get_int32_avg_filter_max_min(iso_item.ad_buf[kIsoVolt_Other], ISO_SAMPLE_CNT);
|
||
insu_sample_calc_insu_res();
|
||
if((iso_item.ctrl == kIsoCtrl_Once) || (iso_item.bsu_ctrl == kIsoCtrl_Disable))
|
||
{
|
||
iso_item.step = kIsoStep_None;
|
||
iso_item.ctrl = kIsoCtrl_Disable;
|
||
}
|
||
else
|
||
{
|
||
//由于总压有独立通道采集，不需要关闭S1
|
||
iso_item.step = kIsoStep_TotalVolt;
|
||
}
|
||
}
|
||
break;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
void bms_set_iso_bsu_ctrl(IsoCtrl ctrl)
|
||
{
|
||
iso_item.bsu_ctrl = ctrl;
|
||
}
|
||
|
||
void bms_init_iso(void)
|
||
{
|
||
iso_item.iso_bat_rate = 1000;
|
||
|
||
kit_set_buf(iso_item.data, kIsoData_End * 2, 0xFF);
|
||
iso_item.ctrl = (IsoCtrl)get_eeprom_data(kEep_InsuPeriod_Enable, kEepromDataType_Low);
|
||
}
|