【3.1.0.26】

Merge branch 'V3-V30XX-6060' into V3-V31XX-7250

# Conflicts:
#	inc/AppConfig.h
#	inc/AppSignal.h
#	inc/EmbeddedCoder_inc/BCUDisp.h
#	src/AppSignal.c
#	src/AppTaskCan.c
#	src/AppTaskTcp.c
#	src/AppTaskUart.c
#	src/EmbeddedCoder_src/BCUDisp.c
#	src/EmbeddedCoder_src/SOC.c
1.合并V3.0.0.26的修改，保持参数不变

ARMCC/Makefile

@@ -32,6 +32,7 @@ obj-y             += PLAT/project/$(TARGET)/apps/qx_app/src/app.o \
 						PLAT/project/$(TARGET)/apps/qx_app/src/EmbeddedCoder_src/SOH.o \
 						PLAT/project/$(TARGET)/apps/qx_app/src/EmbeddedCoder_src/SOC.o \
 						PLAT/project/$(TARGET)/apps/qx_app/src/EmbeddedCoder_src/BLC.o \
+						PLAT/project/$(TARGET)/apps/qx_app/src/EmbeddedCoder_src/CM.o \
 						PLAT/project/$(TARGET)/apps/qx_app/src/EmbeddedCoder_src/look1_is16lu16n16tu16_binlcase.o \
 						PLAT/project/$(TARGET)/apps/qx_app/src/EmbeddedCoder_src/look1_iu16lu16n16tu16_binlcase.o \
 						PLAT/project/$(TARGET)/apps/qx_app/src/EmbeddedCoder_src/BCUCal.o \

inc/AppConfig.h

@@ -2,7 +2,7 @@
  * @Author       : ChenJie
  * @Date         : 2021-10-14 09:27:15
  * @LastEditors  : ChenJie
- * @LastEditTime : 2021-10-20 14:57:34
+ * @LastEditTime : 2021-10-27 11:02:24
  * @Description  : App Config H file 配置文件，可以针对不同参数进行更改
  * @FilePath     : \PLAT\project\ec616_0h00\apps\qx_app\inc\AppConfig.h
  */
@@ -14,7 +14,7 @@
 #define DATA_MODULE_TYPE (1)           //1表示NB模块，2表示4G cat1
 #define EOLSTATE (0)                   //1表示下线检测跳过，使用默认值，0表示使用下线检测
 #define DEFAULT_SN "RLTEST00000000001" //默认上传的SN编码
-#define APPSWVERSION 0x03010013        //数据模块软件版本号
+#define APPSWVERSION 0x0301001A        //数据模块软件版本号
 #define RELAYCONFIG 1                  //继电器配置
 #define TCP_ADD "iotp.fast-fun.cn"     //数据上传的地址
 #define TCP_PORT 8712                  //数据上传的端口

inc/AppSignal.h

@@ -2,7 +2,7 @@
  * @Author       : ChenJie
  * @Date         : 2021-10-14 09:27:15
  * @LastEditors  : ChenJie
- * @LastEditTime : 2021-10-21 11:52:28
+ * @LastEditTime : 2021-10-26 16:07:37
  * @Description  : Global variable definition H file
  * @FilePath     : \PLAT\project\ec616_0h00\apps\qx_app\inc\AppSignal.h
  */
@@ -85,7 +85,7 @@ extern UINT16 nbHwVersion;
 extern UINT16 avrgCellVol;
 extern UINT16 ErrorNum[ErrorNumLength];
 extern UINT16 updateDifferDataPackageCounter;
-
+extern INT16 xyzData[3];
 //declear the UINT32 vars
 extern UINT32 battBalanceoInfo;
 extern UINT32 battProtectState;

inc/AppTaskTcp.h

@@ -30,18 +30,18 @@
 #include "AppConfig.h"
 #include "AppFunc.h"
 
-#define PROC_TCP_TASK_STACK_SIZE           (2048 + 1024)
-#define APP_EVENT_QUEUE_SIZE               (10)
-#define QMSG_ID_BASE               (0x16) 
-#define QMSG_ID_NW_IP_READY        (QMSG_ID_BASE)
-#define QMSG_ID_NW_IP_SUSPEND      (QMSG_ID_BASE + 1)
-#define QMSG_ID_NW_IP_NOREACHABLE  (QMSG_ID_BASE + 2)
-#define QMSG_ID_SOCK_SENDPKG       (QMSG_ID_BASE + 4)
-#define QMSG_ID_SOCK_RECVPKG       (QMSG_ID_BASE + 5)
-#define QMSG_ID_SOCK_LISTEN        (QMSG_ID_BASE + 6)
-#define QMSG_ID_SOCK_EXIT          (QMSG_ID_BASE + 7)
+#define PROC_TCP_TASK_STACK_SIZE (2048 + 1024)
+#define APP_EVENT_QUEUE_SIZE (10)
+#define QMSG_ID_BASE (0x16)
+#define QMSG_ID_NW_IP_READY (QMSG_ID_BASE)
+#define QMSG_ID_NW_IP_SUSPEND (QMSG_ID_BASE + 1)
+#define QMSG_ID_NW_IP_NOREACHABLE (QMSG_ID_BASE + 2)
+#define QMSG_ID_SOCK_SENDPKG (QMSG_ID_BASE + 4)
+#define QMSG_ID_SOCK_RECVPKG (QMSG_ID_BASE + 5)
+#define QMSG_ID_SOCK_LISTEN (QMSG_ID_BASE + 6)
+#define QMSG_ID_SOCK_EXIT (QMSG_ID_BASE + 7)
 // typedef struct BattInfoType
-// {	
+// {
 // 	UINT8	sendTimeUTC[6];
 // 	UINT8	msgMark;
 // 	UINT8	msgCollectionTimeUTC[6];
@@ -67,99 +67,101 @@
 // 	UINT8 	battotherTemp[BMS_OTHER_TEMP+NB_OTHER_TEMP_NUM];
 // }BattInfoType;
 // typedef struct BattMsgtoTcpType
-// {	
+// {
 // 	UINT8 	startSymbol[2];
 // 	UINT8	cmdSymbol;
 // 	UINT8	ansSymbol;
 // 	UINT8	SN[BATT_SN_LEN];
 // 	UINT8	encryptMethod;
-// 	UINT8	dataLength[2];	
-// 	BattInfoType battInfo;	
+// 	UINT8	dataLength[2];
+// 	BattInfoType battInfo;
 // 	UINT8	CRC;
 // }BattMsgtoTcpType;
 /*---------------------------------------------------------------------------*/
 typedef struct HeartMsgtoTcpType
-{	
-	UINT8 	startSymbol[2];
-	UINT8	cmdSymbol;
-	UINT8	ansSymbol;
-	UINT8	SN[BATT_SN_LEN];
-	UINT8	encryptMethod;
-	UINT8	dataLength[2];		
-	UINT8	CRC;
-}HeartMsgtoTcpType;
+{
+	UINT8 startSymbol[2];
+	UINT8 cmdSymbol;
+	UINT8 ansSymbol;
+	UINT8 SN[BATT_SN_LEN];
+	UINT8 encryptMethod;
+	UINT8 dataLength[2];
+	UINT8 CRC;
+} HeartMsgtoTcpType;
 typedef struct _GPSInfoType
 {
-	UINT8	sendTimeUTC[6];
-	UINT8	msgMark;
-	UINT8	msgCollectionTimeUTC[6];
+	UINT8 sendTimeUTC[6];
+	UINT8 msgMark;
+	UINT8 msgCollectionTimeUTC[6];
 	GPSInfo GpsInfoData;
-	UINT8	Tac[2];
-	UINT8	CellID[4];
-	
-}GPSInfoType;
+	UINT8 Tac[2];
+	UINT8 CellID[4];
+	UINT8 xData[2];
+	UINT8 yData[2];
+	UINT8 zData[2];
+} GPSInfoType;
 typedef struct GPSMsgtoTcpType
 {
-	UINT8 	startSymbol[2];
-	UINT8	cmdSymbol;
-	UINT8	ansSymbol;
-	UINT8	SN[BATT_SN_LEN];
-	UINT8	encryptMethod;
-	UINT8	dataLength[2];	
-	GPSInfoType gpsInfo;	
-	UINT8	CRC;
-}GPSMsgtoTcpType;
+	UINT8 startSymbol[2];
+	UINT8 cmdSymbol;
+	UINT8 ansSymbol;
+	UINT8 SN[BATT_SN_LEN];
+	UINT8 encryptMethod;
+	UINT8 dataLength[2];
+	GPSInfoType gpsInfo;
+	UINT8 CRC;
+} GPSMsgtoTcpType;
 /*---------------------------------------------------------------------------*/
 typedef struct _VerInfoType
 {
-	UINT8	sendTimeUTC[6];
-	UINT8	msgMark;
-	UINT8	msgCollectionTimeUTC[6];
-	UINT8	ICCID[20];
-	UINT8	IMEI[15];
-	UINT8	BMSHwVersion[2];
-	UINT8	BMSSwVersion[4];
-	UINT8	NBHwVersion[2];
-	UINT8	BLVersion[4];
-	UINT8	DRVVersion[4];
-	UINT8	APPVersion[4];
-	UINT8	BmsType;
-	UINT8	BmsInfo;
-	UINT8   DataModuleType;
-}VerInfoType;
+	UINT8 sendTimeUTC[6];
+	UINT8 msgMark;
+	UINT8 msgCollectionTimeUTC[6];
+	UINT8 ICCID[20];
+	UINT8 IMEI[15];
+	UINT8 BMSHwVersion[2];
+	UINT8 BMSSwVersion[4];
+	UINT8 NBHwVersion[2];
+	UINT8 BLVersion[4];
+	UINT8 DRVVersion[4];
+	UINT8 APPVersion[4];
+	UINT8 BmsType;
+	UINT8 BmsInfo;
+	UINT8 DataModuleType;
+} VerInfoType;
 typedef struct VersionMsgtoTcpType
 {
-	UINT8 	startSymbol[2];
-	UINT8	cmdSymbol;
-	UINT8	ansSymbol;
-	UINT8	SN[BATT_SN_LEN];
-	UINT8	encryptMethod;
-	UINT8	dataLength[2];	
-	VerInfoType VerInfo;	
-	UINT8	CRC;
-}VersionMsgtoTcpType;
+	UINT8 startSymbol[2];
+	UINT8 cmdSymbol;
+	UINT8 ansSymbol;
+	UINT8 SN[BATT_SN_LEN];
+	UINT8 encryptMethod;
+	UINT8 dataLength[2];
+	VerInfoType VerInfo;
+	UINT8 CRC;
+} VersionMsgtoTcpType;
 typedef struct _DebugInfoType
 {
-	UINT8	sendTimeUTC[6];
-	UINT8	msgMark;
-	UINT8  	DebugLen[2];
-}DebugInfoType;
+	UINT8 sendTimeUTC[6];
+	UINT8 msgMark;
+	UINT8 DebugLen[2];
+} DebugInfoType;
 typedef struct DebugMsgtoTcp_Type
 {
-	UINT8 	startSymbol[2];
-	UINT8	cmdSymbol;
-	UINT8	ansSymbol;
-	UINT8	SN[BATT_SN_LEN];
-	UINT8	encryptMethod;
-	UINT8	dataLength[2];		
+	UINT8 startSymbol[2];
+	UINT8 cmdSymbol;
+	UINT8 ansSymbol;
+	UINT8 SN[BATT_SN_LEN];
+	UINT8 encryptMethod;
+	UINT8 dataLength[2];
 	DebugInfoType DebugInfo;
-	UINT8	CRC;
-}DebugMsgtoTcpType;
+	UINT8 CRC;
+} DebugMsgtoTcpType;
 typedef enum
 {
-    APP_SOCKET_CONNECTION_CLOSED = 0,
-    APP_SOCKET_CONNECTION_CONNECTING,
-    APP_SOCKET_CONNECTION_CONNECTED,    
-}AppSocketConnectionStatus;
+	APP_SOCKET_CONNECTION_CLOSED = 0,
+	APP_SOCKET_CONNECTION_CONNECTING,
+	APP_SOCKET_CONNECTION_CONNECTED,
+} AppSocketConnectionStatus;
 void AppTaskTcpInit(void *arg);
 void AppTaskTcpDeInit(void *arg);

inc/BCU.h

@@ -2,6 +2,8 @@
 #include "AppSignal.h"
 #include "SFM.h"
 #include "SPM.h"
+#include "CM.h"
+#include "TMS.h"
 #include "BCUCal.h"
 #include "BCUDisp.h"
 #define PROC_BCU_TASK_STACK_SIZE           (2048)

inc/EmbeddedCoder_inc/BCUCal.h

@@ -93,6 +93,10 @@ extern const uint16_T socm_V_disChrgCor[3]; /* 放电CCV对应的电压； */
 extern const uint16_T socc_pct_chrgCor;
 extern const uint16_T socc_pct_disChrgCor;
 
+extern const int16_T cmm_T_ChrgCurr[3];
+extern const int16_T cmm_V_ChrgCurr[6];
+extern const int16_T cmm_I_ChrgCurr[3][6];
+
 extern const uint16_T tmsc_T_openThr;                         /* 热管理开启温度； */
 extern const uint16_T tmsc_T_closeMinThr;                     /* 热管理关闭温度（最低模组）； */
 extern const uint16_T tmsc_T_closeMaxThr;                     /* 热管理开启温度（最高模组）； */

inc/EmbeddedCoder_inc/BCUDisp.h

@@ -72,6 +72,8 @@ extern boolean_T sfmd_flg_heatCirClosFltEi;              /*加热回路常闭故
 extern boolean_T sfmd_flg_heatCirClosFltEo;              /*加热回路常闭故障写入量 */  
 extern boolean_T sfmd_flg_heatCirOpenFltEi;              /*加热回路常开故障读取量 */  
 extern boolean_T sfmd_flg_heatCirOpenFltEo;              /*加热回路常开故障写入量 */  
+extern boolean_T sfmd_flg_heatRunFltEi;
+extern boolean_T sfmd_flg_heatRunFltEo;
 
 extern uint16_T socd_pct_ahSoc;                          /*安时SOC； */
 extern uint16_T socd_pct_estSoc;                         /*估算SOC；*/
@@ -103,7 +105,7 @@ extern uint16_T sohv_V_chrgStartStatEo[28];              /*充电开始时刻的
 
 extern uint8_T cmd_st_chrgMod;                           /* 充电模式  */
 extern uint8_T cmd_st_chrgSt;                            /* 充电状态  */
-extern uint16_T cmd_I_chrgCurrReq;                       /* 充电需求电流  */
+extern int16_T cmd_I_chrgCurrReq;                       /* 充电需求电流  */
 extern uint8_T cmd_idx_chrgEndReason;                    /* 充电结束原因  */
 
 extern uint16_T cand_Nr_cellNr;                          /*循环发送的单体编号； */
@@ -116,4 +118,6 @@ extern uint16_T cand_Q_totalCp;                          /*循环发送的单体
 extern uint16_T test_efkSocMin;
 extern uint16_T test_efkSocMax;
 extern real_T test_UpMin;
-extern real_T test_UpMax;
+extern real_T test_UpMax;
+extern uint16_T test_ekfSoc;
+extern uint16_T test_ekfSoc0;

inc/EmbeddedCoder_inc/CM.h

@@ -0,0 +1,11 @@
+#include "rtwtypes.h"
+#include "BCUCal.h"
+#include "BCUDisp.h"
+#include "numeric.h"
+
+extern boolean_T FirstRun_CM;
+extern void CM_Init(void);
+extern void CM(void);
+
+//extern boolean_T JudgeTimeSystem(boolean_T Enable,boolean_T Input, uint8_T *N,uint8_T Thr);
+extern int16_T lookupChrgCurr(int16_T X,int16_T Y,uint16_T xN,uint16_T yN);

inc/EmbeddedCoder_inc/SFM.h

@@ -6,7 +6,8 @@ extern boolean_T FirstRun_SFM;
 extern void SFM_Init(void);
 extern void SFM(void);
 
-typedef struct {
+typedef struct
+{
     uint8_T fltNum81;
     uint8_T recNum81;
     uint8_T fltNum82;
@@ -31,7 +32,7 @@ typedef struct {
     uint8_T recNum97;
     uint8_T fltNum98;
     uint8_T recNum98;
-	uint8_T fltNum99;
+    uint8_T fltNum99;
     uint8_T recNum99;
     uint8_T fltNum100;
     uint8_T recNum100;
@@ -75,24 +76,25 @@ typedef struct {
     uint8_T recNum131;
     uint8_T fltNum132;
     uint8_T recNum132;
-}DiagThrstruct;
+} DiagThrstruct;
 
-typedef struct {
-	uint16_T N22;
-	uint16_T N24;
-	uint16_T N32;
-	uint16_T N33;
-	uint16_T N34;
-	uint16_T N54;
-	uint16_T N56;
-	uint16_T N57;
-	uint16_T N58;
-	uint16_T N59;
-	uint16_T N62;
-	uint16_T N64;
-	uint16_T N65;
-	uint16_T N66;
-	uint16_T N67;
+typedef struct
+{
+    uint16_T N22;
+    uint16_T N24;
+    uint16_T N32;
+    uint16_T N33;
+    uint16_T N34;
+    uint16_T N54;
+    uint16_T N56;
+    uint16_T N57;
+    uint16_T N58;
+    uint16_T N59;
+    uint16_T N62;
+    uint16_T N64;
+    uint16_T N65;
+    uint16_T N66;
+    uint16_T N67;
     uint16_T N151;
     uint16_T N152;
     uint16_T N153;
@@ -105,9 +107,9 @@ typedef struct {
     uint16_T N160;
     uint16_T N161;
     uint16_T N162;
-	uint16_T N179;
-}DiagTimestruct;
+    uint16_T N179;
+} DiagTimestruct;
 
-extern boolean_T DiagThrSystem1(boolean_T Enable, boolean_T precondition, uint16_T Input,uint16_T fltThr,uint16_T recThr,uint16_T fltNumThr,uint16_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,uint16_T fltNumThr,uint16_T recNumThr,uint8_T *fltNum,uint8_T *recNum,boolean_T *fitFlg);
+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);

inc/EmbeddedCoder_inc/SOH.h

@@ -3,5 +3,5 @@
 extern boolean_T FirstRun_SOH;
 extern void SOH_Init(void);
 extern void SOH(void);
-extern void  ArrMax(uint16_T *Data,uint16_T *Max, uint16_T m);
-extern void  ArrMin(uint16_T *Data,uint16_T *Min, uint16_T m);
+extern uint16_T  ArrMax(uint16_T *Data, uint16_T m);
+extern uint16_T  ArrMin(uint16_T *Data, uint16_T m);

inc/EmbeddedCoder_inc/SPM.h

@@ -1,6 +1,12 @@
 #include "rtwtypes.h"
 #include "BCUCal.h"
 #include "BCUDisp.h"
+#include "SOH.h"
+#include "SOC.h"
+#include "BLC.h"
+
+
+
 extern boolean_T FirstRun_SPM;
 
 extern void SPM_Init(void);

src/AppFunc.c

@@ -3,7 +3,7 @@
  * @Date         : 2021-10-14 09:27:15
  * @Version      : V3.0
  * @LastEditors  : ChenJie
- * @LastEditTime : 2021-10-19 11:54:15
+ * @LastEditTime : 2021-10-26 14:54:49
  * @Description  : 应用层函数定义区，UTF-8
  * @FilePath     : \PLAT\project\ec616_0h00\apps\qx_app\src\AppFunc.c
  */
@@ -540,13 +540,12 @@ void LEDDisplay(void)
 		}
 		else if (UartRecvFlag == 1) //电池接收到UART信息，且正确运行
 		{
-			if (battWorkState == 0x02) //充电模式下，如果只有“SOC低故障”，那么就不显示故障灯   zhengchao20210713 add
-			{
-				if ((((battWarningState >> 10) & 0x01) == 0x01) && ((battWarningState & 0xFFFFFBFF) == 0x00))
-					return;
-			}
-			if (getbit(sfmd_st_fltAct, 1) == 1 || AppNVMData.EOLState == 0) //电池存在故障,或者未完成下线检测
+			if (AppNVMData.EOLState == 0) //未完成下线检测，灭其余灯，闪红灯
 			{
+				NetSocDisplay(LED_SOC_0, LED_TURN_OFF);
+				NetSocDisplay(LED_SOC_1, LED_TURN_OFF);
+				NetSocDisplay(LED_SOC_2, LED_TURN_OFF);
+				NetSocDisplay(LED_SOC_3, LED_TURN_OFF);
 				if (ErrorLightTimer < (UINT8)(LEDFlashPeriod * DutyRatio))
 				{
 
@@ -559,17 +558,19 @@ void LEDDisplay(void)
 				}
 				else
 				{
-
 					FaultDisplay(LED_TURN_OFF);
 					ErrorLightTimer = 0;
 				}
+				return;
+			}
+			else if (getbit(sfmd_st_fltAct, 1) == 1) //通过下线检测，有故障亮红灯
+			{
+				FaultDisplay(LED_TURN_ON);
 			}
 			else
 			{
 				FaultDisplay(LED_TURN_OFF);
-				ErrorLightTimer = 0;
 			}
-
 			if (battWorkState == 0 || battWorkState == 1) //静置或放电状态
 			{
 				if (LedBattSoc <= 10)
@@ -1362,6 +1363,7 @@ static void setDefaultBcuDataInfo(void)
 	BcuDataInfo.sfmd_flg_mainCirClosFltE = 0;
 	BcuDataInfo.sfmd_flg_heatCirClosFltE = 0;
 	BcuDataInfo.sfmd_flg_heatCirOpenFltE = 0;
+	BcuDataInfo.sfmd_flg_heatRunFltE = 0;
 	return;
 }
 /**
@@ -2032,6 +2034,62 @@ void CANEncodeFunction(UINT32 ID, UINT8 *msgData)
 		*(UINT8 *)(msgData + 6) = reservedSignal7 & 0xFF;
 		*(UINT8 *)(msgData + 7) = reservedSignal8 & 0xFF;
 		break;
+	case 0x7CE:
+		reservedSignal1 = socd_pct_ahSoc & 0xFF;
+		reservedSignal2 = (socd_pct_ahSoc >> 8) & 0xFF;
+		reservedSignal3 = socd_pct_ekfSoc & 0xFF;
+		reservedSignal4 = (socd_pct_ekfSoc >> 8) & 0xFF;
+		reservedSignal5 = socd_pct_estSoc & 0xFF;
+		reservedSignal6 = (socd_pct_estSoc >> 8) & 0xFF;
+		reservedSignal7 = socd_pct_battSoc & 0xFF;
+		reservedSignal8 = (socd_pct_battSoc >> 8) & 0xFF;
+		*(UINT8 *)(msgData + 0) = reservedSignal1 & 0xFF;
+		*(UINT8 *)(msgData + 1) = reservedSignal2 & 0xFF;
+		*(UINT8 *)(msgData + 2) = reservedSignal3 & 0xFF;
+		*(UINT8 *)(msgData + 3) = reservedSignal4 & 0xFF;
+		*(UINT8 *)(msgData + 4) = reservedSignal5 & 0xFF;
+		*(UINT8 *)(msgData + 5) = reservedSignal6 & 0xFF;
+		*(UINT8 *)(msgData + 6) = reservedSignal7 & 0xFF;
+		*(UINT8 *)(msgData + 7) = reservedSignal8 & 0xFF;
+		break;
+	case 0x7CF:
+		reservedSignal1 = socd_pct_bcuSoc & 0xFF;
+		reservedSignal2 = (socd_pct_bcuSoc >> 8) & 0xFF;
+		reservedSignal3 = test_efkSocMin & 0xFF;
+		reservedSignal4 = (test_efkSocMin >> 8) & 0xFF;
+		reservedSignal5 = test_efkSocMax & 0xFF;
+		reservedSignal6 = (test_efkSocMax >> 8) & 0xFF;
+		reservedSignal7 = 0 & 0xFF;
+		reservedSignal8 = (0 >> 8) & 0xFF;
+		*(UINT8 *)(msgData + 0) = reservedSignal1 & 0xFF;
+		*(UINT8 *)(msgData + 1) = reservedSignal2 & 0xFF;
+		*(UINT8 *)(msgData + 2) = reservedSignal3 & 0xFF;
+		*(UINT8 *)(msgData + 3) = reservedSignal4 & 0xFF;
+		*(UINT8 *)(msgData + 4) = reservedSignal5 & 0xFF;
+		*(UINT8 *)(msgData + 5) = reservedSignal6 & 0xFF;
+		*(UINT8 *)(msgData + 6) = reservedSignal7 & 0xFF;
+		*(UINT8 *)(msgData + 7) = reservedSignal8 & 0xFF;
+		break;
+	case 0x7D0:
+		UINT32 temp1 = (UINT32)(test_UpMin * 100000);
+		UINT32 temp2 = (UINT32)(test_UpMax * 100000);
+		reservedSignal1 = temp1 & 0xFF;
+		reservedSignal2 = (temp1 >> 8) & 0xFF;
+		reservedSignal3 = (temp1 >> 16) & 0xFF;
+		reservedSignal4 = (temp1 >> 24) & 0xFF;
+		reservedSignal5 = temp2 & 0xFF;
+		reservedSignal6 = (temp2 >> 8) & 0xFF;
+		reservedSignal7 = (temp2 >> 16) & 0xFF;
+		reservedSignal8 = (temp2 >> 24) & 0xFF;
+		*(UINT8 *)(msgData + 0) = reservedSignal1 & 0xFF;
+		*(UINT8 *)(msgData + 1) = reservedSignal2 & 0xFF;
+		*(UINT8 *)(msgData + 2) = reservedSignal3 & 0xFF;
+		*(UINT8 *)(msgData + 3) = reservedSignal4 & 0xFF;
+		*(UINT8 *)(msgData + 4) = reservedSignal5 & 0xFF;
+		*(UINT8 *)(msgData + 5) = reservedSignal6 & 0xFF;
+		*(UINT8 *)(msgData + 6) = reservedSignal7 & 0xFF;
+		*(UINT8 *)(msgData + 7) = reservedSignal8 & 0xFF;
+		break;
 	case 0x7B0:
 		*(UINT8 *)(msgData + 0) = (AppNVMData.battSN[0]) & 0xFF;
 		*(UINT8 *)(msgData + 1) = (AppNVMData.battSN[1]) & 0xFF;
@@ -2084,7 +2142,6 @@ void UDS_Service(CAN_Msg_Type *CanRxMsg)
 	UINT32 flashStartAddr = 0x00;
 
 	static UINT32 updateDifferDataSliceCounter = 0;
-	//static UINT8 UDSSwitch = 0;
 	static UINT8 UDSBattSN[BATT_SN_LEN];
 
 	for (i = 0; i < 2; i++)
@@ -2152,6 +2209,42 @@ void UDS_Service(CAN_Msg_Type *CanRxMsg)
 					UDSNegtiveAnswer(0x05, i, 0xEE);
 				}
 				break;
+			case 0x14: //clear the DTC information 14 FF FF FF 00 00 00 00
+				if (UDSDialogMode == 2)
+				{
+					if (UDSSubService[i] == 0xFF)
+					{
+						if (UDSSubServiceActionCode[i] == 0xFF)
+						{
+							if (CanRxMsg[i].Data[3] == 0xFF)
+							{
+								ihd_flg_DTCClear = 0x01;
+								posCode = 0xEEEE;
+								UDSPositiveAnswer(06, i, posCode);
+							}
+							else
+							{
+								negCode = (UDSSubServiceActionCode[i] << 8) | CanRxMsg[i].Data[3];
+								UDSNegtiveAnswer(6, i, negCode);
+							}
+						}
+						else
+						{
+							negCode = (UDSSubServiceActionCode[i] << 8) | CanRxMsg[i].Data[3];
+							UDSNegtiveAnswer(6, i, negCode);
+						}
+					}
+					else
+					{
+						negCode = (UDSSubServiceActionCode[i] << 8) | CanRxMsg[i].Data[3];
+						UDSNegtiveAnswer(6, i, negCode);
+					}
+				}
+				else
+				{
+					UDSNegtiveAnswer(0x05, i, 0xFF);
+				}
+				break;
 			case 0x22:
 				if (UDSDialogMode == 2)
 				{
@@ -2212,6 +2305,46 @@ void UDS_Service(CAN_Msg_Type *CanRxMsg)
 						posCode = GpsFlag;
 						UDSPositiveAnswer(0x05, i, posCode); //gps satellite num(uint8), should be modified
 					}
+					else if (UDSSubService[i] == 0x0E) //read ram data  22 0E xx xx xx xx XX XX
+					{
+						// UINT16 size = CanRxMsg[i].Data[7] | (CanRxMsg[i].Data[6] << 8);
+						// UINT32 retData = 0xFFFFFFFF;
+						// if (size == 1)
+						// {
+						// 	UINT8 *addr = (UINT8 *)(CanRxMsg[i].Data[5] | (CanRxMsg[i].Data[4] << 8) | (CanRxMsg[i].Data[3] << 16) | (CanRxMsg[i].Data[2] << 24));
+						// 	retData = *(addr);
+						// }
+						// else if (size == 2)
+						// {
+						// 	UINT16 *addr = (UINT16 *)(CanRxMsg[i].Data[5] | (CanRxMsg[i].Data[4] << 8) | (CanRxMsg[i].Data[3] << 16) | (CanRxMsg[i].Data[2] << 24));
+						// 	retData = *(addr);
+						// }
+						// else if (size == 4)
+						// {
+						// 	UINT32 *addr = (UINT32 *)(CanRxMsg[i].Data[5] | (CanRxMsg[i].Data[4] << 8) | (CanRxMsg[i].Data[3] << 16) | (CanRxMsg[i].Data[2] << 24));
+						// 	retData = *(addr);
+						// }
+						// else if (size == 8)
+						// {
+						// 	//double
+						// 	UINT32 *addr = (UINT32 *)(CanRxMsg[i].Data[5] | (CanRxMsg[i].Data[4] << 8) | (CanRxMsg[i].Data[3] << 16) | (CanRxMsg[i].Data[2] << 24));
+						// 	retData = *(addr);
+						// 	UDSPositiveAnswer(8, i, retData); //
+						// 	UINT32 *addr1 = *(addr+4);
+						// 	retData = *(addr1);
+						// 	UDSPositiveAnswer(8, i, retData);
+						// }
+
+						// UDSPositiveAnswer(8, i, retData); // 08 78 22 0E xx xx xx xx
+						UINT16 size = CanRxMsg[i].Data[7] | (CanRxMsg[i].Data[6] << 8);
+						UINT8 *addr = (UINT8 *)(CanRxMsg[i].Data[5] | (CanRxMsg[i].Data[4] << 8) | (CanRxMsg[i].Data[3] << 16) | (CanRxMsg[i].Data[2] << 24));
+						for (UINT8 j = 0; j < size; j++)
+						{
+							UINT32 ret = 0xFF;
+							ret = (j << 16) | (*(addr + j));
+							UDSPositiveAnswer(8, i, ret);
+						}
+					}
 					else
 					{
 						UDSNegtiveAnswer(0x05, i, 0xEE);
@@ -2285,6 +2418,14 @@ void UDS_Service(CAN_Msg_Type *CanRxMsg)
 						updateDifferDataByteLen = (CanRxMsg[i].Data[2] << 16) | (CanRxMsg[i].Data[3] << 8) | CanRxMsg[i].Data[4];
 						UDSPositiveAnswer(0x04, i, 00);
 					}
+					else if (UDSSubService[i] == 0x0E) //write ram data  2E 0E xx xx xx xx XX XX
+					{
+						UINT8 *addr = (UINT8 *)(CanRxMsg[i].Data[5] | (CanRxMsg[i].Data[4] << 8) | (CanRxMsg[i].Data[3] << 16) | (CanRxMsg[i].Data[2] << 24));
+						UINT8 data = CanRxMsg[i].Data[7];
+						*(addr) = data;
+						UINT8 retData = *(addr);
+						UDSPositiveAnswer(8, i, retData); // 08 78 2E 0E xx xx xx xx
+					}
 					else
 					{
 						UDSNegtiveAnswer(0x05, i, 0xEE);
@@ -2296,7 +2437,7 @@ void UDS_Service(CAN_Msg_Type *CanRxMsg)
 					UDSNegtiveAnswer(0x05, i, 0xFF);
 				}
 				break;
-			case 0x31: //clear the flash service
+			case 0x31: //clear the flash service and control heat or relay
 				if (UDSDialogMode == 3)
 				{
 					if (UDSSubService[i] == 0x0F) //clear the fota flash
@@ -2314,6 +2455,30 @@ void UDS_Service(CAN_Msg_Type *CanRxMsg)
 							UDSNegtiveAnswer(0x06, i, UDSSubServiceActionCode[i] << 8 | 0xFF); //重点测试
 						}
 					}
+					else if (UDSSubService[i] == 0x10) //control the heater 31 10 00/01/02
+					{
+						if (UDSSubServiceActionCode[i] <= 0x02) //case 00/01/02
+						{
+							HeatForceControl = UDSSubServiceActionCode[i];
+							UDSPositiveAnswer(0x05, i, HeatForceControl);
+						}
+						else
+						{
+							UDSNegtiveAnswer(0x05, i, UDSSubServiceActionCode[i]);
+						}
+					}
+					else if (UDSSubService[i] == 0x11) //control the main relay  31 11 00/01/02
+					{
+						if (UDSSubServiceActionCode[i] <= 0x02) //case 00/01/02
+						{
+							RelayForceControl = UDSSubServiceActionCode[i];
+							UDSPositiveAnswer(0x05, i, RelayForceControl);
+						}
+						else
+						{
+							UDSNegtiveAnswer(0x05, i, UDSSubServiceActionCode[i]);
+						}
+					}
 					else
 					{
 						UDSNegtiveAnswer(0x05, i, 0xEE); //the subservice is not surpported

src/AppSignal.c

@@ -3,7 +3,7 @@
  * @Date         : 2021-10-14 09:27:15
  * @Version      : V3.0
  * @LastEditors  : ChenJie
- * @LastEditTime : 2021-10-21 11:51:54
+ * @LastEditTime : 2021-10-26 16:07:27
  * @Description  : Global variable definition c file,此文件存放App使用到的全局变量
  * @FilePath     : \PLAT\project\ec616_0h00\apps\qx_app\src\AppSignal.c
  */
@@ -81,6 +81,7 @@ UINT16 maxCellVol = 0;                     //最大单体电压
 UINT16 minCellVol = 0;                     //最小单体电压
 UINT16 nbHwVersion = 0;                    //数据模块硬件状态
 UINT16 updateDifferDataPackageCounter = 0; //差分包升级计数器
+INT16 xyzData[3] = {0};                    //xyz三轴数据
 
 /**declear the uint32 vars**/
 UINT32 battBalanceoInfo = 0; //电池均衡状态
@@ -104,4 +105,4 @@ AppAlgorithmData BcuDataInfo;
 
 AppSocketConnectionContext socContext = {-1, 0}; //网络连接状态
 
-process_app gProcess_app; //全局工作状态
+process_app gProcess_app; //全局工作状态

src/AppTaskCan.c

@@ -111,35 +111,6 @@ static void CanTask()
 							msgFlag = 0;
 						}
 					}
-					else if (CanRxMsg[i].Id == 0x7A2)
-					{
-						if (CanRxMsg[i].Data[0] == 0x01)
-						{
-							AppDataInfo.appDataModify = TRUE;
-							AppDataInfo.RelayControl = TRUE;
-						}
-						else
-						{
-							AppDataInfo.appDataModify = TRUE;
-							AppDataInfo.RelayControl = FALSE;
-						}
-						if (CanRxMsg[i].Data[1] == 0x01)
-						{
-							HeatForceControl = 0x01;
-						}
-						else if (CanRxMsg[i].Data[1] == 0x02)
-						{
-							HeatForceControl = 0x02;
-						}
-						else
-						{
-							HeatForceControl = 0x00;
-						}
-						if (CanRxMsg[i].Data[2] == 0x01)
-						{
-							BMS_Fota_update_flag = true;
-						}
-					}
 				}
 				if (udsFlag == 1)
 				{
@@ -161,7 +132,7 @@ static void CanTask()
 				delayconuter = 0;
 				clrbit(PadInterrupt, 0);
 				PROC_CAN_STATE_SWITCH(PROCESS_CAN_STATE_IDLE);
-				if (RelayForbiddenControl == 1)
+				if (RelayForceControl == 1)
 				{
 					RelayForceControl = 2;
 				}

src/AppTaskGps.c

@@ -39,6 +39,11 @@ static void GpsTask(void *arg)
 	UINT16 direction;
 	UINT16 GpsNoDataCounter = 0;
 	UINT8 index = 0;
+	UINT8 xyzCounter = 0;
+	INT16 xData[3] = {0};
+	INT16 yData[3] = {0};
+	INT16 zData[3] = {0};
+	INT16 xyzDataBuffer[3] = {0};
 	GPSInfo GpsInfoData;
 	PROC_GPS_STATE_SWITCH(PROCESS_GPS_STATE_INIT);
 	while (1)
@@ -196,6 +201,26 @@ static void GpsTask(void *arg)
 					free(msg.dataPtr);
 				msg.dataPtr = NULL;
 			}
+			if (getbit(PadInterrupt, 3) == 1 || getbit(PadInterrupt, 4) == 1 || TimeCounter % 50 == 0)
+			{
+				//SL_SC7A20_Reg_read_all();
+				UINT8 ret = SL_SC7A20_Read_XYZ_Data(xyzDataBuffer);
+				xData[xyzCounter] = xyzDataBuffer[0];
+				yData[xyzCounter] = xyzDataBuffer[1];
+				zData[xyzCounter] = xyzDataBuffer[2];
+				xyzCounter++;
+				if (xyzCounter > 2)
+				{
+					xyzCounter = 0;
+				}
+				xyzData[0] = (xData[0] + xData[1] + xData[2]) / 3;
+				xyzData[1] = (yData[0] + yData[1] + yData[2]) / 3;
+				xyzData[2] = (zData[0] + zData[1] + zData[2]) / 3;
+#ifdef USING_PRINTF
+				printf("\n%d %d %d \r\n", xyzData[0], xyzData[1], xyzData[2]);
+#endif
+			}
+
 			PROC_GPS_STATE_SWITCH(PROCESS_GPS_STATE_IDLE);
 			break;
 		}

src/AppTaskMain.c

@@ -3,7 +3,7 @@
  * @Date         : 2021-10-14 09:27:15
  * @Version      : V3.0
  * @LastEditors  : ChenJie
- * @LastEditTime : 2021-10-15 16:33:43
+ * @LastEditTime : 2021-10-26 16:07:52
  * @Description  : 主要负责：1.工作模式转换2.定时器定时3.定时保存运行数据4.异常状态重启
  * @FilePath     : \PLAT\project\ec616_0h00\apps\qx_app\src\AppTaskMain.c
  */
@@ -105,11 +105,11 @@ static void MainTask(void *arg)
                 PROC_MAIN_STATE_SWITCH(PROCESS_STATE_LISTEN);
                 break;
             }
-            if ((BMS_Fota_update_flag != FALSE) || (TCPWorkState != 0x00) || (BattWorkStateDelay != BATT_IDLE_SYM) || (PadInterrupt != 0x00))
+            if ((BMS_Fota_update_flag != FALSE) || (TCPWorkState != 0x00) || (BattWorkStateDelay != BATT_IDLE_SYM) || (battWorkState != BATT_IDLE_SYM) || (PadInterrupt != 0x00))
             {
                 xTimerReset(work_timer, 0);
             }
-            if (TcpErrorcount >= 10 && PadInterrupt == 0x00)
+            if ((TcpErrorcount >= 10 && getbit(PadInterrupt, 0) == 0x00) || (NB_Fota_update_flag == TRUE))
             {
                 AppDataInfo.appDataModify = true;
                 AppDataInfo.SysReStartCount = AppDataInfo.SysReStartCount + 1;
@@ -142,20 +142,6 @@ static void MainTask(void *arg)
         case PROCESS_STATE_LISTEN:
         {
             PROC_APP_STATE_SWITCH(LISTEN);
-            if (NB_Fota_update_flag)
-            {
-                if (AppDataInfo.appDataModify)
-                {
-                    SaveAppData();
-                }
-                if (BcuDataInfo.appDataModify)
-                {
-                    SaveBcuData();
-                }
-                appSetCFUN(0);
-                osDelay(1000);
-                EC_SystemReset();
-            }
             if (UartInterruptcount > 1)
             {
                 if (UartInterruptcount > 10)

src/AppTaskTcp.c

@@ -235,7 +235,7 @@ static void TcpDataInfoAssembleSend()
     UINT16 DataLen = 0;
     UINT8 BattSendFreq = 30;
     UINT8 GpsSendFreq = 30;
-
+    UINT8 DebugFeq = 10;
     UINT16 year;
     UINT8 month, day, hour, minute, sec;
     UTC8TimeType UTC8TimeTcp;
@@ -272,12 +272,13 @@ static void TcpDataInfoAssembleSend()
     if (getbit(PadInterrupt, 3) == 1 || getbit(PadInterrupt, 4) == 1) //有震动产生,提高位置信息发送频率
     {
         GpsSendFreq = AppDataInfo.PosInfoSendFreqHigh;
-        if (send_counter % 10 == 0)
+        if (send_counter % 20 == 0)
         {
             clrbit(PadInterrupt, 3);
             clrbit(PadInterrupt, 4);
         }
     }
+    DebugFeq = BattSendFreq;
     if (send_counter % BattSendFreq == 0 && send_counter != 0)
     {
         UINT16 ErrorTemp = 0x00;
@@ -427,32 +428,56 @@ static void TcpDataInfoAssembleSend()
         if (ret == 0)
         {
             memcpy((UINT8 *)&GpsToTcpInfo.gpsInfo.GpsInfoData, (UINT8 *)&GpsRecvData, sizeof(GPSInfo));
-            GpsToTcpInfo.gpsInfo.Tac[0] = tac >> 8;
-            GpsToTcpInfo.gpsInfo.Tac[1] = tac & 0xFF;
-            GpsToTcpInfo.gpsInfo.CellID[0] = cellId >> 24;
-            GpsToTcpInfo.gpsInfo.CellID[0] = cellId >> 16;
-            GpsToTcpInfo.gpsInfo.CellID[0] = cellId >> 8;
-            GpsToTcpInfo.gpsInfo.CellID[0] = cellId;
-            GpsToTcpInfo.CRC = bcc_chk((UINT8 *)&GpsToTcpInfo, sizeof(GPSMsgtoTcpType) - 1);
-            if (1)
+        }
+        else
+        {
+            memset((UINT8 *)&GpsToTcpInfo.gpsInfo.GpsInfoData, 0x00, sizeof(GPSInfo));
+        }
+        GpsToTcpInfo.gpsInfo.Tac[0] = tac >> 8;
+        GpsToTcpInfo.gpsInfo.Tac[1] = tac & 0xFF;
+        GpsToTcpInfo.gpsInfo.CellID[0] = cellId >> 24;
+        GpsToTcpInfo.gpsInfo.CellID[0] = cellId >> 16;
+        GpsToTcpInfo.gpsInfo.CellID[0] = cellId >> 8;
+        GpsToTcpInfo.gpsInfo.CellID[0] = cellId;
+        UINT16 xyzDatacahce[3] = {0};
+        memcpy(xyzDatacahce, xyzData, 3);
+        for (UINT8 i = 0; i < 3; i++)
+        {
+            if (xyzDatacahce[i] > 0x8000) // 数据为负
             {
-                len = tcpipConnectionSend(socContext.id, (UINT8 *)&GpsToTcpInfo, sizeof(GpsToTcpInfo), 0, 0, 0);
+                xyzDatacahce[i] = (UINT16)((UINT16)(~(xyzDatacahce[i])) + 1);
+                xyzDatacahce[i] = 20000U - xyzDatacahce[i];
+            }
+            else
+            {
+                xyzDatacahce[i] = xyzDatacahce[i] + 20000U;
+            }
+        }
+        GpsToTcpInfo.gpsInfo.xData[0] = xyzDatacahce[0] >> 8;
+        GpsToTcpInfo.gpsInfo.xData[1] = xyzDatacahce[0];
+        GpsToTcpInfo.gpsInfo.yData[0] = xyzDatacahce[1] >> 8;
+        GpsToTcpInfo.gpsInfo.yData[1] = xyzDatacahce[1];
+        GpsToTcpInfo.gpsInfo.zData[0] = xyzDatacahce[2] >> 8;
+        GpsToTcpInfo.gpsInfo.zData[1] = xyzDatacahce[2];
+        GpsToTcpInfo.CRC = bcc_chk((UINT8 *)&GpsToTcpInfo, sizeof(GPSMsgtoTcpType) - 1);
+        if (1)
+        {
+            len = tcpipConnectionSend(socContext.id, (UINT8 *)&GpsToTcpInfo, sizeof(GpsToTcpInfo), 0, 0, 0);
 #ifdef USING_PRINTF1
-                for (int i = 0; i < sizeof(GpsToTcpInfo); i++)
-                {
-                    printf("%02x ", *((UINT8 *)&GpsToTcpInfo + i));
-                }
-                printf("-[%d]-Gpslen:%d\n", __LINE__, len);
+            for (int i = 0; i < sizeof(GpsToTcpInfo); i++)
+            {
+                printf("%02x ", *((UINT8 *)&GpsToTcpInfo + i));
+            }
+            printf("-[%d]-Gpslen:%d\n", __LINE__, len);
 #endif
-                if (len > 0)
-                {
-                    TcpSendLen = 0x04 | TcpSendLen;
-                }
-                else
-                {
-                    TcpSendLen = 0xFB & TcpSendLen;
-                    sendQueueMsg(QMSG_ID_NW_IP_SUSPEND, 0);
-                }
+            if (len > 0)
+            {
+                TcpSendLen = 0x04 | TcpSendLen;
+            }
+            else
+            {
+                TcpSendLen = 0xFB & TcpSendLen;
+                sendQueueMsg(QMSG_ID_NW_IP_SUSPEND, 0);
             }
         }
     }
@@ -529,7 +554,6 @@ static void TcpDataInfoAssembleSend()
             len = tcpipConnectionSend(socContext.id, (UINT8 *)&VerMsgToTcpInfo, sizeof(VerMsgToTcpInfo), 0, 0, 0);
             if (len > 0)
             {
-                BMSupdatestatus = 0xFF;
                 TcpSendLen = 0x01 | TcpSendLen;
             }
             else
@@ -539,26 +563,30 @@ static void TcpDataInfoAssembleSend()
             }
         }
     }
-    UINT8 DebugFeq = 10;
     if (send_counter % DebugFeq == 0)
     {
         DebugMsgtoTcpType DebugMsgInfo;
         UINT16 BufferLen = 0;
         UINT8 rbuf[1000];
-        if (send_counter % (600 / DebugFeq) == 0)
+        static UINT8 Debugcounter = 0;
+        if (Debugcounter % 2 == 0)
         {
-            sprintf((char *)rbuf, "A-%x,%d,%d,%x,%x,%d,%d",
-                    battProtectState, AppDataInfo.RelayControl, chargerConnectState, PowerVoltage, BMSupdatestatus,
+            sprintf((char *)rbuf, "A-%x,%d,%d,%x,%x,%x,,%d,%d",
+                    battProtectState, AppDataInfo.RelayControl, chargerConnectState, PowerVoltage, BMSupdatestatus, PadInterrupt,
                     AppDataInfo.ExpiryTimeArray[1], AppDataInfo.ExpiryTimeArray[2]);
         }
-        else if (send_counter % DebugFeq == 0)
+        else
         {
             sprintf((char *)rbuf, "B-%d,%d,%d,%d,%d,%d,\
-                                %d,%d,%d,%d,%d,%d,%d",
+                                %d,%d,%d",
                     socd_pct_ahSoc, socd_pct_ekfSoc, socd_pct_estSoc, socd_pct_battSoc, socd_pct_bcuSoc, battSOC * 10,
-                    battI, maxCellVol, minCellVol, test_efkSocMin, test_efkSocMax, test_UpMin, test_UpMax);
+                    battI, maxCellVol, minCellVol);
+        }
+        Debugcounter++;
+        if (Debugcounter > 100)
+        {
+            Debugcounter = 0;
         }
-
         BufferLen = strlen(rbuf);
         SendBuffer = malloc(BufferLen + sizeof(DebugMsgInfo));
         memcpy(SendBuffer + sizeof(DebugMsgInfo) - 1, rbuf, BufferLen);

src/AppTaskUart.c

@@ -2,7 +2,7 @@
  * @Author       : ChenJie
  * @Date         : 2021-10-14 09:27:15
  * @LastEditors  : ChenJie
- * @LastEditTime : 2021-10-21 11:54:55
+ * @LastEditTime : 2021-10-26 15:00:40
  * @Description  : file content
  * @FilePath     : \PLAT\project\ec616_0h00\apps\qx_app\src\AppTaskUart.c
  */
@@ -69,14 +69,14 @@ static void UartTask(void *arg)
 #ifdef USING_PRINTF
 			printf("EncryptFlag:%d\n", EncryptFlag);
 #endif
-			// if(EncryptFlag==0x01)
-			// {
-			// 	ihd_st_authFaild = 0;
-			// }
-			// else
-			// {
-			// 	ihd_st_authFaild = 1;
-			// }
+			if (EncryptFlag == 0x01)
+			{
+				ihd_st_authFaild = 0;
+			}
+			else
+			{
+				ihd_st_authFaild = 1;
+			}
 			PROC_UART_STATE_SWITCH(PROCESS_UART_STATE_IDLE);
 			break;
 		}
@@ -182,6 +182,7 @@ static void UartTask(void *arg)
 		}
 		case PROCESS_UART_STATE_UPDATE:
 		{
+			BMSupdatestatus = 0xFF;
 			UartRecvFlag = 0;
 #if BMS_MANUFACTURE == 1
 			BMSupdatestatus = SP_BMS_Update_Service();
@@ -295,11 +296,11 @@ void Uart_Cmd_Control(QueueHandle_t UartWriteCmdHandle, UartBuffer UartAnswerDat
 	//控制充电禁止
 	if (getbit(BcuFltAct, 6) == 0)
 	{
-		if (maxCellVol > 4180 && maxCellVol < 5000)
+		if ((maxCellVol > 4190 && maxCellVol < 5000) && battSOC >= 99)
 		{
 			ChargeForbiddenControl = 1;
 		}
-		else if (maxCellVol < 4130 && maxCellVol > 0)
+		else if (maxCellVol < 4150 && maxCellVol > 0)
 		{
 			ChargeForbiddenControl = 0;
 		}
@@ -355,7 +356,7 @@ void Uart_Cmd_Control(QueueHandle_t UartWriteCmdHandle, UartBuffer UartAnswerDat
 		AppDataInfo.appDataModify = TRUE;
 		AppDataInfo.RelayControl = TRUE;
 	}
-
+	//控制命令发送
 	if (TimeCounter % 10 == 0 && BattHeaterSwitch(&HeatSwitch, HeatForceControl) == TRUE)
 	{
 		UartWriteData.WriteCmd = 0x02;

src/BCU.c

@@ -11,6 +11,7 @@ static process_Bcu gProcess_Bcu_Task;
 void BCU(void)
 {
     uint16_T i;
+
     UINT32 locateTimeCounter = 0;
     osDelay(5000);
     PROC_BCU_STATE_SWITCH(PROCESS_STATE_INIT);
@@ -36,7 +37,9 @@ void BCU(void)
             sfmd_flg_mainCirClosFltEi = BcuDataInfo.sfmd_flg_mainCirClosFltE;
             sfmd_flg_heatCirClosFltEi = BcuDataInfo.sfmd_flg_heatCirClosFltE;
             sfmd_flg_heatCirOpenFltEi = BcuDataInfo.sfmd_flg_heatCirOpenFltE;
-            ihd_st_EOLState = AppNVMData.EOLState;
+			sfmd_flg_heatRunFltEi = BcuDataInfo.sfmd_flg_heatRunFltE;
+			ihd_st_EOLState = AppNVMData.EOLState;
+			
             PROC_BCU_STATE_SWITCH(PROCESS_STATE_WORK);
             break;
         }
@@ -46,9 +49,12 @@ void BCU(void)
             while (TRUE)
             {
                 //osDelay(100);
-                if (locateTimeCounter != TimeCounter)
+                //if (locateTimeCounter != TimeCounter)
+                if (TimeCounter%10==0)
                 {
-                    locateTimeCounter = TimeCounter;
+                    printf("TimeCounter:%d\n",TimeCounter);
+                    //locateTimeCounter = TimeCounter;
+			
                     if (gProcess_app == LISTEN)
                     {
                         PROC_BCU_STATE_SWITCH(PROCESS_STATE_SLEEP);
@@ -93,22 +99,27 @@ void BCU(void)
                     ihd_flg_disChrgModTLowFlt = ((battWarningState >> 22) & 0x01) == 1;
                     ihd_flg_chrgModTLowFlt = ((battWarningState >> 23) & 0x01) == 1;
                     ihd_flg_currOpenFlt = 0;
-                    ihd_st_chrgMosControl = ChargeForbiddenControl == 0;
-                    ihd_st_disChrgMosControl = DisChargeForbiddenControl == 0;
-                    ihd_st_relayControl = RelayForbiddenControl == 0;
-                    ihd_st_heatForceControl = HeatForceControl;
+					ihd_st_chrgMosControl    = ChargeForbiddenControl == 0;
+					ihd_st_disChrgMosControl = DisChargeForbiddenControl  == 0;
+				    ihd_st_relayControl      = RelayForbiddenControl  == 0;
+				    ihd_flg_DTCClear = ihd_flg_disChrgMosClosFlt;  /////
+
                     //=============================================================================
                     SFM();
                     TMS();
-                    SPM();
+					//CM();
 
+					
+                    SPM();
+			
                     //==============================shi shi cun chu=======
-                    if (socd_flg_EEsave == 1)
+                    if (socd_flg_EEsave == 1 || ihd_flg_DTCClear)
                     {
                         BCUEEDataSave();
+						ihd_flg_DTCClear = false;
                     }
                 }
-                osDelay(5);
+                osDelay(100);
             }
             break;
         }
@@ -144,6 +155,7 @@ void BCU_Init(void)
 {
     SFM_Init();
     TMS_Init();
+	//CM_Init();
     SPM_Init();
 }
 
@@ -177,5 +189,6 @@ void BCUEEDataSave(void)
     BcuDataInfo.sfmd_flg_mainCirClosFltE = sfmd_flg_mainCirClosFltEo;
     BcuDataInfo.sfmd_flg_heatCirClosFltE = sfmd_flg_heatCirClosFltEo;
     BcuDataInfo.sfmd_flg_heatCirOpenFltE = sfmd_flg_heatCirOpenFltEo;
+	BcuDataInfo.sfmd_flg_heatRunFltE = sfmd_flg_heatRunFltEo;
     BcuDataInfo.appDataModify = TRUE;
 }

src/EmbeddedCoder_src/BCUCal.c

@@ -13,7 +13,7 @@ const uint16_T cmnc_tm_parkTime = 1800U;
 const uint16_T cmnm_F_polar[13] = {3880U, 5062U, 10906U, 10798U, 12885U, 12990U, 14331U, 10754U, 9378U, 10340U, 10592U, 11946U, 11515U}; /* 电池放电参数的C数组； */
 const uint16_T cmnm_R_ohm[13] = {2181U, 2043U, 2040U, 1927U, 1850U, 1802U, 1771U, 1743U, 1733U, 1728U, 1748U, 1762U, 1789U};             /* 电池放电参数的Ro数组； */
 const uint16_T cmnm_R_polar[13] = {1837U, 1055U, 1352U, 1453U, 1181U, 1125U, 1205U, 1283U, 1228U, 1187U, 1104U, 1170U, 1049U};           /* 电池放电参数的Rp数组； */
-const uint16_T cmnm_V_ocv[13] = {3311U, 3411U, 3476U, 3553U, 3610U, 3638U, 3674U, 3758U, 3845U, 3942U, 4051U, 4106U, 4175U};             /* 电池放电参数的OCV数组； */
+const uint16_T cmnm_V_ocv[13] = {3234U, 3444U, 3476U, 3553U, 3610U, 3638U, 3674U, 3758U, 3845U, 3942U, 4051U, 4106U, 4175U};             /* 电池放电参数的OCV数组； */
 const uint16_T cmnm_pct_soc[13] = {0U, 50U, 100U, 200U, 300U, 400U, 500U, 600U, 700U, 800U, 900U, 950U, 1000U};                          /* 电池放电参数的SOC数组； */
 const uint16_T cmnc_V_chrgFul = 4200U;                                                                                                   /* 充满电的截至电压； */
 
@@ -94,6 +94,9 @@ const uint16_T socm_V_chrgCor[3] = {4160U, 4175U, 4188U};    /* 充电CCV对应
 const uint16_T socm_V_disChrgCor[3] = {3303U, 3359U, 3368U}; /* 放电CCV对应的电压； */
 const uint16_T socc_pct_chrgCor = 980;
 const uint16_T socc_pct_disChrgCor = 50;
+const int16_T cmm_T_ChrgCurr[3] = {40,60,80};
+const int16_T cmm_V_ChrgCurr[6] = {3200,3201,3900,3901,4100,4101};
+const int16_T cmm_I_ChrgCurr[3][6] = {{0,100,100,100,100,50},{50,400,400,200,200,100},{40,300,300,150,150,80}};
 
 const uint16_T tmsc_T_openThr = 45U;     /* 热管理开启温度； */
 const uint16_T tmsc_T_closeMinThr = 50U; /* 热管理关闭温度（最低模组）； */

src/EmbeddedCoder_src/BCUDisp.c

@@ -73,7 +73,8 @@ boolean_T sfmd_flg_heatCirClosFltEi;              /*加热回路常闭故障读
 boolean_T sfmd_flg_heatCirClosFltEo;              /*加热回路常闭故障写入量 */  
 boolean_T sfmd_flg_heatCirOpenFltEi;              /*加热回路常开故障读取量 */  
 boolean_T sfmd_flg_heatCirOpenFltEo;              /*加热回路常开故障写入量 */  
-
+boolean_T sfmd_flg_heatRunFltEi;
+boolean_T sfmd_flg_heatRunFltEo;
 uint16_T socd_pct_ahSoc;                          /*安时SOC； */
 uint16_T socd_pct_estSoc;                         /*估算SOC；*/
 uint16_T socd_flg_EEsave;                         /*实时存储标志位；*/
@@ -104,7 +105,7 @@ uint16_T sohv_V_chrgStartStatEo[28];              /*充电开始时刻的单体
 
 uint8_T cmd_st_chrgMod;                           /* 充电模式  */
 uint8_T cmd_st_chrgSt;                            /* 充电状态  */
-uint16_T cmd_I_chrgCurrReq;                       /* 充电需求电流  */
+int16_T cmd_I_chrgCurrReq;                       /* 充电需求电流  */
 uint8_T cmd_idx_chrgEndReason;                    /* 充电结束原因  */
         
 uint16_T cand_Nr_cellNr;                          /*循环发送的单体编号； */
@@ -116,4 +117,6 @@ uint16_T cand_Q_totalCp;                          /*循环发送的单体累计
 uint16_T test_efkSocMin;
 uint16_T test_efkSocMax;
 real_T test_UpMin;
-real_T test_UpMax;
+real_T test_UpMax;
+uint16_T test_ekfSoc;
+uint16_T test_ekfSoc0;

src/EmbeddedCoder_src/BLC.c

@@ -48,7 +48,7 @@ void BLC(void)
     
     if(FirstRun_BLC)
     {
-        ArrMax(&blcv_Q_reqCpEi[0],&MaxCp, cmnc_num_cellUNum);
+        MaxCp = ArrMax(&blcv_Q_reqCpEi[0], cmnc_num_cellUNum);
         if(MaxCp > cmnc_Q_ratedCp * 100)
         {
             memset(blcn_Q_reqCpEE,0, sizeof(blcn_Q_reqCpEE));
@@ -166,7 +166,7 @@ void BLC(void)
             blcv_flg_excute[i] = false ;
         }
 
-		blcn_Q_impleCp[i] = (uint16_T) (temp[i] /36000 * 1000);
+		blcn_Q_impleCp[i] = (uint16_T) (temp[i] /3600 * 1000);
         blcv_Q_reqCpEo[i] = ( (int16_T)(blcn_Q_reqCpNow[i] - blcn_Q_impleCp[i]) < 0 ? 0 :blcn_Q_reqCpNow[i] - blcn_Q_impleCp[i]);
         blcv_Q_totalCpEo[i] = (uint32_T) (blcn_Q_totalCpEE[i] + blcn_Q_impleCp[i]);  
     }

src/EmbeddedCoder_src/CM.c

@@ -0,0 +1,232 @@
+#include "CM.h"
+
+boolean_T FirstRun_CM;
+void CM_Init(void)
+{
+    FirstRun_CM = true;
+}
+
+void CM(void)
+{
+    static uint16_T cmn_V_cellUMax;
+    static uint16_T cmn_V_cellUMin;
+    static uint16_T chrgFulCnt;
+    static boolean_T cmn_flg_chrgFul;
+    static uint16_T chrgCnt;
+    static uint16_T noChrgCnt;
+    int16_T cmd_I_chrgCurrReq1 = 0;
+    int16_T cmd_I_chrgCurrReq2 = 0;
+    int16_T cmd_I_chrgCurrReq3 = 0;
+    int16_T cmd_I_chrgCurrReq4 = 0;
+	static boolean_T First;
+    static uint8_T cmd_st_chrgSt_end;
+	static boolean_T chrgFul;
+    if(FirstRun_CM)
+    {
+        cmn_V_cellUMax = 0;
+        cmn_V_cellUMin = 0;
+        chrgFulCnt = 0;
+        chrgCnt = 0;
+        noChrgCnt = 0;
+        cmn_flg_chrgFul = false;
+		cmd_st_chrgSt = 0;
+		chrgFul = 0;
+    }
+    //=====================================================================
+    //============================充电模式=================================
+    //=====================================================================
+    if(ihd_st_chrgConnect > 0)
+    {
+        cmd_st_chrgMod = 1;
+    }
+    else
+    {
+        cmd_st_chrgMod = 0;
+    }
+	
+    //=====================================================================
+    //============================充电需求电流=============================
+    //=====================================================================
+    if (cmd_st_chrgMod)
+    {
+        if(sfmd_V_cellUMin > cmn_V_cellUMin)
+        {
+            cmn_V_cellUMin = sfmd_V_cellUMin;
+        }
+        if(sfmd_V_cellUMax > cmn_V_cellUMax)
+        {
+            cmn_V_cellUMax = sfmd_V_cellUMax;
+        }
+        cmd_I_chrgCurrReq1 = lookupChrgCurr((int16_T)sfmd_T_modTMin,(int16_T)cmn_V_cellUMin,3,6);
+        cmd_I_chrgCurrReq2 = lookupChrgCurr((int16_T)sfmd_T_modTMax,(int16_T)cmn_V_cellUMin,3,6);
+        cmd_I_chrgCurrReq3 = lookupChrgCurr((int16_T)sfmd_T_modTMin,(int16_T)cmn_V_cellUMax,3,6);
+        cmd_I_chrgCurrReq4 = lookupChrgCurr((int16_T)sfmd_T_modTMax,(int16_T)cmn_V_cellUMax,3,6);
+		cmd_I_chrgCurrReq =  min( min(cmd_I_chrgCurrReq1, cmd_I_chrgCurrReq2),min(cmd_I_chrgCurrReq3,cmd_I_chrgCurrReq4));
+    }
+    else
+    {
+        cmd_I_chrgCurrReq = 0;
+    }
+	
+    //=====================================================================
+    //============================充电状态=================================
+    //=====================================================================
+    if (cmd_st_chrgMod > 0)
+    {
+        noChrgCnt = 0; 
+        if (sfmd_I_curr > 1)
+        {
+            cmd_st_chrgSt = 1;
+        } 
+		if(JudgeTimeSystem(1,sfmd_V_cellUMax >= cmnc_V_chrgFul ,&chrgFulCnt ,20))
+		{
+		   chrgFul = true;
+           cmn_flg_chrgFul = true;
+		} 
+        if(cmn_flg_chrgFul)
+        {
+            cmd_st_chrgSt = 2;
+            cmd_I_chrgCurrReq = 0;
+        }
+
+		//
+        if (cmd_st_chrgSt != 2 && JudgeTimeSystem(1,sfmd_I_curr < 1 ,&chrgCnt ,100) )
+        {
+            cmd_st_chrgSt = 3;
+            cmd_I_chrgCurrReq = 0;
+        }
+		//
+        if((sfmd_st_fltAct >> 6) & 0x01)
+        {
+            cmd_st_chrgSt = 3;
+            cmd_I_chrgCurrReq = 0; 
+        }
+
+
+
+
+		
+		cmd_st_chrgSt_end = cmd_st_chrgSt;
+    }
+    else
+    {   
+        cmn_flg_chrgFul = false;
+        chrgCnt = 0;
+        noChrgCnt ++;
+        if(cmd_st_chrgSt_end == 1  && noChrgCnt <= 100 )
+        {
+            cmd_st_chrgSt = 3;
+        }
+        else
+        {
+            cmd_st_chrgSt = 0;
+        }
+    }
+	
+    // ===================================================================
+    // =================故障结束原因========================================
+    // ===================================================================
+    if(cmd_st_chrgSt == 3 )
+    {
+        if (ihd_st_chrgConnect == 0 && First)
+        {
+            cmd_idx_chrgEndReason = 1 ; // 用户主动停止
+        }
+        if((sfmd_st_fltAct >> 6) & 0x01 && First)
+        {
+            cmd_idx_chrgEndReason = 2 ; // 因电池故障限制充电；
+        }
+
+
+		
+        First = false;
+    }
+    else
+    {
+        First = true ;
+        cmd_idx_chrgEndReason = 0;
+    }
+
+    FirstRun_CM = false;
+	//printf("fltAct:%d,chrgMod:%d,chrgSt:%d,chrgCurrReq:%d,chrgEndReason:%d\n\n",sfmd_st_fltAct,cmd_st_chrgMod,cmd_st_chrgSt,cmd_I_chrgCurrReq,cmd_idx_chrgEndReason);
+}
+
+
+//==============================================================================================================================
+//==============================================================================================================================
+int16_T lookupChrgCurr(int16_T X,int16_T Y,uint16_T cmm_T_ChrgCurrN,uint16_T cmm_V_ChrgCurrN)
+{
+    int16_T Z;
+    int16_T temp1;
+    int16_T temp2;
+    uint16_T i;
+    uint16_T j;
+	
+    //===================================
+    if (X <= cmm_T_ChrgCurr[0])
+    {
+        if(Y <= cmm_V_ChrgCurr[0])
+        {
+            Z = cmm_I_ChrgCurr[0][0];
+        }
+        for(j = 0;j < cmm_V_ChrgCurrN - 1;j++)
+        {
+            if (Y > cmm_V_ChrgCurr[j] && Y <= cmm_V_ChrgCurr[j+1])
+            {
+                Z = (int16_T) (     (int32_T)(Y - cmm_V_ChrgCurr[i]) * (cmm_I_ChrgCurr[0][j+1] -cmm_I_ChrgCurr[0][j]) /(cmm_V_ChrgCurr[j+1] - cmm_V_ChrgCurr[j])     ) + cmm_I_ChrgCurr[0][j];
+            }
+        }
+        if(Y > cmm_V_ChrgCurr[cmm_V_ChrgCurrN-1])
+        {
+            Z = cmm_I_ChrgCurr[0][cmm_V_ChrgCurrN-1];
+        }
+    }
+    //======================================================================
+    for (i = 0; i < cmm_T_ChrgCurrN - 1;i++ )
+    {
+        if (X > cmm_T_ChrgCurr[i] && X <= cmm_T_ChrgCurr[i+1])
+        {
+            if(Y <= cmm_V_ChrgCurr[0])
+            {
+                Z = (int16_T) ((int32_T)(X - cmm_T_ChrgCurr[i]) * (cmm_I_ChrgCurr[i+1][0] -cmm_I_ChrgCurr[i][0])/(cmm_T_ChrgCurr[i+1] - cmm_T_ChrgCurr[i])) + cmm_I_ChrgCurr[i][0]  ;
+            }
+            for(j = 0;j < cmm_V_ChrgCurrN - 1;j++)
+            { 
+                if (Y > cmm_V_ChrgCurr[j] && Y <= cmm_V_ChrgCurr[j+1])
+                {
+                    temp1 = (int16_T) ( (int32_T)(  (X - cmm_T_ChrgCurr[i]) * (cmm_I_ChrgCurr[i+1][j] - cmm_I_ChrgCurr[i][j])  )/(cmm_T_ChrgCurr[i+1] - cmm_T_ChrgCurr[i])) + cmm_I_ChrgCurr[i][j];
+                    temp2 = (int16_T) ( (int32_T)( (X - cmm_T_ChrgCurr[i]) * (cmm_I_ChrgCurr[i+1][j+1] - cmm_I_ChrgCurr[i][j+1]) )/(cmm_T_ChrgCurr[i+1] - cmm_T_ChrgCurr[i])) + cmm_I_ChrgCurr[i][j+1];
+                    Z =  (int16_T) ( (int32_T)( (Y - cmm_V_ChrgCurr[j]) * (temp2 -temp1)  )/(cmm_V_ChrgCurr[j+1] - cmm_V_ChrgCurr[j])) + temp1 ;
+		
+                    //break;
+                }
+            }
+            if(Y > cmm_V_ChrgCurr[cmm_V_ChrgCurrN-1])
+            {
+                Z = (int16_T) (  (int32_T)( (X - cmm_T_ChrgCurr[i]) * (cmm_I_ChrgCurr[i+1][cmm_V_ChrgCurrN-1] -cmm_I_ChrgCurr[i][cmm_V_ChrgCurrN-1])  )/(cmm_T_ChrgCurr[i+1] - cmm_T_ChrgCurr[i]))+ cmm_I_ChrgCurr[i][cmm_V_ChrgCurrN-1];
+            }
+           // break;
+        }
+    }
+    //=====================================================================
+    if (X > cmm_T_ChrgCurr[cmm_T_ChrgCurrN-1])
+    {
+        if(Y <= cmm_V_ChrgCurr[0])
+        {
+            Z = cmm_I_ChrgCurr[cmm_T_ChrgCurrN-1][0];
+        }
+        for(j = 0;j < cmm_V_ChrgCurrN - 1;j++)
+        {
+            if (Y > cmm_V_ChrgCurr[j] && Y <= cmm_V_ChrgCurr[j+1])
+            {
+                Z = (int16_T) (  (int32_T)( (Y - cmm_V_ChrgCurr[i]) * (cmm_I_ChrgCurr[cmm_T_ChrgCurrN-1][j+1] -cmm_I_ChrgCurr[cmm_T_ChrgCurrN-1][j]) )/(cmm_V_ChrgCurr[j+1] - cmm_V_ChrgCurr[j])) + cmm_I_ChrgCurr[cmm_T_ChrgCurrN-1][j];
+               //  break;
+            }
+        }
+        if(Y > cmm_V_ChrgCurr[cmm_V_ChrgCurrN-1])
+        {
+            Z = cmm_I_ChrgCurr[cmm_T_ChrgCurrN-1][cmm_V_ChrgCurrN-1];
+        }
+    }  
+	return Z;
+}

src/EmbeddedCoder_src/SFM.c

@@ -28,7 +28,7 @@ void SFM(void)
     static uint16_T SumT2;
     
     static uint8_T  Cntl;
-    static uint16_T CellUArry[3][28];
+    static uint16_T CellUArry[4][28];
     static uint8_T  ErrNr[28];
     static uint8_T  RecNr[28];
     static boolean_T sfmv_flg_cellUOpenFlt[28];
@@ -48,7 +48,6 @@ void SFM(void)
     boolean_T sfmd_flg_ACPlugTOpenFlt;
     static boolean_T sfmd_flg_heatPanT1OpenFlt;
     static boolean_T sfmd_flg_heatPanT2OpenFlt;
-    boolean_T sfmd_flg_heatPanTOpenFlt;
     boolean_T sfmd_flg_modTRationFlt;
     boolean_T sfmv_flg_modTRationFlt[6];
     
@@ -130,6 +129,18 @@ void SFM(void)
     boolean_T sfmd_flg_EEsaveFlt;
     static uint16_T socd_pct_bcuSoc_Delay;
     
+    static uint16_T noDisChrgCnt;
+    static int16_T sfmd_I_curr_Delay;
+    static boolean_T sfmd_flg_Udrop;
+    static boolean_T sfmd_flg_heatclear;
+    static boolean_T sfmd_flg_volFlt_keep;
+    static uint16_T CntA;
+    static uint16_T ModTStor[4][6];
+    static boolean_T sfmd_flg_Tup;
+    static boolean_T sfmd_flg_modTOpenFlt_keep;
+    static uint16_T CntB;
+    
+    
     boolean_T FltFlg[200];
     uint16_T  FltLevel[200];
     uint8_T   FltAct[200];
@@ -146,7 +157,7 @@ void SFM(void)
     {
         memset(&DiagThr,0,sizeof(DiagThr));
         memset(&DiagTime,0,sizeof(DiagTime));
-        Cntl = 0;
+        Cntl = 10;
         memset(heatT1Arr,0,sizeof(heatT1Arr));
         memset(heatT2Arr,0,sizeof(heatT2Arr));
         memset(ErrNr,0,sizeof(ErrNr));
@@ -190,6 +201,16 @@ void SFM(void)
         sfmd_flg_chrgCurrOverFlt = false;
         sfmd_flg_disChrgCurrOverFlt = false;
         
+        noDisChrgCnt = 0;
+        sfmd_I_curr_Delay = ihd_I_curr;
+        sfmd_flg_Udrop = false;
+        sfmd_flg_heatclear = false;
+        sfmd_flg_volFlt_keep = false;
+        CntA  = 10;
+        sfmd_flg_Tup = false;
+        sfmd_flg_modTOpenFlt_keep = false;
+        CntB = 0;;
+
         ihd_T_mosT_Delay = ihd_T_mosT;
         ihd_T_DCPlugT_Delay =ihd_T_DCPlugT;
         ihd_T_ACPlugT_Delay = ihd_T_ACPlugT;
@@ -207,6 +228,9 @@ void SFM(void)
         sfmd_flg_heatCirOpenFltEo = false;
         sfmd_flg_heatCirClosFltEi = false;
         sfmd_flg_heatCirClosFltEo = false;
+		sfmd_flg_heatRunFltEi = false;
+		sfmd_flg_heatRunFltEo = false;
+		
     }
     
     //=======================================================================================
@@ -236,13 +260,19 @@ void SFM(void)
     {
         sfmd_flg_HVILFlt = false;
     }
-    
+
+
+
+
+
+
+	
     //22 充电Mos失效故障
-    sfmd_flg_chrgMosClosFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, ihd_flg_chrgMosClosFlt ,&DiagTime.N22 ,20);
+    sfmd_flg_chrgMosClosFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, ihd_flg_chrgMosClosFlt ,&DiagTime.N22 ,2);
 
 	
     //24 放电Mos失效故障
-    sfmd_flg_disChrgMosClosFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, ihd_flg_disChrgMosClosFlt ,&DiagTime.N24 ,20);
+    sfmd_flg_disChrgMosClosFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, ihd_flg_disChrgMosClosFlt ,&DiagTime.N24 ,2);
     
     
     //
@@ -259,7 +289,7 @@ void SFM(void)
         }
         Time1Cntl ++;
         SumT1 = SumT1 + ihd_T_heatPanT1;
-        if(Time1Cntl >= 600)
+        if(Time1Cntl >= 60)
         {
             for(i = 0; i < 9;i++)
             {
@@ -283,7 +313,7 @@ void SFM(void)
         }
         Time2Cntl ++;
         SumT2 = SumT2 + ihd_T_heatPanT2;
-        if(Time2Cntl >= 600)
+        if(Time2Cntl >= 60)
         {
             for(i = 0; i < 9;i++)
             {
@@ -296,7 +326,7 @@ void SFM(void)
     }
 
 	//32 加热回路常闭故障
-    Enable1 = JudgeTimeSystem(1, tmsd_st_heatAct == 0,&DiagTime.N32, 6000);
+    Enable1 = JudgeTimeSystem(1, tmsd_st_heatAct == 0,&DiagTime.N32, 600);
     sfmd_flg_heatCirClosFltEo = ( ((int16_T)(heatT2Arr[9] - heatT2Arr[0]) > 5 || (int16_T)(heatT1Arr[9] - heatT1Arr[0]) > 5) && Enable1) || sfmd_flg_heatCirClosFltEo || sfmd_flg_heatCirClosFltEi;
 
 	//33 加热回路常开故障
@@ -306,16 +336,16 @@ void SFM(void)
     //34 主回路常闭故障   //(充电MOS及放电MOS，主继电器断开  使能)
     sfmd_I_curr = ihd_I_curr;
     sfmd_I_currAbs = (uint16_T) (sfmd_I_curr > 0 ? sfmd_I_curr : -sfmd_I_curr);
-    sfmd_flg_mainCirClosFltEo = JudgeTimeSystem(!ihd_st_chrgMosControl && !ihd_st_disChrgMosControl && !ihd_st_relayControl, sfmd_I_currAbs > 5,&DiagTime.N34, 50) || sfmd_flg_mainCirClosFltEo || sfmd_flg_mainCirClosFltEi ;
+    sfmd_flg_mainCirClosFltEo = JudgeTimeSystem(!ihd_st_chrgMosControl && !ihd_st_disChrgMosControl && !ihd_st_relayControl, sfmd_I_currAbs > 5,&DiagTime.N34, 5) || sfmd_flg_mainCirClosFltEo || sfmd_flg_mainCirClosFltEi ;
     
     
     //52 电压开路故障
     Cntl = Cntl + 1;
-    if(!sfmd_flg_interComFlt && Cntl > 9)
+    if(!sfmd_flg_interComFlt && Cntl >= 0)
     {
         Cntl = 0;
         // 矩阵更新
-        for(j = 0;j < 3 && FirstRun_SFM;j++)
+        for(j = 0;j < 4 && FirstRun_SFM;j++)
         {
             for(i = 0;i < cmnc_num_cellUNum;i++)
             {
@@ -323,7 +353,7 @@ void SFM(void)
             }
         }
         
-        for(j = 0;j < 2;j++)
+        for(j = 0;j < 3;j++)
         {
             for(i = 0;i < cmnc_num_cellUNum;i++)
             {
@@ -337,7 +367,7 @@ void SFM(void)
         // 故障判断
         for(i = 0;i < cmnc_num_cellUNum - 1;i++)
         {
-            if((int16_T)(CellUArry[2][i] - CellUArry[0][i]) < -50 && (int16_T)(CellUArry[2][i+1] - CellUArry[0][i+1]) > 50)
+            if((int16_T)(CellUArry[3][i] - CellUArry[1][i]) < -50 && (int16_T)(CellUArry[3][i+1] - CellUArry[1][i+1]) > 50)
             {
                 ErrNr[i] = ErrNr[i] + 1;
             }
@@ -345,7 +375,7 @@ void SFM(void)
             {
                 ErrNr[i] = 0;
             }
-            if ( ErrNr[i] >= 2|| ((int16_T)(CellUArry[2][i] - CellUArry[0][i]) < -1000 && (int16_T)(CellUArry[2][i+1] - CellUArry[0][i+1]) > 1000 ))
+            if ( ErrNr[i] >= 2|| ((int16_T)(CellUArry[3][i] - CellUArry[2][i]) < -1000 && (int16_T)(CellUArry[3][i+1] - CellUArry[2][i+1]) > 1000 ))
             {
                 sfmv_flg_cellUOpenFlt[i] = true;
                 ErrUFlg[i] = true;
@@ -355,7 +385,7 @@ void SFM(void)
             {
                 for(k = 0;k < i;k++)
                 {
-                    if((int16_T)(CellUArry[2][i-k] - CellUArry[1][i-k]) < -200)
+                    if((int16_T)(CellUArry[3][i-k] - CellUArry[2][i-k]) < -200)
                     {
                         sfmv_flg_cellUOpenFlt[i-k] = true;
                         ErrUFlg[i-k] = true;
@@ -363,6 +393,7 @@ void SFM(void)
                 }
             }
         }
+		
     }
     //
     if(!sfmd_flg_interComFlt)
@@ -387,7 +418,7 @@ void SFM(void)
                 }
             }
         }
-        if(sfmd_V_cellUMax - sfmd_V_cellUMin > 3000)
+        if(sfmd_V_cellUMax - sfmd_V_cellUMin > 5000)  // 2021 10.24
         {
             sfmd_V_cellUAvrg = (SumU - sfmd_V_cellUMax - sfmd_V_cellUMin)/(UNum - 2);
             for(i = 0;i < cmnc_num_cellUNum ;i++)
@@ -421,7 +452,7 @@ void SFM(void)
                 {
                     RecNr[i] = 0;
                 }
-                if( RecNr[i] > 20)
+                if( RecNr[i] >= 2)
                 {
                     sfmv_flg_cellUOpenFlt[i] = false;
                     ErrUFlg[i] = false;
@@ -462,7 +493,7 @@ void SFM(void)
         {
             modTOpenNr[i] = 0;
         }
-        if(modTOpenNr[i] >= 20)
+        if(modTOpenNr[i] >= 2)
         {
             sfmv_flg_modTOpenFlt[i] = true;
         }
@@ -475,16 +506,16 @@ void SFM(void)
     }
     
     //54 Mos温度开路故障
-    sfmd_flg_chrgMosTOpenFlt =  JudgeTimeSystem(!sfmd_flg_interComFlt, ihd_T_mosT == 10, &DiagTime.N54, 20);
+    sfmd_flg_chrgMosTOpenFlt =  JudgeTimeSystem(!sfmd_flg_interComFlt, ihd_T_mosT == 10, &DiagTime.N54, 2);
     
     //56 快充插头温度开路故障
-    sfmd_flg_DCPlugTOpenFlt  = JudgeTimeSystem(!sfmd_flg_interComFlt, ihd_T_DCPlugT == 0, &DiagTime.N56, 20);
+    sfmd_flg_DCPlugTOpenFlt  = JudgeTimeSystem(!sfmd_flg_interComFlt, ihd_T_DCPlugT == 0, &DiagTime.N56, 2);
     //57 慢充插头温度开路故障
-    sfmd_flg_ACPlugTOpenFlt  = JudgeTimeSystem(!sfmd_flg_interComFlt, ihd_T_ACPlugT == 0, &DiagTime.N57, 20);
+    sfmd_flg_ACPlugTOpenFlt  = JudgeTimeSystem(!sfmd_flg_interComFlt, ihd_T_ACPlugT == 0, &DiagTime.N57, 2);
     //58 加热板#1温度开路故障
-    sfmd_flg_heatPanT1OpenFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, ihd_T_heatPanT1 == 0, &DiagTime.N58, 20);
+    sfmd_flg_heatPanT1OpenFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, ihd_T_heatPanT1 == 0, &DiagTime.N58, 2);
     //59 加热板#2温度开路故障
-    sfmd_flg_heatPanT2OpenFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, ihd_T_heatPanT2 == 0, &DiagTime.N59, 20);
+    sfmd_flg_heatPanT2OpenFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, ihd_T_heatPanT2 == 0, &DiagTime.N59, 2);
     
     
     //61 模组温度合理性故障
@@ -504,7 +535,7 @@ void SFM(void)
             modTRationNr[i] = 0;
             modTArr[i] = ihv_T_modT[i];
         }
-        if(modTRationNr[i] >= 20)
+        if(modTRationNr[i] >= 2)
         {
             sfmv_flg_modTRationFlt[i] = true;
         }
@@ -523,7 +554,7 @@ void SFM(void)
     {
         ihd_T_mosT_Delay = ihd_T_mosT;
     }
-    sfmd_flg_chrgMosTRationFlt = JudgeTimeSystem(1, sfmd_flg_chrgMosTRationFltOnce, &DiagTime.N62, 20);
+    sfmd_flg_chrgMosTRationFlt = JudgeTimeSystem(1, sfmd_flg_chrgMosTRationFltOnce, &DiagTime.N62, 2);
     
     //64 快充插头温度合理性故障
     sfmd_flg_DCPlugTRationFltOnce = !sfmd_flg_interComFlt && !sfmd_flg_DCPlugTOpenFlt && ((int16_T)(ihd_T_DCPlugT - ihd_T_DCPlugT_Delay) < -10);
@@ -531,7 +562,7 @@ void SFM(void)
     {
         ihd_T_DCPlugT_Delay = ihd_T_DCPlugT;
     }
-    sfmd_flg_DCPlugTRationFlt = JudgeTimeSystem(1, sfmd_flg_DCPlugTRationFltOnce, &DiagTime.N64, 20);
+    sfmd_flg_DCPlugTRationFlt = JudgeTimeSystem(1, sfmd_flg_DCPlugTRationFltOnce, &DiagTime.N64, 2);
     
     //65 慢插头温度合理性故障
     sfmd_flg_ACPlugTRationFltOnce = !sfmd_flg_interComFlt && !sfmd_flg_ACPlugTOpenFlt && ((int16_T)(ihd_T_ACPlugT - ihd_T_ACPlugT_Delay) < -10);
@@ -539,7 +570,7 @@ void SFM(void)
     {
         ihd_T_ACPlugT_Delay = ihd_T_ACPlugT;
     }
-    sfmd_flg_ACPlugTRationFlt = JudgeTimeSystem(1, sfmd_flg_ACPlugTRationFltOnce, &DiagTime.N65, 20);
+    sfmd_flg_ACPlugTRationFlt = JudgeTimeSystem(1, sfmd_flg_ACPlugTRationFltOnce, &DiagTime.N65, 2);
     
     //66 加热板#1 温度合理性故障
     sfmd_flg_heatPanT1RationFltOnce = !sfmd_flg_interComFlt && !sfmd_flg_heatPanT1OpenFlt && ((int16_T)(ihd_T_heatPanT1 - ihd_T_heatPanT1_Delay) < -10);
@@ -547,7 +578,7 @@ void SFM(void)
     {
         ihd_T_heatPanT1_Delay = ihd_T_heatPanT1;
     }
-    sfmd_flg_heatPanT1RationFlt = JudgeTimeSystem(1, sfmd_flg_heatPanT1RationFltOnce, &DiagTime.N66, 20);
+    sfmd_flg_heatPanT1RationFlt = JudgeTimeSystem(1, sfmd_flg_heatPanT1RationFltOnce, &DiagTime.N66, 2);
     
     //67 加热板#2 温度合理性故障
     sfmd_flg_heatPanT2RationFltOnce = !sfmd_flg_interComFlt && !sfmd_flg_heatPanT2OpenFlt && ((int16_T)(ihd_T_heatPanT2 -ihd_T_heatPanT2_Delay) < -10);
@@ -555,7 +586,7 @@ void SFM(void)
     {
         ihd_T_heatPanT2_Delay = ihd_T_heatPanT2;
     }
-    sfmd_flg_heatPanT2RationFlt = JudgeTimeSystem(1, sfmd_flg_heatPanT2RationFltOnce, &DiagTime.N67, 20);
+    sfmd_flg_heatPanT2RationFlt = JudgeTimeSystem(1, sfmd_flg_heatPanT2RationFltOnce, &DiagTime.N67, 2);
     
     //68 模组温度全部不可用
     sfmd_flg_modTAllFlt = true;
@@ -607,121 +638,120 @@ void SFM(void)
     sfmd_flg_currOpenFlt = false;
     sfmd_I_curr =  ihd_I_curr;
     //81 单体过压故障2级
-    sfmd_flg_cellUOverFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt,sfmd_V_cellUMax,sfmc_V_cellUOverThrFlt2,sfmc_V_cellUOverThrRec2,20,20,&DiagThr.fltNum81,&DiagThr.recNum81,&sfmd_flg_cellUOverFlt2);
+    sfmd_flg_cellUOverFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt,sfmd_V_cellUMax,sfmc_V_cellUOverThrFlt2,sfmc_V_cellUOverThrRec2,2,2,&DiagThr.fltNum81,&DiagThr.recNum81,&sfmd_flg_cellUOverFlt2);
     //82 单体过压故障1级
-    sfmd_flg_cellUOverFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt,sfmd_V_cellUMax,sfmc_V_cellUOverThrFlt1,sfmc_V_cellUOverThrRec1,20,20,&DiagThr.fltNum82,&DiagThr.recNum82,&sfmd_flg_cellUOverFlt1) && !sfmd_flg_cellUOverFlt2;
+    sfmd_flg_cellUOverFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt,sfmd_V_cellUMax,sfmc_V_cellUOverThrFlt1,sfmc_V_cellUOverThrRec1,2,2,&DiagThr.fltNum82,&DiagThr.recNum82,&sfmd_flg_cellUOverFlt1) && !sfmd_flg_cellUOverFlt2;
     
     //83 单体欠压故障2级
-    sfmd_flg_cellULowFlt2 = DiagThrSystem2(1,!sfmd_flg_interComFlt,sfmd_V_cellUMin,sfmc_V_cellULowThrFlt2,sfmc_V_cellULowThrRec2,20,20,&DiagThr.fltNum83,&DiagThr.recNum83,&sfmd_flg_cellULowFlt2);
+    sfmd_flg_cellULowFlt2 = DiagThrSystem2(1,!sfmd_flg_interComFlt,sfmd_V_cellUMin,sfmc_V_cellULowThrFlt2,sfmc_V_cellULowThrRec2,2,2,&DiagThr.fltNum83,&DiagThr.recNum83,&sfmd_flg_cellULowFlt2);
     //84 单体欠压故障1级
-    sfmd_flg_cellULowFlt1 = DiagThrSystem2(0,!sfmd_flg_interComFlt,sfmd_V_cellUMin,sfmc_V_cellULowThrFlt1,sfmc_V_cellULowThrRec1,20,20,&DiagThr.fltNum84,&DiagThr.recNum84,&sfmd_flg_cellULowFlt1) && !sfmd_flg_cellULowFlt2;
+    sfmd_flg_cellULowFlt1 = DiagThrSystem2(0,!sfmd_flg_interComFlt,sfmd_V_cellUMin,sfmc_V_cellULowThrFlt1,sfmc_V_cellULowThrRec1,2,2,&DiagThr.fltNum84,&DiagThr.recNum84,&sfmd_flg_cellULowFlt1) && !sfmd_flg_cellULowFlt2;
     
     //85 压差过大2级
-    sfmd_flg_cellUDiffFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt,sfmd_V_cellUMax - sfmd_V_cellUMin,sfmc_flg_cellUDiffThrFlt2,sfmc_flg_cellUDiffThrRec2,20,20,&DiagThr.fltNum85,&DiagThr.recNum85,&sfmd_flg_cellUDiffFlt2);
+    sfmd_flg_cellUDiffFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt,sfmd_V_cellUMax - sfmd_V_cellUMin,sfmc_flg_cellUDiffThrFlt2,sfmc_flg_cellUDiffThrRec2,2,2,&DiagThr.fltNum85,&DiagThr.recNum85,&sfmd_flg_cellUDiffFlt2);
     //86 压差过大1级
-    sfmd_flg_cellUDiffFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt,sfmd_V_cellUMax - sfmd_V_cellUMin,sfmc_flg_cellUDiffThrFlt1,sfmc_flg_cellUDiffThrRec1,20,20,&DiagThr.fltNum86,&DiagThr.recNum86,&sfmd_flg_cellUDiffFlt1) && !sfmd_flg_cellUDiffFlt2;
+    sfmd_flg_cellUDiffFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt,sfmd_V_cellUMax - sfmd_V_cellUMin,sfmc_flg_cellUDiffThrFlt1,sfmc_flg_cellUDiffThrRec1,2,2,&DiagThr.fltNum86,&DiagThr.recNum86,&sfmd_flg_cellUDiffFlt1) && !sfmd_flg_cellUDiffFlt2;
     
     //87 总压过压2级
-    sfmd_flg_battUOverFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt,sfmd_V_battU,sfmc_V_battUOverThrFlt2,sfmc_V_battUOverThrRec2,20,20,&DiagThr.fltNum87,&DiagThr.recNum87,&sfmd_flg_battUOverFlt2);
+    sfmd_flg_battUOverFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt,sfmd_V_battU,sfmc_V_battUOverThrFlt2,sfmc_V_battUOverThrRec2,2,2,&DiagThr.fltNum87,&DiagThr.recNum87,&sfmd_flg_battUOverFlt2);
     //88 总压过压1级
-    sfmd_flg_battUOverFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt,sfmd_V_battU,sfmc_V_battUOverThrFlt1,sfmc_V_battUOverThrRec1,20,20,&DiagThr.fltNum88,&DiagThr.recNum88,&sfmd_flg_battUOverFlt1) && !sfmd_flg_battUOverFlt2;
+    sfmd_flg_battUOverFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt,sfmd_V_battU,sfmc_V_battUOverThrFlt1,sfmc_V_battUOverThrRec1,2,2,&DiagThr.fltNum88,&DiagThr.recNum88,&sfmd_flg_battUOverFlt1) && !sfmd_flg_battUOverFlt2;
     
     //89 总压欠压2级
-    sfmd_flg_battULowFlt2 = DiagThrSystem2(1,!sfmd_flg_interComFlt,sfmd_V_battU,sfmc_V_battULowThrFlt2,sfmc_V_battULowThrRec2,20,20,&DiagThr.fltNum89,&DiagThr.recNum89,&sfmd_flg_battULowFlt2);
+    sfmd_flg_battULowFlt2 = DiagThrSystem2(1,!sfmd_flg_interComFlt,sfmd_V_battU,sfmc_V_battULowThrFlt2,sfmc_V_battULowThrRec2,2,2,&DiagThr.fltNum89,&DiagThr.recNum89,&sfmd_flg_battULowFlt2);
     //90 总压欠压1级
-    sfmd_flg_battULowFlt1 = DiagThrSystem2(0,!sfmd_flg_interComFlt,sfmd_V_battU,sfmc_V_battULowThrFlt1,sfmc_V_battULowThrRec1,20,20,&DiagThr.fltNum90,&DiagThr.recNum90,&sfmd_flg_battULowFlt1) && !sfmd_flg_battULowFlt2;
+    sfmd_flg_battULowFlt1 = DiagThrSystem2(0,!sfmd_flg_interComFlt,sfmd_V_battU,sfmc_V_battULowThrFlt1,sfmc_V_battULowThrRec1,2,2,&DiagThr.fltNum90,&DiagThr.recNum90,&sfmd_flg_battULowFlt1) && !sfmd_flg_battULowFlt2;
     
     chrgFlg = ihd_st_workStat == 2;
     disChrgFlg = ihd_st_workStat != 2;
 	
     //97 模组充电温度过高2级故障
-    sfmd_flg_chrgModTOverFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt&&chrgFlg && !sfmd_flg_modTAllFlt,sfmd_T_modTMax,sfmc_T_chrgModTOverThrFlt2,sfmc_T_chrgModTOverThrRec2,25,25,&DiagThr.fltNum97,&DiagThr.recNum97,&sfmd_flg_chrgModTOverFlt2);
+    sfmd_flg_chrgModTOverFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt&&chrgFlg && !sfmd_flg_modTAllFlt,sfmd_T_modTMax,sfmc_T_chrgModTOverThrFlt2,sfmc_T_chrgModTOverThrRec2,3,3,&DiagThr.fltNum97,&DiagThr.recNum97,&sfmd_flg_chrgModTOverFlt2);
     //98 模组充电温度过高1级故障
-    sfmd_flg_chrgModTOverFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt&&chrgFlg && !sfmd_flg_modTAllFlt,sfmd_T_modTMax,sfmc_T_chrgModTOverThrFlt1,sfmc_T_chrgModTOverThrRec1,25,25,&DiagThr.fltNum98,&DiagThr.recNum98,&sfmd_flg_chrgModTOverFlt1) && !sfmd_flg_chrgModTOverFlt2;
+    sfmd_flg_chrgModTOverFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt&&chrgFlg && !sfmd_flg_modTAllFlt,sfmd_T_modTMax,sfmc_T_chrgModTOverThrFlt1,sfmc_T_chrgModTOverThrRec1,3,3,&DiagThr.fltNum98,&DiagThr.recNum98,&sfmd_flg_chrgModTOverFlt1) && !sfmd_flg_chrgModTOverFlt2;
     //99 模组充电温度过低2级故障
-    sfmd_flg_chrgModTLowFlt2 = DiagThrSystem2(0,!sfmd_flg_interComFlt&&chrgFlg && !sfmd_flg_modTAllFlt,sfmd_T_modTMin,sfmc_T_chrgModTLowThrFlt2,sfmc_T_chrgModTLowThrRec2,25,25,&DiagThr.fltNum99,&DiagThr.recNum99,&sfmd_flg_chrgModTLowFlt2);
+    sfmd_flg_chrgModTLowFlt2 = DiagThrSystem2(0,!sfmd_flg_interComFlt&&chrgFlg && !sfmd_flg_modTAllFlt,sfmd_T_modTMin,sfmc_T_chrgModTLowThrFlt2,sfmc_T_chrgModTLowThrRec2,3,3,&DiagThr.fltNum99,&DiagThr.recNum99,&sfmd_flg_chrgModTLowFlt2);
     //100 模组充电温度过低1级故障
-    sfmd_flg_chrgModTLowFlt1 = DiagThrSystem2(0,!sfmd_flg_interComFlt&&chrgFlg && !sfmd_flg_modTAllFlt,sfmd_T_modTMin,sfmc_T_chrgModTLowThrFlt1,sfmc_T_chrgModTLowThrRec1,25,25,&DiagThr.fltNum100,&DiagThr.recNum100,&sfmd_flg_chrgModTLowFlt1) && !sfmd_flg_chrgModTLowFlt2;
+    sfmd_flg_chrgModTLowFlt1 = DiagThrSystem2(0,!sfmd_flg_interComFlt&&chrgFlg && !sfmd_flg_modTAllFlt,sfmd_T_modTMin,sfmc_T_chrgModTLowThrFlt1,sfmc_T_chrgModTLowThrRec1,3,3,&DiagThr.fltNum100,&DiagThr.recNum100,&sfmd_flg_chrgModTLowFlt1) && !sfmd_flg_chrgModTLowFlt2;
     
     //101 模组放电温度过高2级故障
-    sfmd_flg_disChrgModTOverFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt&&disChrgFlg && !sfmd_flg_modTAllFlt,sfmd_T_modTMax,sfmc_T_disChrgModTOverThrFlt2,sfmc_T_disChrgModTOverThrRec2,25,25,&DiagThr.fltNum101,&DiagThr.recNum101,&sfmd_flg_disChrgModTOverFlt2);
+    sfmd_flg_disChrgModTOverFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt&&disChrgFlg && !sfmd_flg_modTAllFlt,sfmd_T_modTMax,sfmc_T_disChrgModTOverThrFlt2,sfmc_T_disChrgModTOverThrRec2,3,3,&DiagThr.fltNum101,&DiagThr.recNum101,&sfmd_flg_disChrgModTOverFlt2);
     //102 模组放电温度过高1级故障
-    sfmd_flg_disChrgModTOverFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt&&disChrgFlg && !sfmd_flg_modTAllFlt,sfmd_T_modTMax,sfmc_T_disChrgModTOverThrFlt1,sfmc_T_disChrgModTOverThrRec1,25,25,&DiagThr.fltNum102,&DiagThr.recNum102,&sfmd_flg_disChrgModTOverFlt1) && !sfmd_flg_disChrgModTOverFlt2;
+    sfmd_flg_disChrgModTOverFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt&&disChrgFlg && !sfmd_flg_modTAllFlt,sfmd_T_modTMax,sfmc_T_disChrgModTOverThrFlt1,sfmc_T_disChrgModTOverThrRec1,3,3,&DiagThr.fltNum102,&DiagThr.recNum102,&sfmd_flg_disChrgModTOverFlt1) && !sfmd_flg_disChrgModTOverFlt2;
     //103 模组放电温度过低2级故障
-    sfmd_flg_disChrgModTLowFlt2 = DiagThrSystem2(0,!sfmd_flg_interComFlt&&disChrgFlg && !sfmd_flg_modTAllFlt,sfmd_T_modTMin,sfmc_T_disChrgModTLowThrFlt2,sfmc_T_disChrgModTLowThrRec2,25,25,&DiagThr.fltNum103,&DiagThr.recNum103,&sfmd_flg_disChrgModTLowFlt2);
+    sfmd_flg_disChrgModTLowFlt2 = DiagThrSystem2(0,!sfmd_flg_interComFlt&&disChrgFlg && !sfmd_flg_modTAllFlt,sfmd_T_modTMin,sfmc_T_disChrgModTLowThrFlt2,sfmc_T_disChrgModTLowThrRec2,3,3,&DiagThr.fltNum103,&DiagThr.recNum103,&sfmd_flg_disChrgModTLowFlt2);
     //104 模组放电温度过低1级故障
-    sfmd_flg_disChrgModTLowFlt1 = DiagThrSystem2(0,!sfmd_flg_interComFlt&&disChrgFlg && !sfmd_flg_modTAllFlt,sfmd_T_modTMin,sfmc_T_disChrgModTLowThrFlt1,sfmc_T_disChrgModTLowThrRec1,25,25,&DiagThr.fltNum104,&DiagThr.recNum104,&sfmd_flg_disChrgModTLowFlt1) && !sfmd_flg_disChrgModTLowFlt2;
+    sfmd_flg_disChrgModTLowFlt1 = DiagThrSystem2(0,!sfmd_flg_interComFlt&&disChrgFlg && !sfmd_flg_modTAllFlt,sfmd_T_modTMin,sfmc_T_disChrgModTLowThrFlt1,sfmc_T_disChrgModTLowThrRec1,3,3,&DiagThr.fltNum104,&DiagThr.recNum104,&sfmd_flg_disChrgModTLowFlt1) && !sfmd_flg_disChrgModTLowFlt2;
     
     //
     //105 模组温差2级故障
-    sfmd_flg_modTDiffFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt && !sfmd_flg_modTAllFlt,sfmd_T_modTMax - sfmd_T_modTMin,sfmc_T_modTDiffThrFlt2,sfmc_T_modTDiffThrRec2,25,25,&DiagThr.fltNum105,&DiagThr.recNum105,&sfmd_flg_modTDiffFlt2);
+    sfmd_flg_modTDiffFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt && !sfmd_flg_modTAllFlt,sfmd_T_modTMax - sfmd_T_modTMin,sfmc_T_modTDiffThrFlt2,sfmc_T_modTDiffThrRec2,3,3,&DiagThr.fltNum105,&DiagThr.recNum105,&sfmd_flg_modTDiffFlt2);
     //106 模组温差1级故障
-    sfmd_flg_modTDiffFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt && !sfmd_flg_modTAllFlt,sfmd_T_modTMax - sfmd_T_modTMin,sfmc_T_modTDiffThrFlt1,sfmc_T_modTDiffThrRec1,25,25,&DiagThr.fltNum106,&DiagThr.recNum106,&sfmd_flg_modTDiffFlt1) && !sfmd_flg_modTDiffFlt2;
+    sfmd_flg_modTDiffFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt && !sfmd_flg_modTAllFlt,sfmd_T_modTMax - sfmd_T_modTMin,sfmc_T_modTDiffThrFlt1,sfmc_T_modTDiffThrRec1,3,3,&DiagThr.fltNum106,&DiagThr.recNum106,&sfmd_flg_modTDiffFlt1) && !sfmd_flg_modTDiffFlt2;
     
     
     //107 充电Mos温度过高2级故障
-    sfmd_flg_chrgMosTOverFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt && chrgFlg && sfmd_flg_chrgMosTRationFlt && sfmd_flg_chrgMosTOpenFlt,ihd_T_mosT,sfmc_T_chrgMosTOverThrFlt2,sfmc_T_chrgMosTOverThrRec2,25,25,&DiagThr.fltNum107,&DiagThr.recNum107,&sfmd_flg_chrgMosTOverFlt2);
+    sfmd_flg_chrgMosTOverFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt && chrgFlg && sfmd_flg_chrgMosTRationFlt && sfmd_flg_chrgMosTOpenFlt,ihd_T_mosT,sfmc_T_chrgMosTOverThrFlt2,sfmc_T_chrgMosTOverThrRec2,3,3,&DiagThr.fltNum107,&DiagThr.recNum107,&sfmd_flg_chrgMosTOverFlt2);
     //108 充电Mos温度过高1级故障
-    sfmd_flg_chrgMosTOverFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt && chrgFlg && sfmd_flg_chrgMosTRationFlt && sfmd_flg_chrgMosTOpenFlt,ihd_T_mosT,sfmc_T_chrgMosTOverThrFlt1,sfmc_T_chrgMosTOverThrRec1,25,25,&DiagThr.fltNum108,&DiagThr.recNum108,&sfmd_flg_chrgMosTOverFlt1) && !sfmd_flg_chrgMosTOverFlt2;
+    sfmd_flg_chrgMosTOverFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt && chrgFlg && sfmd_flg_chrgMosTRationFlt && sfmd_flg_chrgMosTOpenFlt,ihd_T_mosT,sfmc_T_chrgMosTOverThrFlt1,sfmc_T_chrgMosTOverThrRec1,3,3,&DiagThr.fltNum108,&DiagThr.recNum108,&sfmd_flg_chrgMosTOverFlt1) && !sfmd_flg_chrgMosTOverFlt2;
     //109 放电Mos温度过高2级故障
-    sfmd_flg_disChrgMosTOverFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt && disChrgFlg && sfmd_flg_chrgMosTRationFlt && sfmd_flg_chrgMosTOpenFlt,ihd_T_mosT,sfmc_T_disChrgMosTOverThrFlt2,sfmc_T_disChrgMosTOverThrRec2,25,25,&DiagThr.fltNum109,&DiagThr.recNum109,&sfmd_flg_disChrgMosTOverFlt2);
+    sfmd_flg_disChrgMosTOverFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt && disChrgFlg && sfmd_flg_chrgMosTRationFlt && sfmd_flg_chrgMosTOpenFlt,ihd_T_mosT,sfmc_T_disChrgMosTOverThrFlt2,sfmc_T_disChrgMosTOverThrRec2,3,3,&DiagThr.fltNum109,&DiagThr.recNum109,&sfmd_flg_disChrgMosTOverFlt2);
     //110 放电Mos温度过高1级故障
-    sfmd_flg_disChrgMosTOverFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt && disChrgFlg && sfmd_flg_chrgMosTRationFlt && sfmd_flg_chrgMosTOpenFlt,ihd_T_mosT,sfmc_T_disChrgMosTOverThrFlt1,sfmc_T_disChrgMosTOverThrRec1,25,25,&DiagThr.fltNum110,&DiagThr.recNum110,&sfmd_flg_disChrgMosTOverFlt1) && !sfmd_flg_disChrgMosTOverFlt2;
+    sfmd_flg_disChrgMosTOverFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt && disChrgFlg && sfmd_flg_chrgMosTRationFlt && sfmd_flg_chrgMosTOpenFlt,ihd_T_mosT,sfmc_T_disChrgMosTOverThrFlt1,sfmc_T_disChrgMosTOverThrRec1,3,3,&DiagThr.fltNum110,&DiagThr.recNum110,&sfmd_flg_disChrgMosTOverFlt1) && !sfmd_flg_disChrgMosTOverFlt2;
     
     
     //111 快充插头温度过高2级故障
-    sfmd_flg_DCPlugTOverFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt && sfmd_flg_DCPlugTRationFlt && sfmd_flg_DCPlugTOpenFlt,ihd_T_DCPlugT,sfmc_T_DCPlugTOverThrFlt2,sfmc_T_DCPlugTOverThrRec2,25,25,&DiagThr.fltNum111,&DiagThr.recNum111,&sfmd_flg_DCPlugTOverFlt2);
+    sfmd_flg_DCPlugTOverFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt && sfmd_flg_DCPlugTRationFlt && sfmd_flg_DCPlugTOpenFlt,ihd_T_DCPlugT,sfmc_T_DCPlugTOverThrFlt2,sfmc_T_DCPlugTOverThrRec2,3,3,&DiagThr.fltNum111,&DiagThr.recNum111,&sfmd_flg_DCPlugTOverFlt2);
     //112 快充插头温度过高1级故障
-    sfmd_flg_DCPlugTOverFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt && sfmd_flg_DCPlugTRationFlt && sfmd_flg_DCPlugTOpenFlt,ihd_T_DCPlugT,sfmc_T_DCPlugTOverThrFlt1,sfmc_T_DCPlugTOverThrRec1,25,25,&DiagThr.fltNum112,&DiagThr.recNum112,&sfmd_flg_DCPlugTOverFlt1) && !sfmd_flg_DCPlugTOverFlt2;
+    sfmd_flg_DCPlugTOverFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt && sfmd_flg_DCPlugTRationFlt && sfmd_flg_DCPlugTOpenFlt,ihd_T_DCPlugT,sfmc_T_DCPlugTOverThrFlt1,sfmc_T_DCPlugTOverThrRec1,3,3,&DiagThr.fltNum112,&DiagThr.recNum112,&sfmd_flg_DCPlugTOverFlt1) && !sfmd_flg_DCPlugTOverFlt2;
     //113 慢充插头温度过高2级故障
-    sfmd_flg_ACPlugTOverFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt && sfmd_flg_ACPlugTRationFlt && sfmd_flg_ACPlugTOpenFlt,ihd_T_ACPlugT,sfmc_T_ACPlugTOverThrFlt2,sfmc_T_ACPlugTOverThrRec2,25,25,&DiagThr.fltNum113,&DiagThr.recNum113,&sfmd_flg_ACPlugTOverFlt2);
+    sfmd_flg_ACPlugTOverFlt2 = DiagThrSystem1(1,!sfmd_flg_interComFlt && sfmd_flg_ACPlugTRationFlt && sfmd_flg_ACPlugTOpenFlt,ihd_T_ACPlugT,sfmc_T_ACPlugTOverThrFlt2,sfmc_T_ACPlugTOverThrRec2,3,3,&DiagThr.fltNum113,&DiagThr.recNum113,&sfmd_flg_ACPlugTOverFlt2);
     //114 慢充插头温度过高1级故障
-    sfmd_flg_ACPlugTOverFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt && sfmd_flg_ACPlugTRationFlt && sfmd_flg_ACPlugTOpenFlt,ihd_T_ACPlugT,sfmc_T_ACPlugTOverThrFlt1,sfmc_T_ACPlugTOverThrRec1,25,25,&DiagThr.fltNum114,&DiagThr.recNum114,&sfmd_flg_ACPlugTOverFlt1) && !sfmd_flg_ACPlugTOverFlt2;
+    sfmd_flg_ACPlugTOverFlt1 = DiagThrSystem1(0,!sfmd_flg_interComFlt && sfmd_flg_ACPlugTRationFlt && sfmd_flg_ACPlugTOpenFlt,ihd_T_ACPlugT,sfmc_T_ACPlugTOverThrFlt1,sfmc_T_ACPlugTOverThrRec1,3,3,&DiagThr.fltNum114,&DiagThr.recNum114,&sfmd_flg_ACPlugTOverFlt1) && !sfmd_flg_ACPlugTOverFlt2;
     
     //115 加热板温度过高2级故障
-    sfmd_flg_heatPanTOverFlt2_1 = DiagThrSystem1(1,!sfmd_flg_interComFlt && sfmd_flg_heatPanT1RationFlt && sfmd_flg_heatPanT1OpenFlt,ihd_T_heatPanT1,sfmc_T_heatPanTOverThrFlt2,sfmc_T_heatPanTOverThrRec2,25,25,&DiagThr.fltNum115_1,&DiagThr.recNum115_1,&sfmd_flg_heatPanTOverFlt2_1);
-    sfmd_flg_heatPanTOverFlt2_2 = DiagThrSystem1(1,!sfmd_flg_interComFlt && sfmd_flg_heatPanT2RationFlt && sfmd_flg_heatPanT2OpenFlt,ihd_T_heatPanT2,sfmc_T_heatPanTOverThrFlt2,sfmc_T_heatPanTOverThrRec2,25,25,&DiagThr.fltNum115_2,&DiagThr.recNum115_2,&sfmd_flg_heatPanTOverFlt2_2);
+    sfmd_flg_heatPanTOverFlt2_1 = DiagThrSystem1(1,!sfmd_flg_interComFlt && sfmd_flg_heatPanT1RationFlt && sfmd_flg_heatPanT1OpenFlt,ihd_T_heatPanT1,sfmc_T_heatPanTOverThrFlt2,sfmc_T_heatPanTOverThrRec2,3,3,&DiagThr.fltNum115_1,&DiagThr.recNum115_1,&sfmd_flg_heatPanTOverFlt2_1);
+    sfmd_flg_heatPanTOverFlt2_2 = DiagThrSystem1(1,!sfmd_flg_interComFlt && sfmd_flg_heatPanT2RationFlt && sfmd_flg_heatPanT2OpenFlt,ihd_T_heatPanT2,sfmc_T_heatPanTOverThrFlt2,sfmc_T_heatPanTOverThrRec2,3,3,&DiagThr.fltNum115_2,&DiagThr.recNum115_2,&sfmd_flg_heatPanTOverFlt2_2);
     sfmd_flg_heatPanTOverFlt2 = sfmd_flg_heatPanTOverFlt2_1 || sfmd_flg_heatPanTOverFlt2_2;
     //116 加热板温度过高1级故障
-    sfmd_flg_heatPanTOverFlt1_1 = DiagThrSystem1(0,!sfmd_flg_interComFlt && sfmd_flg_heatPanT1RationFlt && sfmd_flg_heatPanT1OpenFlt,ihd_T_heatPanT1,sfmc_T_heatPanTOverThrFlt1,sfmc_T_heatPanTOverThrRec1,25,25,&DiagThr.fltNum116_1,&DiagThr.recNum116_1,&sfmd_flg_heatPanTOverFlt1_1) && !sfmd_flg_heatPanTOverFlt2;
-    sfmd_flg_heatPanTOverFlt1_2 = DiagThrSystem1(0,!sfmd_flg_interComFlt && sfmd_flg_heatPanT2RationFlt && sfmd_flg_heatPanT2OpenFlt,ihd_T_heatPanT2,sfmc_T_heatPanTOverThrFlt1,sfmc_T_heatPanTOverThrRec1,25,25,&DiagThr.fltNum116_2,&DiagThr.recNum116_2,&sfmd_flg_heatPanTOverFlt1_2) && !sfmd_flg_heatPanTOverFlt2;
+    sfmd_flg_heatPanTOverFlt1_1 = DiagThrSystem1(0,!sfmd_flg_interComFlt && sfmd_flg_heatPanT1RationFlt && sfmd_flg_heatPanT1OpenFlt,ihd_T_heatPanT1,sfmc_T_heatPanTOverThrFlt1,sfmc_T_heatPanTOverThrRec1,3,3,&DiagThr.fltNum116_1,&DiagThr.recNum116_1,&sfmd_flg_heatPanTOverFlt1_1) && !sfmd_flg_heatPanTOverFlt2;
+    sfmd_flg_heatPanTOverFlt1_2 = DiagThrSystem1(0,!sfmd_flg_interComFlt && sfmd_flg_heatPanT2RationFlt && sfmd_flg_heatPanT2OpenFlt,ihd_T_heatPanT2,sfmc_T_heatPanTOverThrFlt1,sfmc_T_heatPanTOverThrRec1,3,3,&DiagThr.fltNum116_2,&DiagThr.recNum116_2,&sfmd_flg_heatPanTOverFlt1_2) && !sfmd_flg_heatPanTOverFlt2;
     sfmd_flg_heatPanTOverFlt1 = sfmd_flg_heatPanTOverFlt1_1 || sfmd_flg_heatPanTOverFlt1_2;
     
     sfmd_I_curr = ihd_I_curr;
     sfmd_I_currAbs = (uint16_T) (sfmd_I_curr > 0 ? sfmd_I_curr : - sfmd_I_curr);
     //131 充电电流过高
-    sfmd_flg_chrgCurrOverFlt = DiagThrSystem1(1,!sfmd_flg_interComFlt && chrgFlg && !sfmd_flg_currOpenFlt,sfmd_I_currAbs,sfmc_I_chrgCurrOverThr,sfmc_I_chrgCurrOverThr,20,20,&DiagThr.fltNum131,&DiagThr.recNum131,&sfmd_flg_chrgCurrOverFlt);
+    sfmd_flg_chrgCurrOverFlt = DiagThrSystem1(1,!sfmd_flg_interComFlt && chrgFlg && !sfmd_flg_currOpenFlt,sfmd_I_currAbs,sfmc_I_chrgCurrOverThr,sfmc_I_chrgCurrOverThr,2,2,&DiagThr.fltNum131,&DiagThr.recNum131,&sfmd_flg_chrgCurrOverFlt);
     //132 放电电流过高
-    sfmd_flg_disChrgCurrOverFlt = DiagThrSystem1(1,!sfmd_flg_interComFlt && disChrgFlg && !sfmd_flg_currOpenFlt,sfmd_I_currAbs,sfmc_I_disChrgCurrOverThr,sfmc_I_disChrgCurrOverThr ,20,20,&DiagThr.fltNum132,&DiagThr.recNum132,&sfmd_flg_disChrgCurrOverFlt);
-    
-   // printf("sfmd_flg_currOpenFlt:%d,ihd_st_workStat%d,sfmd_I_currAbs:%d,fltNum131:%d,fltNum132:%d,recNum131:%d,recNum132:%d\n",sfmd_flg_currOpenFlt,ihd_st_workStat,sfmd_I_currAbs,DiagThr.fltNum131,DiagThr.fltNum132,DiagThr.recNum131,DiagThr.recNum132);
+    sfmd_flg_disChrgCurrOverFlt = DiagThrSystem1(1,!sfmd_flg_interComFlt && disChrgFlg && !sfmd_flg_currOpenFlt,sfmd_I_currAbs,sfmc_I_disChrgCurrOverThr,sfmc_I_disChrgCurrOverThr ,2,2,&DiagThr.fltNum132,&DiagThr.recNum132,&sfmd_flg_disChrgCurrOverFlt);
+
     //151 充电过流故障不匹配
-    sfmd_flg_chrgCurrOverMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt,sfmd_flg_chrgCurrOverFlt != ihd_flg_chrgCurrOverFlt,&DiagTime.N151,20);
+    sfmd_flg_chrgCurrOverMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt,sfmd_flg_chrgCurrOverFlt != ihd_flg_chrgCurrOverFlt,&DiagTime.N151,2);
     //152 放电过流故障不匹配
-    sfmd_flg_disChrgCurrOverMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt,sfmd_flg_disChrgCurrOverFlt != ihd_flg_disChrgCurrOverFlt,&DiagTime.N152,20);
+    sfmd_flg_disChrgCurrOverMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt,sfmd_flg_disChrgCurrOverFlt != ihd_flg_disChrgCurrOverFlt,&DiagTime.N152,2);
     //153 总压欠压不匹配
-    sfmd_flg_battULowMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, sfmd_flg_battULowFlt2 != ihd_flg_battULowFlt,&DiagTime.N153,20);
+    sfmd_flg_battULowMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, sfmd_flg_battULowFlt2 != ihd_flg_battULowFlt,&DiagTime.N153,2);
     //154 总压过压不匹配
-    sfmd_flg_battUOverMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, sfmd_flg_battUOverFlt2 != ihd_flg_battUOverFlt,&DiagTime.N154,20);
+    sfmd_flg_battUOverMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, sfmd_flg_battUOverFlt2 != ihd_flg_battUOverFlt,&DiagTime.N154,2);
     
     //155 单体欠压故障不匹配
-    sfmd_flg_cellULowMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, sfmd_flg_cellULowFlt2 != ihd_flg_cellULowFlt,&DiagTime.N155,20);
+    sfmd_flg_cellULowMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, sfmd_flg_cellULowFlt2 != ihd_flg_cellULowFlt,&DiagTime.N155,2);
     //156 单体过压故障不匹配
-    sfmd_flg_cellUOverMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, sfmd_flg_cellUOverFlt2 != ihd_flg_cellUOverFlt,&DiagTime.N156,20);
+    sfmd_flg_cellUOverMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, sfmd_flg_cellUOverFlt2 != ihd_flg_cellUOverFlt,&DiagTime.N156,2);
     
     //157 充电模组过温故障不匹配
-    sfmd_flg_chrgModTOverMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, sfmd_flg_chrgModTOverFlt2 != ihd_flg_chrgModTOverFlt,&DiagTime.N157,20);
+    sfmd_flg_chrgModTOverMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, sfmd_flg_chrgModTOverFlt2 != ihd_flg_chrgModTOverFlt,&DiagTime.N157,2);
     //158 充电模组低温故障不匹配
-    sfmd_flg_chrgModTLowMisFlt = JudgeTimeSystem(0 && !sfmd_flg_interComFlt,sfmd_flg_chrgModTLowFlt2 != ihd_flg_chrgModTLowFlt,&DiagTime.N158,20);
+    sfmd_flg_chrgModTLowMisFlt = JudgeTimeSystem(0 && !sfmd_flg_interComFlt,sfmd_flg_chrgModTLowFlt2 != ihd_flg_chrgModTLowFlt,&DiagTime.N158,2);
     //159 放电模组过温故障不匹配
-    sfmd_flg_disChrgModTOverMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt,sfmd_flg_disChrgModTOverFlt2 != ihd_flg_disChrgModTOverFlt,&DiagTime.N159,20);
+    sfmd_flg_disChrgModTOverMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt,sfmd_flg_disChrgModTOverFlt2 != ihd_flg_disChrgModTOverFlt,&DiagTime.N159,2);
     //160 放电模组低温故障不匹配
-    sfmd_flg_disChrgModTLowMisFlt = JudgeTimeSystem(0 &&!sfmd_flg_interComFlt, sfmd_flg_disChrgModTLowFlt2 != ihd_flg_disChrgModTLowFlt,&DiagTime.N160,20);
+    sfmd_flg_disChrgModTLowMisFlt = JudgeTimeSystem(0 &&!sfmd_flg_interComFlt, sfmd_flg_disChrgModTLowFlt2 != ihd_flg_disChrgModTLowFlt,&DiagTime.N160,2);
     
     //161 充电Mos过温故障不匹配
-    sfmd_flg_chrgMosTOverMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, sfmd_flg_chrgMosTOverFlt2 != ihd_flg_chrgMosTOverFlt,&DiagTime.N161,20);
+    sfmd_flg_chrgMosTOverMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, sfmd_flg_chrgMosTOverFlt2 != ihd_flg_chrgMosTOverFlt,&DiagTime.N161,2);
     //162 放电Mos过温故障不匹配
-    sfmd_flg_disChrgMosTOverMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, sfmd_flg_disChrgMosTOverFlt2 != ihd_flg_disChrgMosTOverFlt,&DiagTime.N162,20);
+    sfmd_flg_disChrgMosTOverMisFlt = JudgeTimeSystem(!sfmd_flg_interComFlt, sfmd_flg_disChrgMosTOverFlt2 != ihd_flg_disChrgMosTOverFlt,&DiagTime.N162,2);
     
     
     //163 soc不匹配
@@ -729,14 +759,114 @@ void SFM(void)
     //164 SOH 不匹配
     sfmd_flg_sohMisFlt = !FirstRun_SFM && (!sfmd_flg_interComFlt) && ( (int16_T) (sohd_pct_bcuSoh -ihd_pct_soh ) > 50  || (int16_T) (sohd_pct_bcuSoh -ihd_pct_soh ) <  -50);
     
-    
     //179 soc跳变
-    sfmd_flg_socJumpFlt = ((int16_T) (socd_pct_bcuSoc - socd_pct_bcuSoc_Delay) > 50 ||(int16_T) (socd_pct_bcuSoc - socd_pct_bcuSoc_Delay) < -50) && JudgeTimeSystem(1, 1 ,&DiagTime.N179,2);;
+    sfmd_flg_socJumpFlt = ((int16_T) (socd_pct_bcuSoc - socd_pct_bcuSoc_Delay) > 50 ||(int16_T) (socd_pct_bcuSoc - socd_pct_bcuSoc_Delay) < -50) && JudgeTimeSystem(1, 1 ,&DiagTime.N179,2);
     socd_pct_bcuSoc_Delay = socd_pct_bcuSoc;
     //180 EE失效
     sfmd_flg_EEsaveFlt = ihd_flg_EESaveFlt;
     
+   
+    
+    
+    // 21 热失控故障
+    if (sfmd_I_curr > -2  && (sfmd_I_curr_Delay - sfmd_I_curr) > -2 && (sfmd_I_curr_Delay - sfmd_I_curr) < 2)
+    {
+        noDisChrgCnt = (noDisChrgCnt + 1) > 200 ? 200 : (noDisChrgCnt + 1);
+    }
+    else
+    {
+       noDisChrgCnt = 0; 
+    }
+    sfmd_I_curr_Delay = sfmd_I_curr;
     
+    if (noDisChrgCnt > 60)
+    {
+        for(i = 0; i < cmnc_num_cellUNum;i++)
+        {
+            if((int16_T)(CellUArry[3][i] - CellUArry[0][i]) < -100)
+            {
+                sfmd_flg_Udrop = true;
+            }
+        }
+        if(sfmd_flg_heatclear)
+        {
+            sfmd_flg_Udrop = false;
+        }
+    }
+    ////
+    if(sfmd_flg_volFlt)
+    {
+        sfmd_flg_volFlt_keep = true;
+    }
+    if(sfmd_flg_heatclear)
+    {
+        sfmd_flg_volFlt_keep = false;
+    }
+    //
+    CntA = CntA + 1;
+    if(CntA >= 0)
+    {
+        CntA = 0;
+        for(j = 0;j < 4 && FirstRun_SFM;j++)
+        {
+            for(i = 0; i < cmnc_num_modTNum;i++)
+            {
+                ModTStor[j][i] = sfmv_T_modT[i];
+            }
+        }
+        for(j = 0;j < 3;j++)
+        {
+            for(i = 0; i < cmnc_num_modTNum;i++)
+            {
+                ModTStor[j][i] = ModTStor[j+1][i];
+            }
+        }
+        for(i = 0; i < cmnc_num_modTNum;i++)
+        {
+            ModTStor[j][i] = sfmv_T_modT[i];
+        }
+        for(i = 0; i < cmnc_num_modTNum;i++)
+        {
+            if( (int16_T)(ModTStor[3][i] - ModTStor[2][i]) > 2 && (int16_T)(ModTStor[2][i] - ModTStor[1][i]) > 2 && (int16_T)(ModTStor[1][i] - ModTStor[0][i]) > 2)
+            {
+                sfmd_flg_Tup = true;
+            }
+        }
+    }
+
+    if(sfmd_flg_heatclear)
+    {
+        sfmd_flg_Tup = false;
+    }
+    //
+    if(sfmd_flg_modTOpenFlt)
+    {
+        sfmd_flg_modTOpenFlt_keep = true;
+    }
+    if(sfmd_flg_heatclear)
+    {
+        sfmd_flg_modTOpenFlt_keep = false;
+    }
+    //////
+    if(sfmd_flg_Udrop || sfmd_flg_volFlt_keep || sfmd_flg_Tup || sfmd_flg_modTOpenFlt_keep)
+    {
+        CntB = (CntB + 1) > 200 ? 200 : (CntB + 1);
+    }
+    if((((sfmd_flg_Udrop && sfmd_flg_Tup) || (sfmd_flg_Udrop && sfmd_flg_modTOpenFlt_keep) || (sfmd_flg_volFlt_keep && sfmd_flg_Tup)) && CntB <= 180) || sfmd_flg_heatRunFltEi)
+    {
+        sfmd_flg_heatRunFltEo = true;
+    }
+    if(!sfmd_flg_heatRunFltEo && CntB > 180)       
+    {
+        sfmd_flg_heatclear = true;
+        CntB = 0;
+    }
+	else
+	{ 
+	    sfmd_flg_heatclear = false;
+	}
+    printf("%d   %d  %d   %d   %d  %d  %d\n",sfmd_flg_Udrop,sfmd_flg_volFlt_keep,sfmd_flg_Tup,sfmd_flg_modTOpenFlt_keep,CntB,sfmd_flg_heatclear,sfmd_flg_heatRunFltEo) ;   
+        
     
     //================================================================================================================================================================
     //================================================================================================================================================================
@@ -769,7 +899,7 @@ void SFM(void)
     FltFlg[18] = 0;                                  FltLevel[18] = 0;        FltAct[18] = 0;       //19 预留2
     FltFlg[19] = 0;                                  FltLevel[19] = 0;        FltAct[19] = 0;       //20 预留1
     
-    FltFlg[20] = 0;                                  FltLevel[20] = 0;        FltAct[20] = 0;       //21 热失控；
+    FltFlg[20] = sfmd_flg_heatRunFltEo;              FltLevel[20] = 5;        FltAct[20] = 255;     //21 热失控；
     FltFlg[21] = sfmd_flg_chrgMosClosFlt;            FltLevel[21] = 4;        FltAct[21] = 106;     //22充电MOS常闭故障
     FltFlg[22] = 0;                                  FltLevel[22] = 0;        FltAct[22] = 0;       //23充电MOS常开故障
     FltFlg[23] = sfmd_flg_disChrgMosClosFlt;         FltLevel[23] = 4;        FltAct[23] = 106;     //24 放电Mos常闭故障
@@ -1019,14 +1149,14 @@ void SFM(void)
         }
         sfmd_st_fltAct = sfmd_st_fltAct|FltActArr[i];
     }
-    /*
+    
     for(i=0;i < 20;i++)
     {
         printf("%d  ",sfmv_idx_fltCode[i]);
     }
     printf("\n");
     printf("sfmd_st_fltAct:%d,sfmd_st_fltLevel:%d\n\n",sfmd_st_fltAct,sfmd_st_fltLevel);
-    */
+    
     
     ///=====================================================================
     //======================================================================
@@ -1041,7 +1171,7 @@ void SFM(void)
 
 
 //===============================================================================
-boolean_T DiagThrSystem1(boolean_T Enable, boolean_T precondition, uint16_T Input,uint16_T fltThr,uint16_T recThr,uint16_T fltNumThr,uint16_T recNumThr,uint8_T *fltNum,uint8_T *recNum,boolean_T *fitFlg)
+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)
 {
     
     if(Enable && precondition && Input > fltThr)
@@ -1073,7 +1203,7 @@ boolean_T DiagThrSystem1(boolean_T Enable, boolean_T precondition, uint16_T Inpu
     return *fitFlg;
 }
 //=============================================================================
-boolean_T DiagThrSystem2(boolean_T Enable, boolean_T precondition, uint16_T Input,uint16_T fltThr,uint16_T recThr,uint16_T fltNumThr,uint16_T recNumThr,uint8_T *fltNum,uint8_T *recNum,boolean_T *fitFlg)
+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)
 {
     if(Enable && precondition && Input < fltThr)
     {

src/EmbeddedCoder_src/SOC.c

@@ -2,15 +2,12 @@
 #include "look1_is16lu16n16tu16_binlcase.h"
 #include "look1_iu16lu16n16tu16_binlcase.h"
 
-
 boolean_T FirstRun_SOC;
 
-
 //---------------SOC初始化-----------------------------------------------
 void SOC_Init(void)
-{   
+{
     FirstRun_SOC = true;
-
 }
 //-------------------------------------------------------------------------
 
@@ -20,21 +17,21 @@ void SOC(void)
     static uint16_T socn_pct_bcuSocEE;
     static uint16_T socn_Q_cap;
     //
-    uint16_T   EKFSOCMin;
-    uint16_T   EKFSOCMax;
-    boolean_T  socn_flg_ekfInvalidMin;
-    boolean_T  socn_flg_ekfInvalidMax;
-    boolean_T  socn_flg_ekfInvalid;
+    uint16_T EKFSOCMin;
+    uint16_T EKFSOCMax;
+    boolean_T socn_flg_ekfInvalidMin;
+    boolean_T socn_flg_ekfInvalidMax;
+    boolean_T socn_flg_ekfInvalid;
     real_T ocv;
     real_T Ro;
     real_T Rp;
     real_T C;
-	real_T deltU;
+    real_T deltU;
     real_T A[4];
     real_T B[2];
     real_T H[2];
     real_T K[2];
-	real_T P1[4];
+    real_T P1[4];
     static real_T P_Min_Delay[4];
     static real_T P_Max_Delay[4];
     real_T soc1;
@@ -51,243 +48,243 @@ void SOC(void)
     static real_T ahDelay;
     int16_T ahSoc;
     //
-    static uint16_T  ekfInvalidCntl;
+    static uint16_T ekfInvalidCntl;
     static boolean_T onceFlg_est;
-    static int16_T   ahSoc0_est;
-    static uint16_T  ekfSoc0_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 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 int16_T ahSoc0_utrckOver;
+    static uint16_T estSoc0_utrckOver;
+    static uint16_T Soc_Delay;
     static boolean_T lowFlg;
-    static uint8_T   lowCntl;
+    static uint8_T lowCntl;
     static boolean_T onceFlg_utrckLow;
-    static int16_T   ahSoc0_utrckLow;
-    static uint16_T  estSoc0_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;
+    uint16_T socTemp;
+    static uint8_T chrgCntl;
+    static uint8_T disChrgCntl;
     //
     static uint16_T socd_pct_battSoc_Delay;
     //
-    static uint8_T  statCntl;
+    static uint8_T statCntl;
     boolean_T statFlg;
-    static uint8_T  ihd_st_chrgSta_Delay;
+    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;
+    int16_T coinSoc;
     uint16_T x[3];
     uint16_T y[3];
-	boolean_T Flg;
+    boolean_T Flg;
     static boolean_T onceFlg_chrg;
     static boolean_T onceFlg_dischrg;
 
-	
-	//
-    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)
+    {
+        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))
+    if (FirstRun_SOC)
+    { //
+        if ((socd_pct_battSocEi > 1000) || (socd_pct_bcuSocEi > 1000) || (ihd_st_EOLState == 0))
         {
-            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);
+            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;
+            socn_pct_bcuSocEE = socd_pct_bcuSocEi;
         }
         //
         if (ihd_tm_parkTime > cmnc_tm_parkTime && 0)
         {
-            socn_pct_battSocEE =  look1_iu16lu16n16tu16_binlcase(sfmd_V_cellUAvrg,(&(cmnm_V_ocv[0])), (&(cmnm_pct_soc[0])), 12U);
+            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);
+        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);
+
+    //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;
-	
+    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;
+        Up_Min_Delay = 0;
         P_Min_Delay[0] = 10;
-		P_Min_Delay[1] = 0;
-		P_Min_Delay[2] = 0;
-		P_Min_Delay[3] = 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;
+    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(-0.1/( Rp * C));
-	
-    B[0] = 0.1/Q/3600 * 100;
-    B[1] = Rp * (1- exp(-0.1/(Rp * C)));
-	
+    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;
+    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 );
-	
+    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;
+    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;
+        soc_Min_Delay = (real_T)socc_pct_battSocLow * 0.1;
     }
-    if (soc_Min_Delay > (real_T)socc_pct_battSocUp  * 0.1)
+    if (soc_Min_Delay > (real_T)socc_pct_battSocUp * 0.1)
     {
-        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;
-	test_UpMin = Up_Min_Delay;
+
+    Up_Min_Delay = Up1 + K[1] * deltU;
+    test_UpMin = Up_Min_Delay;
     //P更新
-    P_Min_Delay[0] = (1 - K[0] * H[0]) * P1[0] -K[0]* P1[1];
+    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[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.005)||(deltU < -0.005);
+    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;
+        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;
+        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;
+    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(-0.1/( Rp * C));
-	
-    B[0] = 0.1/Q/3600 * 100;
-    B[1] = Rp * (1- exp(-0.1/(Rp * C)));
-	
+    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;
-	
+    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 );
-	
+    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;
+    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;
+        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;
+        soc_Max_Delay = (real_T)socc_pct_battSocUp * 0.1;
     }
-    Up_Max_Delay  = Up1 + K[1] * deltU;
+    Up_Max_Delay = Up1 + K[1] * deltU;
 
-	test_UpMax = Up_Max_Delay;
+    test_UpMax = Up_Max_Delay;
     //P更新
-    P_Max_Delay[0] = (1 - K[0] * H[0]) * P1[0] -K[0] * P1[1];
+    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[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.005)||(deltU < -0.005);
-	//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);
+    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)
+    if (EKFSOCMax > 800)
     {
         factor = 100;
     }
-    else if(EKFSOCMin < 200)
+    else if (EKFSOCMin < 200)
     {
         factor = 0;
     }
     else
     {
-        factor=(uint16_T)(((uint16_T)((  (uint32_T)  (EKFSOCMin - 200)  << 6) / (800 - (EKFSOCMax-EKFSOCMin) - 200)) * 25U) >> 4);
+        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); 
-	test_efkSocMin  = EKFSOCMin;
-	test_efkSocMax  = EKFSOCMax;
+    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);
+    test_efkSocMin = EKFSOCMin;
+    test_efkSocMax = EKFSOCMax;
 
-	//printf("4----factor:%d,socd_pct_ekfSoc:%d,EKFSOCMax:%d,EKFSOCMin:%d,\n",factor,socd_pct_ekfSoc,EKFSOCMax,EKFSOCMin);
+    //printf("4----factor:%d,socd_pct_ekfSoc:%d,EKFSOCMax:%d,EKFSOCMin:%d,\n",factor,socd_pct_ekfSoc,EKFSOCMax,EKFSOCMin);
     //======================================================================
     ////////////////////////AhSOC//////////////////////////////////////////
     //======================================================================
@@ -297,9 +294,9 @@ void SOC(void)
     }
     else
     {
-        ahDelay = ahDelay +  battcurr / (real_T)(cmnc_Q_ratedCp * 0.1) /360.0;
+        ahDelay = ahDelay + battcurr / (real_T)(cmnc_Q_ratedCp * 0.1) / 36.0;
     }
-    ahSoc =(int16_T)(ahDelay * 10);
+    ahSoc = (int16_T)(ahDelay * 10);
     if (ahSoc > socc_pct_battSocUp)
     {
         socd_pct_ahSoc = socc_pct_battSocUp;
@@ -312,35 +309,35 @@ void SOC(void)
     {
         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);
+    //printf("5----ahDelay:%f,ahSoc:%d,battcurr:%f,sfmd_I_curr:%d\n",ahDelay,ahSoc,battcurr,sfmd_I_curr);
     //======================================================================
     ///////////////////////estSOC//////////////////////////////////////////
     //======================================================================
-    if(!socn_flg_ekfInvalid)
+    if (!socn_flg_ekfInvalid)
     {
-        ekfInvalidCntl = (ekfInvalidCntl + 1) > 250 ? 250 :(ekfInvalidCntl + 1);
+        ekfInvalidCntl = (ekfInvalidCntl + 1) > 250 ? 250 : (ekfInvalidCntl + 1);
     }
     else
     {
         ekfInvalidCntl = 0;
     }
-    
-    if (ekfInvalidCntl < 200)
+
+    if (ekfInvalidCntl < 20)
     {
         if (onceFlg_est)
         {
-            ahSoc0_est  = ahSoc;
+            ahSoc0_est = ahSoc;
             ekfSoc0_est = socd_pct_ekfSoc;
-			onceFlg_est = false;
+            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)
     {
@@ -350,30 +347,30 @@ void SOC(void)
     {
         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);
+    //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))
+        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;
+            overCntl = 0;
         }
-        if(overCntl > 20 || overFlg)
+        if (overCntl > 2 || overFlg)
         {
             overFlg = 1;
         }
         //
-        if(sfmd_V_cellUMax >= cmnc_V_chrgFul)
+        if (sfmd_V_cellUMax >= cmnc_V_chrgFul)
         {
             fulCntl = (fulCntl + 1) > 250 ? 250 : (fulCntl + 1);
         }
@@ -381,129 +378,129 @@ void SOC(void)
         {
             fulCntl = 0;
         }
-        if(fulCntl > 20 || fulFlg)
+        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;
+                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 > 20)
-        {   
+        else if (chrgCntl > 2)
+        {
             onceFlg_utrckOver = true;
             socTemp = socd_pct_estSoc > socc_pct_chrgCor ? socc_pct_chrgCor : socd_pct_estSoc;
         }
-		else
+        else
         {
             socTemp = socd_pct_estSoc;
         }
-		// 
-        socn_pct_utrackSoc = Soc_Delay + (socTemp > Soc_Delay ?  (socTemp - Soc_Delay) : 0);
+        //
+        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;
+            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);
+
+        //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);
+        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))
+        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;
+            lowCntl = 0;
         }
-        if(lowCntl > 20 || lowFlg)
+        if (lowCntl > 2 || lowFlg)
         {
             lowFlg = true;
         }
         //
         if (lowFlg)
         {
-            if (onceFlg_utrckLow)	
+            if (onceFlg_utrckLow)
             {
-                onceFlg_utrckLow = false; 
-                ahSoc0_utrckLow  = ahSoc;
-				estSoc0_utrckLow = socd_pct_estSoc < socc_pct_disChrgCor ? socd_pct_estSoc : socc_pct_disChrgCor;
+                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;
+            socn_pct_utrackSoc = (int16_T)(ahSoc - ahSoc0_utrckLow + estSoc0_utrckLow) > 0 ? (uint16_T)(ahSoc - ahSoc0_utrckLow + estSoc0_utrckLow) : 0;
         }
-        else if(disChrgCntl > 20)
+        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;
-	 }
-	 
-	//=====================================================================
+        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)
+    if (sfmd_I_curr < 10 && sfmd_I_curr > -10)
     {
-        statCntl= (statCntl + 1) > 250 ? 250 :  (statCntl + 1);
+        statCntl = (statCntl + 1) > 250 ? 250 : (statCntl + 1);
     }
     else
     {
         statCntl = 0;
     }
-    statFlg = statCntl > 20;
+    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;
+    ihd_st_chrgSta_Delay = ihd_st_workStat;
     socn_pct_utrackSoc_Delay = socn_pct_utrackSoc;
-	
+
     //
-    if(Flg)
+    if (Flg)
     {
         socd_pct_battSoc0 = socd_pct_battSoc;
-        if(FirstRun_SOC)
+        if (FirstRun_SOC)
         {
             socd_pct_bcuSoc0 = socn_pct_bcuSocEE;
         }
@@ -512,84 +509,83 @@ void SOC(void)
             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);
+
+    //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 (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_chrg = false;
             onceFlg_dischrg = true;
-            socd_pct_bcuSoc = 2000;	
+            socd_pct_bcuSoc = 2000;
         }
-        socd_pct_bcuSoc = SOCfitSystem(bcuSoc,&socd_pct_bcuSoc, 1);
-		
+        socd_pct_bcuSoc = SOCfitSystem(bcuSoc, &socd_pct_bcuSoc, 1);
+
         //
         if (fulFlg)
         {
-            socd_pct_bcuSoc =socc_pct_battSocUp;
+            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);
+        //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)
+        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_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;
+    FirstRun_SOC = false;
+
     //printf("\n");
 }
 
-
 //-------------------------------------------------------------------------
 real_T docvmath(real_T soc)
 {   
     real_T docv;
-    docv = ((((((-2.8104E-13 * pow(soc, 7.0) + 1.0283E-10 * pow(soc, 6.0)) +
-            -1.5072E-8 * pow(soc, 5.0)) + 1.1295E-6 * pow(soc, 4.0)) +
-            -4.588E-5 * pow(soc, 3.0)) + soc * soc * 0.000993) +
-            -0.010548 * soc) + 0.04876;
+    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;
 }
 ////
@@ -623,19 +619,19 @@ uint16_T SOC_LookUp(uint16_T battsoc, uint16_T x[3], uint16_T y[3])
 uint16_T SOCfitSystem(uint16_T SOC,uint16_T *SOCfit, uint16_T m)
 {
     int16_T socdelt;
-	socdelt = (int16_T)(SOC - *SOCfit);
-	if( socdelt > m)
+    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;
-	}
+    if (socdelt < -m && socdelt > -1000)
+    {
+        *SOCfit = *SOCfit + (socdelt < -m ? -m : socdelt);
+    }
+    if ((socdelt <= m && socdelt >= -m) || socdelt <= -1000)
+    {
+        *SOCfit = SOC;
+    }
 
     return *SOCfit;
 }

src/EmbeddedCoder_src/SOH.c

@@ -64,10 +64,10 @@ void SOH(void)
         //=======================================================================================
         //--------------------------EE校验-------------------------------------------------------
         //=======================================================================================
-        ArrMin((&(sohv_Q_cellCapArrEi[0])), &cellCapArr_Min , cmnc_num_cellUNum);
-        ArrMax((&(sohv_Q_cellCapArrEi[0])), &cellCapArr_Max , cmnc_num_cellUNum);
-        ArrMin((&(sohv_Q_packCapArrEi[0])), &packCapArr_Min , 10);
-        ArrMax((&(sohv_Q_packCapArrEi[0])), &packCapArr_Max , 10);
+        cellCapArr_Min = ArrMin((&(sohv_Q_cellCapArrEi[0])) , cmnc_num_cellUNum);
+        cellCapArr_Max = ArrMax((&(sohv_Q_cellCapArrEi[0])) , cmnc_num_cellUNum);
+        packCapArr_Min = ArrMin((&(sohv_Q_packCapArrEi[0])) , 10);
+        packCapArr_Max = ArrMax((&(sohv_Q_packCapArrEi[0])) , 10);
         if(sohd_Q_chrgEi > cmnc_Q_ratedCp || cellCapArr_Min < 10 || cellCapArr_Max > cmnc_Q_ratedCp + 100 || packCapArr_Min < 10 || packCapArr_Max > cmnc_Q_ratedCp + 100 )
         {
             memset(sohn_V_chrgStartStatEE, 0 , sizeof(sohn_V_chrgStartStatEE));
@@ -134,8 +134,8 @@ void SOH(void)
                      chrgCellCapArr[i] =    (uint16_T)( (real_T)(sohv_Q_cellCapArrEo[i] * 0.1)*(1 - temp[i]) * 10);
                      disChrgCellCapArr[i] = (uint16_T)( (real_T)(sohv_Q_cellCapArrEo[i] * 0.1)*(    temp[i]) * 10);
  				} 
-                 ArrMin((&(chrgCellCapArr[0])), &chrgCap_Min , cmnc_num_cellUNum);
-                 ArrMin((&(disChrgCellCapArr[0])), &disChrgCap_Min , cmnc_num_cellUNum);
+                 chrgCap_Min = ArrMin((&(chrgCellCapArr[0])) , cmnc_num_cellUNum);
+                 disChrgCap_Min = ArrMin((&(disChrgCellCapArr[0])) , cmnc_num_cellUNum);
                  for(i = 0; i < 9;i++)
                  {
                      sohv_Q_packCapArrEo[i] = sohn_Q_packCapArrEE[i+1];
@@ -155,10 +155,10 @@ void SOH(void)
          }
  
  
-         SumQ=0;
+         SumQ = 0;
          for(i = 0; i < 10;i++)
          {
-             SumQ =SumQ + sohv_Q_packCapArrEo[i];
+             SumQ = SumQ + sohv_Q_packCapArrEo[i];
          }
          sohd_pct_bcuSoh = (uint16_T)(((real_T)(SumQ  * 0.1)/10.0/ (real_T)(cmnc_Q_ratedCp * 0.1) * 100) * 10);  
          sohd_pct_bcuSoh = (sohd_pct_bcuSoh > 1000 ? 1000 : sohd_pct_bcuSoh);
@@ -172,7 +172,7 @@ void SOH(void)
      	memcpy(sohn_V_chrgStartStat,appv_V_cellU, sizeof(appv_V_cellU));
      }
      
-     if(sfmd_I_curr < 10 && sfmd_I_curr > -10)
+     if(sfmd_I_curr < 10 && sfmd_I_curr > -10 && !sfmd_flg_cellUDisable)
      {
          memcpy(sohn_V_chrgStartStat,sfmv_V_cellU, sizeof(sfmv_V_cellU));
      }
@@ -188,7 +188,7 @@ void SOH(void)
     if(ihd_st_workStat == 2)
     {
         Ahincr = Ahincr + (real_T)sfmd_I_curr * 0.1;
-        tmp_0 = (int16_T)(Ahincr/36000 * 10);
+        tmp_0 = (int16_T)(Ahincr/3600 * 10);
 		
         sohd_Q_chrgEo = tmp_0 > 0 ? (uint16_T)tmp_0 : 0;  
         if(sfmd_flg_currDisable)
@@ -224,29 +224,34 @@ void SOH(void)
 
 
 ////////////////////////////////////////////////////////////////
-void  ArrMax(uint16_T *Data,uint16_T *Max, uint16_T m)
+uint16_T  ArrMax(uint16_T *Data, uint16_T m)
 {
     uint16_T i;
-    *Max = Data[0];
+	uint16_T DataMax;
+    DataMax = Data[0];
     for(i = 0; i < m; i++)
     {
-        if (*Max < Data[i])
+        if (DataMax < Data[i])
         {
-            *Max = Data[i];
+            DataMax = Data[i];
         }
     }
+	return DataMax;
 }
 
 ///////////////////////////////////////////////////////////////////
-void  ArrMin(uint16_T *Data,uint16_T *Min, uint16_T m)
+uint16_T  ArrMin(uint16_T *Data, uint16_T m)
 {
     uint16_T i;
-    *Min = Data[0];
+	uint16_T DataMin;
+    DataMin = Data[0];
     for(i = 0; i < m; i++)
     {
-        if (*Min > Data[i])
+        if (DataMin > Data[i])
         {
-            *Min = Data[i];
+            DataMin = Data[i];
         }
     }
+	return DataMin;
 }
+

src/EmbeddedCoder_src/SPM.c

@@ -16,7 +16,6 @@ void SPM(void)
 {
 
   static uint16_T spmd_Nr_cellNr;
-
   uint8_T Feq = 10;
   if (FirstRun_SPM)
   {
@@ -37,12 +36,11 @@ void SPM(void)
     cand_Nr_cellNr = 1;
   }
 
-  
   SOH();
   BLC();
-  if (!ihd_flg_urtRecFlt)
+  if (!ihd_flg_urtRecFlt || sfmd_V_cellUMax == 0)
   {
-    SOC();
+      SOC();
   }
 
   

src/sensor.c

@@ -260,30 +260,30 @@ INT8 SL_SC7A20_INT_Config(void)
     UINT8 SL_Read_Reg;
 
     /*******************AOI1  IN  INT1********************/
-    SL_Read_Reg = 0x08; //AOI1 LATCH
+    SL_Read_Reg = 0x00; //AOI1 LATCH
     GSENSOR_WriteReg(SL_SC7A20_CTRL_REG5, SL_Read_Reg);
 
-    SL_Read_Reg = SL_SC7A20_INT_ACTIVE_HIGH_LEVEL;
+    SL_Read_Reg = SL_SC7A20_INT_ACTIVE_LOWER_LEVEL;
     //interrupt happen,int pin output lower level
     GSENSOR_WriteReg(SL_SC7A20_CTRL_REG6, SL_Read_Reg);
 
     //AOI1 CONFIG
     SL_Read_Reg = 0x00;               //0x7F
-    SL_Read_Reg = SL_Read_Reg | 0x40; //�����˶�ʶ��ģʽ
-    SL_Read_Reg = SL_Read_Reg | 0x03; //X ��ߵ��¼� X�����ݵľ���ֵ������ֵ
-    SL_Read_Reg = SL_Read_Reg | 0x0C; //Y ��ߵ��¼� Y�����ݵľ���ֵ������ֵ
-    SL_Read_Reg = SL_Read_Reg | 0x30; //Z ��ߵ��¼� Z�����ݵľ���ֵ������ֵ
+    SL_Read_Reg = SL_Read_Reg | 0x00; //�����˶�ʶ��ģʽ
+    SL_Read_Reg = SL_Read_Reg | 0x02; //X ��ߵ��¼� X�����ݵľ���ֵ������ֵ
+    SL_Read_Reg = SL_Read_Reg | 0x08; //Y ��ߵ��¼� Y�����ݵľ���ֵ������ֵ
+    SL_Read_Reg = SL_Read_Reg | 0x20; //Z ��ߵ��¼� Z�����ݵľ���ֵ������ֵ
     GSENSOR_WriteReg(SL_SC7A20_INT1_CFG, SL_Read_Reg);
 
     //HPF SET
     //  GSENSOR_ReadReg(SL_SC7A20_CTRL_REG2,1, &SL_Read_Reg);
-    SL_Read_Reg = 0xcf; //SL_Read_Reg|0x81;//Normal HP , HPF TO AOI1
+    SL_Read_Reg = 0x81; //SL_Read_Reg|0x81;//Normal HP , HPF TO AOI1
     GSENSOR_WriteReg(SL_SC7A20_CTRL_REG2, SL_Read_Reg);
 
     //�ж���ֵ����
     GSENSOR_WriteReg(SL_SC7A20_INT1_THS, SL_SC7A20_INT_THS_20PERCENT);
     //������ֵ����ʱ�䴥���ж�
-    GSENSOR_WriteReg(SL_SC7A20_INT1_DURATION, SL_SC7A20_INT_DURATION_30CLK);
+    GSENSOR_WriteReg(SL_SC7A20_INT1_DURATION, SL_SC7A20_INT_DURATION_10CLK);
 
     //AOI1 TO INT1
     // GSENSOR_ReadReg(SL_SC7A20_CTRL_REG3,1, &SL_Read_Reg);
@@ -310,9 +310,9 @@ INT8 SL_SC7A20_INT_Config(void)
     //    GSENSOR_WriteReg(SL_SC7A20_CTRL_REG2, SL_Read_Reg);
 
     //�ж���ֵ����
-    GSENSOR_WriteReg(SL_SC7A20_INT2_THS, SL_SC7A20_INT_THS_5PERCENT);
+    GSENSOR_WriteReg(SL_SC7A20_INT2_THS, SL_SC7A20_INT_THS_20PERCENT);
     //������ֵ����ʱ�䴥���ж�
-    GSENSOR_WriteReg(SL_SC7A20_INT2_DURATION, SL_SC7A20_INT_DURATION_2CLK);
+    GSENSOR_WriteReg(SL_SC7A20_INT2_DURATION, SL_SC7A20_INT_DURATION_10CLK);
 
     //AOI2 TO INT2
     GSENSOR_ReadReg(SL_SC7A20_CTRL_REG6, 1, &SL_Read_Reg);
@@ -489,9 +489,8 @@ void SL_SC7A20_Reg_read_all(void)
     for (adr = 0x20; adr <= 0x3F; adr++)
     {
         GSENSOR_ReadReg(adr, 1, &SL_Read_Reg);
-        printf("%#x = %#x\r\n", adr, SL_Read_Reg);
+        printf("%#x = %#x, ", adr, SL_Read_Reg);
     }
-
     printf("SL_SC7A20_Reg_readall --------\r\n");
 }
 
@@ -548,23 +547,22 @@ INT8 SL_SC7A20_Read_XYZ_Data(INT8 *SL_SC7A20_Data_XYZ_Buf)
 INT8 SL_SC7A20_Read_XYZ_Data(INT16 *SL_SC7A20_Data_XYZ_Buf)
 {
     UINT8 SL_Read_Buf[7];
-
     GSENSOR_ReadReg(SL_SC7A20_STATUS_REG, 1, &SL_Read_Buf[0]);
-#ifdef USING_PRINTF
-    printf("SL_SC7A20_STATUS_REG = %#x\r\n", SL_Read_Buf[0]);
-#endif
     if ((SL_Read_Buf[0] & 0x0f) == 0x0f)
     {
-        GSENSOR_ReadReg(SL_SC7A20_DATA_OUT, 6, &SL_Read_Buf[1]);
-#ifdef USING_PRINTF
-        printf("SL_SC7A20_DATA_OUT22 = %#x\r\n", SL_Read_Buf[1]);
-#endif
+        for (UINT8 i = 1; i < 7; i++)
+        {
+            GSENSOR_ReadReg(SL_SC7A20_STATUS_REG + i, 1, &SL_Read_Buf[i]);
+        }
         SL_SC7A20_Data_XYZ_Buf[0] = (INT16)((SL_Read_Buf[2] << 8) + SL_Read_Buf[1]);
         SL_SC7A20_Data_XYZ_Buf[1] = (INT16)((SL_Read_Buf[4] << 8) + SL_Read_Buf[3]);
         SL_SC7A20_Data_XYZ_Buf[2] = (INT16)((SL_Read_Buf[6] << 8) + SL_Read_Buf[5]);
-#ifdef USING_PRINTF
-        printf("SL_SC7A20_DATA_OUT BUF:X = %d,Y = %d,Z = %d\r\n", SL_SC7A20_Data_XYZ_Buf[0], SL_SC7A20_Data_XYZ_Buf[1], SL_SC7A20_Data_XYZ_Buf[2]);
-#endif
+        SL_SC7A20_Data_XYZ_Buf[0] = SL_SC7A20_Data_XYZ_Buf[0] >> 4;
+        SL_SC7A20_Data_XYZ_Buf[1] = SL_SC7A20_Data_XYZ_Buf[1] >> 4;
+        SL_SC7A20_Data_XYZ_Buf[2] = SL_SC7A20_Data_XYZ_Buf[2] >> 4;
+        SL_SC7A20_Data_XYZ_Buf[0] = SL_SC7A20_Data_XYZ_Buf[0] * 2;
+        SL_SC7A20_Data_XYZ_Buf[1] = SL_SC7A20_Data_XYZ_Buf[1] * 2;
+        SL_SC7A20_Data_XYZ_Buf[2] = SL_SC7A20_Data_XYZ_Buf[2] * 2;
         return 1;
     }
     else
@@ -584,7 +582,7 @@ void GsensorInit(void)
         SL_SC7A20_FS_Config(SL_SC7A20_FS_4G);
         SL_SC7A20_INT_Config();
         //SL_SC7A20_INT_RESET();
-        SL_SC7A20_Power_Config(SL_SC7A20_LOWER_POWER_ODR_400HZ);
+        SL_SC7A20_Power_Config(SL_SC7A20_ODR_400HZ);
     }
 }
 #endif