1.优化CDM与PIM代码

2.新增FFT

inc/EmbeddedCoder_inc/funlib.h

@@ -17,6 +17,11 @@ extern uint16_T look2_u16u16tu16(uint16_T x, uint16_T y, const uint16_T xTable[]
 extern boolean_T DiagThrSystem1(boolean_T Enable, boolean_T precondition, uint16_T Input, uint16_T fltThr, uint16_T recThr, uint8_T fltNumThr, uint8_T recNumThr, uint8_T *fltNum, uint8_T *recNum, boolean_T *fitFlg);
 extern boolean_T DiagThrSystem2(boolean_T Enable, boolean_T precondition, uint16_T Input, uint16_T fltThr, uint16_T recThr, uint8_T fltNumThr, uint8_T recNumThr, uint8_T *fltNum, uint8_T *recNum, boolean_T *fitFlg);
 extern boolean_T JudgeTimeSystem(boolean_T Enable, boolean_T Input, uint16_T *N, uint16_T Thr);
+extern void RaderReverse(creal_T *X, uint16_T N);
+extern void fft(creal_T *X,uint16_T N);
+extern void cmul(creal_T a,creal_T b,creal_T *c);
+extern void cadd(creal_T a,creal_T b,creal_T *c);
+extern void csub(creal_T a,creal_T b,creal_T *c);
 
 
 

src/AppTaskTcp.c

@@ -578,20 +578,18 @@ static void TcpDataInfoAssembleSend()
         UINT16 BufferLen = 0;
         UINT8 rbuf[1000];
         static UINT8 Debugcounter = 0;
-        sprintf((char *)rbuf, "B-%d,%d,%d,%d,,\
+        sprintf((char *)rbuf, "B-%d,%d,%d,%d,%d,,\
                                 %d,%d,%d,%d,%d,%d,,\
                                 %d,%d,%d,%d,%d,%d,,\
                                 %d,%d,%d,%d,,\
-                                %d,%d,%d,%d,%d,%d,,\
-                                %d,%d,%d,,\
-                                %.3f,%.3f,%.3f,%.3f",
-                socd_pct_ahSoc, socd_pct_ekfSoc, socd_pct_battSoc, socd_pct_vcuSoc,
-                sfmd_I_curr, maxCellVol, minCellVol, sfmv_V_cellU[0], sfmv_V_cellU[1], sfmv_V_cellU[2],
+                                %d,%d,%d,%d,%d,%d,%d,,\
+                                %d,%d,%d,,",
+                socd_pct_ahSoc, socd_pct_ekfSoc, socd_pct_battSoc, socd_pct_bcuSoc,pimd_pct_battSoc,
+                sfmd_I_curr, sfmd_V_cellUMax, sfmd_V_cellUMin, sfmv_V_cellU[0], sfmv_V_cellU[1], sfmv_V_cellU[2],
                 cand_idx_cellNr, cand_Q_cellCap, ihd_tm_parkTime, sohd_Q_chrgEo, iscv_Q_remainCpEi[cand_idx_cellNr - 1], iscd_tm_totalEi,
                 pimd_V_ocv, pimd_R_ohm, pimd_R_polar, pimd_F_polar,
-                pimv_V_cellOcv[0], pimv_V_cellOcv[1], pimv_V_cellOcv[2], pimv_R_cellOhm[0], pimv_R_cellOhm[1], pimv_R_cellOhm[2],
-                iscd_flg_flt[0], iscd_flg_flt[1], iscd_flg_flt[2],
-                test_U1, test_U[0], test_U[1], test_U[2]);
+                pimv_V_cellOcv[0], pimv_V_cellOcv[1], pimv_V_cellOcv[2], pimv_R_cellOhm[0], pimv_R_cellOhm[1], pimv_R_cellOhm[2],pimv_R_cellOhm[sfmd_idx_cellUMin],
+                iscd_flg_flt[0], iscd_flg_flt[1], iscd_flg_flt[2]);
         if (Debugcounter % 5 == 0)
         {
             sprintf((char *)rbuf + strlen(rbuf), ",A-%x,%d,%d,%x,%x,,%d,%d,,%x,%d",

src/EmbeddedCoder_src/CDM.c

@@ -44,7 +44,7 @@ void CDM(void)
         }
     }
     //记忆区间-----------------------------------
-    if (pimd_flg_firstRun)
+    if (cdmd_flg_firstRun)
     {
         for (i = 0; i < cdmd_L_rls; i++)
         {
@@ -61,7 +61,7 @@ void CDM(void)
     }
 
     //放电使能----------------------------------------
-    if (ihd_st_workStat == 1 && ihd_st_heatStat == 0)
+    if (cmdn_st_workStat_Delay == 1 && ihd_st_heatStat == 0)
     {
         for (i = 1; i < cmnc_num_cellUNum; i++)
         {
@@ -90,7 +90,7 @@ void CDM(void)
             cdmn_M_P[1][1][i] = P[1][1];
 
             //删去
-            if (cdmn_num_Cnt > cdmd_L_rls)
+            if (cdmn_num_Cnt > cdmd_L_rls + 1)
             {
                 A[0] = 1;
                 A[1] = (real_T)cdmn_I_curr[0] * 0.1;
@@ -114,7 +114,7 @@ void CDM(void)
 
             deltaE[i] = theta[0];
             deltaR[i] = theta[1];
-            test_U[i] = arf;
+			
             cdmv_V_deltOCV[i] = (int16_T)(deltaE[i] * 1000);
             cdmv_R_deltOhm[i] = (int16_T)(deltaR[i] * 1000 * 1000);
         }
@@ -123,7 +123,7 @@ void CDM(void)
     }
     else
     {
-        memset(cdmv_flg_inval, 0, sizeof(cdmv_V_deltOCV));
+        memset(cdmv_flg_inval, 0, sizeof(cdmv_flg_inval));
         memset(cdmv_V_deltOCV, 0, sizeof(cdmv_V_deltOCV));
         memset(cdmv_R_deltOhm, 0, sizeof(cdmv_R_deltOhm));
     }

src/EmbeddedCoder_src/PIM.c

@@ -26,15 +26,17 @@ void PIM(void)
     real_T A[4];
     real_T K[4];
     real_T temp;
-	boolean_T pimn_flg_inval;
-	
+    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)
     {
@@ -43,7 +45,7 @@ void PIM(void)
             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++)
@@ -54,8 +56,8 @@ void PIM(void)
         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;
@@ -65,10 +67,10 @@ void PIM(void)
         
         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 = (real_T)look1_u16tu16(socd_pct_battSoc, cmnm_pct_soc, cmnm_R_polar, 13) * 0.001 * 0.001;;
+        pimn_R_polar = 0.001;;
         pimn_F_polar = 5;
-        pimn_num_cnt = 0;  
-		pimn_st_workStat_Delay = 0;
+        pimn_num_cnt = 0;
+        pimn_st_workStat_Delay = 0;
     }
     //放电使能
     if(pimn_st_workStat_Delay == 1 && ihd_st_heatStat == 0)
@@ -85,7 +87,7 @@ void PIM(void)
         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] +
@@ -98,11 +100,11 @@ void PIM(void)
         //参数更新
         
         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]);
-		test_U1 =temp;
-		if (temp > 0.03 || temp < -0.03)
-		{
-             pimn_flg_inval = 1;
-		}
+        test_U1 =temp;
+        if (temp > 0.03 || temp < -0.03)
+        {
+            pimn_flg_inval = 1;
+        }
         for(i = 0;i < 4;i++)
         {
             theta[i] = theta[i] + K[i] * temp;
@@ -114,27 +116,27 @@ void PIM(void)
             {
                 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)
+        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] +
@@ -144,7 +146,7 @@ void PIM(void)
             {
                 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++)
             {
@@ -159,7 +161,7 @@ void PIM(void)
                 for(j = 0;j < 4;j++)
                 {
                     pimn_M_P[i][j] = P[i][j];
-                }  
+                }
             }
         }
         //参数求解
@@ -167,47 +169,83 @@ void PIM(void)
         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);
-
-
+        //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;
+    else
+    {
+        pimd_V_ocv   = 0;
         pimd_R_ohm   = 0;
         pimd_R_polar = 0;
         pimd_F_polar = 0;
-		pimn_flg_inval = 1;
-	}
-
-	//全部单体参数---------------------
+        pimn_flg_inval = 1;
+    }
+    
+    //全部单体参数---------------------
     for(i = 0;i < cmnc_num_cellUNum;i++)
     {
-        if((!pimn_flg_inval && !cdmv_flg_inval[i]) || 1)
+        if((!pimn_flg_inval && !cdmv_flg_inval[i]))
         {
             pimv_flg_inval[i]   = 0;
             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);
+            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 (pimv_V_cellOcv[i] > 4400 || pimv_V_cellOcv[i] < 1000 || pimv_R_cellOhm[i] < 1000 || pimv_R_cellOhm[i] > 10000)
+			{ 
+			    pimv_flg_inval[i]   = 1;
+			}
+        }
+        else
+        {
+            pimv_flg_inval[i]   = 1;
         }
-		else
-		{
-           pimv_flg_inval[i]   = 1;
-		}
-	
     }
+    // 整包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; 
+    pimd_flg_firstRun = false;
 }
 
 

src/EmbeddedCoder_src/SOC.c

@@ -193,7 +193,7 @@ void SOC(void)
     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;
+    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]);
@@ -202,7 +202,7 @@ void SOC(void)
     
     //输出
     EKFSOCMin = (uint16_T)(soc_Min_Delay * 10);
-    socn_flg_ekfInvalidMin = (deltU > 0.01) || (deltU < -0.01);
+    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)
@@ -258,7 +258,7 @@ void SOC(void)
     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);
+    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;
@@ -447,7 +447,7 @@ void SOC(void)
         overFlg = false;
         fulFlg = false;
         fulCntl = 0;
-		if (!pimv_flg_inval[sfmd_idx_cellUMin])
+		if (!pimv_flg_inval[sfmd_idx_cellUMin] && 0)
 		{
 		    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);
 		}

src/EmbeddedCoder_src/funlib.c

@@ -1,4 +1,5 @@
 #include "funlib.h"
+#define PI 3.14159265358979
 
 
 
@@ -463,3 +464,78 @@ boolean_T JudgeTimeSystem(boolean_T Enable, boolean_T Input, uint16_T *N, uint16
 
 
 
+//
+void fft(creal_T *X,uint16_T N)
+{
+    uint16_T i = 0;
+    uint16_T j = 0;
+    uint16_T k = 0;
+    uint16_T l = 0;
+    creal_T up;
+    creal_T down;
+    creal_T product;
+    creal_T W[N];
+    RaderReverse(X,N);
+    for(i = 0;i < N;i++)
+    {
+        W[i].re = cos(2*PI/N*i);
+		W[i].im = -sin(2*PI/N*i);
+    }
+    for(i = 0;i < log(N)/log(2);i++)
+    {
+        l = 1<<i;
+        for(j=0;j < N;j += 2*l)
+        {
+            for(k = 0;k < l;k++)
+            {
+                cmul(X[j+k+l],W[N*k/2/l],&product);
+                cadd(X[j+k],product,&up); 
+                csub(X[j+k],product,&down);
+                X[j+k] = up;
+                X[j+k+l] =down;
+            }
+        }
+    }
+}
+//
+void RaderReverse(creal_T *X, uint16_T N)
+{
+    uint16_T i;
+    uint16_T j;
+    uint16_T k;
+    creal_T temp;
+    j = N/2;
+    for(i = 1; i < N - 1; i++)
+    {
+        if(i < j)
+        {
+            temp = X[j];
+            X[j] = X[i];
+            X[i] = temp;
+        }
+        k = N/2;
+        while(k <= j)
+        {
+            j = j - k;
+            k = k/2;
+        }
+        j = j + k;
+    }
+}
+//
+void cmul(creal_T a,creal_T b,creal_T *c)
+{
+   c->re = a.re * b.re - a.im * b.im;
+   c->im = a.re * b.im + a.im * b.re;
+}
+void cadd(creal_T a,creal_T b,creal_T *c)
+{
+   c->re = a.re + b.re;
+   c->im = a.im + b.im;
+}
+void csub(creal_T a,creal_T b,creal_T *c)
+{
+	c->re = a.re - b.re;
+	c->im = a.im - b.im;
+}
+