#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; // 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 uint16_T socd_pct_battSoc_Delay; // static uint8_T statCntl; boolean_T statFlg; static uint8_T ihd_st_chrgSta_Delay; static uint16_T socn_pct_utrackSoc_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; 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; socn_pct_utrackSoc_Delay = 0; onceFlg_chrg = true; onceFlg_dischrg = true; } //===================================================================== ////////////////////////初始值获取////////////////////////////////////// //===================================================================== 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)) { 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) { 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----socmin:%f,Up:%f,U1:%d,sfmd_V_cellUMin:%d,deltU:%f,flg:%d,soc1:%f,K[0]:%f,K[1]:%f\n",soc_Min_Delay,Up_Min_Delay,ihv_V_cellU[0],sfmd_V_cellUMin,deltU,socn_flg_ekfInvalidMin,soc1,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("3----socmax:%f,Up:%f,sfmd_V_cellUMax:%d,deltU:%f,flg:%d\n",soc_Max_Delay,Up_Max_Delay,sfmd_V_cellUMax,deltU,socn_flg_ekfInvalidMax); //-----------------------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); //-------------------------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_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_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); // 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); } //socn_flg_ekfInvalid = 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); //printf("4----factor:%d,socd_pct_ekfSoc:%d,EKFSOCMax:%d,EKFSOCMin:%d,\n",factor,socd_pct_ekfSoc,EKFSOCMax,EKFSOCMin); //====================================================================== ////////////////////////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; } 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; if (sfmd_V_cellUMax >= look1_is16lu16n16tu16_binlcase(sfmd_I_curr, (&(socm_I_chrgCor[0])), (&(socm_V_chrgCor[0])), 2U)) { 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 (sfmd_V_cellUMin <= look1_is16lu16n16tu16_binlcase(sfmd_I_curr, (&(socm_I_disChrgCor[0])), (&(socm_V_disChrgCor[0])), 2U)) { 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); } //=================================================================== //------------------EEsave //================================================================== socd_pct_battSoc = socn_pct_utrackSoc; socd_pct_battSocEo = socn_pct_utrackSoc; if ((int16_T)(socd_pct_battSoc - socd_pct_battSoc_Delay) > 10 || (int16_T)(socd_pct_battSoc - socd_pct_battSoc_Delay) < -10) { socd_flg_EEsave = 1; socd_pct_battSoc_Delay = socd_pct_battSoc; } else { socd_flg_EEsave = 0; } //===================================================================== //////////////////////////////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)(socn_pct_utrackSoc - socn_pct_utrackSoc_Delay) > 20 || (int16_T)(socn_pct_utrackSoc - socn_pct_utrackSoc_Delay) < -20) || statFlg); ihd_st_chrgSta_Delay = ihd_st_workStat; socn_pct_utrackSoc_Delay = socn_pct_utrackSoc; // 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 > 1000 ? 1000 : (uint16_T)coinSoc; y[2] = 1000; 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] = 0; y[1] = coinSoc > 0 ? (uint16_T)coinSoc : 0; 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; FirstRun_SOC = false; //printf("\n"); } //------------------------------------------------------------------------- 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; }