/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  ** This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * COPYRIGHT(c) 2018 STMicroelectronics
  *
  * Redistribution and use in source and binary forms, with or without modification,
  * are permitted provided that the following conditions are met:
  *   1. Redistributions of source code must retain the above copyright notice,
  *      this list of conditions and the following disclaimer.
  *   2. Redistributions in binary form must reproduce the above copyright notice,
  *      this list of conditions and the following disclaimer in the documentation
  *      and/or other materials provided with the distribution.
  *   3. Neither the name of STMicroelectronics nor the names of its contributors
  *      may be used to endorse or promote products derived from this software
  *      without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "stm32f0xx_hal.h"

/* USER CODE BEGIN Includes */
#include "Globaldef.h"
#include "debug.h"
#include "Functions.h"
#include "KMachine.h"
#include "PLCfunctions.h"
//#include "KBus.h"
#include "KLink.h"
#include "string.h"
#include "BSP.h"
#include "ModbusRTU.h"
#if (BOARD_TYPE == 13)
#include "w5500_port.h"
#include "../src/Ethernet/socket.h"
#include "../src/Ethernet/loopback.h"
#elif (BOARD_TYPE == 14)
#include "FPx.h"
#elif (BOARD_TYPE == 15 || BOARD_TYPE == 16)
#include "KWireless.h"
//#include "user.h"
//#include "../src/radio/inc/sx126x-board.h"
#endif
#include "SLP.h"
//#include "YDLidar.h"
#include "OrdLidar.h"

/* USER CODE END Includes */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
/* Private variables ---------------------------------------------------------*/

#define RX2BUFSIZE 64
#define TX2BUFSIZE 64


unsigned char Uart1RxBuf[RX1BUFSIZE];
unsigned char Uart1TxBuf[TX1BUFSIZE];

unsigned char Uart2RxBuf[RX2BUFSIZE];
unsigned char Uart2TxBuf[TX2BUFSIZE];

unsigned char SlowFlicker=0;
unsigned char FastFlicker=0;

unsigned int Uart1IdelTimer = 0;
#if (ENABLE_PLC)
stBinProg1 * pProgs = (stBinProg1 *)STORE_PRG_BASE;
#endif

uint32_t us1,us2,us3,us4,us5,us6;

stKBusDef KBus1;							// 

extern 	stDeviceInfo MyDeviceInfo;

unsigned char bSLPMaster =1;
unsigned char nSLPStation = 1;

stSLPDef SLP1;
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/


/* USER CODE BEGIN PFP */
/* Private function prototypes -----------------------------------------------*/

const unsigned char LEDSEGTAB[]={
0x3f,0x06,0x5b,0x4f,0x66,0x6d,0x7d,0x07,0x7f,0x6f,0x77,0x7c,0x39,0x5e,0x79,0x71,	//0-F
0xbf,0x86,0xdb,0xcf,0xe6,0xed,0xfd,0x87,0xff,0xef,0xf7,0xfc,0xb9,0xde,0xf9,0xf1,  //0.-F.
0x00,0x40,			//  ,-,_,~,o,n,N,<,>,J,r,
};

/* USER CODE END PFP */

/* USER CODE BEGIN 0 */

int HexToInt(char ch)
{
	if (ch>='0' && ch <='9') return ch-'0';
	if (ch>='A' && ch <='F') return ch-'A'+10;
	if (ch>='a' && ch <='f') return ch-'a'+10;
	return 0;
}

void HAL_SYSTICK_Callback(void)
{
static int Count=0;
	CurTickuS += 100;	
	nCurTick++;
	KBus1.nSlaveTick++;
	Count++;
	if (Count>=10000) 
	{
		Count=0; 
		KMem.CurTimeSec++;
		KMem.ThisRunTime++; KMem.TotalRunTime++;
		if (KMRunStat.bLEDFlick) KMRunStat.bLEDFlick--;
		if (KMRunStat.bLEDFlick >120) KMRunStat.bLEDFlick=120;
	}

	return;
}

void PendSvCallBack()
{
#if (BOARD_TYPE == 14)
///*	
		if (bSPI1RecvDone)
		{
			bSPI1RecvDone=0;
			FPxParsePkt(SPI1RecvBuf,nSPI1RecvLenInBuf);
		}
//*/	
#endif		
	if (Uart2Stat.bPacketRecved)
	{
		KBusParsePacket(&KBus1, (pKBPacket)Uart2RecvBuf1, Uart2RecvBuf1DataLen);		
		Uart2RecvBuf1DataLen=0;
		Uart2Stat.bPacketRecved=0;
		Uart2RecvDMA(Uart2RecvBuf1,sizeof(Uart2RecvBuf1));		
	}
}

void * KBusEvCallBackFunc(void * pParam, int nEvent, void *pBuf, int nLen1)
{
	switch (nEvent){
		
		case KBusEvNone:
			break;
		case KBusEvCreate:
			break;
		case KBusEvConnected:
			break;
		case KBusEvDisConnected:
			break;
		case KBusEvClosed:
			break;
		case KBusEvStateChange:
			break;
		case KBusEvTimeSync:
			break;
		case KBusEvDataUpdate:
			KMem.WY[0]=KBusMem.WLY[0];			//KBus Slave
			KBusMem.WLX[0]=KMem.WX[0];
			KBusMem.WLX[1]=KMem.WX[1];
			KBusMem.WLX[2]=KMem.WX[2];
			KBusMem.WLX[3]=KMem.WX[3];	
			break;
		case KBusEvCmdResponse:
			break;
		
		default:
			break;
	}
	return 0;
}

//#define RAM_START_ADDR 0x20000000    // SRAM_BASE
#define VECTOR_SIZE 46 
#define  ApplicationAddress  0x08001000  //应用程序首地址定义
/*
static void RemapIrqVector(void)
{
	memcpy((void*)RAM_START_ADDR, (void *)ApplicationAddress, VECTOR_SIZE * 4);
	LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_SYSCFG);
	LL_SYSCFG_SetRemapMemory(LL_SYSCFG_REMAP_SRAM);
}
*/
/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  *
  * @retval None
  */
int main(void)
{
  /* USER CODE BEGIN 1 */
//	RemapIrqVector();
	  __set_PRIMASK(0);    //打开全局中断
	
	KMRunStat.bLEDFlick = 1;
	
	InitUartstat(&Uart1Stat,Uart1RxBuf,sizeof(Uart1RxBuf),Uart1TxBuf,sizeof(Uart1TxBuf));
	InitUartstat(&Uart2Stat,Uart2RxBuf,sizeof(Uart2RxBuf),Uart2TxBuf,sizeof(Uart2TxBuf));
  /* USER CODE END 1 */

  /* MCU Configuration----------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

	
		KMem.LastScanTime=0;
		KMem.ScanTimeuS=0;
		KMem.MinScanTimeuS=99999;
		KMem.MaxScanTimeuS=0;

//		KMem.SDD[14]=(unsigned int)&KMStoreSysCfg;
//		KMem.SDD[15]=(unsigned int)&KMStoreSysCfg1;
		KMem.SDD[12]=((uint32_t *)UID_BASE)[0];
//		KMem.SDD[13]=((uint32_t *)UID_BASE)[1];
//		KMem.SDD[14]=((uint32_t *)UID_BASE)[2];
		KMem.SDD[13]=PendSvCount;
		KMem.SDD[14]=RCC->CSR;
//		KMem.SDD[15]=*(uint32_t *)FLASHSIZE_BASE;
//		KMem.SDD[16]=(unsigned int)&KMSysCfg;
	
  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */
	TickFreq=10000;		//Tick频率
	InituS(TickFreq);	
 // HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/TickFreq);	//重新定义SysTick的频率

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
	
	KMachineInit();
	ReadSysCfgFromFlash(&storedKMSysCfg);
	
	KMRunStat.bLEDFlick = 1;
	int bKBusMaster,bKBusSlave,bKBusRepeater;;
	int nKBusStationId;
	int nKBusChilds;
	KMem.CurJumperSW=ReadJumperSW();
	KMem.EffJumperSW=KMem.CurJumperSW;
	nKBusStationId=KMem.EffJumperSW&0x0f;	
	
	nKBusChilds = nKBusStationId;
	
	bSLPMaster = 1; // KMem.EffJumperSW&0x20 ; //master?
	nSLPStation = 1;
	SLP1.bSLPMaster = 1;
	SLP1.nStation = 1;
	SLPInit(&SLP1,Uart6SendPacket);
//	Uart2Baud = AlterUart2Baud;
	

	
#if (BOARD_TYPE == 14)
	KMem.EffJumperSW|=0x10;
	int nKBusChilds=KMem.EffJumperSW&0x0f;
  if ((KMem.EffJumperSW&0x10)!=0) {bKBusMaster=1;bKBusSlave=0;}
	else{bKBusMaster=0;bKBusSlave=1;}

	FPxSetCallBackFunc(&FPxCallBackFunc);
	FPx_Init(nKBusChilds);

	int IOByteCount = nKBusChilds;
	FPx_SetIOCount(IOByteCount,IOByteCount);
	

#elif (BOARD_TYPE == 15 || BOARD_TYPE == 16)
	nStationID=1 ;//KMem.EffJumperSW&0x0f;
//	if (KMem.EffJumperSW == 0x1f) {bKBusRepeater=1;bKBusMaster=1;bKBusSlave=0;}
//  else if ((KMem.EffJumperSW&0x10)!=0) {bKBusMaster=1;bKBusSlave=0;}
//	else
		{bKBusMaster=0;bKBusSlave=1;}	
#else
	nKBusStationId=KMem.EffJumperSW&0x0f;
	if (KMem.EffJumperSW == 0x3f) {bKBusRepeater=1;bKBusMaster=1;bKBusSlave=0;}
  else if ((KMem.EffJumperSW&0x20)!=0) {bKBusMaster=1;bKBusSlave=0;}
	else{bKBusMaster=0;bKBusSlave=1;}
#endif
	
	if (bKBusMaster) {
		KBusInitMaster(&KBus1, (KBusSendPktFuncDef)PutStr2, nKBusChilds);
	
	} else if (bKBusSlave) {
		KBusInitSlave(&KBus1, (KBusSendPktFuncDef)PutStr2, nKBusStationId,&MyDeviceInfo);
	}
	
	KBusSetEvCallBackFunc(&KBus1, &KBusEvCallBackFunc);	

	//if (KMem.EffJumperSW == 0x00)
		Uart1Baud = DefaultUart1Baud;
  MX_USART1_UART_Init();
  MX_USART2_UART_Init();

	MX_USART3_UART_Init();
	
	LL_USART_EnableIT_RXNE(USART3);
	LL_USART_EnableIT_IDLE(USART3);	
	
	MX_USART5_UART_Init();
	LL_USART_EnableIT_RXNE(USART5);
	LL_USART_EnableIT_IDLE(USART5);
	
	MX_USART6_UART_Init();
	LL_USART_EnableIT_RXNE(USART6);
	LL_USART_EnableIT_IDLE(USART6);

	
//	MX_SPI1_Init();
//	LL_SPI_EnableIT_RXNE(SPI1);

#if (BOARD_TYPE == 14)
//	MX_SPI2_Init();
//  MX_ADC_Init();
#else
//	MX_SPI2_Init();
  MX_ADC_Init();
#endif

	MX_IWDG_Init();

	MX_TIM6_Init();
	LL_TIM_EnableCounter(TIM6);
	
  /* USER CODE BEGIN 2 */
	LL_USART_EnableIT_RXNE(USART1);
	LL_USART_EnableIT_IDLE(USART1);
	LL_USART_EnableIT_TC(USART1);

//	LL_USART_EnableIT_RXNE(USART2);
	Uart2RecvDMA(Uart2RecvBuf1,sizeof(Uart2RecvBuf1));	
	LL_USART_EnableIT_IDLE(USART2);
	LL_USART_EnableIT_TC(USART2);
#if (BOARD_TYPE == 13)
	int res;
	res = w5500_init();
	KMem.SDD[28]=res;
	
//	res=socket(0,Sn_MR_TCP,5000,0);
	KMem.SDD[29]=res;	
	
//	res = listen(0);
#endif	
//	if (bKBusSlave)
	{
	//	LL_USART_EnableAutoBaudRate(USART1);
	//	LL_USART_SetAutoBaudRateMode(USART1, LL_USART_AUTOBAUD_DETECT_ON_FALLINGEDGE);		
	//	LL_USART_EnableAutoBaudRate(USART2);
	//	LL_USART_SetAutoBaudRateMode(USART2, LL_USART_AUTOBAUD_DETECT_ON_FALLINGEDGE);		
	}
	//LL_USART_EnableIT_TXE(USART1);
  /* USER CODE END 2 */

	
  /* Infinite loop */
  /* USER CODE BEGIN WHILE */

	HAL_Delay(10);				
	SetRunLed(1);				//Turn On Run Led
	SetErrLed(0);				//Turn Off Err Led

#if (BOARD_TYPE == 14)
//	PutOutput (0);					//Clear all Output
//	Enable595(1);						//Enable 595 Output 
#else
	PutOutput (0);					//Clear all Output
	Enable595(1);						//Enable 595 Output 
#endif

		if (GetBoardType() == 7 || GetBoardType() ==8 
			|| GetBoardType() == 9 || GetBoardType() ==10 ||GetBoardType() ==13 ||GetBoardType() ==15 || BOARD_TYPE == 16) 
		{
			displayInput(0xffff);		//
			EnableDisIn(1);				//Input Diaplay Enable 595 
		}
	SetOutStat(0);			//OK Good, signal
	ShowInitInfo();
	KMem.LastScanTime = GetuS();

	KMRunStat.WorkMode=0;
	KMRunStat.WorkMode2=0;
		
	KMRunStat.WorkMode = storedKMSysCfg.theKMSysCfg.workmode;
#if (ENABLE_PLC)		
	if (KMRunStat.WorkMode == 1){
		InitPLC();
		KMRunStat.WorkMode2 = KMem.CurJumperSW&0x20 ;
		if (KMRunStat.WorkMode2) {
			StartPLC(); }
	}
#endif	

#if (BOARD_TYPE == 15 || BOARD_TYPE == 16)
	KWireLessInit(KMem.EffJumperSW&0x20,KMem.EffJumperSW&0x0f);
	KWireLessStart();
#endif
	
	OrdLidarStart(-1);
	
  while (1)
  {
		//int MyKeyStat1,MyKeyStat2;
		//MyKeyStat1=GetInput();

		//*((unsigned int *)&(PLCMem.SDT[10]))=nRunCount;
	//	KMem.nRunCount=nRunCount;
		SlowFlicker=0;
		FastFlicker=1;		
		us1=GetuS();
		int haltick=HAL_GetTick();
		
		int thisJumperSW=ReadJumperSW();
#if (ENABLE_PLC)		
		if (KMRunStat.WorkMode&1){
			if (thisJumperSW&0x20 && !(KMem.CurJumperSW&0x20))  // Run 开关 正 跳变。
			{StartPLC();}
			if (!(thisJumperSW&0x20) && (KMem.CurJumperSW&0x20))  // Run 开关 负 跳变。
			{StopPLC();}
		}
#endif		
		KMem.CurJumperSW=thisJumperSW;
		KMem.haltick=haltick;
//		KMem.TotalRunTime=TotalRunTime;
//		KMem.ThisRunTime=ThisRunTime;		
		
//		*((unsigned int *)&(PLCMem.SDT[2]))=nChilds;
//		KMem.SDD[13]=PendSvCount;
//		KMem.SDD[14]=RCC->CSR;		
		
		int a;
		a		= LL_GPIO_ReadInputPort(GPIOA);
		KMem.WDT[120]=a;
		a		= LL_GPIO_ReadInputPort(GPIOB);
		KMem.WDT[121]=a;
		a		= LL_GPIO_ReadInputPort(GPIOC);
		KMem.WDT[122]=a;
		a		= LL_GPIO_ReadInputPort(GPIOD);
		KMem.WDT[123]=a;
		

		KMem.WXB[0]= GetInput();		
		
		us2=GetuS();
		if (PowerDownEvent) {		KMem.WX[0]=0;}
// /*
		if ((KMem.nRunCount &0x1f) == 0x02)
		{
			ADCProcess();
			if (PowerDownEvent)
			{
				KMem.WX[0]=0;
				if (!OldPowerDownEvent)
				{
					OldPowerDownEvent = PowerDownEvent;
					OldPowerDownEventTime = nCurTick;
					PowerDownProcess();
				}
			}else
			{
				if (OldPowerDownEvent)
				{
					OldPowerDownEvent=PowerDownEvent;
					PowerRecoverProcess();
					
				}
			}
		}
// */

//		pProgs = (stBinProg1 *) STORE_PRG_BASE;
#if (ENABLE_PLC)
		if (	KMRunStat.WorkMode==1 ) //&& bKBusMaster)
		{
			if (KMRunStat.nBinProgBank == 0){
				pProgs=(stBinProg1 *)STORE_PRG_BASE;
			}else {
				pProgs=(stBinProg1 *)ALT_PRG_BASE;
			}
			nSizeProg1=KMRunStat.nBinProgSize;
		//	pProgs=(stBinProg1 *)prog1;
			
			ProcessPLCBinProg(pProgs, nSizeProg1);
		}
#endif
		
		KMem.ScanTimeuS=us2-KMem.LastScanTime;
		KMem.LastScanTime = us2;
		if (KMem.ScanTimeuS < KMem.MinScanTimeuS) {KMem.MinScanTimeuS = KMem.ScanTimeuS;}
		if (KMem.ScanTimeuS > KMem.MaxScanTimeuS) {KMem.MaxScanTimeuS = KMem.ScanTimeuS;}

		//		if (bKBusRepeater)		{	KBusRepeaterFunc();	}

		us3=GetuS();

		if (bKBusMaster)		
		{
			KBusLoopProcess(&KBus1);
		}
			if (haltick&0x00002000) SlowFlicker=1;
			else SlowFlicker=0;
			if (haltick&0x00000800) FastFlicker=1;
			else FastFlicker=0;	

		if (bKBusSlave)		
		{

			KBusLoopProcess(&KBus1);	
		//	if (! KBus1.RunStat) {KBusMem.WLY[0]=0;}
			KMem.WLY[0]=KBusMem.WLY[0];

			if (KBus1.nSlaveTick&0x00002000) SlowFlicker=1;
			else SlowFlicker=0;
			if (KBus1.nSlaveTick&0x00000800) FastFlicker=1;
			else FastFlicker=0;			

		}
		KBusMem.WLX[0]=KMem.WX[0];
		KMem.WY[0]=KBusMem.WLY[0];
		KBusMem.WLX[1]=KMem.WX[1];
		KBusMem.WLX[2]=KMem.WX[2];
		KBusMem.WLX[3]=KMem.WX[3];
		
		
		if (KMRunStat.bLEDFlick)
		{
			SetRunLed(FastFlicker);
			SetErrLed(FastFlicker);
			SetErr2Led(FastFlicker);
			SetOutStat(!FastFlicker);
			//KMRunStat.bLEDFlick-- ;
		}
		else
		{
#if (ENABLE_PLC)			
			if (KMRunStat.WorkMode==1 ) {
				if (PLCMem.bPLCRunning){SetRunLed(SlowFlicker);}
					else {SetRunLed(0);}
			}
			else 
#endif				
			{
					if (!KMem.RunStat) SetRunLed(SlowFlicker);
					else SetRunLed(FastFlicker);
			}
			KMem.ErrStat = KBus1.ErrStat + SLP1.SLPErrSign;
			if (!KMem.ErrStat) 
			{
				SetErrLed(0);
				SetErr2Led(0);
				SetOutStat(1);
			}
			else 
			{
				SetErrLed(FastFlicker);
				SetErr2Led(FastFlicker);
				SetOutStat(0);
				
			}
		}
		
//		SetRunLed(RunStat);
//		SetErrLed(ErrStat);
		
		us4=GetuS();
//		EffJumperSW = GetInput(20)&0xff;


		us5=GetuS();
		

		//PutOutput (KMem.nRunCount>>8);
		//PutOutput(0x0f70);

//		if (bKBusMaster) ShowInfo();
//		if (bKBusSlave) ShowInfo();
		us6=GetuS();
		add1(10,10);
		for (int i=0;i<64;i++)
		{
//			ProcessTimer(i);
		}
		KMem.nRunCount++;
//		int nSize=sizeof(stKBusChnStat);
//		memcpy(&KMem.SDT[64],&KBusChnStats[1],nSize);
//		memcpy(&KMem.SDT[64+nSize/2],&KBusChnStats[2],nSize);
//		for (int i=0;i<128;i++)	{		SDT[i]=i;	}
//		SDT[48]=55;
		if (Uart1Stat.bPacketRecved && Uart1RecvBuf1DataLen >0)
		{
			int res1 = -1;
			if (Uart1RecvBuf1[0] == KLSignStart) {
				res1 = KLParsePacket(1, Uart1RecvBuf1, Uart1RecvBuf1DataLen);
			}else {
				res1 = ModBusSlaveParsePkg(1, Uart1RecvBuf1, Uart1RecvBuf1DataLen);
			}
			Uart1Stat.bPacketRecved=0;
			Uart1RecvBuf1DataLen=0;
			Uart1IdelTimer = 0;
		}else {
			if (Uart1IdelTimer>600000) { // 超过60秒没有数据传输，重新进入自适应波特率状态
				LL_USART_EnableAutoBaudRate(USART1);
				LL_USART_SetAutoBaudRateMode(USART1, LL_USART_AUTOBAUD_DETECT_ON_FALLINGEDGE);
			}else {
					Uart1IdelTimer++;
			}
		}
	if (bKBusSlave)	HAL_Delay(0);
		
	if (Uart6Stat.bPacketRecved){
		SLPparsePacket(&SLP1,Uart6RxBuf,Uart6RecvBuf1DataLen);
		Uart6RecvBuf1DataLen =0;
		Uart6Stat.bPacketRecved = 0;
	}
		SLP1.SLPinputB =  KMem.WYB[1];
		SLPProcess(&SLP1);
		KMem.WXB[1] = SLP1.SLPoutputB;

		// YDLiDar process;

	if (Uart3Stat.bPacketRecved){
		KMem.WDT[8]++;
		OrdLidarParsePkt(0,(OradarLidarFrame *)Uart3RxBuf,Uart3RecvBuf1DataLen);
		Uart3RecvBuf1DataLen =0;
		Uart3Stat.bPacketRecved = 0;
	}
	KMem.WDT[9]=pCount1;
	KMem.WDT[10]=dCount1;
	KMem.WDT[11]=vCount1;
	
	KMem.WDT[12] = eCount1;
	KMem.WDT[13] = eCount2;
		
	if (Uart5Stat.bPacketRecved){
		KMem.WDT[16]++;
		OrdLidarParsePkt(1,(OradarLidarFrame *)Uart5RxBuf,Uart5RecvBuf1DataLen);
		Uart5RecvBuf1DataLen =0;
		Uart5Stat.bPacketRecved = 0;
	}
		
//	nPosX,nPosY,nPosZ,nPosZ1,nPosZ2;
	
	KMem.WDT[17]=pCount2;
	KMem.WDT[18]=dCount2;
	KMem.WDT[19]=vCount2;
	
	KMem.WDT[24]=nPosX;
	KMem.WDT[25]=nPosY;
	KMem.WDT[26]=nPosZ;
	KMem.WDT[27]=nPosZ1;
	KMem.WDT[28]=nPosZ2;
	
	KMem.WDT[32]=results[0];
	KMem.WDT[33]=results[1];
	KMem.WDT[34]=results[2];
	KMem.WDT[35]=results[3];
	KMem.WDT[36]=results[4];
	KMem.WDT[37]=results[5];
	KMem.WDT[38]=results[6];
	KMem.WDT[39]=results[7];
	
	
	KMem.WX[1] = nPosX ;
	KMem.WX[2] = nPosY ;
	KMem.WX[3] = nPosZ;
	
//	KMem.WX[1]++ ;
//	KMem.WX[2]++;
	
//	KMem.WYB[0]=1;
	PutOutput (KMem.WY[0]);

	 LL_IWDG_ReloadCounter(IWDG);
		
  }	//while (1) ;
  /* USER CODE END WHILE */

  /* USER CODE BEGIN 3 */

  /* USER CODE END 3 */

}


/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @param  file: The file name as string.
  * @param  line: The line in file as a number.
  * @retval None
  */
void _Error_Handler(char *file, int line)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  while(1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t* file, uint32_t line)
{ 
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

/**
  * @}
  */

/**
  * @}
  */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/