BatterySwap/System/Vehicle/AppFunc.c

/*
 * @Author       : ChenJie
 * @Date         : 2021-12-15 10:40:06
 * @Version      : V3.0
 * @LastEditors  : ChenJie
 * @LastEditTime : 2022-05-10 19:26:35
 * @Description  : AppFunc
 * @FilePath     : \VehicleControl\VehicleControl\src\System\Vehicle\AppFunc.c
 */
#include "string.h"
#include "stdlib.h"
#include "HardwareLib.h"
#include "CanVar.h"
#include "math.h"
#include "AppFunc.h"
#include "Std_Types.h"
#define Lock 0
#define Unlock 1
#define LockCtrl() PSwtDrv_Interface(_PSWT_INDEX_HBAK1_CHAN, Lock)
#define UnlockCtrl() PSwtDrv_Interface(_PSWT_INDEX_HBAK1_CHAN, Unlock)
void LockAndUnlockCtrl()
{
	static uint16 LockDelay = 0;
	static uint16 UnlockDelay = 0;
	static uint16 BleConCounter = 0;
	static uint16 battSwpeModeCounter = 0;
	if (ebcd_flg_ebcManCtrlEn) //手动控制使能
	{
		if (ebcd_flg_ebcManCtrlMode == 1) //循环控制模式
		{
			if (ebcd_st_lockSensor == 1) //处于锁紧状态，传感器判定一个
			{
				LockDelay = LockDelay + 10;
				if (LockDelay > LockDelayTime) //锁紧状态超时就解锁
				{
					ManuControlTimes++;
					LockDelay = 0;
					ebcd_st_SwitchCtrlFlg = Unlock;
				}
			}
			else if (ebcd_st_unlockSensor == 1) //处于解锁状态，传感器判定一个
			{
				UnlockDelay = UnlockDelay + 10;
				if (UnlockDelay > UnlockDelayTime)
				{
					UnlockDelay = 0;
					ebcd_st_SwitchCtrlFlg = Lock;
				}
			}
		}
		else if (ebcd_flg_ebcManCtrlMode == 2) //控制锁紧
		{
			ebcd_st_SwitchCtrlFlg = Lock;
		}
		else if (ebcd_flg_ebcManCtrlMode == 3) //控制解锁
		{
			ebcd_st_SwitchCtrlFlg = Unlock;
		}
	}
	else //正常工作模式
	{
		switch (ebcd_st_ebcWork)
		{
		case DriveMode: //行车状态，自动进入，默认状态
		{
			PSwtDrv_Interface(_PSWT_INDEX_EBCLEDCONTROL, 0); //行车状态换电指示灯关闭
			if (bled_flg_handShake == 1)
			{
				ebcd_st_ebcWork = CommuMode; //进入通讯状态
				bled_flg_handShake = 0;
				break;
			}
			break;
		}
		case CommuMode: //通讯状态，握手才能进入
		{
			BleConCounter = BleConCounter + 10;
			if ((BleConCounter % 1000) < 500) //换电指示灯闪烁
			{
				PSwtDrv_Interface(_PSWT_INDEX_EBCLEDCONTROL, 1);
			}
			else
			{
				PSwtDrv_Interface(_PSWT_INDEX_EBCLEDCONTROL, 0);
			}

			if (BleConCounter >= 60 * 1000 || bled_flg_backToDrv == 1)
			{
				ebcd_st_ebcWork = DriveMode; //切换到行车状态
				BleConCounter = 0;
				bled_flg_backToDrv = 0;
				break;
			}
			else if (BattCD_stWakeupU.B.bLogic == 0 && BattCD_stWakeupU.B.bWkpSig1 == 1)
			{
				ebcd_st_ebcWork = SwapMode; //切换到换电状态
				BleConCounter = 0;
				break;
			}
			if (bled_st_dataTrans == 1)
			{
				BleConCounter = 0;
				bled_st_dataTrans = 0;
			}
			break;
		}
		case SwapMode: //换电状态
		{
			PSwtDrv_Interface(_PSWT_INDEX_EBCLEDCONTROL, 1); //换电状态换电指示灯常亮
			battSwpeModeCounter = battSwpeModeCounter + 10;
			if (bled_flg_swapBattDone == 1)
			{
				ebcd_Nr_swapBatt++;
				ebcd_Nr_swapSucBatt++;
				bled_flg_swapBattDone = 0;
				ebcd_st_ebcWork = CommuMode; //切换到通讯状态
				break;
			}
			else if (battSwpeModeCounter >= 60 * 1000)
			{
				ebcd_Nr_swapBatt++;
				battSwpeModeCounter = DriveMode; //切换到行车状态
				ebcd_st_ebcWork = 0;
				break;
			}
			if (bled_st_dataTrans == 1)
			{
				battSwpeModeCounter = 0;
				bled_st_dataTrans = 0;
			}
			if (bled_flg_unlockCmdForce)
			{
				ebcd_st_SwitchCtrlFlg = Unlock;
				bled_flg_unlockCmdForce = 0;
			}
			if (bled_flg_lockCmdForce)
			{
				ebcd_st_SwitchCtrlFlg = Lock;
				bled_flg_lockCmdForce = 0;
			}
			break;
		}
		}
	}

	//锁紧解锁执行
	if (ebcd_st_SwitchCtrlFlg == Unlock)
	{
		UnlockCtrl();
	}
	else
	{
		LockCtrl();
	}
}
/**
 * @brief : 互锁检测函数
 * @param {*}
 * @return {*}
 */
void GetHVLockState(void)
{
	// PWM输出，互锁1检测，频率100HZ，占空比30%
	uint16 VehCo_fTestUI = 1000;
	uint16 VehCo_rTestUW = 3000;
	uint32 PwmFreAcq = 0;
	uint16 PwmDutyAcq = 0;
	PPWMDrv_Interface(_PPWM_INDEX_HVLOCK2, VehCo_fTestUI, VehCo_rTestUW);
	PulseAcqDrv_GetChanFreq(_PULSEACQ_INDEX_HVLOCK2, &PwmFreAcq, &PwmDutyAcq);
	if (abs(PwmFreAcq - VehCo_fTestUI) < 100 && abs(PwmDutyAcq - VehCo_rTestUW) < 500)
	{
		m_flg_HVlock1 = 1;
	}
	else
	{
		m_flg_HVlock1 = 0;
	}
	//互锁2检测，配置高有效，悬空为0则未接入，高电平为1则接入，
	// DINDrv_SetChanThres(_DIN_INDEX_PLUGHVLOCK, 0, 4095 * 3);
	m_flg_HVlock2 = DINDrv_GetChanState(_DIN_INDEX_PLUGHVLOCK);
}
/**
 * @brief : 数字量传感器信号检测函数
 * @param {*}
 * @return {*}
 */
void GetDIOState(void)
{
	uint8 temp[4];
	//松开传感器检测，配置低有效，底层悬空为1，触发为0,应用层输出悬空为0，触发为1
	memset(temp, 0x00, 4);
	ebcd_st_unlockSensor = 0;
	DINDrv_SetChanThres(_DIN_INDEX_UNLOCKSENSOR1, 1, 4095U);
	DINDrv_SetChanThres(_DIN_INDEX_UNLOCKSENSOR2, 1, 4095U);
	DINDrv_SetChanThres(_DIN_INDEX_UNLOCKSENSOR3, 1, 4095U);
	DINDrv_SetChanThres(_DIN_INDEX_UNLOCKSENSOR4, 1, 4095U);
	temp[0] = !DINDrv_GetChanState(_DIN_INDEX_UNLOCKSENSOR1);
	temp[1] = !DINDrv_GetChanState(_DIN_INDEX_UNLOCKSENSOR2);
	temp[2] = !DINDrv_GetChanState(_DIN_INDEX_UNLOCKSENSOR3);
	temp[3] = !DINDrv_GetChanState(_DIN_INDEX_UNLOCKSENSOR4);
	ebcd_st_unlockSensor = (getbit(temp[3], 0) << 3) | (getbit(temp[2], 0) << 2) | (getbit(temp[1], 0) << 1) | (getbit(temp[0], 0) << 0);

	//夹紧传感器检测，配置低有效，底层悬空为1，触发为0,应用层输出悬空为0，触发为1
	memset(temp, 0x00, 4);
	ebcd_st_lockSensor = 0;
	DINDrv_SetChanThres(_DIN_INDEX_LOCKSENSOR1, 1, 4095U);
	DINDrv_SetChanThres(_DIN_INDEX_LOCKSENSOR2, 1, 4095U);
	DINDrv_SetChanThres(_DIN_INDEX_LOCKSENSOR3, 1, 4095U);
	DINDrv_SetChanThres(_DIN_INDEX_LOCKSENSOR4, 1, 4095U);
	temp[0] = !DINDrv_GetChanState(_DIN_INDEX_LOCKSENSOR1);
	temp[1] = !DINDrv_GetChanState(_DIN_INDEX_LOCKSENSOR2);
	temp[2] = !DINDrv_GetChanState(_DIN_INDEX_LOCKSENSOR3);
	temp[3] = !DINDrv_GetChanState(_DIN_INDEX_LOCKSENSOR4);
	ebcd_st_lockSensor = (getbit(temp[3], 0) << 3) | (getbit(temp[2], 0) << 2) | (getbit(temp[1], 0) << 1) | (getbit(temp[0], 0) << 0);

	//落座传感器检测，高有效，悬空为0则未接入，高电平为1则接入
	memset(temp, 0x00, 4);
	temp[0] = !DINDrv_GetChanState(_DIN_INDEX_READYSENSOR1);
	temp[1] = !DINDrv_GetChanState(_DIN_INDEX_READYSENSOR2);
	ebcd_st_pedstSensor = (getbit(temp[1], 0) << 1) | (getbit(temp[0], 0) << 0);
}

/**
 * @brief : 获取模拟量输入值，并进行转换
 * @param {*}
 * @return {*}
 */
void GetAIOValue(void)
{
	uint16 AirPressureTemp_Vol = 0;
	uint16 PluginTemp1_Vol = 0;
	uint32 PluginTemp1_R = 0;
	uint16 PluginTemp2_Vol = 0;
	uint32 PluginTemp2_R = 0;
	uint16 PluginTemp3_Vol = 0;
	uint32 PluginTemp3_R = 0;
	uint16 PluginTemp4_Vol = 0;
	uint32 PluginTemp4_R = 0;

	AirPressureTemp_Vol = ATDDrv_GetChanResult(_ATD_INDEX_AIRPRESSURE);
	AirPressureTemp_Vol = (uint16)(AirPressureTemp_Vol * 1000 / 4095.0);
	Test_LockPressure = AirPressureTemp_Vol; //锁紧力测试变量
	/*气压数据转换*/
	AirPressureTemp_Vol = MAX(AirPressureTemp_Vol, 500);
	AirPressureTemp_Vol = MIN(AirPressureTemp_Vol, 4500);
	ebcd_P_airSensor = (uint8)((AirPressureTemp_Vol - 500) / 40);
	/*温度采集获取*/
	PluginTemp1_Vol = ATDDrv_GetChanResult(_ATD_INDEX_PLUGINTEMP1);
	PluginTemp2_Vol = ATDDrv_GetChanResult(_ATD_INDEX_PLUGINTEMP2);
	PluginTemp3_Vol = ATDDrv_GetChanResult(_ATD_INDEX_PLUGINTEMP3);
	PluginTemp4_Vol = ATDDrv_GetChanResult(_ATD_INDEX_PLUGINTEMP4);
	PluginTemp1_R = (uint32)((PluginTemp1_Vol / (5.0 * 4095 - PluginTemp1_Vol)) * 1000.0);
	PluginTemp2_R = (uint32)((PluginTemp2_Vol / (5.0 * 4095 - PluginTemp2_Vol)) * 1000.0);
	PluginTemp3_R = (uint32)((PluginTemp3_Vol / (5.0 * 4095 - PluginTemp3_Vol)) * 1000.0);
	PluginTemp4_R = (uint32)((PluginTemp4_Vol / (5.0 * 4095 - PluginTemp4_Vol)) * 1000.0);
	ebcd_T_plugin[0] = (uint8)Look1_u32u8(PluginTemp1_R, R_table, T_table, 240);
	ebcd_T_plugin[1] = (uint8)Look1_u32u8(PluginTemp2_R, R_table, T_table, 240);
	ebcd_T_plugin[2] = (uint8)Look1_u32u8(PluginTemp3_R, R_table, T_table, 240);
	ebcd_T_plugin[3] = (uint8)Look1_u32u8(PluginTemp4_R, R_table, T_table, 240);
}
/**
 * @brief : lookUp Table Fun
 * @param {uint32} u0 x
 * @param {uint32} bp0 x_table
 * @param {uint16} table y_table
 * @param {uint16} maxIndex
 * @return {*}
 */
uint16 Look1_u32u8(uint32 u0, uint32 *bp0, uint8 *table, uint16 MaxLen)
{
	uint32 bpIdx = 0;
	uint32 iLeft = 0;
	uint32 iRght = 0;
	uint16 y = 0;
	uint32 yL_0d0 = 0;
	uint32 yR_0d0 = 0;
	uint32 maxIndex = MaxLen - 1;
	if (u0 <= bp0[0U])
	{
		iLeft = 0U;
		iRght = 0U;
	}
	else if (u0 < bp0[maxIndex])
	{
		//对折法寻找u0的位置
		bpIdx = maxIndex >> 1U;
		iLeft = 0U;
		iRght = maxIndex;
		while ((iRght - iLeft) > 1)
		{
			if (u0 < bp0[bpIdx])
			{
				iRght = bpIdx;
			}
			else
			{
				iLeft = bpIdx;
			}
			bpIdx = (iRght + iLeft) >> 1U;
		}
	}
	else
	{
		iLeft = maxIndex;
		iRght = maxIndex;
	}
	//找到位置以后计算插值
	if (iLeft != iRght)
	{
		//线性插值
		yR_0d0 = table[iLeft + 1U];
		yL_0d0 = table[iLeft];
		if (yR_0d0 >= yL_0d0)
		{
			y = (uint16)(((uint32)(u0 - bp0[iLeft]) * (yR_0d0 - yL_0d0)) / (bp0[iLeft + 1] - bp0[iLeft]) + yL_0d0);
		}
		else
		{
			y = (uint16)(yL_0d0 - ((uint32)(u0 - bp0[iLeft]) * (yL_0d0 - yR_0d0)) / (bp0[iLeft + 1] - bp0[iLeft]));
		}
	}
	else
	{
		y = (uint16)table[iLeft];
	}
	return y;
}
/**
 * @brief : 故障诊断函数，运行时间100ms
 * @param {*}
 * @return {*}
 */
void FaultDiagnosisFunc(void)
{
	UINT8 ErrorNumTemp = 0, ErrRank = 0;
	static BOOL FuncFirstRun = TRUE;
	static BOOL sfmd_flg_Temp1Over2;
	static BOOL sfmd_flg_Temp2Over2;
	static BOOL sfmd_flg_Temp3Over2;
	static BOOL sfmd_flg_Temp4Over2;
	static BOOL sfmd_flg_Temp1Over3;
	static BOOL sfmd_flg_Temp2Over3;
	static BOOL sfmd_flg_Temp3Over3;
	static BOOL sfmd_flg_Temp4Over3;
	BOOL sfmd_flg_Temp1Openflt = FALSE;
	BOOL sfmd_flg_Temp2Openflt = FALSE;
	BOOL sfmd_flg_Temp3Openflt = FALSE;
	BOOL sfmd_flg_Temp4Openflt = FALSE;
	if (FuncFirstRun)
	{
		FuncFirstRun = FALSE;
		sfmd_flg_Temp1Over2 = FALSE;
		sfmd_flg_Temp2Over2 = FALSE;
		sfmd_flg_Temp3Over2 = FALSE;
		sfmd_flg_Temp4Over2 = FALSE;
		sfmd_flg_Temp1Over3 = getbit(sfmv_Num_FltEEsave,0);
		sfmd_flg_Temp2Over3 = getbit(sfmv_Num_FltEEsave,1);
		sfmd_flg_Temp3Over3 = getbit(sfmv_Num_FltEEsave,2);
		sfmd_flg_Temp4Over3 = getbit(sfmv_Num_FltEEsave,3);
	}
	/*锁紧与解锁诊断*/
	if (ebcd_st_SwitchCtrlFlg == Unlock) //解锁诊断
	{
		if (ebcd_st_unlockSensor == 0x0F)
		{
			ebcd_st_unlockSucJug = 1;
		}
		else if (ebcd_st_unlockSensor == 0x00)
		{
			ebcd_st_unlockSucJug = 0;
		}
		else
		{
			ebcd_st_unlockSucJug = 2;
		}
	}
	else //锁紧诊断
	{
		if (ebcd_st_lockSensor == 0x0F)
		{
			ebcd_st_lockSucJug = 1;
		}
		else if (ebcd_st_lockSensor == 0x00)
		{
			ebcd_st_lockSucJug = 0;
		}
		else
		{
			ebcd_st_lockSucJug = 2;
		}
	}
	/*底座到位判断*/
	if (ebcd_st_pedstSucJug == 0x03)
	{
		ebcd_st_pedstSucJug = 1;
	}
	else if (ebcd_st_pedstSucJug == 0x00)
	{
		ebcd_st_pedstSucJug = 0;
	}
	else
	{
		ebcd_st_pedstSucJug = 2;
	}
	/*温度传感器开路诊断*/
	sfmd_flg_Temp1Openflt = JudgeTimeSystem(1U, (ebcd_T_plugin[0] == 0), DiagErrFaulNum.DiagErrFaulN25, 5U);
	sfmd_flg_Temp2Openflt = JudgeTimeSystem(1U, (ebcd_T_plugin[1] == 0), DiagErrFaulNum.DiagErrFaulN26, 5U);
	sfmd_flg_Temp3Openflt = JudgeTimeSystem(1U, (ebcd_T_plugin[2] == 0), DiagErrFaulNum.DiagErrFaulN27, 5U);
	sfmd_flg_Temp4Openflt = JudgeTimeSystem(1U, (ebcd_T_plugin[3] == 0), DiagErrFaulNum.DiagErrFaulN28, 5U);
	/*温度传感器三级过温诊断*/
	sfmd_flg_Temp1Over3 = DiagThrSystem1(1, !sfmd_flg_Temp1Openflt, ebcd_T_plugin[0], sfmd_Th_Temp1Over3Faul, sfmd_Th_Temp1Over3Reco, 30, 30, DiagErrFaulNum.DiagErrFaulN17, DiagErrRecoNum.DiagErrRecoN17, sfmd_flg_Temp1Over3);
	sfmd_flg_Temp2Over3 = DiagThrSystem1(1, !sfmd_flg_Temp2Openflt, ebcd_T_plugin[1], sfmd_Th_Temp2Over3Faul, sfmd_Th_Temp2Over3Reco, 30, 30, DiagErrFaulNum.DiagErrFaulN18, DiagErrRecoNum.DiagErrRecoN18, sfmd_flg_Temp2Over3);
	sfmd_flg_Temp3Over3 = DiagThrSystem1(1, !sfmd_flg_Temp3Openflt, ebcd_T_plugin[2], sfmd_Th_Temp3Over3Faul, sfmd_Th_Temp3Over3Reco, 30, 30, DiagErrFaulNum.DiagErrFaulN19, DiagErrRecoNum.DiagErrRecoN19, sfmd_flg_Temp3Over3);
	sfmd_flg_Temp4Over3 = DiagThrSystem1(1, !sfmd_flg_Temp4Openflt, ebcd_T_plugin[3], sfmd_Th_Temp4Over3Faul, sfmd_Th_Temp4Over3Reco, 30, 30, DiagErrFaulNum.DiagErrFaulN20, DiagErrRecoNum.DiagErrRecoN20, sfmd_flg_Temp4Over3);
	/*温度传感器二级过温诊断*/
	sfmd_flg_Temp1Over2 = DiagThrSystem1(1, !sfmd_flg_Temp1Over3 && !sfmd_flg_Temp1Openflt, ebcd_T_plugin[0], sfmd_Th_Temp1Over2Faul, sfmd_Th_Temp1Over2Reco, 30, 30, DiagErrFaulNum.DiagErrFaulN9, DiagErrRecoNum.DiagErrRecoN9, sfmd_flg_Temp1Over2);
	sfmd_flg_Temp2Over2 = DiagThrSystem1(1, !sfmd_flg_Temp2Over3 && !sfmd_flg_Temp2Openflt, ebcd_T_plugin[1], sfmd_Th_Temp2Over2Faul, sfmd_Th_Temp2Over2Reco, 30, 30, DiagErrFaulNum.DiagErrFaulN10, DiagErrRecoNum.DiagErrRecoN10, sfmd_flg_Temp2Over2);
	sfmd_flg_Temp3Over2 = DiagThrSystem1(1, !sfmd_flg_Temp3Over3 && !sfmd_flg_Temp3Openflt, ebcd_T_plugin[2], sfmd_Th_Temp3Over2Faul, sfmd_Th_Temp3Over2Reco, 30, 30, DiagErrFaulNum.DiagErrFaulN11, DiagErrRecoNum.DiagErrRecoN11, sfmd_flg_Temp3Over2);
	sfmd_flg_Temp4Over2 = DiagThrSystem1(1, !sfmd_flg_Temp4Over3 && !sfmd_flg_Temp4Openflt, ebcd_T_plugin[3], sfmd_Th_Temp4Over2Faul, sfmd_Th_Temp4Over2Reco, 30, 30, DiagErrFaulNum.DiagErrFaulN12, DiagErrRecoNum.DiagErrRecoN12, sfmd_flg_Temp4Over2);

	sfmv_Num_FltEEsave = ((sfmd_flg_Temp4Over3&0x01)<<3)|((sfmd_flg_Temp3Over3&0x01)<<2)|((sfmd_flg_Temp2Over3&0x01)<<1)|(sfmd_flg_Temp1Over3);
	/*故障代码写入*/
	if (sfmd_flg_Temp1Openflt)
	{
		ErrorNumTemp = 25;
		ErrRank = 2;
		PutErrorNum(ErrArray, sizeof(ErrArray) / 2, ErrorNumTemp + ErrRank * 1000);
	}
	if (sfmd_flg_Temp2Openflt)
	{
		ErrorNumTemp = 26;
		ErrRank = 2;
		PutErrorNum(ErrArray, sizeof(ErrArray) / 2, ErrorNumTemp + ErrRank * 1000);
	}
	if (sfmd_flg_Temp3Openflt)
	{
		ErrorNumTemp = 27;
		ErrRank = 2;
		PutErrorNum(ErrArray, sizeof(ErrArray) / 2, ErrorNumTemp + ErrRank * 1000);
	}
	if (sfmd_flg_Temp4Openflt)
	{
		ErrorNumTemp = 28;
		ErrRank = 2;
		PutErrorNum(ErrArray, sizeof(ErrArray) / 2, ErrorNumTemp + ErrRank * 1000);
	}
	if (sfmd_flg_Temp1Over3)
	{
		ErrorNumTemp = 17;
		ErrRank = 3;
		PutErrorNum(ErrArray, sizeof(ErrArray) / 2, ErrorNumTemp + ErrRank * 1000);
	}
	if (sfmd_flg_Temp2Over3)
	{
		ErrorNumTemp = 18;
		ErrRank = 3;
		PutErrorNum(ErrArray, sizeof(ErrArray) / 2, ErrorNumTemp + ErrRank * 1000);
	}
	if (sfmd_flg_Temp3Over3)
	{
		ErrorNumTemp = 19;
		ErrRank = 3;
		PutErrorNum(ErrArray, sizeof(ErrArray) / 2, ErrorNumTemp + ErrRank * 1000);
	}
	if (sfmd_flg_Temp4Over3)
	{
		ErrorNumTemp = 20;
		ErrRank = 3;
		PutErrorNum(ErrArray, sizeof(ErrArray) / 2, ErrorNumTemp + ErrRank * 1000);
	}
	if (sfmd_flg_Temp1Over2)
	{
		ErrorNumTemp = 9;
		ErrRank = 2;
		PutErrorNum(ErrArray, sizeof(ErrArray) / 2, ErrorNumTemp + ErrRank * 1000);
	}
	if (sfmd_flg_Temp2Over2)
	{
		ErrorNumTemp = 10;
		ErrRank = 2;
		PutErrorNum(ErrArray, sizeof(ErrArray) / 2, ErrorNumTemp + ErrRank * 1000);
	}
	if (sfmd_flg_Temp3Over2)
	{
		ErrorNumTemp = 11;
		ErrRank = 2;
		PutErrorNum(ErrArray, sizeof(ErrArray) / 2, ErrorNumTemp + ErrRank * 1000);
	}
	if (sfmd_flg_Temp4Over2)
	{
		ErrorNumTemp = 12;
		ErrRank = 2;
		PutErrorNum(ErrArray, sizeof(ErrArray) / 2, ErrorNumTemp + ErrRank * 1000);
	}
}
/**
 * @brief : 故障判断函数
 * @param {BOOL} Enable 使能状态
 * @param {BOOL} Input 判断输入
 * @param {UINT16} *N 判断次数累计
 * @param {UINT16} Thr 判断次数阈值
 * @return {*}
 */
BOOL JudgeTimeSystem(BOOL Enable, BOOL Input, UINT8 *N, UINT8 Thr)
{
	BOOL Flg = FALSE;
	if (Input && Enable)
	{
		*N = (*N + 1) > 2008 ? 2008 : (*N + 1);
	}
	else
	{
		*N = 0;
	}
	if (*N > Thr && Enable)
	{
		Flg = TRUE;
	}
	else
	{
		Flg = FALSE;
	}
	return Flg;
}
BOOL DiagThrSystem1(BOOL Enable, BOOL precondition, UINT16 Input, UINT16 fltThr, UINT16 recThr, UINT16 fltNumThr, UINT16 recNumThr, UINT16 *fltNum, UINT16 *recNum, UINT8 *fitFlg)
{
	if (Enable && precondition && Input > fltThr)
	{
		*fltNum = (*fltNum + 1) > 20000 ? 20000 : (*fltNum + 1);
	}
	else
	{
		*fltNum = 0;
	}
	if (Enable && precondition && Input < recThr)
	{
		*recNum = (*recNum + 1) > 20000 ? 20000 : (*recNum + 1);
	}
	else
	{
		*recNum = 0;
	}
	if ((*fltNum > fltNumThr || (*fitFlg && *recNum < recNumThr)) && precondition)
	{
		*fitFlg = TRUE;
	}
	else
	{
		*fitFlg = FALSE;
	}
	return *fitFlg;
}
/**
 * @brief : 获取故障码函数，从故障数组中获取故障码，并将之前的故障码向前移动
 * @param {UINT16} *ErrorArray
 * @param {UINT8} Errorlen
 * @return {*}
 */
UINT16 GetErrorNum(UINT16 *ErrorArray, UINT8 Errorlen)
{
	UINT16 OutNum;
	OutNum = *(ErrorArray);
	for (UINT8 i = 0; i < Errorlen - 1; i++)
	{
		*(ErrorArray + i) = *(ErrorArray + i + 1);
		if (*(ErrorArray + i + 1) == 0)
			break;
	}
	return OutNum;
}
/**
 * @brief : 故障码注入函数，将故障码写入到故障数组中，如果存在则不写入，如果存在则不写入
 * @param {UINT16} *ErrorArray
 * @param {UINT8} Errorlen
 * @param {UINT16} ErrorNum
 * @return {*}
 */
UINT8 PutErrorNum(UINT16 *ErrorArray, UINT8 Errorlen, UINT16 ErrorNum)
{
	for (UINT8 i = 0; i < Errorlen; i++)
	{
		if (*(ErrorArray + i) == 0)
		{
			*(ErrorArray + i) = ErrorNum;
			return 0;
		}
		else
		{
			if (*(ErrorArray + i) == ErrorNum)
			{
				return 1;
			}
			else
			{
				continue;
			}
		}
	}
	return 2;
}