4G/code/app/bms/SOC.c

#include "SOC.h"

//---------------SOC初始化-----------------------------------------------
void SOC_Init(void)
{
    socd_flg_firstRun = true;
}


//-------------------------------------------------------------------------
//---------------------------SOC-------------------------------------------
//-------------------------------------------------------------------------
void SOC(void)
{
    static uint16_T socn_pct_battSocEE;
    static uint16_T socn_pct_bcuSocEE;
    static uint16_T socn_Q_cap;
    //
    real_T Q;
    real_T battcurr;
    boolean_T socn_flg_ekfInvalidMin;
    boolean_T socn_flg_ekfInvalidMax;
    boolean_T socn_flg_ekfInvalidAvrg;
    boolean_T socn_flg_ekfInvalid;
    real_T ocv;
    real_T Ro;
    real_T Rp;
    real_T RC;
    real_T deltU;
	real_T UL;
    real_T A[4];
    real_T B[2];
    real_T H[2];
    real_T K[2];
    real_T P1[4];
    static real_T P_Min_Delay[4];
    static real_T P_Max_Delay[4];
    static real_T P_avrg_Delay[4];
    real_T soc1;
    static real_T soc_Min_Delay;
    static real_T soc_Avrg_Delay;
    static real_T soc_Max_Delay;
    real_T Up1;
    static real_T Up_Min_Delay;
    static real_T Up_Max_Delay;
    static real_T Up_Avrg_Delay;
	uint16_T EKFSOCMin;
    uint16_T EKFSOCMax;
	
    uint16_T EKFSOCAvrg;
	uint16_T cellSocMax;
    uint16_T cellSocMin;
    uint16_T factor;
    //
    static real_T ahDelay;
    int16_T ahSoc;
    //
    boolean_T socn_flg_ekfDisable;
    static uint16_T  ekfInvalidCntl;
    static boolean_T onceFlg_est;
    static int16_T   ahSoc0_est;
    static uint16_T  ekfSoc0_est;
    // 
    uint16_T socn_V_chrgCCV;
	uint16_T socn_V_disChrgCCV;
    static boolean_T overFlg;
    static boolean_T fulFlg;
    static uint16_T overCntl;
    static uint16_T fulCntl;
    static boolean_T onceFlg_utrckOver;
    static int16_T   ahSoc0_utrckOver;
    static uint16_T  estSoc0_utrckOver;
    static uint16_T Soc_Delay;
    static boolean_T lowFlg;
    static uint16_T lowCntl;
    static boolean_T onceFlg_utrckLow;
    static int16_T ahSoc0_utrckLow;
    static uint16_T estSoc0_utrckLow;
    uint16_T socn_pct_utrackSoc;
    uint16_T socTemp;
    //
    //
    static uint16_T statCntl;
    static uint8_T  socn_st_workStat_Delay;
    static uint16_T socn_pct_battSoc_Delay;
    static uint16_T socn_pct_battSoc0;
    static uint16_T socn_pct_bcuSoc0;
    uint16_T delSOC;
    uint16_T bcuSoc;
    int16_T  coinSoc;
    uint16_T x[3];
    uint16_T y[3];
    boolean_T Flg;
    static boolean_T onceFlg_chrg;
    static boolean_T onceFlg_dischrg;

	static uint16_T socn_pct_battSoc_save;
	static uint16_T socn_pct_bcuSoc_save;
    uint16_T i;
    //
    if (socd_flg_firstRun)
    {
        onceFlg_est = true;
        ekfInvalidCntl = 0;
        overCntl = 0;
        fulCntl = 0;
        lowCntl = 0;
        overFlg = false;
        fulFlg = false;
        lowFlg = false;
		statCntl = 0;
        Soc_Delay = 0;
        onceFlg_utrckOver = true;
        onceFlg_utrckLow = true;
        socn_st_workStat_Delay = 0;
        socn_pct_battSoc_Delay = 0;
        onceFlg_chrg = true;
        onceFlg_dischrg = true;
		socn_pct_battSoc_save = 0;
		socn_pct_bcuSoc_save = 0;
    }
    
    //=====================================================================
    ////////////////////////初始值获取/EE校验//////////////////////////////
    //=====================================================================
    if (socd_flg_firstRun)
    { //
        if (socd_pct_battSocEi > socc_pct_battSocUp || socd_pct_bcuSocEi > socc_pct_battSocUp || ihd_st_EOLState == 0  ||
           (((int16_T)(socd_pct_battSocEi - socd_pct_bcuSocEi) > 300 || (int16_T)(socd_pct_battSocEi - socd_pct_bcuSocEi) < -300) && ihd_tm_parkTime >= cmnc_tm_parkTime && sfmd_I_curr < 10 && sfmd_I_curr > -10))
        {
            socn_pct_battSocEE = look1_u16tu16(sfmd_V_cellUAvrg, cmnm_V_ocv, cmnm_pct_soc, 13);
            socn_pct_bcuSocEE  = look1_u16tu16(sfmd_V_cellUAvrg, cmnm_V_ocv, cmnm_pct_soc, 13);
        }
        else
        {
            socn_pct_battSocEE = socd_pct_battSocEi;
            socn_pct_bcuSocEE  = socd_pct_bcuSocEi;
        }
        //
        if (ihd_tm_parkTime >= cmnc_tm_parkTime  && sfmd_I_curr < 10 && sfmd_I_curr > -10)
        {
            socn_pct_battSocEE = look1_u16tu16(sfmd_V_cellUAvrg, cmnm_V_ocv, cmnm_pct_soc, 13);
        }
        socn_Q_cap = (uint16_T)((uint16_T)((uint32_T)sohd_pct_bcuSoh * cmnc_Q_ratedCp / 2000U) << 1);
    }
    //printf("1----  battSocEi:%d,bcuSocEi:%d,battSocEE:%d,bcuSocEE:%d\n",socd_pct_battSocEi,socd_pct_bcuSocEi,socn_pct_battSocEE,socn_pct_bcuSocEE);
    //======================================================================
    ////////////////////////EKFSOC//////////////////////////////////////////
    //======================================================================
    battcurr = (real_T)sfmd_I_curr * 0.1;
    Q = (real_T)socn_Q_cap * 0.1;
    
    //-------------------------EKFmin---------------------------------------
    if (socd_flg_firstRun)
    {
        soc_Min_Delay = (real_T)socn_pct_battSocEE * 0.1;
        Up_Min_Delay = 0;
        P_Min_Delay[0] = 10;
        P_Min_Delay[1] = 0;
        P_Min_Delay[2] = 0;
        P_Min_Delay[3] = 10;
    }
    //参数查表
    ocv = (real_T)look1_u16tu16((uint16_T)(soc_Min_Delay * 10), cmnm_pct_soc, cmnm_V_ocv,  13) * 0.001;
    Ro  = (real_T)look1_u16tu16((uint16_T)(soc_Min_Delay * 10), cmnm_pct_soc, cmnm_R_ohm,  13) * 0.001 * 0.001;
    Rp  = (real_T)look1_u16tu16((uint16_T)(soc_Min_Delay * 10), cmnm_pct_soc, cmnm_R_polar,13) * 0.001 * 0.001;
    RC  = (real_T)look1_u16tu16((uint16_T)(soc_Min_Delay * 10), cmnm_pct_soc, cmnm_F_polar,13) * 0.001;
    A[0] = 1;
    A[1] = 0;
    A[2] = 0;
    A[3] = exp(-1 / RC);
    
    B[0] = 1 / Q / 3600 * 100;
    B[1] = Rp * (1 - exp(-1 / RC));
    
    H[0] = docvmath(soc_Min_Delay);
    H[1] = 1;
    
    //先验
    soc1 = soc_Min_Delay * A[0] + B[0] * battcurr;
    Up1 = Up_Min_Delay * A[3] + B[1] * battcurr;
    UL = ocv + battcurr * Ro + Up1;
    P1[0] = P_Min_Delay[0] + 0.001;
    P1[1] = P_Min_Delay[1] * A[3] + 0.001;
    P1[2] = P_Min_Delay[2] * A[3] + 0.001;
    P1[3] = P_Min_Delay[3] * A[3] * A[3] + 0.001;
    
    //增益
    K[0] = (P1[0] * H[0] + P1[2]) / (H[0] * P1[0] * H[0] + P1[1] * H[0] + H[0] * P1[2] + P1[3] + 0.5);
    K[1] = (P1[1] * H[0] + P1[3]) / (H[0] * P1[0] * H[0] + P1[1] * H[0] + H[0] * P1[2] + P1[3] + 0.5);
    
    //后验
    deltU = (real_T)sfmd_V_cellUMin * 0.001 - UL;
    soc_Min_Delay = soc1 + K[0] * deltU;
    soc_Min_Delay = Saturation_r(soc_Min_Delay, (real_T)socc_pct_battSocLow * 0.1, (real_T)socc_pct_battSocUp * 0.1);
    Up_Min_Delay  = Up1 + K[1] * deltU;
    //P更新
    P_Min_Delay[0] = (1 - K[0] * H[0]) * P1[0] - K[0] * P1[1];
    P_Min_Delay[1] = -K[1] * H[0] * P1[0] + P1[1] * (1 - K[1]);
    P_Min_Delay[2] = (1 - K[0] * H[0]) * P1[2] - K[0] * P1[3];
    P_Min_Delay[3] = -K[1] * H[0] * P1[2] + P1[3] * (1 - K[1]);
    
    //输出
    EKFSOCMin = (uint16_T)(soc_Min_Delay * 10);
    socn_flg_ekfInvalidMin = (deltU > 0.02) || (deltU < -0.02);
    //printf("2----socmin:%f,U:%d,R:%f\n",soc_Min_Delay,sfmd_V_cellUMin,Ro);
	//------------------------EKFSOCmax-----------------------------------
    if (socd_flg_firstRun)
    {
        soc_Max_Delay = (real_T)socn_pct_battSocEE * 0.1;
        Up_Max_Delay = 0;
        P_Max_Delay[0] = 10;
        P_Max_Delay[1] = 0;
        P_Max_Delay[2] = 0;
        P_Max_Delay[3] = 10;
    }
    // 参数查表
    ocv = (real_T)look1_u16tu16((uint16_T)(soc_Max_Delay * 10), cmnm_pct_soc, cmnm_V_ocv,  13) * 0.001;
    Ro  = (real_T)look1_u16tu16((uint16_T)(soc_Max_Delay * 10), cmnm_pct_soc, cmnm_R_ohm,  13) * 0.001 * 0.001;
    Rp  = (real_T)look1_u16tu16((uint16_T)(soc_Max_Delay * 10), cmnm_pct_soc, cmnm_R_polar,13) * 0.001 * 0.001;
    RC  = (real_T)look1_u16tu16((uint16_T)(soc_Max_Delay * 10), cmnm_pct_soc, cmnm_F_polar,13) * 0.001;

    A[0] = 1;
    A[1] = 0;
    A[2] = 0;
    A[3] = exp(-1 / RC);
    
    B[0] = 1 / Q / 3600 * 100;
    B[1] = Rp * (1 - exp(-1 / RC));
    
    H[0] = docvmath(soc_Max_Delay);
    H[1] = 1;
    
    //先验
    soc1 = soc_Max_Delay * A[0] + B[0] * battcurr;
    Up1 = Up_Max_Delay * A[3] + B[1] * battcurr;
    UL = ocv + battcurr * Ro + Up1;
    
    P1[0] = P_Max_Delay[0] + 0.001;
    P1[1] = P_Max_Delay[1] * A[3] + 0.001;
    P1[2] = P_Max_Delay[2] * A[3] + 0.002;
    P1[3] = P_Max_Delay[3] * A[3] * A[3] + 0.001;
    
    //增益
    K[0] = (P1[0] * H[0] + P1[2]) / (H[0] * P1[0] * H[0] + P1[1] * H[0] + H[0] * P1[2] + P1[3] + 0.5);
    K[1] = (P1[1] * H[0] + P1[3]) / (H[0] * P1[0] * H[0] + P1[1] * H[0] + H[0] * P1[2] + P1[3] + 0.5);
    
    //后验
    deltU = (real_T)sfmd_V_cellUMax * 0.001 - UL;
    soc_Max_Delay = soc1 + K[0] * deltU;
	soc_Max_Delay = Saturation_r(soc_Max_Delay, (real_T)socc_pct_battSocLow * 0.1, (real_T)socc_pct_battSocUp * 0.1);
    Up_Max_Delay = Up1 + K[1] * deltU;
    
    //P更新
    P_Max_Delay[0] = (1 - K[0] * H[0]) * P1[0] - K[0] * P1[1];
    P_Max_Delay[1] = -K[1] * H[0] * P1[0] + P1[1] * (1 - K[1]);
    P_Max_Delay[2] = (1 - K[0] * H[0]) * P1[2] - K[0] * P1[3];
    P_Max_Delay[3] = -K[1] * H[0] * P1[2] + P1[3] * (1 - K[1]);
    //输出
    EKFSOCMax = (uint16_T)(soc_Max_Delay * 10);
    socn_flg_ekfInvalidMax = (deltU > 0.02) || (deltU < -0.02);
    //printf("3----socmax:%f,U:%d,R:%f\n\n",soc_Max_Delay,sfmd_V_cellUMax,Ro);
    //-----------------------EKFSOC----------------------------------------
    socn_flg_ekfInvalid = socn_flg_ekfInvalidMax || socn_flg_ekfInvalidMin;
    if (EKFSOCMax > 800)
    {
        factor = 100;
    }
    else if (EKFSOCMin < 200)
    {
        factor = 0;
    }
    else
    {
        factor = (uint16_T)(((uint16_T)(((uint32_T)(EKFSOCMin - 200) << 6) / (800 - (EKFSOCMax - EKFSOCMin) - 200)) * 25U) >> 4);
    }
    socd_pct_ekfSoc = (uint16_T)(((1 - (real_T)(factor * 0.01)) * (real_T)(EKFSOCMin * 0.1) + (real_T)(factor * 0.01) * (real_T)(EKFSOCMax * 0.1)) * 10);
   
   //printf("4----socd_pct_ekfSoc:%d,EKFSOCMax:%d,EKFSOCMin:%d,\n",socd_pct_ekfSoc,EKFSOCMax,EKFSOCMin);
/*
   //-------------------------EKFavrg---------------------------------------
    if (socd_flg_firstRun)
    {
        soc_Avrg_Delay = (real_T)socn_pct_battSocEE * 0.1;
        Up_Avrg_Delay = 0;
        P_avrg_Delay[0] = 10;
        P_avrg_Delay[1] = 0;
        P_avrg_Delay[2] = 0;
        P_avrg_Delay[3] = 10;
    }
    //参数查表
    ocv = (real_T)look1_u16tu16((uint16_T)(soc_Avrg_Delay * 10), cmnm_pct_soc, cmnm_V_ocv,  13) * 0.001;
    Ro  = (real_T)look1_u16tu16((uint16_T)(soc_Avrg_Delay * 10), cmnm_pct_soc, cmnm_R_ohm,  13) * 0.001 * 0.001;
    Rp  = (real_T)look1_u16tu16((uint16_T)(soc_Avrg_Delay * 10), cmnm_pct_soc, cmnm_R_polar,13) * 0.001 * 0.001;
    RC  = (real_T)look1_u16tu16((uint16_T)(soc_Avrg_Delay * 10), cmnm_pct_soc, cmnm_F_polar,13) * 0.001;

    A[0] = 1;
    A[1] = 0;
    A[2] = 0;
    A[3] = exp(-1 / RC);
    
    B[0] = 1 / Q / 3600 * 100;
    B[1] = Rp * (1 - exp(-1 / RC));
    
    H[0] = docvmath(soc_Avrg_Delay);
    H[1] = 1;
    
    //先验
    soc1 = soc_Avrg_Delay * A[0] + B[0] * battcurr;
    Up1 = Up_Avrg_Delay * A[3] + B[1] * battcurr;
    UL = ocv + battcurr * Ro + Up1;
    P1[0] = P_avrg_Delay[0] + 0.001;
    P1[1] = P_avrg_Delay[1] * A[3] + 0.001;
    P1[2] = P_avrg_Delay[2] * A[3] + 0.001;
    P1[3] = P_avrg_Delay[3] * A[3] * A[3] + 0.001;
    
    //增益
    K[0] = (P1[0] * H[0] + P1[2]) / (H[0] * P1[0] * H[0] + P1[1] * H[0] + H[0] * P1[2] + P1[3] + 0.5);
    K[1] = (P1[1] * H[0] + P1[3]) / (H[0] * P1[0] * H[0] + P1[1] * H[0] + H[0] * P1[2] + P1[3] + 0.5);
    
    //后验
    deltU = (real_T)sfmd_V_cellUAvrg * 0.001 - UL;
    soc_Avrg_Delay = soc1 + K[0] * deltU;
    
    soc_Avrg_Delay = Saturation_r(soc_Avrg_Delay, (real_T)socc_pct_battSocLow * 0.1, (real_T)socc_pct_battSocUp * 0.1);
    Up_Avrg_Delay = Up1 + K[1] * deltU;
    //P更新
    P_avrg_Delay[0] = (1 - K[0] * H[0]) * P1[0] - K[0] * P1[1];
    P_avrg_Delay[1] = -K[1] * H[0] * P1[0] + P1[1] * (1 - K[1]);
    P_avrg_Delay[2] = (1 - K[0] * H[0]) * P1[2] - K[0] * P1[3];
    P_avrg_Delay[3] = -K[1] * H[0] * P1[2] + P1[3] * (1 - K[1]);
    
    //输出
    EKFSOCAvrg = (uint16_T)(soc_Avrg_Delay * 10);
    socn_flg_ekfInvalidAvrg = (deltU > 0.01) || (deltU < -0.01);
	//printf("4----soc:%f,Up:%f,U:%d,deltU:%f,K[0]:%f,K[1]:%f\n\n",soc_Avrg_Delay,Up_Avrg_Delay,sfmd_V_cellUAvrg,deltU,K[0],K[1]);
    //
    for(i = 0;i < cmnc_num_cellUNum;i++)
    {
        socv_pct_cellSoc[i] = look1_u16tu16((look1_u16tu16(EKFSOCAvrg, cmnm_pct_soc, cmnm_V_ocv, 13) + cdmv_V_deltOCV[i]), cmnm_V_ocv, cmnm_pct_soc, 13);
    }
    socd_flg_cellSocDisable = socn_flg_ekfInvalidAvrg || cdmd_flg_deltOCVDisable;
	
    cellSocMax = ArrMax(socv_pct_cellSoc,cmnc_num_cellUNum);
    cellSocMin = ArrMin(socv_pct_cellSoc,cmnc_num_cellUNum);
	
    if (cellSocMax > 800)
    {
        factor = 100;
    }
    else if (cellSocMin < 200)
    {
        factor = 0;
    }
    else
    {
        factor = (uint16_T)(((uint16_T)(((uint32_T)(cellSocMin - 200) << 6) / (800 - (cellSocMax - cellSocMin) - 200)) * 25U) >> 4);
    }
    socd_pct_cellBattSoc = (uint16_T)(((1 - (real_T)(factor * 0.01)) * (real_T)(cellSocMin * 0.1) + (real_T)(factor * 0.01) * (real_T)(cellSocMax * 0.1)) * 10);
    
*/
    //======================================================================
    ////////////////////////AhSOC//////////////////////////////////////////
    //======================================================================
    if (socd_flg_firstRun)
    {
        ahDelay = (real_T)(socn_pct_battSocEE * 0.1);
    }
    else
    {
        ahDelay = ahDelay + battcurr / (real_T)(cmnc_Q_ratedCp * 0.1) / 36.0;//cmnc_Q_ratedCp
    }
    ahSoc = (int16_T)(ahDelay * 10);
	
	socd_pct_ahSoc = Saturation_u(ahSoc < 0 ? 0 : (uint16_T)ahSoc, socc_pct_battSocLow, socc_pct_battSocUp);
	//printf("5----ahDelay:%f,ahSoc:%d,battcurr:%f,sfmd_I_curr:%d\n",ahDelay,ahSoc,battcurr,sfmd_I_curr);
	
    //======================================================================
    ///////////////////////estSOC//////////////////////////////////////////
    //======================================================================
    socn_flg_ekfDisable = !JudgeTimeSystem(1,!socn_flg_ekfInvalid,&ekfInvalidCntl,2);
    if (socn_flg_ekfDisable)
    {
        if (onceFlg_est)
        {  
            ahSoc0_est  = ahSoc;
            ekfSoc0_est = socd_pct_ekfSoc;
            onceFlg_est = false;
        }
        socd_pct_estSoc = (int16_T)(ahSoc - ahSoc0_est + ekfSoc0_est) > 0 ? (uint16_T)(ahSoc - ahSoc0_est + ekfSoc0_est) : 0;
    }
    else
    {
        onceFlg_est = true;
        socd_pct_estSoc = socd_pct_ekfSoc;
    }
    socd_pct_estSoc = Saturation_u(socd_pct_estSoc, socc_pct_battSocLow, socc_pct_battSocUp);
    //
    //printf("6----ahSoc0_est:%d,ekfSoc0_est:%d,socd_pct_estSoc:%d\n",ahSoc0_est,ekfSoc0_est,socd_pct_estSoc);
	
    //======================================================================
    ////////////////////////UtrackSOC//////////////////////////////////////////
    //======================================================================
    if (ihd_st_workStat == 2)
    {
        lowCntl = 0;
        lowFlg = false;
		
	    socn_V_chrgCCV = look1_i16tu16(sfmd_I_curr,socm_I_chrgCor,socm_V_chrgCor,3); 
		overFlg = JudgeTimeSystem(1,sfmd_V_cellUMax >= socn_V_chrgCCV,&overCntl,2) || overFlg;
        fulFlg  = JudgeTimeSystem(1,sfmd_V_cellUMax >= cmnc_V_chrgFul,&fulCntl ,2) || fulFlg;
        //
        if (overFlg)
        {
            if (onceFlg_utrckOver)
            {
                onceFlg_utrckOver = false;
                ahSoc0_utrckOver = ahSoc;
                estSoc0_utrckOver = socd_pct_estSoc > socc_pct_chrgCor ? socd_pct_estSoc : socc_pct_chrgCor;
            }
            socTemp = (uint16_T)(ahSoc - ahSoc0_utrckOver + estSoc0_utrckOver);
        }
        else
        {
            onceFlg_utrckOver = true;
            socTemp = socd_pct_estSoc > socc_pct_chrgCor ? socc_pct_chrgCor : socd_pct_estSoc;
        }
        //
        socn_pct_utrackSoc = Soc_Delay + (socTemp > Soc_Delay ? (socTemp - Soc_Delay) : 0);
        Soc_Delay = socn_pct_utrackSoc;
        
        if (fulFlg)
        {
            socn_pct_utrackSoc = socc_pct_battSocUp;
        }
        else
        {
            socn_pct_utrackSoc = socn_pct_utrackSoc > (socc_pct_battSocUp - 1) ? (socc_pct_battSocUp - 1) : socn_pct_utrackSoc;
        }
        
       // printf("7----overCntl:%d,overFlg:%d,fulCntl:%d,fulFlg:%d,ahSoc0_utrckOver:%d,estSoc0_utrckOver:%d,socn_pct_utrackSoc:%d,socTemp:%d\n",overCntl,overFlg,fulCntl,fulFlg,ahSoc0_utrckOver,estSoc0_utrckOver,socn_pct_utrackSoc,socTemp);
    }
    else
    {
        Soc_Delay = 0;
        overCntl = 0;
        overFlg = false;
        fulFlg = false;
        fulCntl = 0;
		if (!pimv_flg_inval[sfmd_idx_cellUMin])
		{
		    socn_V_disChrgCCV = look1_i16tu16(sfmd_I_curr,socm_I_disChrgCor,socm_V_disChrgCor,3) + (uint16_T)(sfmd_I_curr * (pimv_R_cellOhm[sfmd_idx_cellUMin]) * 0.001 * 0.1);
		}
		else
		{
		    socn_V_disChrgCCV = look1_i16tu16(sfmd_I_curr,socm_I_disChrgCor,socm_V_disChrgCor,3);
		}
		lowFlg = JudgeTimeSystem(1,sfmd_V_cellUMin <= socn_V_disChrgCCV,&lowCntl,2) || lowFlg;
        //
        if (lowFlg)
        {
            if (onceFlg_utrckLow)
            {
                onceFlg_utrckLow = false;
                ahSoc0_utrckLow  = ahSoc;
                estSoc0_utrckLow = socc_pct_disChrgCor;
            }
            socn_pct_utrackSoc = (int16_T)(ahSoc - ahSoc0_utrckLow + estSoc0_utrckLow) > 0 ? (uint16_T)(ahSoc - ahSoc0_utrckLow + estSoc0_utrckLow) : 0;
        }
        else 
        {
            onceFlg_utrckLow = true;
            socn_pct_utrackSoc = socd_pct_estSoc < socc_pct_disChrgCor ? socc_pct_disChrgCor : socd_pct_estSoc;
        }
		
        //printf("8----lowCntl:%d,lowFlg:%d,ahSoc0_utrckLow:%d,estSoc0_utrckLow:%d,socn_pct_utrackSoc:%d\n",lowCntl,lowFlg,ahSoc0_utrckLow,estSoc0_utrckLow,socn_pct_utrackSoc);
    }
	socd_pct_battSoc = Saturation_u(socn_pct_utrackSoc, socc_pct_battSocLow, socc_pct_battSocUp);
    socd_pct_battSocEo = socd_pct_battSoc;

    
    //===============================================================================================================================================================
    ////////////////////////////////////////////////BCUSOC///////////////////////////////////////////////////////////////////////////////////////////////////////////
    //=============================================================================================================================================================== 
    Flg = (socd_flg_firstRun || (socn_st_workStat_Delay == 2 && ihd_st_workStat != 2) || (socn_st_workStat_Delay != 2 && ihd_st_workStat == 2) ||
          ((int16_T)(socd_pct_battSoc  - socn_pct_battSoc_Delay) > 5 || (int16_T)(socd_pct_battSoc - socn_pct_battSoc_Delay) < -5) ||
          JudgeTimeSystem(1,sfmd_I_curr < 10 && sfmd_I_curr > -10,&statCntl ,3));
	
    socn_st_workStat_Delay = ihd_st_workStat;
    socn_pct_battSoc_Delay = socd_pct_battSoc;
    
    //
    if (Flg)
    {
        socn_pct_battSoc0 = socd_pct_battSoc;
        if (socd_flg_firstRun)
        {
            socn_pct_bcuSoc0 = socn_pct_bcuSocEE;
        }
        else
        {
            socn_pct_bcuSoc0 = socd_pct_bcuSoc;
        }
    }
    
    //
    if (ihd_st_workStat == 2)
    {
        delSOC = socn_pct_battSoc0 > socn_pct_bcuSoc0 ? (socn_pct_battSoc0 - socn_pct_bcuSoc0) : (socn_pct_bcuSoc0 - socn_pct_battSoc0);
        coinSoc = (socn_pct_battSoc0 > socn_pct_bcuSoc0 ? socn_pct_battSoc0 : socn_pct_bcuSoc0) + (delSOC > 50 ? 50 : delSOC);
        
        x[0] = socn_pct_battSoc0 > socc_pct_battSocUp - 2 ? socc_pct_battSocUp - 2 : socn_pct_battSoc0;
        x[1] = coinSoc > socc_pct_battSocUp - 1 ? socc_pct_battSocUp - 1: (uint16_T)coinSoc;
        x[2] = socc_pct_battSocUp;
        
        y[0] = socn_pct_bcuSoc0 > socc_pct_battSocUp - 2 ? socc_pct_battSocUp - 2 : socn_pct_bcuSoc0;;
        y[1] = coinSoc > socc_pct_battSocUp - 1 ? socc_pct_battSocUp - 1: (uint16_T)coinSoc;
        y[2] = socc_pct_battSocUp;
        bcuSoc = look1_u16tu16(socd_pct_battSoc, x, y,3);
        
        //
        if (onceFlg_chrg)
        {
            onceFlg_chrg = false;
            onceFlg_dischrg = true;
            socd_pct_bcuSoc = bcuSoc;
        }
		
        socd_pct_bcuSoc = DataFilt(bcuSoc, &socd_pct_bcuSoc, 1);
        
        //
        if (fulFlg)
        {
            socd_pct_bcuSoc = socc_pct_battSocUp;
        }
        else
        {
            socd_pct_bcuSoc = socd_pct_bcuSoc > (socc_pct_battSocUp - 1) ? (socc_pct_battSocUp - 1) : socd_pct_bcuSoc;
        }
       //printf("10-----x:[%d-%d-%d],y:[%d-%d-%d],battSOC:%d,bcusoc:%d,socd_pct_bcuSoc:%d\n",x[0],x[1],x[2],y[0],y[1],y[2],socd_pct_battSoc,bcuSoc,socd_pct_bcuSoc);
    }
    else
    {
        //
        delSOC = socn_pct_battSoc0 > socn_pct_bcuSoc0 ? (socn_pct_battSoc0 - socn_pct_bcuSoc0) : (socn_pct_bcuSoc0 - socn_pct_battSoc0);
        coinSoc = (int16_T)((socn_pct_battSoc0 < socn_pct_bcuSoc0 ? socn_pct_battSoc0 : socn_pct_bcuSoc0) - (delSOC > 50 ? 50 : delSOC));
        
        x[0] = socc_pct_battSocLow;
        x[1] = coinSoc < socc_pct_battSocLow + 1 ? socc_pct_battSocLow + 1 : (uint16_T)coinSoc;
        x[2] = socn_pct_battSoc0 < socc_pct_battSocLow + 2 ? socc_pct_battSocLow + 2 : (uint16_T)socn_pct_battSoc0;
		
        y[0] = socc_pct_battSocLow;
        y[1] = coinSoc < socc_pct_battSocLow + 1 ? socc_pct_battSocLow + 1 : (uint16_T)coinSoc;
        y[2] = socn_pct_bcuSoc0 < socc_pct_battSocLow + 2 ? socc_pct_battSocLow + 2 : (uint16_T)socn_pct_bcuSoc0;
        bcuSoc = look1_u16tu16(socd_pct_battSoc, x, y,3);
        
        //
        if (onceFlg_dischrg)
        {
            onceFlg_chrg = true;
            onceFlg_dischrg = false;
            socd_pct_bcuSoc = bcuSoc;
        }
		
        socd_pct_bcuSoc = DataFilt(bcuSoc, &socd_pct_bcuSoc, 1);
		
        //printf("11-----x:[%d-%d-%d],y:[%d-%d-%d],bcusoc:%d,socd_pct_bcuSoc:%d\n",x[0],x[1],x[2],y[0],y[1],y[2],bcuSoc,socd_pct_bcuSoc);
    }
	socd_pct_bcuSoc = Saturation_u(socd_pct_bcuSoc, socc_pct_battSocLow, socc_pct_battSocUp);
    socd_pct_bcuSocEo = socd_pct_bcuSoc;

	//=======================================VCUSOC========================================================================================
	
    socd_pct_vcuSoc =  (uint16_T)((uint32_T)((socd_pct_bcuSoc - socc_pct_battSocLow) * 1000) / (socc_pct_battSocUp - socc_pct_battSocLow)); 
	
    //================================================================================================
    //------------------EEsave-------------------------------------------=============================
    //================================================================================================
    if ((int16_T)(socd_pct_battSoc - socn_pct_battSoc_save) > 10 || (int16_T)(socd_pct_battSoc - socn_pct_battSoc_save) < -10 || (int16_T)(socd_pct_bcuSoc - socn_pct_bcuSoc_save) > 10 || (int16_T)(socd_pct_bcuSoc - socn_pct_bcuSoc_save) < -10 )
    {
        socd_flg_EEsave = 1;
        socn_pct_battSoc_save = socd_pct_battSoc;
	    socn_pct_bcuSoc_save  = socd_pct_bcuSoc;
    }
    else
    {
        socd_flg_EEsave = 0;
    }

	
    socd_flg_firstRun = false;

	//printf("\n");
}



//===================================================================================================================
//----------------------------function-------------------------------------------------------------------------------
//===================================================================================================================
real_T docvmath(real_T soc)
{
    real_T docv;
    docv = ((((((-1.0936E-13 * pow(soc, 7.0) +3.9249E-11 * pow(soc, 6.0)) +
            -5.5776E-9 * pow(soc, 5.0)) + 3.996E-7 * pow(soc, 4.0)) +
            -1.5332E-5 * pow(soc, 3.0)) +soc * soc * 0.0003192) +
           -0.00371 * soc) +0.02732;
    return docv;
}

////

//-------------------------------------------------------------------------