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

#include "PIM.h"

#define pimd_L_rls 51

void PIM_Init(void)
{
    pimd_flg_firstRun = true;
}

void PIM(void)
{
    static uint8_T pimn_st_workStat_Delay;
    uint16_T i;
    uint16_T j;
    static uint16_T pimn_V_volt[pimd_L_rls];
    static int16_T  pimn_I_curr[pimd_L_rls];
    static real_T  pimn_V_ocv;
    static real_T  pimn_R_ohm;
    static real_T  pimn_R_polar;
    static real_T  pimn_F_polar;
    
    static uint16_T pimn_num_cnt;
    static real_T pimn_M_P[4][4];
    real_T theta[4];
    real_T P[4][4];
    real_T A[4];
    real_T K[4];
    real_T temp;
    uint16_T socMax;
    uint16_T socMin;
    boolean_T pimn_flg_inval;
    uint16_T factor;
    //重置--------------------
    if (pimn_st_workStat_Delay != 1 && ihd_st_workStat == 1)
    {
        pimd_flg_firstRun = true;
    }
    
    
    //限定记忆长度------------------------
    if (pimd_flg_firstRun)
    {
        for (i = 0; i < pimd_L_rls; i++)
        {
            pimn_V_volt[i] = sfmv_V_cellU[0];   //和CMD匹配
            pimn_I_curr[i] = sfmd_I_curr;
        }
    }
    else
    {
        for (i = 0; i < pimd_L_rls - 1; i++)
        {
            pimn_V_volt[i]  = pimn_V_volt[i+1];
            pimn_I_curr[i] = pimn_I_curr[i+1];
        }
        pimn_V_volt[i] = sfmv_V_cellU[0];
        pimn_I_curr[i] = sfmd_I_curr;
    }
    
    //初值-------------------
    if (pimd_flg_firstRun)
    {
        pimn_M_P[0][0] = 10; pimn_M_P[0][1] =  0;  pimn_M_P[0][2] =  0;  pimn_M_P[0][3] =  0;
        pimn_M_P[1][0] =  0; pimn_M_P[1][1] = 10;  pimn_M_P[1][2] =  0;  pimn_M_P[1][3] =  0;
        pimn_M_P[2][0] =  0; pimn_M_P[2][1] =  0;  pimn_M_P[2][2] = 10;  pimn_M_P[2][3] =  0;
        pimn_M_P[3][0] =  0; pimn_M_P[3][1] =  0;  pimn_M_P[3][2] =  0;  pimn_M_P[3][3] = 10;
        
        pimn_V_ocv   = (real_T)look1_u16tu16(socd_pct_battSoc, cmnm_pct_soc, cmnm_V_ocv,   13) * 0.001;
        pimn_R_ohm   = (real_T)look1_u16tu16(socd_pct_battSoc, cmnm_pct_soc, cmnm_R_ohm,   13) * 0.001 * 0.001;
        pimn_R_polar = 0.001;;
        pimn_F_polar = 5;
        pimn_num_cnt = 0;
        pimn_st_workStat_Delay = 0;
    }
    //放电使能
    if(pimn_st_workStat_Delay == 1 && ihd_st_heatStat == 0)
    {
        pimn_num_cnt = pimn_num_cnt + 1;
        //新增-------------------
        A[0] = (real_T)pimn_V_volt[pimd_L_rls - 2] * 0.001;
        A[1] = (real_T)pimn_I_curr[pimd_L_rls - 1] * 0.1;
        A[2] =-(real_T)pimn_I_curr[pimd_L_rls - 2] * 0.1;
        A[3] = 1;
        //参数形变
        
        theta[0] = exp(-1/pimn_F_polar);
        theta[1] = pimn_R_ohm;
        theta[2] = pimn_R_ohm * exp(-1/pimn_F_polar) -pimn_R_polar * (1-exp(-1/pimn_F_polar));
        theta[3] = (1 -theta[0]) * pimn_V_ocv;
        
        //增益矩阵
        temp =  (A[0] * pimn_M_P[0][0] +A[1] * pimn_M_P[1][0] + A[2] * pimn_M_P[2][0] + A[3] * pimn_M_P[3][0]) * A[0] +
                (A[0] * pimn_M_P[0][1] +A[1] * pimn_M_P[1][1] + A[2] * pimn_M_P[2][1] + A[3] * pimn_M_P[3][1]) * A[1] +
                (A[0] * pimn_M_P[0][2] +A[1] * pimn_M_P[1][2] + A[2] * pimn_M_P[2][2] + A[3] * pimn_M_P[3][2]) * A[2] +
                (A[0] * pimn_M_P[0][3] +A[1] * pimn_M_P[1][3] + A[2] * pimn_M_P[2][3] + A[3] * pimn_M_P[3][3]) * A[3];
        for(i = 0;i < 4;i++)
        {
            K[i] = (pimn_M_P[i][0] * A[0] + pimn_M_P[i][1] * A[1] + pimn_M_P[i][2] * A[2] + pimn_M_P[i][3] * A[3])/(1 + temp);
        }
        //参数更新
        
        temp = (real_T)pimn_V_volt[pimd_L_rls - 1] * 0.001 - (theta[0] * A[0] + theta[1] * A[1] + theta[2] * A[2] + theta[3] * A[3]);
		pimn_flg_inval = (temp > 0.03 || temp < -0.03);
        for(i = 0;i < 4;i++)
        {
            theta[i] = theta[i] + K[i] * temp;
        }
        //协方差更新
        for(i = 0;i < 4;i++)
        {
            for(j = 0;j < 4;j++)
            {
                P[i][j] = pimn_M_P[i][j] - K[i] * (A[0] * pimn_M_P[0][j] + A[1] * pimn_M_P[1][j] + A[2] * pimn_M_P[2][j] + A[3] * pimn_M_P[3][j]);
            }
            
        }
        
        //传递下循环
        for(i = 0;i < 4;i++)
        {
            for(j = 0;j < 4;j++)
            {
                pimn_M_P[i][j] = P[i][j];
            }
        }
        
        //删去---------------
        if(pimn_num_cnt > pimd_L_rls + 1)
        {
            //输入
            A[0] = (real_T)pimn_V_volt[0] * 0.001;
            A[1] = (real_T)pimn_I_curr[1] * 0.1;
            A[2] =-(real_T)pimn_I_curr[0] * 0.1;
            A[3] = 1;
            
            //增益矩阵
            temp =  (A[0] * pimn_M_P[0][0] +A[1] * pimn_M_P[1][0] + A[2] * pimn_M_P[2][0] + A[3] * pimn_M_P[3][0]) * A[0] +
                    (A[0] * pimn_M_P[0][1] +A[1] * pimn_M_P[1][1] + A[2] * pimn_M_P[2][1] + A[3] * pimn_M_P[3][1]) * A[1] +
                    (A[0] * pimn_M_P[0][2] +A[1] * pimn_M_P[1][2] + A[2] * pimn_M_P[2][2] + A[3] * pimn_M_P[3][2]) * A[2] +
                    (A[0] * pimn_M_P[0][3] +A[1] * pimn_M_P[1][3] + A[2] * pimn_M_P[2][3] + A[3] * pimn_M_P[3][3]) * A[3];
            for(i = 0;i <4;i++)
            {
                K[i] = (pimn_M_P[i][0] * A[0] + pimn_M_P[i][1] * A[1] + pimn_M_P[i][2] * A[2] + pimn_M_P[i][3] * A[3])/(1 + temp);
            }
            
            //协方差更新
            for(i = 0;i < 4;i++)
            {
                for(j = 0;j < 4;j++)
                {
                    P[i][j] = pimn_M_P[i][j] + K[i] * (A[0] * pimn_M_P[0][j] + A[1] * pimn_M_P[1][j] + A[2] * pimn_M_P[2][j] + A[3] * pimn_M_P[3][j]);
                }
            }
            //传递下一循环
            for(i = 0;i < 4;i++)
            {
                for(j = 0;j < 4;j++)
                {
                    pimn_M_P[i][j] = P[i][j];
                }
            }
        }
        //参数求解
        pimn_V_ocv   = theta[3]/(1 - theta[0]);
        pimn_R_ohm   = theta[1];
        pimn_R_polar = (theta[0] * theta[1] -theta[2])/(1 -theta[0]);
        pimn_F_polar = -1/log(theta[0]);
        
        //pimn_V_ocv   = Saturation_r(pimn_V_ocv,3.2, 4.4);
        //pimn_R_ohm   = Saturation_r(pimn_V_ocv,0.001, 1);
        //pimn_R_polar = Saturation_r(pimn_V_ocv,0.0002, 1);
        //pimn_F_polar = Saturation_r(pimn_V_ocv,0, 200);
        
        
        pimd_V_ocv   = (uint16_T)(pimn_V_ocv   * 1000);
        pimd_R_ohm   = (uint16_T)(pimn_R_ohm   * 1000 * 1000);
        pimd_R_polar = (uint16_T)(pimn_R_polar * 1000 * 1000);
        pimd_F_polar = (uint16_T)(pimn_F_polar * 1000);
        if(pimd_V_ocv > 4400 || pimd_V_ocv < 1000 || pimd_R_ohm < 1000 || pimd_R_ohm > 10000)
        {
            pimn_flg_inval = 1;
        }
    }
    else
    {
        pimd_V_ocv   = 0;
        pimd_R_ohm   = 0;
        pimd_R_polar = 0;
        pimd_F_polar = 0;
        pimn_flg_inval = 1;
    }
    
    //全部单体参数---------------------
    for(i = 0;i < cmnc_num_cellUNum;i++)
    {
        pimv_V_cellOcv[i]   = pimd_V_ocv + cdmv_V_deltOCV[i];
        pimv_R_cellOhm[i]   = pimd_R_ohm + cdmv_R_deltOhm[i];
        pimv_pct_cellSoc[i] = look1_u16tu16(pimv_V_cellOcv[i], cmnm_V_ocv, cmnm_pct_soc, 13);
        if((!pimn_flg_inval && !cdmv_flg_inval[i]))
        {
            pimv_flg_inval[i] = (pimv_V_cellOcv[i] > 4400 || pimv_V_cellOcv[i] < 1000 || pimv_R_cellOhm[i] < 1000 || pimv_R_cellOhm[i] > 10000);
        }
        else
        {
            pimv_flg_inval[i] = 1;
        }
    }
    pimd_flg_inval = 0;
    for(i = 0;i < cmnc_num_cellUNum;i++)
    {
        if(pimv_flg_inval[i] == 1)
        {
            pimd_flg_inval = 1;
            break;
        }
    }
    // 整包SOC
    socMin = 1000;
    socMax = 0;
    for(i = 0;i < cmnc_num_cellUNum;i++)
    {
        if(!pimv_flg_inval[i] && pimv_pct_cellSoc[i] > socMax)
        {
            socMax = pimv_pct_cellSoc[i];
        }
        if(!pimv_flg_inval[i] && pimv_pct_cellSoc[i] < socMin)
        {
            socMin = pimv_pct_cellSoc[i];
        }
    }
    
    if (socMax > 800)
    {
        factor = 100;
    }
    else if (socMin < 200)
    {
        factor = 0;
    }
    else
    {
        factor = (uint16_T)(((uint16_T)(((uint32_T)(socMin - 200) << 6) / (800 - (socMax - socMin) - 200)) * 25U) >> 4);
    }
    pimd_pct_battSoc = (uint16_T)(((1 - (real_T)(factor * 0.01)) * (real_T)(socMin * 0.1) + (real_T)(factor * 0.01) * (real_T)(socMax * 0.1)) * 10);
    
    
    //
    pimn_st_workStat_Delay = ihd_st_workStat;
    pimd_flg_firstRun = false;
}