/*
 * AppTaskCan.c
 *
 *  Created on: 2022年2月8日
 *      Author: QiXiang_CHENJIE
 */
#include "AppTaskCan.h"
static void CanDataAsk(QueueDataTrans_Type AskData);
uint8 DataPackCounter[21] = {1,1,7,5,16,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};
static uint32 timerConuter = 0;
static void UdsAns(bool PosFlg,uint8 UdsService,uint8 UdsSubService,uint8 Idx,uint8 *AnsData,uint8 AnsDataLen);
void udsService(uint8 *DataIn);
static void dataInit();
static void appFunc();
static void CanSend100ms(void);
static void CanDecode(uint32 AskCanID, uint8 AskData[]);
static void uartDataDecode(uint8 Data[]);
static void vCanRxCallback(TimerHandle_t pxTimer)
{
	uint32 ulTimerID;
	ulTimerID = (uint32)pvTimerGetTimerID(pxTimer);
	if(ulTimerID==0)
	{
		Can_MainFunction_Read();
		timerConuter++;
	}
}
void CanTask()
{
	CanRecvQueueHandle = xQueueCreate(16, sizeof(Can_Msg_Type_Data)); // CAN获取消息队列创建
	Uart2ToCanDataHandle = xQueueCreate(2, sizeof(QueueDataTrans_Type)); // CAN数据查询队列创建
	CanToUart2DataHandle = xQueueCreate(1, sizeof(QueueDataTrans_Type)); // CAN数据回复队列创建
	uint8 CanState = 0;
	CanIf_bRxFlag = false;
	Can_Msg_Type_Data CanRxMsg;
	QueueDataTrans_Type CanAskData={0,NULL};

	TimerHandle_t monitorTimer1ms;
	monitorTimer1ms = xTimerCreate("monitor1ms",1,pdTRUE,(void *)0,vCanRxCallback);
	xTimerStart(monitorTimer1ms,0);
	uint32 canRecvDelay = 0;
	sint8 QueueRet = -1;
	uint8 uartData[10] = {0};
	uint32 canSendCounter = 0;
	while (TRUE)
	{
		switch (CanState)
		{
			case 0:
			{
				CanState = 1;
				break;
			}
			case 1:
			{
				memset(&CanRxMsg, 0, sizeof(CanRxMsg));
				BaseType_t ret = xQueueReceive(CanRecvQueueHandle, &CanRxMsg, 10);
				if (ret == pdPASS)
				{
					CanDecode(CanRxMsg.id, CanRxMsg.data);
					canRecvDelay = 0;
				}
				else
				{
					canRecvDelay = canRecvDelay + 10;
				}
				if(canRecvDelay>10*1000)
				{
					canRecvDelay = 0;
					dataInit();
				}
				QueueRet = xQueueReceive(McuDataSendHandle,uartData,0);
				if(QueueRet==pdPASS)//串口数据解析
				{
					uartDataDecode(uartData);// 解码
				}
				if((timerConuter - canSendCounter)>100)//100ms
				{
					canSendCounter = timerConuter;
					appFunc();
					CanSend100ms();
				}
				break;
			}
		}
	}
}
static void uartDataDecode(uint8 Data[])
{

    uint8 CmdIndex = Data[0];
	BleRecvDataFromUart2 DataSend={0,NULL};
	uint8 ReadLen = 0;
	sint8 McuDataRet = -1;
    switch (CmdIndex)
    {
    case 0x01: //心跳数据，为了维持通讯
    {
    	uint16 CrcChkCal = 0;
    	ReadLen = 9;
    	DataSend.RealLen = ReadLen;
    	DataSend.DataPtr = malloc(ReadLen+1);
    	memset(DataSend.DataPtr,0x00,ReadLen+1);
    	*(DataSend.DataPtr+0) = 0x11;
    	*(DataSend.DataPtr+1) = 0x02;
    	*(DataSend.DataPtr+2) = 0x00;
    	*(DataSend.DataPtr+3) = 0x01;
    	CrcChkCal = CRC16_Modbus(DataSend.DataPtr,ReadLen-2);
    	*(DataSend.DataPtr+7) = CrcChkCal;
    	*(DataSend.DataPtr+8) = CrcChkCal>>8;
    	McuDataRet= xQueueSend(McuDataRecvHandle,&DataSend,1000*2);
        break;
    }
    case 0x02: //获取换电控制器信息
    {
    	uint16 CrcChkCal = 0;
    	ReadLen = 51;
    	DataSend.RealLen = ReadLen;
    	DataSend.DataPtr = malloc(ReadLen+1);
    	memset(DataSend.DataPtr,0x00,ReadLen+1);
    	*(DataSend.DataPtr+7*0 + 0) = 0x21;
    	*(DataSend.DataPtr+7*0 + 1) = unlockFlg;
    	*(DataSend.DataPtr+7*0 + 2) = lockFlg;
    	*(DataSend.DataPtr+7*0 + 3) = VehPowerMode;
    	*(DataSend.DataPtr+7*1) = 0x22;
    	*(DataSend.DataPtr+7*2) = 0x23;
    	*(DataSend.DataPtr+7*3) = 0x24;
    	*(DataSend.DataPtr+7*4) = 0x25;
    	*(DataSend.DataPtr+7*5) = 0x26;
    	*(DataSend.DataPtr+7*6) = 0x27;
    	CrcChkCal = CRC16_Modbus(DataSend.DataPtr,ReadLen-2);
    	*(DataSend.DataPtr+49) = CrcChkCal;
    	*(DataSend.DataPtr+50) = CrcChkCal>>8;
    	McuDataRet= xQueueSend(McuDataRecvHandle,&DataSend,1000*2);
        break;
    }
    case 0x03: //获得车辆信息
    {
    	uint16 CrcChkCal = 0;
    	ReadLen = 37;
    	DataSend.RealLen = ReadLen;
    	DataSend.DataPtr = malloc(ReadLen+1);
    	memset(DataSend.DataPtr,0x00,ReadLen+1);
    	*(DataSend.DataPtr+7*0 + 0) = 0x31;
    	*(DataSend.DataPtr+7*0 + 1) = EPBWorkingSts;
    	*(DataSend.DataPtr+7*0 + 2) = BrakeSwitchSts;
    	*(DataSend.DataPtr+7*0 + 3) = 0x00;
    	*(DataSend.DataPtr+7*0 + 3) = VehPowerMode;
    	*(DataSend.DataPtr+7*1) = 0x32;
    	*(DataSend.DataPtr+7*2) = 0x33;
    	*(DataSend.DataPtr+7*3) = 0x34;
    	*(DataSend.DataPtr+7*4) = 0x35;
    	CrcChkCal = CRC16_Modbus(DataSend.DataPtr,ReadLen-2);
    	*(DataSend.DataPtr+35) = CrcChkCal;
    	*(DataSend.DataPtr+36) = CrcChkCal>>8;
    	McuDataRet= xQueueSend(McuDataRecvHandle,&DataSend,1000*2);
        break;
    }
    case 0x04: //获取电池信息
    {
    	uint16 CrcChkCal = 0;
    	ReadLen = 16*7+2;
    	DataSend.RealLen = ReadLen;
    	DataSend.DataPtr = malloc(ReadLen+1);
    	memset(DataSend.DataPtr,0x00,ReadLen+1);
    	*(DataSend.DataPtr+7*0) = 0x40;
    	*(DataSend.DataPtr+7*1) = 0x41;
    	*(DataSend.DataPtr+7*2) = 0x42;
    	*(DataSend.DataPtr+7*3) = 0x43;
    	*(DataSend.DataPtr+7*4) = 0x44;
    	*(DataSend.DataPtr+7*5) = 0x45;
    	*(DataSend.DataPtr+7*6) = 0x46;
    	*(DataSend.DataPtr+7*7) = 0x47;
    	*(DataSend.DataPtr+7*8) = 0x48;
    	*(DataSend.DataPtr+7*9) = 0x49;
    	*(DataSend.DataPtr+7*10) = 0x4A;
    	*(DataSend.DataPtr+7*11) = 0x4B;
    	*(DataSend.DataPtr+7*12) = 0x4C;
    	*(DataSend.DataPtr+7*13) = 0x4D;
    	*(DataSend.DataPtr+7*14) = 0x4E;
    	*(DataSend.DataPtr+7*15) = 0x4F;
    	CrcChkCal = CRC16_Modbus(DataSend.DataPtr,ReadLen-2);
    	*(DataSend.DataPtr+112) = CrcChkCal;
    	*(DataSend.DataPtr+113) = CrcChkCal>>8;
    	McuDataRet= xQueueSend(McuDataRecvHandle,&DataSend,1000*2);
        break;
    }
    case 0x05: //握手
    {
    	uint16 CrcChkCal = 0;
    	ReadLen = 1*7+2;
    	DataSend.RealLen = ReadLen;
    	DataSend.DataPtr = malloc(ReadLen+1);
    	memset(DataSend.DataPtr,0x00,ReadLen+1);
    	*(DataSend.DataPtr+7*0) = 0x51;
    	CrcChkCal = CRC16_Modbus(DataSend.DataPtr,ReadLen-2);
    	*(DataSend.DataPtr+1*7+0) = CrcChkCal;
    	*(DataSend.DataPtr+1*7+1) = CrcChkCal>>8;
    	McuDataRet= xQueueSend(McuDataRecvHandle,&DataSend,1000*2);
        break;
    }
    case 0x06: //解锁
    {
        break;
    }
    case 0x07: //锁紧
    {
        break;
    }
    case 0x08: //完成换电
    {
    	swapBattDone = 1;
    	uint16 CrcChkCal = 0;
    	ReadLen = 1*7+2;
    	DataSend.RealLen = ReadLen;
    	DataSend.DataPtr = malloc(ReadLen+1);
    	memset(DataSend.DataPtr,0x00,ReadLen+1);
    	*(DataSend.DataPtr+7*0) = 0x81;
    	CrcChkCal = CRC16_Modbus(DataSend.DataPtr,ReadLen-2);
    	*(DataSend.DataPtr+1*7+0) = CrcChkCal;
    	*(DataSend.DataPtr+1*7+1) = CrcChkCal>>8;
    	McuDataRet= xQueueSend(McuDataRecvHandle,&DataSend,1000*2);
        break;
    }
    case 0x09: //强制解锁，强制上升
    {
    	unlockCmdFlg = 1;
    	uint16 CrcChkCal = 0;
    	ReadLen = 1*7+2;
    	DataSend.RealLen = ReadLen;
    	DataSend.DataPtr = malloc(ReadLen+1);
    	memset(DataSend.DataPtr,0x00,ReadLen+1);
    	*(DataSend.DataPtr+7*0) = 0x91;
    	CrcChkCal = CRC16_Modbus(DataSend.DataPtr,ReadLen-2);
    	*(DataSend.DataPtr+1*7+0) = CrcChkCal;
    	*(DataSend.DataPtr+1*7+1) = CrcChkCal>>8;
    	McuDataRet= xQueueSend(McuDataRecvHandle,&DataSend,1000*2);
        break;
    }
    case 0x0A: //强制锁紧，强制下降
    {
    	lockCmdFlg = 1;
    	uint16 CrcChkCal = 0;
    	ReadLen = 1*7+2;
    	DataSend.RealLen = ReadLen;
    	DataSend.DataPtr = malloc(ReadLen+1);
    	memset(DataSend.DataPtr,0x00,ReadLen+1);
    	*(DataSend.DataPtr+7*0) = 0xA1;
    	CrcChkCal = CRC16_Modbus(DataSend.DataPtr,ReadLen-2);
    	*(DataSend.DataPtr+1*7+0) = CrcChkCal;
    	*(DataSend.DataPtr+1*7+1) = CrcChkCal>>8;
    	McuDataRet= xQueueSend(McuDataRecvHandle,&DataSend,1000*2);
        break;
    }
    case 0x0B: //回到行车状态
    {
    	swapBattDone = 0;
    	backToDrv = 1;
        break;
    }
    case 0x0C: //顶板举升
    {
        break;
    }
    case 0x0D: //顶板下降
    {
        break;
    }
    case 0x0E: //换电开始自动解锁
    {
        break;
    }
    case 0x0F: //换电完成自动锁紧
    {
        break;
    }
    case 0x10: //除尘开始
    {
        break;
    }
    case 0x11: //除尘关闭
    {
        break;
    }
    case 0x12: //切换到无线通信状态
    {
        break;
    }
    case 0x13: //进入换电状态
    {
        break;
    }
    case 0x14: //急停指令
    {
        break;
    }
    default:
        break;
    }
	if(McuDataRet!=pdPASS)//发送不成功则释放
	{
		if(DataSend.DataPtr != NULL)
		{
			free(DataSend.DataPtr);
		}
		DataSend.DataPtr = NULL;
	}
}
/*通讯板串口转CAN函数*/
static void CanSend100ms(void)
{
	Can_Msg_Type CanTxMsg;
	uint8 DataSend[8]={0};
	uint8 cmdIdx = 1;
	switch(cmdIdx)
	{
	case 1:
		/*301发送*/
		memset(DataSend,0x00,8);
		DataSend[0] = packReadySts;
		DataSend[1] = 0;
		DataSend[2] = 0;
		DataSend[3] = 0;
		DataSend[4] = 0;
		DataSend[5] = 0;
		DataSend[6] = 0;
		DataSend[7] = 0;
		CanTxMsg.id = 0x301;
		CanTxMsg.idFrame = CAN_STANDARD_ID_TYPE;
		CanTxMsg.sdu = DataSend;
		CanTxMsg.length = 8;
		CanIf_SendMessage(0, CanTxMsg);
//		break;
	case 2:
		/*302发送*/
		memset(DataSend,0x00,8);
		DataSend[0] = ((installPackNum<<4)|installPackLoc)&0xFF;
		DataSend[1] = cmdSwapBatt;
		DataSend[2] = 0;
		DataSend[3] = 0;
		DataSend[4] = 0;
		DataSend[5] = 0;
		DataSend[6] = 0;
		DataSend[7] = 0;
		CanTxMsg.id = 0x302;
		CanTxMsg.idFrame = CAN_STANDARD_ID_TYPE;
		CanTxMsg.sdu = DataSend;
		CanTxMsg.length = 8;
		CanIf_SendMessage(0, CanTxMsg);
//		break;
	case 3:
		/*303发送*/
		if(BleConnectEnable==1)
		{
			memcpy(canTargetMac,BleTargetMacAddr,6);
			memset(DataSend,0x00,8);
			DataSend[0] = 0;
			DataSend[1] = connecMacUb;
			DataSend[2] = canTargetMac[0];
			DataSend[3] = canTargetMac[1];
			DataSend[4] = canTargetMac[2];
			DataSend[5] = canTargetMac[3];
			DataSend[6] = canTargetMac[4];
			DataSend[7] = canTargetMac[5];
			CanTxMsg.id = 0x303;
			CanTxMsg.idFrame = CAN_STANDARD_ID_TYPE;
			CanTxMsg.sdu = DataSend;
			CanTxMsg.length = 8;
			CanIf_SendMessage(0, CanTxMsg);
			connecMacUb++;
		}
		break;
	default:
		break;
	}
	return;
}
static void CanDecode(uint32 AskCanID, uint8 AskData[])
{
	switch(AskCanID)
	{
	case 0x201:
		PackExcSts= AskData[0];
//		VehPowerMode = (AskData[1]>>3)&0x01;
		GearSts = (AskData[1]>>4)&0x01;
		EPBWorkingSts = (AskData[1]>>5)&0x01;
		BrakeSwitchSts = (AskData[1]>>6)&0x01;
		DoorSts = (AskData[1]>>7)&0x01;
		SteeringWheelAngle = ((AskData[2]&0x07)<<13)|(AskData[2]<<5)|(AskData[3]>>3);
		BattSwappingAllw = (AskData[3]>>2)&0x01;
		BlueDisconnectFailFlg = (AskData[3]>>1)&0x01;
		AllPackPwrDnSts= (AskData[4]>>4)&0x0F;
		break;
	case 0x202:
		break;
	case 0x203:
		carBleConnectSts = AskData[0]&0x0F;
		BleConnectFlg = carBleConnectSts;
		break;
	case 0x205:
		lockAndUnlockSensor[0] = (AskData[1]>>4)&0x0F;
		lockAndUnlockSensor[1] = (AskData[1])&0x0F;
		lockAndUnlockSensor[2] = (AskData[2]>>4)&0x0F;
		lockAndUnlockSensor[3] = (AskData[2])&0x0F;
		for(uint8 i=0;i<4;i++)
		{
			if(lockAndUnlockSensor[i]==0x01)
			{
				setbit(lockFlg,i);
				clrbit(unlockFlg,i);
			}
			else if(lockAndUnlockSensor[i]==0x02)
			{
				setbit(unlockFlg,i);
				clrbit(lockFlg,i);
			}
		}
		break;
	case 0x206:
	{
		VehPowerMode = AskData[4];
		break;
	}
	default:
		break;
	}
}
static void dataInit()
{
	uint8  AskData[8] = {0};
	PackExcSts= AskData[0];
	VehPowerMode = (AskData[1]>>3)&0x01;
	GearSts = (AskData[1]>>4)&0x01;
	EPBWorkingSts = (AskData[1]>>5)&0x01;
	BrakeSwitchSts = (AskData[1]>>6)&0x01;
	DoorSts = (AskData[1]>>7)&0x01;
	SteeringWheelAngle = ((AskData[2]&0x07)<<13)|(AskData[2]<<5)|(AskData[3]>>3);
	BattSwappingAllw = (AskData[3]>>2)&0x01;
	BlueDisconnectFailFlg = (AskData[3]>>1)&0x01;
	AllPackPwrDnSts= (AskData[4]>>4)&0x0F;
	carBleConnectSts = AskData[0]&0x0F;
}
static void appFunc()
{
	if(BleDisconnectEnable==1)
	{
		connecMacUb = 0;
		cmdSwapBatt = 2;
		packReadySts = 0;
		installPackNum = 0;
		installPackLoc = 0;
		swapBattDone = 0;
		lockCmdFlg = 0;
	}
	if(carBleConnectSts==1 && BleDisconnectEnable==0)//蓝牙连接成功
	{
		xSemaphoreGive(ConnSemaphore);
		cmdSwapBatt = 0;
	}
	if(unlockCmdFlg)//开始换电
	{
		installPackNum = 1;
		installPackLoc = 1;
		cmdSwapBatt = 1;
	}
	if(swapBattDone||lockCmdFlg)//锁紧到位
	{
		packReadySts = 1;
		connecMacUb = 0;
		unlockCmdFlg = 0;
	}
	if(PackExcSts==0x03)//换电电池检测完成
	{
		packReadySts = 0;
		installPackNum = 0;
		installPackLoc = 0;
		cmdSwapBatt = 0;
	}

}








/*常规蓝牙程序函数*/
static void CanDataAsk(QueueDataTrans_Type AskData)
{
	Can_Msg_Type CanTxMsg;
	uint16 CrcGet = 0x00;
	uint16 CrcCal = 0xff;
	if(AskData.RealLen==10)
	{
		CrcGet = (*(AskData.DataPtr + AskData.RealLen - 2)<<0 )| (*(AskData.DataPtr + AskData.RealLen - 1)<<8 );
		CrcCal = CRC16_Modbus(AskData.DataPtr,AskData.RealLen-2);
		if(CrcGet==CrcCal)
		{
			CanTxMsg.id = 0X1CEF0000;
			CanTxMsg.idFrame = CAN_EXTENDED_ID_TYPE;
			CanTxMsg.sdu = AskData.DataPtr;
			CanTxMsg.length = 8;
			CanIf_SendMessage(0, CanTxMsg);
		}
	}
	else if(AskData.RealLen==1)//从机连接与断开，主动给EBC发送一帧状态
	{
		CanTxMsg.id = 0X1CEF0002;
		CanTxMsg.idFrame = CAN_EXTENDED_ID_TYPE;
		CanTxMsg.sdu = AskData.DataPtr;
		CanTxMsg.length = 8;
		CanIf_SendMessage(0, CanTxMsg);
	}

	if(AskData.DataPtr)
	{
		free(AskData.DataPtr);
	}
	AskData.DataPtr = NULL;
}
void BleCanTransmit(uint32 AskCanID, uint8 AskData[])
{
	Can_Msg_Type CanTxMsg;
	uint32 AnsCanID = 0;
	uint8 AnsData[8];
	memset(AnsData, 0x00, 8);
	static QueueDataTrans_Type CanAnsData={0,NULL};
	static uint8 CanDataCounter = 0;//数据打包需要的包数
	static uint8 DataPackDone = 0;//已打包数目
	static uint8 CanByte0Idx = 0;
	static volatile uint8 PackState = 0;//0-未打包，1-正在打包，2-打包完成
	uint8 CanAnsPerLen = 7;
	uint8 CanByte0Seq = 0;
	switch (AskCanID)
	{
	case 0X1CEE0000: //数据通道，换电控制的数据发过来在此打包后转uart2
	{
		if(PackState==0)
		{
			DataPackDone = 0;
			CanDataCounter = DataPackCounter[(AskData[0]>>4)];
			CanAnsData.RealLen = CanDataCounter*CanAnsPerLen + 2;
			CanAnsData.DataPtr = malloc(CanAnsData.RealLen);
			memset(CanAnsData.DataPtr,0x00,CanAnsData.RealLen);
			CanByte0Idx = (AskData[0]>>4);
			PackState = 1;
		}
		if(PackState==1)
		{
			if(CanByte0Idx==(AskData[0]>>4))
			{
				CanByte0Seq = (AskData[0])&0x0F;
				if(CanByte0Idx==4)
				{
					CanByte0Seq = CanByte0Seq + 1;
				}
				if(DataPackDone<CanDataCounter)
				{
					memcpy(CanAnsData.DataPtr+(CanByte0Seq - 1)*CanAnsPerLen,AskData,CanAnsPerLen);//从CAN信息中只截取前7个数据
					DataPackDone++;
				}
				if(DataPackDone==CanDataCounter)
				{
					CanAnsData.RealLen = DataPackDone*CanAnsPerLen + 2;
					PackState = 2;
				}
			}
			else//序号不对，打包立刻完成，更改数据长度
			{
				CanAnsData.RealLen = DataPackDone*CanAnsPerLen + 2;
				PackState = 2;
			}
		}
		if(PackState==2)
		{
			uint16 CrcCal = 0;
			CrcCal = CRC16_Modbus(CanAnsData.DataPtr,CanAnsData.RealLen-2);
			*(CanAnsData.DataPtr + CanAnsData.RealLen-2) = CrcCal &0xFF;
			*(CanAnsData.DataPtr + CanAnsData.RealLen-1) = (CrcCal>>8) &0xFF;
			if(xQueueSend(CanToUart2DataHandle,&CanAnsData,1000*10)!=pdPASS)
			{
				if(CanAnsData.DataPtr)
				{
					free(CanAnsData.DataPtr);
				}
				CanAnsData.DataPtr = NULL;
				CanAnsData.RealLen = 0;
			}
			PackState = 0;
		}
		CanTxMsg.id = AnsCanID;
		CanTxMsg.idFrame = CAN_EXTENDED_ID_TYPE;
		CanTxMsg.sdu = AnsData;
		CanTxMsg.length = 8;
		CanIf_SendMessage(0, CanTxMsg);
		return;
	}
	case 0X1CEE0001: //查询
	{
		if (AskData[0] == 1) //软件版本查询
		{
			AnsData[0] = 0x01;
			AnsData[1] = (BleVersion >> 24) & 0xFF;
			AnsData[2] = (BleVersion >> 16) & 0xFF;
			AnsData[3] = (BleVersion >> 8) & 0xFF;
			AnsData[4] = (BleVersion >> 0) & 0xFF;
		}
		else if (AskData[0] == 2) // Mac地址查询
		{
			AnsData[0] = 0x02;
			AnsData[1] = (BleMacAddr[0]) & 0xFF;
			AnsData[2] = (BleMacAddr[1]) & 0xFF;
			AnsData[3] = (BleMacAddr[2]) & 0xFF;
			AnsData[4] = (BleMacAddr[3]) & 0xFF;
			AnsData[5] = (BleMacAddr[4]) & 0xFF;
			AnsData[6] = (BleMacAddr[5]) & 0xFF;
		}
		AnsCanID = 0X1CEF0001;
		CanTxMsg.id = AnsCanID;
		CanTxMsg.idFrame = CAN_EXTENDED_ID_TYPE;
		CanTxMsg.sdu = AnsData;
		CanTxMsg.length = 8;
		CanIf_SendMessage(0, CanTxMsg);
		break;
	}
	case 0X1CEE0002: //连接控制
	{
		if (AskData[0] == 1) //连接状态查询
		{
			AnsData[0] = 0x01;
			AnsData[1] = (BleDisconnectFlg<<1)|(BleConnectFlg);
		}
		else if (AskData[0] == 2) //连接指定Mac地址
		{
			BleTargetMacAddr[0] = AskData[1];
			BleTargetMacAddr[1] = AskData[2];
			BleTargetMacAddr[2] = AskData[3];
			BleTargetMacAddr[3] = AskData[4];
			BleTargetMacAddr[4] = AskData[5];
			BleTargetMacAddr[5] = AskData[6];
			BleConnectEnable = 1;
			AnsData[0] = 2;
			AnsData[1] = 2;
		}
		else if (AskData[0] == 3) //断开连接
		{
			BleDisconnectEnable = 1;
			AnsData[0] = 3;
			AnsData[1] = 3;
		}
		AnsCanID = 0X1CEF0002;
		CanTxMsg.id = AnsCanID;
		CanTxMsg.idFrame = CAN_EXTENDED_ID_TYPE;
		CanTxMsg.sdu = AnsData;
		CanTxMsg.length = 8;
		CanIf_SendMessage(0, CanTxMsg);
		break;
	}
	case 0X1CEE0003: //角色控制
	{
		if (AskData[0] == 1) //当前角色查询
		{
			AnsData[0] = 0x01;
			AnsData[1] = BleCurrentRole;
		}
		else if (AskData[0] == 2) //角色设置
		{
			BleSetRoleEnable = 1;
			BleSetRole = AskData[1] & 0x03;
			AnsData[0] = 0x02;
			AnsData[1] = BleSetRole;
		}
		AnsCanID = 0X1CEF0003;
		CanTxMsg.id = AnsCanID;
		CanTxMsg.idFrame = CAN_EXTENDED_ID_TYPE;
		CanTxMsg.sdu = AnsData;
		CanTxMsg.length = 8;
		CanIf_SendMessage(0, CanTxMsg);
		break;
	}
	case 0X1CEE0004: //蓝牙开关机控制
	{
		if (AskData[0] == 1)
		{
			if (AskData[1] == 0) //开机
			{
				BlePwrControlEnable = 1;
				BlePwrControlFlg = 0;
			}
			else if (AskData[1] == 1) //关机
			{
				BlePwrControlEnable = 1;
				BlePwrControlFlg = 1;
			}
			else if (AskData[1] == 2) //重启
			{
				BlePwrControlEnable = 1;
				BlePwrControlFlg = 2;
			}
			AnsData[0] = 1;
			AnsData[1] = AskData[1];
		}
		AnsCanID = 0X1CEF0004;
		CanTxMsg.id = AnsCanID;
		CanTxMsg.idFrame = CAN_EXTENDED_ID_TYPE;
		CanTxMsg.sdu = AnsData;
		CanTxMsg.length = 8;
		CanIf_SendMessage(0, CanTxMsg);
		break;
	}
	case 0x1C00EBEA:
	{
		udsService(AskData);
		break;
	}
	default:
		return;
		break;
	}
	return;
}
void udsService(uint8 *DataIn)
{
	static uint8 udsMode = 0;
	uint8 UdsService = *(DataIn + 0);
	uint8 UdsSubService = *(DataIn + 1);
	uint8 AnsData[8]={0};
	switch(UdsService)
	{
	case 0x10:
	{
		if(UdsSubService==0x01)
		{
			udsMode = 1;
			UdsAns(true,UdsService,UdsSubService,0x00,NULL,0);
		}
		else if(*(DataIn+1)==0x02)
		{
			if(udsMode==1||udsMode==2)
			{
				udsMode = 2;
				UdsAns(true,UdsService,UdsSubService,0x00,NULL,0);
			}
			else
			{
				udsMode = 0;
				UdsAns(false,UdsService,UdsSubService,0x00,NULL,0);
			}
		}
		else
		{
			udsMode = 0;
			UdsAns(false,UdsService,UdsSubService,0x00,NULL,0);
		}
		break;
	}
	case 0x22:
	{
		if(udsMode==2)
		{
			switch(UdsSubService)
			{
			case 0x01://软件版本查询
			{
				AnsData[0] = (BleVersion >> 24) & 0xFF;
				AnsData[1] = (BleVersion >> 16) & 0xFF;
				AnsData[2] = (BleVersion >> 8) & 0xFF;
				AnsData[3] = (BleVersion >> 0) & 0xFF;
				UdsAns(true,UdsService,UdsSubService,0x00,AnsData,4);
				break;
			}
			case 0x02://MAC地址查询
			{
				memcpy(AnsData,BleMacAddr,6);
				for(uint8 i=0;i<2;i++)
				{
					UdsAns(true,UdsService,UdsSubService,i,&AnsData[i*4],4);
				}
				break;
			}
			case 0x03://角色查询
			{
				AnsData[0] = (BleCurrentRole) & 0xFF;
				UdsAns(true,UdsService,UdsSubService,0x00,AnsData,1);
				break;
			}
			case 0x04://连接查询
			{
				AnsData[0] = (BleConnectFlg) & 0xFF;
				UdsAns(true,UdsService,UdsSubService,0x00,AnsData,1);
				break;
			}
			default:
			{
				UdsAns(false,UdsService,UdsSubService,0x00,NULL,0);
				break;
			}
			}
		}
		else
		{
			UdsAns(false,UdsService,UdsSubService,0x00,NULL,0);
		}
		break;
	}
	default:
	{
		UdsAns(false,0x00,0x00,0x00,NULL,0);
		break;
	}
	}
}
static void UdsAns(bool PosFlg,uint8 UdsService,uint8 UdsSubService,uint8 Idx,uint8 *AnsData,uint8 AnsDataLen)
{
	Can_Msg_Type CanTxMsg;
	uint32 CanAnsId = 0x1C00EAEB;
	uint8 SendData[8] = {0};
	if(PosFlg)
	{
		SendData[0] = 0x78;
		SendData[1] = UdsService;
		SendData[2] = UdsSubService;
		SendData[3] = Idx;
		if(AnsData!=NULL&&AnsDataLen!=0x00)
		{
			memcpy(&SendData[4],AnsData,AnsDataLen);
		}
	}
	else
	{
		SendData[0] = 0x7F;
		SendData[1] = UdsService;
		SendData[2] = UdsSubService;
		SendData[3] = 0x00;
	}
	CanTxMsg.id = CanAnsId;
	CanTxMsg.idFrame = CAN_EXTENDED_ID_TYPE;
	CanTxMsg.sdu = SendData;
	CanTxMsg.length = 8;
	CanIf_SendMessage(0, CanTxMsg);
}
