NB_APP_Code/src/EmbeddedCoder_src/SOC.c

#include "SOC.h"
#include "look1_is16lu16n16tu16_binlcase.h"
#include "look1_iu16lu16n16tu16_binlcase.h"

boolean_T FirstRun_SOC;

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

void SOC(void)
{
    static uint16_T socn_pct_battSocEE;
    static uint16_T socn_pct_bcuSocEE;
    static uint16_T socn_Q_cap;
    //
    uint16_T EKFSOCMin;
    uint16_T EKFSOCMax;
    uint16_T EKFSOCAvrg;
    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 C;
    real_T deltU;
    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 cellSocMax;
    uint16_T cellSocMin;
    real_T Q;
    real_T battcurr;
    real_T UL;
    uint16_T factor;
    //
    static real_T ahDelay;
    int16_T ahSoc;
    //
    static uint16_T ekfInvalidCntl;
    static boolean_T onceFlg_est;
    static int16_T ahSoc0_est;
    static uint16_T ekfSoc0_est;
    //
    uint16_T socd_V_chrgCCV;
	uint16_T socd_V_disChrgCCV;
    static boolean_T overFlg;
    static boolean_T fulFlg;
    static uint8_T overCntl;
    static uint8_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 uint8_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 uint8_T chrgCntl;
    static uint8_T disChrgCntl;
    //
    //
    static uint8_T statCntl;
    boolean_T statFlg;
    static uint8_T ihd_st_chrgSta_Delay;
    static uint16_T socd_pct_battSoc_Delay;
    static uint16_T socd_pct_battSoc0;
    static uint16_T socd_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 socd_pct_battSoc_save;
	static uint16_T socd_pct_bcuSoc_save;
    uint16_T i;
    //
    if (FirstRun_SOC)
    {
        onceFlg_est = true;
        ekfInvalidCntl = 0;
        overCntl = 0;
        fulCntl = 0;
        lowCntl = 0;
        overFlg = false;
        fulFlg = false;
        lowFlg = false;
        Soc_Delay = 0;
        onceFlg_utrckOver = true;
        onceFlg_utrckLow = true;
        ihd_st_chrgSta_Delay = 0;
        socd_pct_battSoc_Delay = 0;
        onceFlg_chrg = true;
        onceFlg_dischrg = true;
		socd_pct_battSoc_save = 0;
		socd_pct_bcuSoc_save = 0;	
    }
    
    //=====================================================================
    ////////////////////////初始值获取//////////////////////////////////////
    //=====================================================================
    if (FirstRun_SOC)
    { //
        if (socd_pct_battSocEi > 1000 || socd_pct_bcuSocEi > 1000 || 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_iu16lu16n16tu16_binlcase(sfmd_V_cellUAvrg, (&(cmnm_V_ocv[0])), (&(cmnm_pct_soc[0])), 12U);
            socn_pct_bcuSocEE  = look1_iu16lu16n16tu16_binlcase(sfmd_V_cellUAvrg, (&(cmnm_V_ocv[0])), (&(cmnm_pct_soc[0])), 12U);
        }
        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_iu16lu16n16tu16_binlcase(sfmd_V_cellUAvrg, (&(cmnm_V_ocv[0])), (&(cmnm_pct_soc[0])), 12U);
        }
        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 (FirstRun_SOC)
    {
        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_iu16lu16n16tu16_binlcase((uint16_T)(soc_Min_Delay * 10), (&(cmnm_pct_soc[0])), (&(cmnm_V_ocv[0])),   12U) * 0.001;
    Ro  = (real_T)look1_iu16lu16n16tu16_binlcase((uint16_T)(soc_Min_Delay * 10), (&(cmnm_pct_soc[0])), (&(cmnm_R_ohm[0])),   12U) * 0.001 * 0.001;
    Rp  = (real_T)look1_iu16lu16n16tu16_binlcase((uint16_T)(soc_Min_Delay * 10), (&(cmnm_pct_soc[0])), (&(cmnm_R_polar[0])), 12U) * 0.001 * 0.001;
    C   = (real_T)look1_iu16lu16n16tu16_binlcase((uint16_T)(soc_Min_Delay * 10), (&(cmnm_pct_soc[0])), (&(cmnm_F_polar[0])), 12U) * 0.001 * 1000;
    A[0] = 1;
    A[1] = 0;
    A[2] = 0;
    A[3] = exp(-1 / (Rp * C));
    
    B[0] = 1 / Q / 3600 * 100;
    B[1] = Rp * (1 - exp(-1 / (Rp * C)));
    
    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;
    
    if (soc_Min_Delay < (real_T)socc_pct_battSocLow * 0.1)
    {
        soc_Min_Delay = (real_T)socc_pct_battSocLow * 0.1;
    }
    if (soc_Min_Delay > (real_T)socc_pct_battSocUp * 0.1)
    {
        soc_Min_Delay = (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.01) || (deltU < -0.01);
    //printf("2----soc:%f,Up:%f,U:%d,deltU:%f,K[0]:%f,K[1]:%f\n",soc_Min_Delay,Up_Min_Delay,sfmd_V_cellUMin,deltU,K[0],K[1]);  
	//------------------------EKFSOCmax-----------------------------------
    if (FirstRun_SOC)
    {
        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_iu16lu16n16tu16_binlcase((uint16_T)(soc_Max_Delay * 10), (&(cmnm_pct_soc[0])), (&(cmnm_V_ocv[0])),   12U) * 0.001;
    Ro  = (real_T)look1_iu16lu16n16tu16_binlcase((uint16_T)(soc_Max_Delay * 10), (&(cmnm_pct_soc[0])), (&(cmnm_R_ohm[0])),   12U) * 0.001 * 0.001;
    Rp  = (real_T)look1_iu16lu16n16tu16_binlcase((uint16_T)(soc_Max_Delay * 10), (&(cmnm_pct_soc[0])), (&(cmnm_R_polar[0])), 12U) * 0.001 * 0.001;
    C   = (real_T)look1_iu16lu16n16tu16_binlcase((uint16_T)(soc_Max_Delay * 10), (&(cmnm_pct_soc[0])), (&(cmnm_F_polar[0])), 12U) * 0.001 * 1000;
    A[0] = 1;
    A[1] = 0;
    A[2] = 0;
    A[3] = exp(-1 / (Rp * C));
    
    B[0] = 1 / Q / 3600 * 100;
    B[1] = Rp * (1 - exp(-1 / (Rp * C)));
    
    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;
    
    if (soc_Max_Delay < (real_T)socc_pct_battSocLow * 0.1)
    {
        soc_Max_Delay = (real_T)socc_pct_battSocLow * 0.1;
    }
    if (soc_Max_Delay > (real_T)socc_pct_battSocUp * 0.1)
    {
        soc_Max_Delay = (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.01) || (deltU < -0.01);
    //printf("4----soc:%f,Up:%f,U:%d,deltU:%f,K[0]:%f,K[1]:%f\n",soc_Max_Delay,Up_Max_Delay,sfmd_V_cellUMax,deltU,K[0],K[1]);
    //-----------------------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 (FirstRun_SOC)
    {
        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_iu16lu16n16tu16_binlcase((uint16_T)(soc_Avrg_Delay * 10), (&(cmnm_pct_soc[0])), (&(cmnm_V_ocv[0])),   12U) * 0.001;
    Ro  = (real_T)look1_iu16lu16n16tu16_binlcase((uint16_T)(soc_Avrg_Delay * 10), (&(cmnm_pct_soc[0])), (&(cmnm_R_ohm[0])),   12U) * 0.001 * 0.001;
    Rp  = (real_T)look1_iu16lu16n16tu16_binlcase((uint16_T)(soc_Avrg_Delay * 10), (&(cmnm_pct_soc[0])), (&(cmnm_R_polar[0])), 12U) * 0.001 * 0.001;
    C   = (real_T)look1_iu16lu16n16tu16_binlcase((uint16_T)(soc_Avrg_Delay * 10), (&(cmnm_pct_soc[0])), (&(cmnm_F_polar[0])), 12U) * 0.001 * 1000;
    A[0] = 1;
    A[1] = 0;
    A[2] = 0;
    A[3] = exp(-1 / (Rp * C));
    
    B[0] = 1 / Q / 3600 * 100;
    B[1] = Rp * (1 - exp(-1 / (Rp * C)));
    
    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;
    
    if (soc_Avrg_Delay < (real_T)socc_pct_battSocLow * 0.1)
    {
        soc_Avrg_Delay = (real_T)socc_pct_battSocLow * 0.1;
    }
    if (soc_Avrg_Delay > (real_T)socc_pct_battSocUp * 0.1)
    {
        soc_Avrg_Delay = (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_iu16lu16n16tu16_binlcase((look1_iu16lu16n16tu16_binlcase(EKFSOCAvrg, (&(cmnm_pct_soc[0])), (&(cmnm_V_ocv[0])), 12U) + cdmv_V_deltOCV[i]), (&(cmnm_V_ocv[0])),(&(cmnm_pct_soc[0])),12U);
    }
    socd_flg_cellSocDisable = socn_flg_ekfInvalidAvrg || cdmd_flg_deltOCVDisable;
    cellSocMax = ArrMax(&socv_pct_cellSoc[0],cmnc_num_cellUNum);
    cellSocMin = ArrMin(&socv_pct_cellSoc[0],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 (FirstRun_SOC)
    {
        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);
    if (ahSoc > socc_pct_battSocUp)
    {
        socd_pct_ahSoc = socc_pct_battSocUp;
    }
    else if (ahSoc < socc_pct_battSocLow)
    {
        socd_pct_ahSoc = socc_pct_battSocLow;
    }
    else
    {
        socd_pct_ahSoc = (uint16_T)ahSoc;
    }
	//printf("5----ahDelay:%f,ahSoc:%d,battcurr:%f,sfmd_I_curr:%d\n",ahDelay,ahSoc,battcurr,sfmd_I_curr);
    //======================================================================
    ///////////////////////estSOC//////////////////////////////////////////
    //======================================================================
    if (!socn_flg_ekfInvalid)
    {
        ekfInvalidCntl = (ekfInvalidCntl + 1) > 250 ? 250 : (ekfInvalidCntl + 1);
    }
    else
    {
        ekfInvalidCntl = 0;
    }
    
    if (ekfInvalidCntl < 20)
    {
        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;
    }
    
    //
    if (socd_pct_estSoc > socc_pct_battSocUp)
    {
        socd_pct_estSoc = socc_pct_battSocUp;
    }
    if (socd_pct_estSoc < socc_pct_battSocLow)
    {
        socd_pct_estSoc = socc_pct_battSocLow;
    }
    //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)
    {
        disChrgCntl = 0;
        chrgCntl = (chrgCntl + 1) > 250 ? 250 : (chrgCntl + 1);
        lowCntl = 0;
        lowFlg = false;
	    socd_V_chrgCCV = look1_is16lu16n16tu16_binlcase(sfmd_I_curr,&socm_I_chrgCor[0],&socm_V_chrgCor[0],2U); 
		if (sfmd_V_cellUMax >= socd_V_chrgCCV)
        {
            overCntl = (overCntl + 1) > 250 ? 250 : (overCntl + 1);
        }
        else
        {
            overCntl = 0;
        }
        if (overCntl > 2 || overFlg)
        {
            overFlg = 1;
        }
        //
        if (sfmd_V_cellUMax >= cmnc_V_chrgFul)
        {
            fulCntl = (fulCntl + 1) > 250 ? 250 : (fulCntl + 1);
        }
        else
        {
            fulCntl = 0;
        }
        if ((fulCntl > 2) || fulFlg)
        {
            fulFlg = 1;
        }
        
        //
        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 if (chrgCntl > 2)
        {
            onceFlg_utrckOver = true;
            socTemp = socd_pct_estSoc > socc_pct_chrgCor ? socc_pct_chrgCor : socd_pct_estSoc;
        }
        else
        {
            socTemp = 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
    {
        chrgCntl = 0;
        disChrgCntl = (disChrgCntl + 1) > 250 ? 250 : (disChrgCntl + 1);
        Soc_Delay = 0;
        overCntl = 0;
        overFlg = false;
        fulFlg = false;
        fulCntl = 0;
		if (!cdmd_flg_deltOCVDisable)
		{
		    socd_V_disChrgCCV = look1_is16lu16n16tu16_binlcase(sfmd_I_curr,&socm_I_disChrgCor[0],&socm_V_disChrgCor[0],2U) + sfmd_I_curr * (cdmv_ohm_deltR[sfmd_idx_cellUMin]) * 0.001 * 0.1;
		}
		else
		{
		     socd_V_disChrgCCV = look1_is16lu16n16tu16_binlcase(sfmd_I_curr,&socm_I_disChrgCor[0],&socm_V_disChrgCor[0],2U);
		}
		
        if (sfmd_V_cellUMin <= socd_V_disChrgCCV)
        {
            lowCntl = (lowCntl + 1) > 250 ? 250 : (lowCntl + 1);
        }
        else
        {
            lowCntl = 0;
        }
        if (lowCntl > 2 || lowFlg)
        {
            lowFlg = true;
        }
        //
        if (lowFlg)
        {
            if (onceFlg_utrckLow)
            {
                onceFlg_utrckLow = false;
                ahSoc0_utrckLow = ahSoc;
                estSoc0_utrckLow = socd_pct_estSoc < socc_pct_disChrgCor ? socd_pct_estSoc : socc_pct_disChrgCor;
            }
            socn_pct_utrackSoc = (int16_T)(ahSoc - ahSoc0_utrckLow + estSoc0_utrckLow) > 0 ? (uint16_T)(ahSoc - ahSoc0_utrckLow + estSoc0_utrckLow) : 0;
        }
        else if (disChrgCntl > 2)
        {
            onceFlg_utrckLow = true;
            socn_pct_utrackSoc = socd_pct_estSoc < socc_pct_disChrgCor ? socc_pct_disChrgCor : socd_pct_estSoc;
        }
        else
        {
            socn_pct_utrackSoc = 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 = socn_pct_utrackSoc;
    socd_pct_battSocEo = socn_pct_utrackSoc;

    
    //===============================================================================================================================================================
    ////////////////////////////////////////////////BCUSOC///////////////////////////////////////////////////////////////////////////////////////////////////////////
    //===============================================================================================================================================================
    
    if (sfmd_I_curr < 10 && sfmd_I_curr > -10)
    {
        statCntl = (statCntl + 1) > 250 ? 250 : (statCntl + 1);
    }
    else
    {
        statCntl = 0;
    }
    statFlg = statCntl > 2;
    
    Flg = (FirstRun_SOC || (ihd_st_chrgSta_Delay == 2 && ihd_st_workStat != 2) || (ihd_st_chrgSta_Delay != 2 && ihd_st_workStat == 2) || ((int16_T)(socd_pct_battSoc  - socd_pct_battSoc_Delay) > 20 || (int16_T)(socd_pct_battSoc - socd_pct_battSoc_Delay) < -20) || statFlg);
    
    ihd_st_chrgSta_Delay = ihd_st_workStat;
    socd_pct_battSoc_Delay = socd_pct_battSoc;
    
    //
    if (Flg)
    {
        socd_pct_battSoc0 = socd_pct_battSoc;
        if (FirstRun_SOC)
        {
            socd_pct_bcuSoc0 = socn_pct_bcuSocEE;
        }
        else
        {
            socd_pct_bcuSoc0 = socd_pct_bcuSoc;
        }
    }
    
    //printf("9----statCntl:%d,statFlg:%d,Flg:%d,socd_pct_bcuSoc0:%d,socd_pct_bcuSoc0:%d,Flg:%d\n",statCntl,statFlg,Flg,socd_pct_bcuSoc0,socd_pct_bcuSoc0,Flg);
    //
    if (ihd_st_workStat == 2)
    {
        delSOC = socd_pct_battSoc0 > socd_pct_bcuSoc0 ? (socd_pct_battSoc0 - socd_pct_bcuSoc0) : (socd_pct_bcuSoc0 - socd_pct_battSoc0);
        coinSoc = (socd_pct_battSoc0 > socd_pct_bcuSoc0 ? socd_pct_battSoc0 : socd_pct_bcuSoc0) + (delSOC > 50 ? 50 : delSOC);
        
        x[0] = socd_pct_battSoc0;
        x[1] = coinSoc > socc_pct_battSocUp ? socc_pct_battSocUp : (uint16_T)coinSoc;
        x[2] = socc_pct_battSocUp;
        
        y[0] = socd_pct_bcuSoc0;
        y[1] = coinSoc > socc_pct_battSocUp ? socc_pct_battSocUp : (uint16_T)coinSoc;
        y[2] = socc_pct_battSocUp;
        bcuSoc = SOC_LookUp(socd_pct_battSoc, &x[0], &y[0]);
        
        //
        if (onceFlg_chrg)
        {
            onceFlg_chrg = false;
            onceFlg_dischrg = true;
            socd_pct_bcuSoc = 2000;
        }
        socd_pct_bcuSoc = SOCfitSystem(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],bcusoc:%d,socd_pct_bcuSoc:%d\n",x[0],x[1],x[2],y[0],y[1],y[2],bcuSoc,socd_pct_bcuSoc);
    }
    else
    {
        //
        delSOC = socd_pct_battSoc0 > socd_pct_bcuSoc0 ? (socd_pct_battSoc0 - socd_pct_bcuSoc0) : (socd_pct_bcuSoc0 - socd_pct_battSoc0);
        coinSoc = (int16_T)((socd_pct_battSoc0 < socd_pct_bcuSoc0 ? socd_pct_battSoc0 : socd_pct_bcuSoc0) - (delSOC > 50 ? 50 : delSOC));
        
        x[0] = socc_pct_battSocLow;
        x[1] = coinSoc > socc_pct_battSocLow ? (uint16_T)coinSoc : socc_pct_battSocLow;
        x[2] = socd_pct_battSoc0;
        y[0] = socc_pct_battSocLow;
        y[1] = coinSoc > socc_pct_battSocLow ? (uint16_T)coinSoc : socc_pct_battSocLow;
        y[2] = socd_pct_bcuSoc0;
        bcuSoc = SOC_LookUp(socd_pct_battSoc, &x[0], &y[0]);
        
        //
        if (onceFlg_dischrg)
        {
            onceFlg_chrg = true;
            onceFlg_dischrg = false;
            socd_pct_bcuSoc = 2000;
        }
        
        socd_pct_bcuSoc = SOCfitSystem(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_bcuSocEo = socd_pct_bcuSoc;
    socd_pct_vcuSoc =  (uint16_T)((uint32_T)((socd_pct_bcuSoc - socc_pct_battSocLow) * 1000) / (socc_pct_battSocUp - socc_pct_battSocLow)); 
    //printf("BattSOC:%d,BCUSOC:%d,VcuSoc:%d\n",socd_pct_battSoc,socd_pct_bcuSoc,socd_pct_vcuSoc);
    //===================================================================
    //------------------EEsave-------------------------------------------
    //================================================================================================
    if ((int16_T)(socd_pct_battSoc - socd_pct_battSoc_save) > 10 || (int16_T)(socd_pct_battSoc - socd_pct_battSoc_save) < -10 || (int16_T)(socd_pct_bcuSoc - socd_pct_bcuSoc_save) > 10 || (int16_T)(socd_pct_bcuSoc - socd_pct_bcuSoc_save) < -10 )
    {
        socd_flg_EEsave = 1;
        socd_pct_battSoc_save = socd_pct_battSoc;
	    socd_pct_bcuSoc_save  = socd_pct_bcuSoc;
    }
    else
    {
        socd_flg_EEsave = 0;
    }
	
    FirstRun_SOC = false;
}
//===================================================================================================================
//----------------------------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;
}
////
uint16_T SOC_LookUp(uint16_T battsoc, uint16_T x[3], uint16_T y[3])
{
    uint16_T bcusoc;
    if (battsoc <= x[0])
    {
        bcusoc = y[0];
    }
    //
    if (battsoc >= x[2])
    {
        bcusoc = y[2];
    }
    //
    if (battsoc > x[0] && battsoc < x[1]) //(x-x0)*(y1-y0)/(x1-x0)+y0
    {
        
        bcusoc = (uint16_T)((real_T)((battsoc - x[0]) * 0.1) * (real_T)((y[1] - y[0]) * 0.1) / (real_T)((x[1] - x[0]) * 0.1) * 10) + y[0];
    }
    //
    if (battsoc >= x[1] && battsoc < x[2]) //(x-x1)*(y2-y1)/(x2-x1)+y1
    {
        bcusoc = (uint16_T)((real_T)((battsoc - x[1]) * 0.1) * (real_T)((y[2] - y[1]) * 0.1) / (real_T)((x[2] - x[1]) * 0.1) * 10) + y[1];
    }
    return bcusoc;
}
//-------------------------------------------------------------------------

uint16_T SOCfitSystem(uint16_T SOC, uint16_T *SOCfit, uint16_T m)
{
    int16_T socdelt;
    socdelt = (int16_T)(SOC - *SOCfit);
    if (socdelt > m)
    {
        *SOCfit = *SOCfit + (socdelt > m ? m : socdelt);
    }
    if (socdelt < -m && socdelt > -1000)
    {
        *SOCfit = *SOCfit + (socdelt < -m ? -m : socdelt);
    }
    if ((socdelt <= m && socdelt >= -m) || socdelt <= -1000)
    {
        *SOCfit = SOC;
    }
    
    return *SOCfit;
}