#include "stdafx.h"
#include "resource.h"
#include "Global.h"
#include "math.h"
#include "GlobalMessage.h"
#include "GlobalMember.h"
#include "GetElevation.h"
#include "CH91PayloadSoftwareDlg.h"
#include "MapPrj.h"
#include <winsock2.h>
#include <ws2tcpip.h>
#include <opencv2\opencv.hpp>  
#include "deinterlace.h"
#include "basetype.h"
#pragma comment(lib,"ws2_32.lib")
using namespace cv;

CFrameFK_U_CAMERA g_FrameCamera;
CMUDP g_MUDPCamera;
BOOL  g_IsSending;

// CRCУÑé
//struct struCHECK
//{   
//	struCHECK()
//	{
//		C0 = C1 = 0;
//	};
//
//	BYTE C0;
//	BYTE C1;
//};

const UINT16  Crc_Table[256] =                                      // X16+X12+X5+1 1021Óàʽ±í CCITT
{                      
	0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
	0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
	0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
	0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
	0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
	0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
	0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
	0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
	0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823,
	0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
	0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12,
	0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
	0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41,
	0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
	0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
	0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
	0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
	0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
	0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
	0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
	0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
	0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
	0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
	0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
	0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
	0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3,
	0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
	0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92,
	0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
	0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
	0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
	0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0
};

struCHECK MakeCheckCRC16(UINT8 *ptr,UINT8 len) 
{
	UINT8 temp,i;
	UINT16 crc = 0;

	for(i = 0; i < len; i++)
	{
		temp = (UINT8) (crc / 256);                   // ÒÔ8λ¶þ½øÖÆÊýÔÝ´æCRC µÄ¸ß8 λ
		crc <<= 8;                                // ×óÒÆ 8 λ
		crc ^= Crc_Table[temp ^ *ptr];              // ¸ß×ֽں͵±Ç°Êý¾ÝXOR ÔÙ²é±í
		ptr++;
	}
	struCHECK ck;
	ck.C1 = (crc>>8);
	ck.C0 = (crc&0xff);
	return ck;
}

#define PLOYNOMAL 0x1021
unsigned int CreateCRC2(unsigned char lpCRCBlock[], unsigned int nCRCLen)
{
	unsigned char* lpBuffer = NULL;

	if (1 == nCRCLen % 2)
	{
		nCRCLen += 1;
		lpBuffer = new unsigned char[nCRCLen];
		memset(lpBuffer, 0, nCRCLen);
		memcpy(&lpBuffer[1], lpCRCBlock, nCRCLen-1);
	}

	else
	{
		lpBuffer = new unsigned char[nCRCLen];
		memcpy(lpBuffer, lpCRCBlock, nCRCLen);
	}

	unsigned long crc, ploy;
	unsigned int crcCode;
	int i=0, j=0, k=0;	

	crc = 0;
	ploy = 0x80000000;	
	ploy = ploy ^ ((unsigned long)PLOYNOMAL << 15);	

	crc = crc ^ ((unsigned long)lpBuffer[0] << 24) ^ ((unsigned long)lpBuffer[1] << 16);


	for (i = 2; i < (int)(nCRCLen + 1); i = i + 2 )
	{

		if (i < (int)nCRCLen)
		{
			crc = crc ^ ((unsigned long)lpBuffer[i] << 8) ^ ((unsigned long)lpBuffer[i + 1]);

		}
		else
			crc = crc & 0xffff0000;		
		if (i == 2)
		{

			while ( ! (crc & 0x80000000))
			{
				crc = crc << 1;
				k = k + 1;
				if(crc==0)
					break;

			}

			for (j = 0; j < (16 - k); j ++)
			{

				if (crc & 0x80000000)
					crc = ((crc ^ ploy) << 1);
				else
					crc = crc << 1;
			}
		}

		else
		{
			for (j = 0; j < 16; j ++)
			{
				if (crc & 0x80000000)
					crc = (crc ^ ploy) << 1;
				else
					crc = crc << 1;
			}
		}	
	}

	crcCode = (unsigned int)(crc >> 16);


	if (NULL != lpBuffer)
	{
		delete[] lpBuffer;
	}


	return crcCode;
}
// »ñÈ¡Ö¸¶¨´ÅÅÌÊ£Óà¿Õ¼ä
long GetFreeVolumeLen(char *p)
{
	if (p == nullptr)
	{
		return 0;
	}

	ULARGE_INTEGER lpuse;
	ULARGE_INTEGER lptotal;
	ULARGE_INTEGER lpfree;

	char chvolume[10] = {0};
	StrCat(chvolume, p);
	StrCat(chvolume, ":\\");

	GetDiskFreeSpaceExA(chvolume, &lpuse, &lptotal, &lpfree);

	long len = static_cast<long>((((lpfree.QuadPart / 1024) / 1024) / 1024));
	return len;
}

// ¹¦ÄÜ£º»ñÈ¡Èí¼þµ±Ç°¹¤×÷Ŀ¼
CString GetSoftwareCurrentDirectory()
{
	char exeFullPath[MAX_PATH];
	GetModuleFileName(NULL, exeFullPath, MAX_PATH);

	CString Directy;
	Directy.Format("%s", exeFullPath);

	int index = Directy.ReverseFind('\\');
	Directy.Delete(index, Directy.GetLength() - index);

	return Directy;
}

//¹¦ÄÜ£ºÔÚÎļþ¼ÐÖвéÕÒÎļþ£¬¿´ÆäÊÇ·ñ´æÔÚ
bool SearchDirectory(const CString &strDir)
{
	CFileFind fileFind;

	if (fileFind.FindFile(strDir))
	{
		fileFind.Close();
		return true;
	}
	else
	{
		fileFind.Close();
		return false;
	}
}

// »ñÈ¡¿É¼û¹â¡¢ºìÍâÖ¡µÄµ¥Ö¡Êý¾Ý³¤¶È
unsigned int getFrameLen(BYTE B0, BYTE B1, BYTE B2, BYTE B3)
{
	BYTE a[4];
	a[0] = B3;
	a[1] = B2;
	a[2] = B1;
	a[3] = B0;
	unsigned int val = *((unsigned int*)(a)); //Ò»¸öunsigned intÊÇ4¸ö×Ö½Ú£¬¿ªÍ·*ºÅÊÇÈ¡ÖµµÄÒâ˼

	return val;
}

// »ñÈ¡ÊýÂëÕÕƬ֡µÄµ¥Ö¡Êý¾Ý³¤¶È 
unsigned int getFrameLenDC(BYTE B0, BYTE B1)
{
	BYTE a[2];
  	a[0] = B1;
  	a[1] = B0;
	wchar_t val = *((wchar_t*)(a)); //Ò»¸öwchar_tÊÇ2¸ö×Ö½Ú£¬¿ªÍ·*ºÅÊÇÈ¡ÖµµÄÒâ˼
	return val;
}

// »ñȡͼÏñË«»÷µãÆ«ÒÆÁ¿
void CALLBACK GetEOImgCenterOffset(CPoint offset, int flag)
{
	g_payloadCtrlProtocol = 11;
	g_TrackOffset = offset;
}

// ÓÒ¼üµã»÷¿É¼û¹â»òºìÍ⣬ת»»ÎªÊÖ¶¯¸ú×Ù
void CALLBACK GetEOIRImgRightButtonDown(int type)
{
	// Çл»ÎªÊÖ¶¯ËÑËØ
	if (type == 1)
	{
		g_payloadCtrlProtocol = 10;
	}
	// Í£Ö¹ÏìÓ¦ÊÓ³¡´óС
	else if (type == 2)
	{
		g_payloadCtrlProtocol = 0;
	}
}


//  ͨ¹ýͼÏñ¿ØÖÆËÅ·þ
void CALLBACK GetDistRatioAndAngleThroughImg(double distRatio, double angle)
{
	if (distRatio >= 0 && distRatio <= 1 && abs(angle) <= 180)
	{
		if (distRatio > 0.5)
		{
			distRatio = 0.5;
		}

		g_MS_Ratio = distRatio;
		g_MS_Angle = angle;
		::SendMessage(g_SendControlCommandDlgHwnd, WM_SHOW_MANUAL_SEARCH , WPARAM(0), LPARAM(0));
	}
}


//µØͼģ¿é»Øµ÷º¯Êý
void CALLBACK GetLeadLonLat(double Lon, double Lat, double Alt)
{
	//»Øµ÷º¯Êý£¬»ùÓÚµØͼģ¿éÉϵľ­Î³¶È»ñÈ¡¸ÃµãµÄ¸ß³ÌÖµ
	CString rootDir = GetSoftwareCurrentDirectory() + "\\ElevationData";
	float fElevator = 0;
	bool ret = GetDem( fElevator, float(Lon), float(Lat), rootDir.GetBuffer(0));//¾­Î³¸ß¶È¶¼¿ÉÒÔ»ñµÃÁË

	if (ret == false)
	{
		fElevator = g_DEMSetting;
	}
	g_UAVLon = g_CH96TransFJData.Satelite_lon;
	g_UAVLat = g_CH96TransFJData.Satelite_lat;
	g_UAVAlt = g_CH96TransFJData.Satelite_height;
	double uavPos[3] = {g_UAVLon, g_UAVLat, g_UAVAlt};
	double tgtPos[3] = {Lon, Lat, fElevator};
	double uavPose[3] = {g_UAVPitch, g_UAVRoll, g_UAVYaw};

	double guidePitch = -90;
	double guideAzimuth = 0;
	ret = CalLoadAttitudeForLoadGuide(guidePitch, guideAzimuth, uavPos, tgtPos, uavPose);

	if (ret == true)
	{
		// ´Ë´¦Ð­ÒéÓë91²»Í¬£¬·½Î»½Ç×óΪÕý£¬¹Ê´Ë´¦·½Î»½ÇÈ¡·´ÏÔʾ£¬ºóÐø¹¤×÷Ó¦Óë91ЭÒéͳһ
		g_Guide_Azimuth = -static_cast<float>(guideAzimuth); //·½Î»½Ç
		g_Guide_Pitch = static_cast<float>(guidePitch);      //¸©Ñö½Ç

		// ·¢ËÍÏûÏ¢ ¸üеØͼÖ÷½çÃæÔغÉÊýÒýÐÅÏ¢
		::SendMessageA(g_CH96CtrlDlgHwnd, WM_SHOW_MAP_GUIDE_POS, WPARAM(0), LPARAM(0));
	}
}

// »Ø·ÅģʽÊäÈëÊý¾Ý
void ReplayInputData(const void* buffer, int length)
{
	// »Ø·ÅģʽÅж¨
	if (g_bWorkMode == true)
	{
		SocketReceiveFH96Data(buffer, length);

		memset(Buffer,0,sizeof(Buffer));
		memcpy(Buffer, buffer, length);
		if (g_udpStarted)
		{
			int iTimes = length/512;
			for (int i = 0; i < iTimes; i++)
			{
	//			g_UDPTrans2Server.WriteBuffer(Buffer + i * 512, 512);
			}
			if (length%512 > 0)
			{
	//			g_UDPTrans2Server.WriteBuffer(Buffer + iTimes * 512, length%512);
			}
			
			//g_UDPTrans2Server.WriteBuffer(buffer, length);
		}
	}
}

 //UDP ÊÕÊý£¨À´×ÔÔغɣ©
void CALLBACK UDPRevTgtInfo(const void* buffer, int length, UINT32 data)
{
	if (g_bWorkMode == false) // ʵʱģʽ
	{
		if (buffer == nullptr || length < 1) 
		{
			return;
		}
		else
		{
			try
			{
				// ¸´½ÓÊý¾ÝÌî³ä
				memcpy(&tgtRecvInfo, buffer, length);
				::PostMessageA(g_MapGuideHwnd, WM_SHOW_QB_TARGET, 0, 0);
			}
			catch(...)
			{
				
			}
		}
	}
}

// LÁ´×é²¥ÊÕÊý
void GetFH96DataFromNet()
{
	// ʵʱ״̬
	if (g_bWorkMode == false)
	{
		BYTE Buffer[512000];
		sockaddr_in loc;
		while (true && g_socketFH96Data != INVALID_SOCKET)
		{
			// ½ÓÊÕÊý¾Ý
			int len = sizeof(SOCKADDR);
			int ret = recvfrom(g_socketFH96Data, 
				(char* )&Buffer, 
				512000, 
				0, 
				(struct sockaddr*)&loc, 
				&len);
			if (ret != SOCKET_ERROR)
			{


			//if (g_udpStarted)
			//	{
			//		int iTimes = ret/512;
			//		for (int i = 0; i < iTimes; i++)
			//		{
			//			g_UDPTrans2Server.WriteBuffer(Buffer + i * 512, 512);
			//		}
			//		if (ret%512 > 0)
			//		{
			//			g_UDPTrans2Server.WriteBuffer(Buffer + iTimes * 512, ret%512);
			//		}
			//		
			//		//g_UDPTrans2Server.WriteBuffer(Buffer, ret);
			//	}

				// ´¦ÀíÊý¾Ý
				SocketReceiveFH96Data(Buffer, ret);
				/*if (g_tcpStarted)
				{
					g_TCPTrans2Server.WriteBuffer(Buffer, ret);
				}*/

			}
		}
	}
}

void SocketReceiveFH96Data(const void* buffer, int length)
{
	if (buffer == nullptr || length < 1) 
	{
		return;
	}
	else
	{
		// ´æÈëÄÚ´æ³Ø¡ª¡ª¶àÏ߳̿ª¹Ø£¬ÓÃÓÚProcessData
		WaitForSingleObject(g_hMutex_ProcessDataCH96, INFINITE);
		try
		{
			if (g_bWorkMode == false) // ʵʱģʽÏÂдÈëÎļþ
			{
				//дÈë±¾µØÎļþ
				if (g_pFileSaveCH96txData != NULL) 
				{
					fwrite(buffer, static_cast<size_t>(1), length, g_pFileSaveCH96txData); // дÊý¾Ý

					// ÏòÒƶ¯Öն˷¢ËÍÊý¾Ý
			//		g_UdpSend2Mobile.WriteBuffer(buffer, length);
				}
			}

			// Åж¨Êý¾Ý
			if (length + g_validDataLen < g_MaxBufferLen) //³õʼg_validDataLenΪ0£¬¼´ÎªÈç¹ûÊÕÊýµÄlengthСÓÚ×î´ó×Ö³¤£¬Ôò¡­¡­
			{
				memcpy(g_receiveDataBuffer + g_validDataLen, buffer, length); //½«bufferÄÚÈÝ¿½±´µ½g_receiveDataBufferÑÓ³¤g_validDataLen¸ö×Ö½ÚÖ®ºóµÄg_receiveDataBufferÖÐ
				g_validDataLen = length + g_validDataLen; //ÓÐЧ×Ö³¤ÖÃΪÊÕÊý×Ö³¤
			}
			else
			{
				if (length <= g_MaxBufferLen)  // Ð޸ģ¨ÔöÌí£©
				{
					// ÇåÀí»º´æ
					g_validDataLen = 0;

					// ¼Ç¼±¾´ÎÊý¾Ý
					memcpy(g_receiveDataBuffer + g_validDataLen, buffer, length);
					g_validDataLen = length + g_validDataLen;
				}
			}
			ReleaseMutex(g_hMutex_ProcessDataCH96);
		}
		catch(...)
		{
			ReleaseMutex(g_hMutex_ProcessDataCH96);
		}
	}
}
// h264½âÂë½á¹û»Øµ÷º¯Êý
void CH96_h264DecodeResultCallbackFun(unsigned char* data,int width,int height)
{
	if (data == nullptr || width != 1920 || height != 1080)
	{
		return;
	}
	// Èç¹ûµçÄÔÅäÖýϵͣ¬Ôò¡­¡­
	if (g_bGoodComputer == FALSE)
	{
		// Ó²½âÂëÖ±½Ó½«½âÂë½á¹ûÏÔʾÔÚÁ˿ؼþÉÏ£¬´Ë´¦½«Í¼Ïñ¸´ÖÆÀ´½øÐб¸ÓÃ
		memcpy(g_ImgforLowComputer.data, data, 1080*1920*3);
	}
	// Èç¹ûµçÄÔÅäÖýϸߣ¬»òÅäÖÃÀàÐÍÊäÈëÓÐÎó£¬ÔòÒÀÕÕÔ­ÏÈÏÔʾ·½Ê½½øÐд¦Àí
	else
	{
		// ¿É¼û¹âͼÏñÈë¶Ó
		WaitForSingleObject(g_hMutex_DisplayVideoCH96, INFINITE); 
		if (g_CH96VideoDeque.size() > 15)
		{
			g_CH96VideoDeque.pop_front();
		}

		cv::Mat img(1080,  1920,  CV_8UC3, cv::Scalar(0,0,0));
		if (img.empty() == false)
		{
			memcpy(img.data,  data, 1080*1920*3);
			g_CH96VideoDeque.push_back(img);
			img.data = NULL;
		}
	}
	ReleaseMutex(g_hMutex_DisplayVideoCH96);
}

void ProcessCH96Data()
{
	while(g_bProcessDataFlag)
	{
		try
		{
			WaitForSingleObject(g_hMutex_ProcessDataCH96, INFINITE);
			// ÊÇ·ñ³É¹¦½«Êý¾Ý¸´ÖƵ½ÁÙʱλÖ㬱íÕ÷±äÁ¿
			bool bHaveRelease = false;
			// ×îС֡³¤
			if (g_validDataLen >= 512)
			{
				// ÕÒÖ¡Í·
				int _head = 0;  // head ´Ó0¿ªÊ¼¼ÆÊý
				bool _bfindhead = false;
				int SetMaxLoc = g_validDataLen - 256;  // ×î¶àÕÒµ½Àëβ256¸ö×Ö½Ú´¦

				unsigned char midBuffer[256];

				for (_head = 0; ((_head < SetMaxLoc) && (_bfindhead == false)); _head++)
				{
					if (g_receiveDataBuffer[_head + 0] == 0xEB &&
						g_receiveDataBuffer[_head + 1] == 0x90)
					{
						_bfindhead = true;
						break; // Ìø³öforÑ­»·
					}
				} // for Ñ­»·ÕÒÖ¡Í·

				// Èç¹ûÕÒµ½ÁËÖ¡Í·
				if (_bfindhead == true)
				{
					// Êý¾Ý¸´ÖÆ
					memcpy(midBuffer, g_receiveDataBuffer + _head, 256);
					// ¸üÐÂÄÚ´æ
					unsigned int frameLen = 256;
					g_validDataLen = g_validDataLen - _head - frameLen;
					memcpy(g_receiveDataBuffer_middle, g_receiveDataBuffer + _head + frameLen, g_validDataLen);
					memcpy(g_receiveDataBuffer, g_receiveDataBuffer_middle, g_validDataLen);
					ReleaseMutex(g_hMutex_ProcessDataCH96);
					// Êý¾Ý³É¹¦¸´ÖƵ½ÁÙʱλÖÃ
					bHaveRelease = true;
					// ·ÉÐÐÒ£²âÊý¾Ý
					if (midBuffer[4] == 0x2A)
					{

					}
					// ¸´½ÓÊý¾Ý
					else if (midBuffer[4] == 0x50)
					{
						// »ñÈ¡¸´½ÓÊý¾Ý£¨Ö»±£Áô¸´½ÓÊý¾ÝÖеÄ9-158×Ö½Ú£¬¹²150×Ö½Ú£©
						WaitForSingleObject(g_hMutex_SaveFJData, INFINITE);
						memcpy(&g_CH96FJData, midBuffer + 8, 150);
						// ¸´½ÓÊý¾Ýת»»
						TransCH96FJData(g_CH96TransFJData, g_CH96FJData); 
						// ¸´½ÓÊý¾Ýת»»ÎªÇ鱨´¦ÀíÓø´½ÓÊý¾Ý
						TransCH96FJDatatoQBData(g_CH96QBData, g_CH96TransFJData);
						ReleaseMutex(g_hMutex_SaveFJData);
						// ×é²¥Ò³ÃæÏÔʾFJÊý¾ÝÄÚÈÝ
						::PostMessageA(g_MulticastSocketReceiveDlgHwnd, WN_SHOW_CH96_FJDATA, 0, 0);
						// ·¢ËÍÏûÏ¢£¬¸´½ÓÊý¾ÝÀ¸ÏÔʾ¸´½ÓÊý¾Ý
						//::PostMessageA(g_mainDlgHwnd, WM_SHOW_CH96_INFO, 0, 0);
						// ÏÔʾ¼¤¹â²â¾àÖµ
						::PostMessageA(g_QBTargetTrackFunctionsHwnd, WM_SHOW_LASERDIS_QBFUNCTIONSDLG, 0, 0);
						// ¸üеØͼ״̬
						::PostMessageA(g_MapGuideHwnd, WM_UPDATA_GOOGLEMAP, 0, 0);
					}
					// ͼÏñÊý¾Ý
					else if (midBuffer[4] == 0xD5)
					{
						// ½âÂë
						if (g_CH96_h264decoder != nullptr)
						{
							g_CH96_h264decoder->decoding(midBuffer + 8, 248);
							g_UDPTrans2Server.WriteBuffer(midBuffer + 8, 248);
						}
						/*
						if(g_tcpStarted==true){
						
							memcpy(g_tcpData+26,midBuffer + 8, 248);
							//Ö¡¼ÆÊý
							memcpy(g_tcpData+5,&g_frameCountTX,4);
							g_frameCountTX++;
							unsigned int crc_;
							crc_=CreateCRC2(g_tcpData+2, 272);
							memcpy(g_tcpData+274,&crc_,2);
							g_TCPYYDT.WriteBuffer(g_tcpData,276);

							//g_TCPYYDT.WriteBuffer(g_tcpData + 5,4);
							//g_TCPYYDT.WriteBuffer(g_tcpData + 26,248);
						}
						*/

						// ´æ´¢Êý¾Ý
						// дÈë±¾µØÎļþ
						if (g_pFileSaveVideoDataCH96 != NULL && g_bVideoCapture == TRUE) 
						{
							fwrite(midBuffer + 8, static_cast<size_t>(1), 248, g_pFileSaveVideoDataCH96); // дÊý¾Ý
						}
					}
					// Êý¾ÝÀàÐÍÓÐÎó
					else
					{
						// do nothing
					}
				}
				// ûÕÒµ½Ö¡Í·
				else
				{
					// head֮ǰµÄɾµô
					g_validDataLen = g_validDataLen - _head;
					memcpy(g_receiveDataBuffer_middle, g_receiveDataBuffer + _head, g_validDataLen);
					memcpy(g_receiveDataBuffer, g_receiveDataBuffer_middle, g_validDataLen);
				}
			}

			if (bHaveRelease == false)
			{
				ReleaseMutex(g_hMutex_ProcessDataCH96);
			}
		}
		catch(...)
		{
			//ReleaseMutex(g_hMutex_ProcessDataCH96);
		}
	}
}
// »ñÈ¡CH96¸´½ÓÊý¾Ý
void TransCH96FJData(Data96TXD &dst, Protocal96TXD &src)
{
		dst.head[0] = 0xEE;													// Ö¡Í·
		dst.head[1] = 0x16;
		// ÔغÉÏà¹Ø
		dst.mem1 = 0x00;														// Ô¤Áô
		dst.SwitchCmd = src.SwitchCmd;								// ¿ª¹ØÖ¸Áî
		dst.Status0 = src.Status0;											// ״̬×Ö0
		dst.Status1 = src.Status1;											// ״̬×Ö1
		dst.EOFocus = (float)(src.EOFocus * 0.1);					// EO½¹¾àmm
		dst.Status2 = src.Status2;											// ״̬×Ö2
		dst.FrameCount = src.FrameCount;							// ʱÂë
		dst.dx = (int)src.dx;													// ¸©ÑöÍÑ°ÐÁ¿
		dst.dy = (int)src.dy;													// ·½Î»ÍÑ°ÐÁ¿
		dst.IRFocus = (float)(src.IRFocus * 0.1);					// IR½¹¾àmm
		dst.LaserDis = (int)src.LaserDis;								// ¼¤¹â²â¾àÖµ
		dst.LaserState = src.LaserState;									// ¼¤¹â״̬
		dst.None_1 = src.None_1;											// ÎÞ
		dst.None_2 = src.None_2;											// ÎÞ
		dst.ZH_Azimuth = (float)(src.ZH_Azimuth * 0.01);		// Ôغɸ©Ñö½Ç
		dst.ZH_Pitch = (float)(src.ZH_Pitch * 0.01);				// Ôغɷ½Î»½Ç
		dst.PixelSize = (float)(src.PixelSize*1e-3F);				// ÏñÔª³ß´ç
		// ·É»úÏà¹Ø
		dst.PlaneID = (int)src.PlaneID;																				// ·É»úID
		dst.Pitch = (float)(src.Pitch * 0.01);																		// ·É»ú¸©Ñö½Ç
		dst.Roll = (float)(src.Roll * 0.01F);																			// ·É»ú¹öת½Ç
		dst.Yaw = (float)(src.Yaw * 0.01F);																		// ·É»ú·½Î»½Ç
		dst.Satelite_lat = (double)(src.Satelite_lat * 1e-7);													// ·É»ú²â×˾­¶È
		dst.Satelite_lon = (double)(src.Satelite_lon * 1e-7);													// ·É»ú²â×Ëγ¶È
		dst.Satelite_height = (double)(src.Satelite_height * 0.01);											// ·É»ú²â×˸߶È
		dst.Satelite_Ground_Speed = (float)(src.Satelite_Ground_Speed * 0.01);					// ·É»úGPSµØËÙ
		dst.Satelite_Ground_Speed_Dir = (float)(src.Satelite_Ground_Speed_Dir * 0.01);		// ·É»úGPSµØËÙ·½Ïò
		dst.Satelite_Sky_Speed = (float)(src.Satelite_Sky_Speed * 0.01);								// ·É»úGPSÌìËÙ
		dst.MTI_Pitch = (float)(src.MTI_Pitch*0.01);															// MTI¸©Ñö½Ç
		dst.MTI_Roll = (float)(src.MTI_Roll*0.01);															// MTI¹öת½Ç
		dst.MTI_Yaw = (float)(src.MTI_Yaw*0.01);															// MTIº½Ïò½Ç

		dst.MTI_lat = (double)(src.MTI_lat * 1e-7);															// MTI¾­¶È
		dst.MTI_lon = (double)(src.MTI_lon * 1e-7);															// MTIγ¶È
		dst.MTI_height = (double)(src.MTI_height * 0.2 - 500);											// MTI¸ß¶È
		dst.MTI_Ground_Speed = (float)(src.MTI_Ground_Speed * 0.01);							// MTIµØËÙ
		dst.MTI_Ground_Speed_Dir = (float)(src.MTI_Ground_Speed_Dir * 0.01);				// MTIµØËÙ·½Ïò
		dst.MTI_Sky_Speed = (float)(src.MTI_Sky_Speed * 0.01);										// MTIÌìËÙ
		
		dst.UTC_Second = (int)(src.UTC_Second * 0.001);												//UTCÌìÃë
		dst.UTC_Year = (int)(src.UTC_Year);																	// UTCÄê
		dst.UTC_Month = (int)(src.UTC_Month);																// UTCÔÂ
		dst.UTC_Day = (int)(src.UTC_Day);																		// UTCÈÕ

		dst.NavigationState = src.NavigationState;																// µ¼º½×´Ì¬
		dst.PressureHEG = (float)(src.PressureHEG * 0.2 - 500);										// Æøѹ¸ß¶È

		dst.AirSpeed = (float)(src.AirSpeed * 0.01);															// ¿ÕËÙ
		dst.Engine_Speed = (int)(src.Engine_Speed);															// ·¢¶¯»úתËÙ
		dst.Left_Tmp = (int)(src.Left_Tmp * 2);																	// ×ó¸×ÎÂ
		dst.Right_Tmp = (int)(src.Right_Tmp * 2);																// ÓÒ¸×ÎÂ

		dst.Pitch_Install_Err = (float)(src.Pitch_Install_Err * 0.02);										// Ôغɸ©Ñö°²×°Îó²î
		dst.Roll_Install_Err = (float)(src.Roll_Install_Err * 0.02);											// Ôغɷ½Î»°²×°Îó²î
		dst.Yaw_Install_Err = (float)(src.Yaw_Install_Err * 0.02);										// ÔغÉÆ«º½°²×°Îó²î

		dst.mem3 = src.mem3;																							// Ô¤Áô
		dst.CRC = src.CRC;																								// CRCУÑé
}

void TransCH96FJDatatoQBData(struQB_FJ &QB_FJ, Data96TXD &CH96FJ)
{
	QB_FJ.bValid = true;   // ½á¹¹ÌåÊý¾ÝÊÇ·ñÓÐЧ£º TRUE¡ªÓÐЧ    FALSE¡ªÎÞЧ

	// ÔغÉ×Ë̬ºÍÊôÐÔ
	QB_FJ.ZH_StablePlatformWorkState = 0x01;  // Îȶ¨Æ½Ì¨¹¤×÷״̬£º000¡ªÊÖ¶¯¸ú×Ù 001¡ª×Ô¶¯¸ú×Ù 010¡ªËø¶¨ 011¡ªÊýÒý 100¡ªÉ¨Ãè 101¡ªÉ²³µ 110¡ª³õʼ»¯ 111¡ªÆ«Ðĸú×Ù
	QB_FJ.ZH_TrackState = 0x11;               // ¸ú×ÙÆ÷״̬£º000¡ªÕý³£ 001¡ªÒì³£ 010¡ªÄ¿±ê¶ªÊ§ 011¡ªÏà¹Ø¸ú×Ù 100¡ªÖØÐÄºÚ 101¡ªÖØÐÄ°×

	QB_FJ.ZH_Pitch = CH96FJ.ZH_Pitch;   // Ôغɸ©Ñö½Ç
	QB_FJ.ZH_Azimuth = -CH96FJ.ZH_Azimuth; // Ôغɷ½Î»½Ç

	QB_FJ.ZH_LaserOn = !(CH96FJ.LaserState & 0x01); // ¼¤¹â²â¾à¿ª¹Ø£¬trueΪ¿ª£¨×îµÍλΪ¼¤¹â²â¾à¿ª¹ØλÖã©

	QB_FJ.ZH_LaserDist = CH96FJ.LaserDis;  // ¼¤¹â²â¾àÖµ
	QB_FJ.ZH_FocalLen = CH96FJ.EOFocus;   //EO½¹¾à mm
//	QB_FJ.ZH_IRFocalLen = CH96FJ.IRFocus;   //IR½¹¾à mm
	QB_FJ.ZH_DeltaX = CH96FJ.dx;		 //¸©ÑöÍÑ°ÐÁ¿
	QB_FJ.ZH_DeltaY = CH96FJ.dy;		 //·½Î»ÍÑ°ÐÁ¿ 
	QB_FJ.ZH_PixelSize = CH96FJ.PixelSize * 1e-6;  //ÏñÔª³ß´ç¿É¼û¹â2.8um£¬ºìÍâ15um»ò30um
	QB_FJ.ZH_ImgWidth = 1920;  // ͼÏñ¿í
	QB_FJ.ZH_ImgHeight = 1080; // ͼÏñ¸ß

	// ·É»úλÖÃ×Ë̬GPS
	QB_FJ.UAV_GPS_Lon = CH96FJ.Satelite_lon; // ÎÞÈË»ú¾­¶È
	QB_FJ.UAV_GPS_Lat = CH96FJ.Satelite_lat; // ÎÞÈË»úγ¶È
	QB_FJ.UAV_GPS_Alt = float(CH96FJ.Satelite_height); // ÎÞÈË»ú¸ß¶È
	QB_FJ.UAV_GPS_HSpeed = CH96FJ.Satelite_Ground_Speed; // ˮƽËÙ¶È
	QB_FJ.UAV_Pitch = CH96FJ.Pitch;  // ¸©Ñö½Ç
	QB_FJ.UAV_Yaw = CH96FJ.Yaw;      // º½Ïò½Ç£¨»úÍ··½Ïò£©
	QB_FJ.UAV_Roll = CH96FJ.Roll;    // ¹öת½Ç
	QB_FJ.UAV_GroundHeight = 1280;   // ÎÞÈË»úÏÂÊÓµãµØÃæ¸ß¶È£ºµØÃæ¼ÆËã»ñÈ¡30Ã׸߳Ì

	// GPSʱ¼ä
	QB_FJ.UAV_GPS_Year = CH96FJ.UTC_Year;  // ÎÀµ¼1Äê
	QB_FJ.UAV_GPS_Month = CH96FJ.UTC_Month; // ÎÀµ¼1ÔÂ
	QB_FJ.UAV_GPS_Day = CH96FJ.UTC_Day;   // ÎÀµ¼1ÈÕ
	QB_FJ.UAV_GPS_UTC = CH96FJ.UTC_Second;   // ÎÀµ¼1ʱ·ÖÃë
}
// λÖÃÐÅÏ¢½á¹¹Ìå½âÎöº¯Êý
void TransLocationSocketStruct(struSocketProtocal &dst, const struQB_FJ & src)
{
	dst.Socket_Head[0] = 0x55;
	dst.Socket_Head[1] = 0xAA;
	dst.Socket_Marker = 0x11;
	dst.Socket_Sender = 0x20;
	dst.UAV_Lon = (INT32)(src.UAV_GPS_Lon * 10000000);
	dst.UAV_Lat = (INT32)(src.UAV_GPS_Lat * 10000000);
	dst.UAV_Alt = (INT16)(src.UAV_GPS_Alt * 10);
// 	dst.Target_Lon = 0xFFFFFFFF;
// 	dst.Target_Lat = 0xFFFFFFFF;
// 	dst.Target_Alt = 0xFFFF;
	// »ñÈ¡CRCУÑéλ ¼ÆËãdst[26-27]
	BYTE _LocationSocketData[26];
	memcpy(_LocationSocketData, &dst, 26);
	struCHECK tempstruCheck = MakeCheckCRC16(_LocationSocketData, 26);
	dst.CRC[0] = tempstruCheck.C0;
	dst.CRC[1] = tempstruCheck.C1;
	// ֡β
	dst.Socket_End[0] = 0xAF;
	dst.Socket_End[1] = 0xAF;
}
// ¸´½ÓÊý¾Ý½á¹¹Ìå½âÎöº¯Êý
void TransFJStruct( struTranslateProtocal_FJ &dst,  const struProtocal_FJ &src)
{
	// ÅжÏsrcÓÐЧÐÔ
	if (src.Head[0] == 0xEB && src.Head[1] == 0x90)
	{
		dst.bValid = TRUE;

		// ת»»½âÎö
		//½âÎöÔغÉʶ±ðÂë
		dst.ZH_Type = src.ZH_Type;  

		//½âÎö¿ª¹ØÖ¸Áî
		dst.ZH_CmdType = src.ZH_CmdType;

		//½âÎöÔغɹ¤×÷״̬¡ª¡ªÎȶ¨Æ½Ì¨¹¤×÷״̬
		dst.ZH_StablePlatformWorkState = (src.ZH_WorkState & 0xE0)>>5;
		//½âÎöÔغɹ¤×÷״̬¡ª¡ª¼¤¹â²â¾àÒÇ
		dst.ZH_LaserRangeWorkState = (src.ZH_WorkState & 0x1C)>>2;

		// Åж¨¼¤¹â²â¾à¿ª¹Ø
		if (dst.ZH_LaserRangeWorkState == 0x04)
		{
			dst.ZH_LaserOn = true;
		}
		else
		{
			dst.ZH_LaserOn = false;
		}

		//½âÎöÔغɹ¤×÷״̬¡ª¡ªÉý½µ»ú¹¹¡ª¡ªÖ÷½çÃæÏÔʾ
		dst.ZH_ElevatorWorkState = src.ZH_WorkState & 0x03;

		//½âÎöÖ÷¿Ø״̬×Ö0
		dst.ZH_CtrlState0 = src.ZH_CtrlState0;

		//½âÎöÖ÷¿Ø״̬×Ó1
		dst.ZH_CtrlState1 = src.ZH_CtrlState1;

		//½âÎöËÅ·þ״̬×Ö
		dst.ZH_ServoState = src.ZH_ServoState;

		//½âÎö¿É¼û¹â״̬×Ö0¡ª¡ª¿É¼û¹âÏà»ú״̬
		dst.ZH_EOCameraWorkState = (src.ZH_EOState0 & 0x80) >> 7;

		//½âÎö¿É¼û¹â״̬×Ö0¡ª¡ª¿É¼û¹âͼÏñ״̬
		dst.ZH_EOCameraImgState = (src.ZH_EOState0 & 0x40) >> 6;

		//½âÎöºìÍâ״̬×Ö¡ª¡ªºìÍâÏà»ú״̬
		dst.ZH_IRCameraWorkState = (src.ZH_IRState & 0x80) >> 7;
		//½âÎöºìÍâ״̬×Ö¡ª¡ªÍ¼Ïñ״̬
		dst.ZH_IRCameraImgState = (src.ZH_IRState & 0x40) >> 6;
		//½âÎöºìÍâ״̬×Ö¡ª¡ªÈÈÏñÒÇÊý×ֱ䱶
		dst.ZH_IRCameraDigitalDouble = (src.ZH_IRState & 0x08) >> 3;

		dst.ZH_SMExposalTime = src.ZH_IRState * 20;

		//½âÎö¸ú×ÙÆ÷״̬×Ö¡ª¡ª¿É¼û¹â½ÓÊÕÇòÉÏÖ÷¿ØÊý¾Ý
		dst.ZH_EOTrackState_Control = (src.ZH_TrackState & 0x80) >> 7;
		//½âÎö¸ú×ÙÆ÷״̬×Ö¡ª¡ª¿É¼û¹â¸ú×ÙÆ÷״̬
		dst.ZH_EOTrackState = (src.ZH_TrackState & 0x70) >> 4;
		//½âÎö¸ú×ÙÆ÷״̬×Ö¡ª¡ªºìÍâ½ÓÊÕÇòÉÏÖ÷¿ØÊý¾Ý
		dst.ZH_IRTrackState_Control = (src.ZH_TrackState & 0x08) >> 3;
		//½âÎö¸ú×ÙÆ÷״̬×Ö¡ª¡ªºìÍâ¸ú×ÙÆ÷״̬
		dst.ZH_IRTrackState = (src.ZH_TrackState & 0x07);

		//½âÎö¼¤¹â²âÊÔ״̬×Ö
		dst.ZH_LASERTEST_D7 = (src.ZH_EOState1 & 0x80) >> 7;
		dst.ZH_LASERTEST_D6 = (src.ZH_EOState1 & 0x40) >> 6;
		dst.ZH_LASERTEST_D5 = (src.ZH_EOState1 & 0x20) >> 5;
		dst.ZH_LASERTEST_D4 = (src.ZH_EOState1 & 0x10) >> 4;
		dst.ZH_LASERTEST_D3 = (src.ZH_EOState1 & 0x08) >> 3;
		dst.ZH_LASERTEST_D2 = (src.ZH_EOState1 & 0x04) >> 2;
		dst.ZH_LASERTEST_D1D0 = (src.ZH_EOState1 & 0x03);

		//½âÎö¼¤¹â²â¾àÖµ
		dst.ZH_LaserDist = (unsigned int)src.ZH_LaserDist;

		//¿É¼û¹â½¹¾àÖµ
		dst.ZH_EOFocalLen = (float)(src.ZH_EOFocalLen * 0.01);

		//ºìÍâ½¹¾àÖµ
		dst.ZH_IRFocalLen = (float)(src.ZH_IRFocalLen * 0.01);

		dst.ZH_SMSerialNumber = src.ZH_IRFocalLen;  // ÊýÂëÕÕƬ±àºÅ

		// ÏñÔª³ß´ç ¹Ì¶¨Öµ
		dst.ZH_EOPixelSize = 4.5e-6;
		dst.ZH_IRPixelSize = 15e-6;
		dst.ZH_SMPixelSize = 2.8e-6;  // ÔÝʱ


		//ͼÏñÊä³öÀàÐÍ
		dst.ZH_ImgType = src.ZH_ImgType;

		//ͼÏñÊä³öÂëÁ÷
		dst.ZH_ImgFlow = src.ZH_ImgFlow;

		dst.ZH_SMOverlapRatio = float(src.ZH_SMOverlapRatio * 0.5);


		/*¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ªÏÔʾ·É»úÊý¾Ý¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª*/
		//ÎÞÈË»úÐͺÅ
		dst.UAV_Type = src.UAV_Type;

		//·É»úID
		dst.UAV_ID = src.UAV_ID;

		//ÎÞÈË»ú·ÉÐÐ״̬
		for (int i = 0; i<10; i++)
		{
			dst.UAV_States[i] = src.UAV_States[i];
		}

		//´¹Ö±ÍÓÂݸ©Ñö½Ç¡ª¡ª¸º180~Õý180¶È
		dst.UAV_GyroPitch = (float)(src.UAV_GyroPitch * 0.01);

		//´¹Ö±ÍÓÂݹöת½Ç¡ª¡ª¸º180¶È~Õý180¶È
		dst.UAV_GyroRoll = (float)(src.UAV_GyroRoll * 0.01);

		//Æøѹ¸ß¶È
		dst.UAV_AirPresAlt = (float)(src.UAV_AirPresAlt * 0.2 - 500.0);

		//´Åº½Ïò½Ç¡ª¡ª±±Æ«¶«ÎªÕý£¬0~360¶È
		dst.UAV_MagHeading = (float)(src.UAV_MagHeading * 0.1);

		//ÎÀµ¼1¶¨Î»²â×Ë״̬
		dst.GPS_LR_WorkState = src.GPS_LR_WorkState;
		dst.GPS_LR_Location = dst.GPS_LR_WorkState & 0x0F;
		dst.GPS_LR_Attitude = (dst.GPS_LR_WorkState & 0xF0) >> 4;

		//ÎÀµ¼1UTCÌìÃë
		dst.GPS_LR_UTC = (unsigned int)(src.GPS_LR_UTC * 0.001);

		//ÎÀµ¼1UTCÈÕ
		dst.GPS_LR_Day = (unsigned int)src.GPS_LR_Day;

		//ÎÀµ¼1UTCÔÂ
		dst.GPS_LR_Month = (unsigned int)src.GPS_LR_Month;

		// ´íÎóÔ­Òò²»Ïê
		if (dst.GPS_LR_Month > 48)
		{
			dst.GPS_LR_Month -= 48;
		}

		//ÎÀµ¼1UTCÄê
		dst.GPS_LR_Year = (unsigned int)(src.GPS_LR_Year + 2000);

		//ÎÀµ¼1¾­¶È¡ª¡ª¶«¾­ÎªÕý£¬Î÷¾­Îª¸º£¨µÍ×Ö½ÚÔÚÇ°£¬¸ß×Ö½ÚÔÚºó£©
		dst.GPS_LR_Lon = (float)(src.GPS_LR_Lon * 1.0 / 10000000.0);

		//ÎÀµ¼1γ¶È¡ª¡ª±±Î³ÎªÕý£¬ÄÏγΪ¸º
		dst.GPS_LR_Lat = (float)(src.GPS_LR_Lat * 1.0 / 10000000.0);

		//ÎÀµ¼1¸ß¶È
		dst.GPS_LR_Alt = (float)(src.GPS_LR_Alt * 0.2 - 500.0);

		//ÎÀµ¼1µØËÙ·½Ïò¡ª¡ª±±Æ«¶«ÎªÕý£¬0~360¶È
		dst.GPS_LR_VHeading = (float)(src.GPS_LR_VHeading * 0.01);

		//ÎÀµ¼1ˮƽËÙ¶È
		dst.GPS_LR_HoriSpeed = (float)(src.GPS_LR_HoriSpeed * 0.01);

		//ÎÀµ¼1ÌìËÙ¡ª¡ªÉÏÕýϸº
		dst.GPS_LR_VertSpeed = (float)(src.GPS_LR_VertSpeed * 0.01);

		//ÎÀµ¼1º½Ïò½Ç¡ª¡ª±±Æ«¶«ÎªÕý£¬0~360¶È
		dst.GPS_LR_Heading = (float)(src.GPS_LR_Heading * 0.01);

		//ÎÀµ¼1¹öת½Ç
		dst.GPS_LR_Roll = (float)(src.GPS_LR_Roll * 0.01);

		//ÎÀµ¼2¶¨Î»²â×Ë״̬
		dst.GPS_FB_WorkState = src.GPS_FB_WorkState;
		dst.GPS_FB_Location = dst.GPS_FB_WorkState & 0x0F;
		dst.GPS_FB_Attitude = (dst.GPS_FB_WorkState & 0xF0) >> 4;

		//ÎÀµ¼2UTCÌìÃë
		dst.GPS_FB_UTC = (unsigned int)(src.GPS_FB_UTC * 0.001);

		//ÎÀµ¼2UTCÈÕ
		dst.GPS_FB_Day = (unsigned int)src.GPS_FB_Day;

		//ÎÀµ¼2UTCÔÂ
		dst.GPS_FB_Month = (unsigned int)src.GPS_FB_Month;

		// ´íÎóÔ­Òò²»Ïê
		if (dst.GPS_FB_Month > 48)
		{
			dst.GPS_FB_Month -= 48;
		}

		//ÎÀµ¼2UTCÄê
		dst.GPS_FB_Year = (unsigned int)(src.GPS_FB_Year + 2000);

		//ÎÀµ¼2¾­¶È¡ª¡ª¶«¾­ÎªÕý£¬Î÷¾­Îª¸º
		dst.GPS_FB_Lon = (float)(src.GPS_FB_Lon * 1.0 / 10000000.0);

		//ÎÀµ¼2γ¶È¡ª¡ª±±Î³ÎªÕý£¬ÄÏγΪ¸º
		dst.GPS_FB_Lat = (float)(src.GPS_FB_Lat * 1.0 / 10000000.0);

		//ÎÀµ¼2¸ß¶È
		dst.GPS_FB_Alt = (float)(src.GPS_FB_Alt * 0.2 - 500.0);

		//ÎÀµ¼2µØËÙ·½Ïò¡ª¡ª±±Æ«¶«ÎªÕý£¬0~360¶È
		dst.GPS_FB_VHeading = (float)(src.GPS_FB_VHeading * 0.01);

		//ÎÀµ¼2ˮƽËÙ¶È
		dst.GPS_FB_HoriSpeed = (float)(src.GPS_FB_HoriSpeed * 0.01);

		//ÎÀµ¼2ÌìËÙ¡ª¡ªÉÏÕýϸº
		dst.GPS_FB_VertSpeed = (float)(src.GPS_FB_VertSpeed * 0.01);

		//ÎÀµ¼2º½Ïò½Ç¡ª¡ª±±Æ«¶«ÎªÕý£¬0~360¶È
		dst.GPS_FB_Heading = (float)(src.GPS_FB_Heading * 0.01);

		//ÎÀµ¼2¸©Ñö½Ç¡ª¡ªÌ§Í·ÏòÉÏΪÕý£¬¸º180~Õý180¶È
		dst.GPS_FB_Pitch = (float)(src.GPS_FB_Pitch * 0.01);
	}
	else
	{
		dst.bValid = FALSE;
	}
}

// ¸´½ÓÊý¾ÝÈë¶ÓÁÐ
void PushToFrameDataDeque(const struQB_FJ &TgtLocStru)
{
	// Ï̻߳¥³â
	WaitForSingleObject(g_hMutex_TgtLoc, INFINITE);

	if ( g_TgtLocStructDeque.size() < 2)
	{
		g_TgtLocStructDeque.push_back(TgtLocStru); // Èë¶Ó
	}
	else
	{
		while(g_TgtLocStructDeque.size() >= 2) // Éè¼ÆÔ­Òò£ºÖÐ;¿ÉÄÜÉèÖÃСÓÚµ±Ç°ÊýÄ¿µÄÈÝÁ¿
		{
			// µ¯³ö
			g_TgtLocStructDeque.pop_front(); 
		}

		g_TgtLocStructDeque.push_back(TgtLocStru); // Èë¶Ó
	}


	ReleaseMutex(g_hMutex_TgtLoc);
}
// תÇ鱨ÓÃEO¸´½ÓÊý¾Ý
void TransToEOQBFJ(struQB_FJ &dst,  const struTranslateProtocal_FJ &src)
{
	dst.bValid = true;
	if (src.bValid == true)
	{
		// ÔغÉ×Ë̬ºÍÊôÐÔ
		dst.ZH_StablePlatformWorkState = src.ZH_StablePlatformWorkState; // ƽ̨״̬
		dst.ZH_TrackState = src.ZH_EOTrackState; // ¸ú×Ù״̬
		dst.ZH_Pitch = src.ZH_Pitch;   // Ôغɸ©Ñö½Ç
		dst.ZH_Azimuth = src.ZH_Azimuth; // Ôغɷ½Î»½Ç

		if (dst.ZH_Pitch > 10.5 || dst.ZH_Pitch < -110.1)
		{
			dst.bValid = false;
		}

		if (abs(dst.ZH_Azimuth) > 180)
		{
			dst.bValid = false;
		}

		dst.ZH_LaserOn = src.ZH_LaserOn;   // ¼¤¹â²â¾à¿ª¹Ø£¬trueΪ¿ª
		dst.ZH_LaserDist = src.ZH_LaserDist;     // ¼¤¹â²â¾àÖµ

		dst.ZH_FocalLen = src.ZH_EOFocalLen;   //½¹¾à

		if (dst.ZH_FocalLen < 1e-6)
		{
			dst.bValid = false;
		}

		dst.ZH_DeltaX = src.ZH_DeltaX;		 //¸©ÑöÍÑ°ÐÁ¿
		dst.ZH_DeltaY = src.ZH_DeltaY;		 //·½Î»ÍÑ°ÐÁ¿ 

		if (abs(dst.ZH_DeltaX) > 960)
		{
			dst.bValid = false;
		}

		if (abs(dst.ZH_DeltaY) > 540)
		{
			dst.bValid = false;
		}

		dst.ZH_PixelSize = src.ZH_EOPixelSize;  

		dst.ZH_ImgWidth = 1920;  // ͼÏñ¿í
		dst.ZH_ImgHeight = 1080; // ͼÏñ¸ß

		UINT8 GPS_LR_LocState = src.GPS_LR_WorkState & 0x0F;
		UINT8 GPS_LR_PosState = src.GPS_LR_WorkState >> 4;

		UINT8 GPS_FB_LocState = src.GPS_FB_WorkState & 0x0F;
		UINT8 GPS_FB_PosState = src.GPS_FB_WorkState >> 4;

		// ¶¨Î»×´Ì¬£º
		//          0x00 µ¥µã¶¨Î»
		//          0x02 Ôز¨Ïàλ²î·Ö¶¨Î»
		// ²â×Ê״̬£º
		//          0x00 Õý³£²â×Ê

		if (GPS_FB_LocState == 0x00 || GPS_FB_LocState == 0x02) // Ç°ºóÕý³£¶¨Î»
		{
			dst.UAV_GPS_Lon = src.GPS_FB_Lon; // ÎÞÈË»ú¾­¶È
			dst.UAV_GPS_Lat = src.GPS_FB_Lat; // ÎÞÈË»úγ¶È
			dst.UAV_GPS_Alt = src.GPS_FB_Alt; // ÎÞÈË»ú¸ß¶È
			dst.UAV_GPS_HSpeed = src.GPS_FB_HoriSpeed; // ˮƽËÙ¶È

			dst.UAV_GPS_Year = src.GPS_FB_Year;  
			dst.UAV_GPS_Month = src.GPS_FB_Month;
			dst.UAV_GPS_Day = src.GPS_FB_Day;   
			dst.UAV_GPS_UTC = src.GPS_FB_UTC; 
		}
		else if (GPS_LR_LocState == 0x00 || GPS_LR_LocState == 0x02)  // ×óÓÒÕý³£¶¨Î»
		{
			dst.UAV_GPS_Lon = src.GPS_LR_Lon; // ÎÞÈË»ú¾­¶È
			dst.UAV_GPS_Lat = src.GPS_LR_Lat; // ÎÞÈË»úγ¶È
			dst.UAV_GPS_Alt = src.GPS_LR_Alt; // ÎÞÈË»ú¸ß¶È
			dst.UAV_GPS_HSpeed = src.GPS_LR_HoriSpeed; // ˮƽËÙ¶È

			dst.UAV_GPS_Year = src.GPS_LR_Year;  
			dst.UAV_GPS_Month = src.GPS_LR_Month;  
			dst.UAV_GPS_Day = src.GPS_LR_Day;   
			dst.UAV_GPS_UTC = src.GPS_LR_UTC;  
		}
		else if (GPS_FB_LocState == 0x01 || GPS_FB_LocState == 0x03)
		{
			dst.UAV_GPS_Lon = src.GPS_FB_Lon; // ÎÞÈË»ú¾­¶È
			dst.UAV_GPS_Lat = src.GPS_FB_Lat; // ÎÞÈË»úγ¶È
			dst.UAV_GPS_Alt = src.GPS_FB_Alt; // ÎÞÈË»ú¸ß¶È
			dst.UAV_GPS_HSpeed = src.GPS_FB_HoriSpeed; // ˮƽËÙ¶È

			dst.UAV_GPS_Year = src.GPS_FB_Year;  
			dst.UAV_GPS_Month = src.GPS_FB_Month;
			dst.UAV_GPS_Day = src.GPS_FB_Day;   
			dst.UAV_GPS_UTC = src.GPS_FB_UTC; 
		}
		else if (GPS_LR_LocState == 0x01 || GPS_LR_LocState == 0x03)
		{
			dst.UAV_GPS_Lon = src.GPS_LR_Lon; // ÎÞÈË»ú¾­¶È
			dst.UAV_GPS_Lat = src.GPS_LR_Lat; // ÎÞÈË»úγ¶È
			dst.UAV_GPS_Alt = src.GPS_LR_Alt; // ÎÞÈË»ú¸ß¶È
			dst.UAV_GPS_HSpeed = src.GPS_LR_HoriSpeed; // ˮƽËÙ¶È

			dst.UAV_GPS_Year = src.GPS_LR_Year;  
			dst.UAV_GPS_Month = src.GPS_LR_Month;  
			dst.UAV_GPS_Day = src.GPS_LR_Day;   
			dst.UAV_GPS_UTC = src.GPS_LR_UTC;  
		}
		else  // ·É»ú²»¶¨Î»
		{
			dst.bValid = false;
		}

		// ²â×ÊÅжÏ
		if (GPS_LR_PosState == 0x00 && GPS_FB_PosState == 0x00)
		{
			dst.UAV_Yaw = src.GPS_LR_Heading; // º½Ïò½Ç
			dst.UAV_Roll = src.GPS_LR_Roll;   // ¹öת½Ç 
			dst.UAV_Pitch = src.GPS_FB_Pitch; // ¸©Ñö½Ç
		}
		else 
		{
			if (GPS_LR_PosState == 0x00)
			{
				// ×óÓÒGPS²â£ºº½Ïò½Ç+¹öת½Ç
				dst.UAV_Yaw = src.GPS_LR_Heading; // º½Ïò½Ç
				dst.UAV_Roll = src.GPS_LR_Roll;   // ¹öת½Ç 

				// ´¹Ö±ÍÓÂݸ©Ñö½Ç
				dst.UAV_Pitch = src.UAV_GyroPitch;
			} 
			else if (GPS_FB_PosState == 0x00)
			{
				// Ç°ºóGPS²â£ºº½Ïò½Ç+¸©Ñö½Ç
				dst.UAV_Yaw = src.GPS_FB_Heading; 
				dst.UAV_Pitch = src.GPS_FB_Pitch;   

				// ´¹Ö±ÍÓÂݲâ¹öת½Ç
				dst.UAV_Roll = src.UAV_GyroRoll;
			}
			else
			{
				// ÍÓÂÝ + ´Åº½Ïò
				dst.UAV_Yaw = src.UAV_MagHeading; // º½Ïò½Ç
				dst.UAV_Roll = src.UAV_GyroRoll;   // ¹öת½Ç 
				dst.UAV_Pitch = src.UAV_GyroPitch;
			}
		}
	}
}



// תÇ鱨ÓÃIR¸´½ÓÊý¾Ý
void TransToIRQBFJ(struQB_FJ &dst,  const struTranslateProtocal_FJ &src)
{
	dst.bValid = true;
	if (src.bValid == true)
	{
		// ÔغÉ×Ë̬ºÍÊôÐÔ
		dst.ZH_StablePlatformWorkState = src.ZH_StablePlatformWorkState;  // ƽ̨״̬
		dst.ZH_TrackState = src.ZH_IRTrackState; // ¸ú×Ù״̬
		dst.ZH_Pitch = src.ZH_Pitch;     // Ôغɸ©Ñö½Ç
		dst.ZH_Azimuth = src.ZH_Azimuth; // Ôغɷ½Î»½Ç

		if (dst.ZH_Pitch > 10.5 || dst.ZH_Pitch < -110.1)
		{
			dst.bValid = false;;
		}

		if (abs(dst.ZH_Azimuth) > 180)
		{
			dst.bValid = false;;
		}

		dst.ZH_LaserOn = src.ZH_LaserOn;   // ¼¤¹â²â¾à¿ª¹Ø£¬trueΪ¿ª
		dst.ZH_LaserDist = src.ZH_LaserDist; // ¼¤¹â²â¾àÖµ

		dst.ZH_FocalLen = src.ZH_IRFocalLen;   //½¹¾à

		// ÊÊÓ¦Êý×ֱ䱶 2
		if (src.ZH_IRCameraDigitalDouble == 0x01)
		{
			dst.ZH_FocalLen = 2 * dst.ZH_FocalLen;
		}

		if (dst.ZH_FocalLen < 1e-6)
		{
			dst.bValid = false;
		}

		dst.ZH_DeltaX = src.ZH_DeltaX;		 //¸©ÑöÍÑ°ÐÁ¿
		dst.ZH_DeltaY = src.ZH_DeltaY;		 //·½Î»ÍÑ°ÐÁ¿ 

		if (abs(dst.ZH_DeltaX) > 320)
		{
			dst.bValid = false;;
		}

		if (abs(dst.ZH_DeltaY) > 256)
		{
			dst.bValid = false;;
		}

		dst.ZH_PixelSize = src.ZH_IRPixelSize;  

		dst.ZH_ImgWidth = 720;  // ͼÏñ¿í
		dst.ZH_ImgHeight = 576; // ͼÏñ¸ß

		UINT8 GPS_LR_LocState = src.GPS_LR_WorkState & 0x0F;
		UINT8 GPS_LR_PosState = src.GPS_LR_WorkState >> 4;

		UINT8 GPS_FB_LocState = src.GPS_FB_WorkState & 0x0F;
		UINT8 GPS_FB_PosState = src.GPS_FB_WorkState >> 4;

		// ¶¨Î»×´Ì¬£º
		//          0x00 µ¥µã¶¨Î»
		//          0x02 Ôز¨Ïàλ²î·Ö¶¨Î»
		// ²â×Ê״̬£º
		//          0x00 Õý³£²â×Ê
		if (GPS_FB_LocState == 0x00 || GPS_FB_LocState == 0x02) // Ç°ºóÕý³£¶¨Î»
		{
			dst.UAV_GPS_Lon = src.GPS_FB_Lon; // ÎÞÈË»ú¾­¶È
			dst.UAV_GPS_Lat = src.GPS_FB_Lat; // ÎÞÈË»úγ¶È
			dst.UAV_GPS_Alt = src.GPS_FB_Alt; // ÎÞÈË»ú¸ß¶È
			dst.UAV_GPS_HSpeed = src.GPS_FB_HoriSpeed; // ˮƽËÙ¶È

			dst.UAV_GPS_Year = src.GPS_FB_Year;  
			dst.UAV_GPS_Month = src.GPS_FB_Month;
			dst.UAV_GPS_Day = src.GPS_FB_Day;   
			dst.UAV_GPS_UTC = src.GPS_FB_UTC; 
		}
		else if (GPS_LR_LocState == 0x00 || GPS_LR_LocState == 0x02)  // ×óÓÒÕý³£¶¨Î»
		{
			dst.UAV_GPS_Lon = src.GPS_LR_Lon; // ÎÞÈË»ú¾­¶È
			dst.UAV_GPS_Lat = src.GPS_LR_Lat; // ÎÞÈË»úγ¶È
			dst.UAV_GPS_Alt = src.GPS_LR_Alt; // ÎÞÈË»ú¸ß¶È
			dst.UAV_GPS_HSpeed = src.GPS_LR_HoriSpeed; // ˮƽËÙ¶È

			dst.UAV_GPS_Year = src.GPS_LR_Year;  
			dst.UAV_GPS_Month = src.GPS_LR_Month;  
			dst.UAV_GPS_Day = src.GPS_LR_Day;   
			dst.UAV_GPS_UTC = src.GPS_LR_UTC;  
		}
		else if (GPS_FB_LocState == 0x01 || GPS_FB_LocState == 0x03) // Ç°ºóÕý³£¶¨Î»
		{
			dst.UAV_GPS_Lon = src.GPS_FB_Lon; // ÎÞÈË»ú¾­¶È
			dst.UAV_GPS_Lat = src.GPS_FB_Lat; // ÎÞÈË»úγ¶È
			dst.UAV_GPS_Alt = src.GPS_FB_Alt; // ÎÞÈË»ú¸ß¶È
			dst.UAV_GPS_HSpeed = src.GPS_FB_HoriSpeed; // ˮƽËÙ¶È

			dst.UAV_GPS_Year = src.GPS_FB_Year;  
			dst.UAV_GPS_Month = src.GPS_FB_Month;
			dst.UAV_GPS_Day = src.GPS_FB_Day;   
			dst.UAV_GPS_UTC = src.GPS_FB_UTC; 
		}
		else if (GPS_LR_LocState == 0x01 || GPS_LR_LocState == 0x03)  // ×óÓÒÕý³£¶¨Î»
		{
			dst.UAV_GPS_Lon = src.GPS_LR_Lon; // ÎÞÈË»ú¾­¶È
			dst.UAV_GPS_Lat = src.GPS_LR_Lat; // ÎÞÈË»úγ¶È
			dst.UAV_GPS_Alt = src.GPS_LR_Alt; // ÎÞÈË»ú¸ß¶È
			dst.UAV_GPS_HSpeed = src.GPS_LR_HoriSpeed; // ˮƽËÙ¶È

			dst.UAV_GPS_Year = src.GPS_LR_Year;  
			dst.UAV_GPS_Month = src.GPS_LR_Month;  
			dst.UAV_GPS_Day = src.GPS_LR_Day;   
			dst.UAV_GPS_UTC = src.GPS_LR_UTC;  
		}
		else  // ·É»ú²»¶¨Î»
		{
			dst.bValid = false;;
		}

		// ²â×ÊÅжÏ
		if (GPS_LR_PosState == 0x00 && GPS_FB_PosState == 0x00)
		{
			dst.UAV_Yaw = src.GPS_LR_Heading; // º½Ïò½Ç
			dst.UAV_Roll = src.GPS_LR_Roll;   // ¹öת½Ç 
			dst.UAV_Pitch = src.GPS_FB_Pitch; // ¸©Ñö½Ç
		}
		else 
		{
			if (GPS_LR_PosState == 0x00)
			{
				// ×óÓÒGPS²â£ºº½Ïò½Ç+¹öת½Ç
				dst.UAV_Yaw = src.GPS_LR_Heading; // º½Ïò½Ç
				dst.UAV_Roll = src.GPS_LR_Roll;   // ¹öת½Ç 

				// ´¹Ö±ÍÓÂݸ©Ñö½Ç
				dst.UAV_Pitch = src.UAV_GyroPitch;
			} 
			else if (GPS_FB_PosState == 0x00)
			{
				// Ç°ºóGPS²â£ºº½Ïò½Ç+¸©Ñö½Ç
				dst.UAV_Yaw = src.GPS_FB_Heading; 
				dst.UAV_Pitch = src.GPS_FB_Pitch;   

				// ´¹Ö±ÍÓÂݲâ¹öת½Ç
				dst.UAV_Roll = src.UAV_GyroRoll;
			}
			else
			{
				// ÍÓÂÝ + ´Åº½Ïò
				dst.UAV_Yaw = src.UAV_MagHeading; // º½Ïò½Ç
				dst.UAV_Roll = src.UAV_GyroRoll;   // ¹öת½Ç 
				dst.UAV_Pitch = src.UAV_GyroPitch;
			}
		}
	}
}


// ¹¦ÄÜ£ºÔغɵ¼Òý£¬¼ÆËãÔغÉ×Ë̬½Ç()
// ÊäÈ룺
//      1. uavPos£º·É»úλÖã¬double uavPos[GPS¾­¶È£¬GPSγ¶È£¬GPS¸ß¶È]
//      2. tgtPos£ºÄ¿±êλÖã¬double tgtPos[GPS¾­¶È£¬GPSγ¶È£¬GPS¸ß¶È]
//      3. uavAttitude: ·É»ú×Ë̬£¬double uavAttitude[¸©Ñö£¬¹öת£¬Æ«º½]
// Êä³ö£º
//      1. loadPitch Ôغɸߵͽǣ¬µ¥Î»¶È
//      2. loadAzimuth Ôغɷ½Î»½Ç£¬µ¥Î»¶È
// ·µ»ØÖµ£º¼ÆËã³É¹¦·µ»Øtrue,·ñÔò·µ»Øfalse¡£
bool CalLoadAttitudeForLoadGuide(double &loadPitch, double &loadAzimuth, const double *uavPos, const double *tgtPos, const double *uavAttitude)
{
	const double g_PI = 3.1415926;

	// ÊäÈëÅжÏ
	if (uavPos == nullptr || tgtPos == nullptr || uavAttitude == nullptr)
	{
		return false;
	}

	double uavLon = uavPos[0];
	double uavLat = uavPos[1];
	double uavAlt = uavPos[2];

	double tgtLon = tgtPos[0];
	double tgtLat = tgtPos[1];
	double tgtAlt = tgtPos[2];

	double uavPitch = uavAttitude[0];
	double uavRoll = uavAttitude[1];
	double uavYaw = uavAttitude[2];

	if (abs(uavLon) > 180 || abs(uavLat) > 90 || uavLat < 0)
	{
		return false;
	}

	if (abs(tgtLon) > 180 || abs(tgtLat) > 90 || tgtLat < 0)
	{
		return false;
	}

	// Ä¿±ê ·É»úÏÞÖÆÔÚ¾­Î³¶È10¶È·¶Î§ÄÚ£¬ÇÒ·É»ú±ÈÄ¿±êÒª¸ß
	if (abs(uavLon - tgtLon) > 10 || abs(uavLat - tgtLat) > 10 || uavAlt - tgtAlt < 0)
	{
		return false;
	}

	// ¼ÆËã·É»úÓëÄ¿±êµÄ¾àÀë
	double uavTgtDist = 0;
	CalculateTwoPtsSlopeDistance(uavTgtDist, uavPos, tgtPos, 3);

	// ×î½ü¾àÀëÒªÇó
	if (uavTgtDist < 1)
	{
		return false;
	}

	// ·É»úÈý¸ö×Ë̬½Ç
	uavPitch = uavAttitude[0] / 180 * CV_PI;
	uavRoll  = uavAttitude[1] / 180 * CV_PI;
	uavYaw   = uavAttitude[2] / 180 * CV_PI;

	// ·É»ú¸©Ñö½Ç
	cv::Mat m3(3, 3, CV_64FC1);
	m3.at<double>(0, 0) = 1;
	m3.at<double>(0, 1) = 0;
	m3.at<double>(0, 2) = 0;

	m3.at<double>(1, 0) = 0;
	m3.at<double>(1, 1) = cos(uavPitch);
	m3.at<double>(1, 2) = -sin(uavPitch);

	m3.at<double>(2, 0) = 0;
	m3.at<double>(2, 1) = sin(uavPitch);
	m3.at<double>(2, 2) = cos(uavPitch);

	// ·É»ú¹öת½Ç
	cv::Mat m4(3, 3, CV_64FC1);
	m4.at<double>(0, 0) = cos(uavRoll);
	m4.at<double>(0, 1) = 0;
	m4.at<double>(0, 2) = sin(uavRoll);

	m4.at<double>(1, 0) = 0;
	m4.at<double>(1, 1) = 1;
	m4.at<double>(1, 2) = 0;

	m4.at<double>(2, 0) = -sin(uavRoll);
	m4.at<double>(2, 1) = 0;
	m4.at<double>(2, 2) = cos(uavRoll);

	// ·É»úº½Ïò½Ç
	cv::Mat m5(3, 3, CV_64FC1);
	m5.at<double>(0, 0) = cos(uavYaw);
	m5.at<double>(0, 1) = sin(uavYaw);
	m5.at<double>(0, 2) = 0;

	m5.at<double>(1, 0) = -sin(uavYaw);
	m5.at<double>(1, 1) = cos(uavYaw);
	m5.at<double>(1, 2) = 0;

	m5.at<double>(2, 0) = 0;
	m5.at<double>(2, 1) = 0;
	m5.at<double>(2, 2) = 1;


	// ¾ØÕó³Ë·¨
	cv::Mat m543;
	cv::gemm(m4, m3, 1, cv::noArray(), 0, m543);
	cv::gemm(m5, m543, 1, cv::noArray(), 0, m543);

	double a1 = m543.at<double>(0, 0);
	double a2 = m543.at<double>(0, 1);
	double a3 = m543.at<double>(0, 2);

	double a4 = m543.at<double>(1, 0);
	double a5 = m543.at<double>(1, 1);
	double a6 = m543.at<double>(1, 2);

	double a7 = m543.at<double>(2, 0);
	double a8 = m543.at<double>(2, 1);
	double a9 = m543.at<double>(2, 2);


	// »ùÓÚ·É»úλÖúÍÄ¿±êλÖÃÇó£ºx3  x6  x9
	double x3 = 0;
	double x6 = 0;
	double x9 = 0;

	// ·É»úÏà¶ÔÓÚÄ¿±êµÄ¸ß¶È
	double height = uavAlt - tgtAlt; 
	x9 = height / uavTgtDist;

	// Èý½ÇÐεױß
	double distance = sqrt(pow(uavTgtDist, 2) - pow(height, 2));

	// ÇóÄ¿±êÏà¶ÔÓÚ·É»úµØÃæͶӰµÄ·½Î»½Ç
	double azAngle = 0;
	CalculateTwoPtsAzimuth(azAngle, uavLon, uavLat, tgtLon, tgtLat, 3);

	x3 = -distance * sin(azAngle / 180.0 * g_PI) / uavTgtDist;
	x6 = -distance * cos(azAngle / 180.0 * g_PI) / uavTgtDist;


	double delta1 = a1 * x3 + a4 * x6 + a7 * x9;
	double delta2 = a2 * x3 + a5 * x6 + a8 * x9;
	double delta3 = a3 * x3 + a6 * x6 + a9 * x9;

	loadPitch = acos(delta3) * 180 / g_PI - 90;

	double sinLoadAzimuth = -delta1 / sqrt(1 - delta3 * delta3);
	double cosLoadAzimuth = -delta2 / sqrt(1 - delta3 * delta3);

	if (sinLoadAzimuth > 0 && cosLoadAzimuth > 0)
	{
		loadAzimuth = asin(sinLoadAzimuth) * 180 / g_PI;
	}
	else if (sinLoadAzimuth > 0 && cosLoadAzimuth < 0)
	{
		loadAzimuth = 180 - asin(sinLoadAzimuth) * 180 / g_PI;
	}
	else if (sinLoadAzimuth < 0 && cosLoadAzimuth < 0)
	{
		loadAzimuth = -180 - asin(sinLoadAzimuth) * 180 / g_PI;
	}
	else
	{
		loadAzimuth = asin(sinLoadAzimuth) * 180 / g_PI;
	}

	return true;
}

// »ñÈ¡·É»ú×Ë̬
bool GetUAVLocAndPose(double &UAVLon, double &UAVLat,double &UAVAlt, 
	                                    double &UAVRoll, double &UAVYaw,double &UAVPitch, 
	                                    const struTranslateProtocal_FJ &src)
{
	UINT8 GPS_LR_LocState = src.GPS_LR_WorkState & 0x0F;
	UINT8 GPS_LR_PosState = src.GPS_LR_WorkState >> 4;

	UINT8 GPS_FB_LocState = src.GPS_FB_WorkState & 0x0F;
	UINT8 GPS_FB_PosState = src.GPS_FB_WorkState >> 4;
	if (src.bValid == TRUE)
	{
		// ¶¨Î»ÅжÏ
		if (GPS_FB_LocState == 0x00 || GPS_FB_LocState == 0x02) // Ç°ºóÕý³£¶¨Î»
		{
			UAVLon = src.GPS_LR_Lon; // ÎÞÈË»ú¾­¶È
			UAVLat = src.GPS_LR_Lat; // ÎÞÈË»úγ¶È
			UAVAlt = src.GPS_LR_Alt; // ÎÞÈË»ú¸ß¶È
		}
		else if (GPS_LR_LocState == 0x00 || GPS_LR_LocState == 0x02)  // ×óÓÒÕý³£¶¨Î»
		{
			UAVLon = src.GPS_LR_Lon; // ÎÞÈË»ú¾­¶È
			UAVLat = src.GPS_LR_Lat; // ÎÞÈË»úγ¶È
			UAVAlt = src.GPS_LR_Alt; // ÎÞÈË»ú¸ß¶È

			UAVYaw = src.GPS_LR_Heading; // ÎÞÈË»úº½Ïò½Ç
			UAVRoll = src.GPS_LR_Roll;   // ÎÞÈË»ú¹öת½Ç 
		}
		else if (GPS_FB_LocState == 0x01 || GPS_FB_LocState == 0x03)
		{
			UAVLon = src.GPS_LR_Lon; // ÎÞÈË»ú¾­¶È
			UAVLat = src.GPS_LR_Lat; // ÎÞÈË»úγ¶È
			UAVAlt = src.GPS_LR_Alt; // ÎÞÈË»ú¸ß¶È

			UAVYaw = src.GPS_LR_Heading; // ÎÞÈË»úº½Ïò½Ç
			UAVRoll = src.GPS_LR_Roll;   // ÎÞÈË»ú¹öת½Ç 
		}
		else if (GPS_LR_LocState == 0x01 || GPS_LR_LocState == 0x03)
		{
			UAVLon = src.GPS_LR_Lon; // ÎÞÈË»ú¾­¶È
			UAVLat = src.GPS_LR_Lat; // ÎÞÈË»úγ¶È
			UAVAlt = src.GPS_LR_Alt; // ÎÞÈË»ú¸ß¶È

			UAVYaw = src.GPS_LR_Heading; // ÎÞÈË»úº½Ïò½Ç
			UAVRoll = src.GPS_LR_Roll;   // ÎÞÈË»ú¹öת½Ç 
		}
		else
		{
			return false;
		}
		// ²â×ËÅжÏ
		// ²â×ÊÅжÏ
		if (GPS_LR_PosState == 0x00 && GPS_FB_PosState == 0x00)
		{
			UAVYaw = src.GPS_LR_Heading; // ÎÞÈË»úº½Ïò½Ç
			UAVRoll = src.GPS_LR_Roll;   // ÎÞÈË»ú¹öת½Ç 
			UAVPitch = src.GPS_FB_Pitch;   // ÎÞÈË»ú¸©Ñö½Ç
		}
		else 
		{
			if (GPS_LR_PosState == 0x00)
			{
				// ×óÓÒGPS²â£ºº½Ïò½Ç+¹öת½Ç
				UAVYaw = src.GPS_LR_Heading; // ÎÞÈË»úº½Ïò½Ç
				UAVRoll = src.GPS_LR_Roll;   // ÎÞÈË»ú¹öת½Ç 

				// ´¹Ö±ÍÓÂݸ©Ñö½Ç
				UAVPitch = src.UAV_GyroPitch;
			} 
			else if (GPS_FB_PosState == 0x00)
			{
				// Ç°ºóGPS²â£ºº½Ïò½Ç+¸©Ñö½Ç
				UAVYaw = src.GPS_FB_Heading; 
				UAVPitch = src.GPS_FB_Pitch;   

				// ´¹Ö±ÍÓÂݲâ¹öת½Ç
				UAVRoll = src.UAV_GyroRoll;
			}
			else
			{
				// ÍÓÂÝ + ´Åº½Ïò
				UAVYaw = src.UAV_MagHeading; // º½Ïò½Ç
				UAVRoll = src.UAV_GyroRoll;   // ¹öת½Ç 
				UAVPitch = src.UAV_GyroPitch;
			}
		}
	}
	return true;
}


// ¼ÆË㷽λ½Ç
double getYaw(double XA, double YA)
{
	// Êä³öºÏ³Éº½Ïò½Ç Yaw 
	double PayLoad_Yaw = 0;

	// ÔÚ×ø±êÖáÉÏ
	if (XA > 0 && YA == 0)
	{
		PayLoad_Yaw = 90;
	}
	else if (XA < 0 && YA == 0)
	{
		PayLoad_Yaw = -90;
	}
	else if (XA == 0 && YA > 0)
	{
		PayLoad_Yaw = 0;
	}
	else if (XA == 0 && YA < 0)
	{
		PayLoad_Yaw = 180;
	}
	else
	{
		double tanYaw = 0.0; 
		if (YA > 0 && XA > 0)
		{
			// µÚ1ÏóÏÞ
			tanYaw = XA / YA;
			PayLoad_Yaw = atan(tanYaw) * 180 / CV_PI; 
		}
		else if (YA < 0 && XA > 0)
		{
			// µÚ2ÏóÏÞ
			tanYaw = -XA / YA;
			PayLoad_Yaw = atan(tanYaw) * 180 / CV_PI; 
			PayLoad_Yaw = 180 - PayLoad_Yaw;
		}
		else if (YA < 0 && XA < 0)
		{
			// µÚ3ÏóÏÞ
			tanYaw = XA / YA;
			PayLoad_Yaw = atan(tanYaw) * 180 / CV_PI; 
			PayLoad_Yaw = -180 + PayLoad_Yaw;
		}
		else if (YA > 0 && XA < 0)
		{
			// µÚ4ÏóÏÞ
			tanYaw = -XA / YA;
			PayLoad_Yaw = atan(tanYaw) * 180 / CV_PI; 
			PayLoad_Yaw = -PayLoad_Yaw;
		}
	}

	return PayLoad_Yaw;
}


//¹¦ÄÜ£º¼ÆËãͼÏñÖÐÈÎÒâµã¾­Î³¶È×ø±ê ÎÞÎó²î²¹³¥
//ÊäÈ룺	1. frame  ÊäÈ븴½ÓÊý¾Ý
//          2. pt_x£º ÁÐ
//          3. pt_y£º ÐÐ
//      
//Êä³ö£º	1. Lon     ¾­¶È
//          2. Lat£º   γ¶È
//          3. ·µ»ØÖµ£º´¦Àí³É¹¦·µ»Øtrue£¬Ê§°Ü·µ»Øfalse
bool CalAnyPtCoordinateWithoutErrorCompensate(double &Lon, double &Lat, double &H, const struQB_FJ *frame, int pt_x, int pt_y)
{
	try
	{
		// »ù´¡Ìõ¼þÅжÏ
		if (frame == NULL || frame->bValid == FALSE)
		{
			return false;
		}

		// Åжϸ´½ÓÊý¾ÝÓÐЧÐÔ
		double uavLon = frame->UAV_GPS_Lon;
		double uavLat = frame->UAV_GPS_Lat;
		double uavAlt = frame->UAV_GPS_Alt;

		double uavGroundHeight = frame->UAV_GroundHeight; 

		double uavYaw = frame->UAV_Yaw;
		double uavPitch = frame->UAV_Pitch;
		double uavRoll = frame->UAV_Roll;

		double zhAz = frame->ZH_Azimuth;
		double zhPitch = frame->ZH_Pitch;

		// 2. Ô½½çÅжÏ
		if (abs(uavLon) > 180) // ¾­¶È
		{
			return false;
		}

		if (abs(uavLat) > 90)  // γ¶È
		{
			return false;
		}

		if (uavAlt < 0 || uavAlt > 10000) // ¸ß¶È
		{
			return false;
		}

		if (uavGroundHeight < 0 || (uavAlt - uavGroundHeight < 30))
		{
			return false;
		}

		if (abs(uavYaw) > 360) // ·É»úº½Ïò½Ç
		{
			return false;
		}

		if (abs(uavPitch) > 30) // ·É»ú¸©Ñö½Ç
		{
			return false;
		}

		if (abs(uavRoll) > 30)  // ·É»ú¹öת½Ç
		{
			return false;
		}

		if (abs(zhAz) > 180) // Ôغɷ½Î»½Ç
		{
			return false;
		}

		if (zhPitch > -15 || zhPitch < -110) 
		{
			return false;
		}

		// ͼÏñ³ß´ç
		int imgWidth = frame->ZH_ImgWidth;
		int imgHeight = frame->ZH_ImgHeight;

		// ÅжÏ
		if (imgWidth <= 0 || imgHeight <= 0)
		{
			return false;
		}

		// ÊäÈëλÖÃÅжÏ
		if (pt_x < 0 || pt_x >= imgWidth)
		{
			return false;
		}

		if (pt_y < 0 || pt_y >= imgHeight)
		{
			return false;
		}

		// ½¹¾à
		double f = frame->ZH_FocalLen / 1000;  // Ã×
		if (f <= 0)
		{
			return false;
		}

		// ÏñÔª³ß´ç Ã×
		double pixelSize = frame->ZH_PixelSize;
		if (pixelSize <= 0)
		{
			return false;
		}

		// ·É»ú
		double UAV_ROLL  = uavRoll;  // ·É»ú¹öת½Ç  £¨½Ç¶È£©
		double UAV_PITCH = uavPitch; // ·É»ú¸©Ñö½Ç  £¨½Ç¶È£©
		double UAV_YAW   = uavYaw;   // ·É»úº½Ïò½Ç  £¨½Ç¶È£©

		// ÔغÉ
		double ZH_PITCH = zhPitch; // ÔØºÉ¸ßµÍ½Ç  £¨½Ç¶È£©
		double ZH_YAW   = zhAz;    // Ôغɷ½Î»½Ç  £¨½Ç¶È£©

		// Ôغɸ©Ñö½ÇÐÞÕý
		ZH_PITCH = ZH_PITCH + 90; // [0 90]

		// ¸÷¸ö½Çת»»Îª»¡¶È
		ZH_PITCH = ZH_PITCH / 180 * CV_PI;   // 1
		ZH_YAW   =   ZH_YAW / 180 * CV_PI;   // 2

		UAV_PITCH = UAV_PITCH / 180 * CV_PI; // 3
		UAV_ROLL  = UAV_ROLL  / 180 * CV_PI; // 4
		UAV_YAW   = UAV_YAW   / 180 * CV_PI; // 5


		double BaseLon = uavLon; // ¾­¶È
		double BaseLat = uavLat; // ¾­¶È

		double height = uavAlt - uavGroundHeight; // Ïà¶Ô¸ß¶È = ·ÉÐÐ¸ß¶È - µØÃæ¸ß³Ì

		bool bLaser = frame->ZH_LaserOn;
		double LaserDistance = frame->ZH_LaserDist;

		// ¼¤¹â²â¾àÓÐЧÐÔÅжÏ
		if (bLaser == true && LaserDistance < 100)
		{
			return false;
		}

		// ¼ÆËãÐýת¾ØÕó 
		// x1  x2  x3
		// x4  x5  x6
		// x7  x8  x9

		// Ôغɸ©Ñö½Ç
		cv::Mat m1(3,3,CV_64FC1);
		m1.at<double>(0,0) = 1;
		m1.at<double>(0,1) = 0;
		m1.at<double>(0,2) = 0;

		m1.at<double>(1,0) = 0;
		m1.at<double>(1,1) = cos(ZH_PITCH);
		m1.at<double>(1,2) = -sin(ZH_PITCH);

		m1.at<double>(2,0) = 0;
		m1.at<double>(2,1) = sin(ZH_PITCH);
		m1.at<double>(2,2) = cos(ZH_PITCH);

		// Ôغɷ½Î»½Ç
		cv::Mat m2(3,3,CV_64FC1);
		m2.at<double>(0,0) = cos(ZH_YAW);
		m2.at<double>(0,1) = sin(ZH_YAW);
		m2.at<double>(0,2) = 0;

		m2.at<double>(1,0) = -sin(ZH_YAW);
		m2.at<double>(1,1) = cos(ZH_YAW);
		m2.at<double>(1,2) = 0;

		m2.at<double>(2,0) = 0;
		m2.at<double>(2,1) = 0;
		m2.at<double>(2,2) = 1;

		// ·É»ú¸©Ñö½Ç
		cv::Mat m3(3,3,CV_64FC1);
		m3.at<double>(0,0) = 1;
		m3.at<double>(0,1) = 0;
		m3.at<double>(0,2) = 0;

		m3.at<double>(1,0) = 0;
		m3.at<double>(1,1) = cos(UAV_PITCH);
		m3.at<double>(1,2) = -sin(UAV_PITCH);

		m3.at<double>(2,0) = 0;
		m3.at<double>(2,1) = sin(UAV_PITCH);
		m3.at<double>(2,2) = cos(UAV_PITCH);

		// ·É»ú¹öת½Ç
		cv::Mat m4(3,3,CV_64FC1);
		m4.at<double>(0,0) = cos(UAV_ROLL);
		m4.at<double>(0,1) = 0;
		m4.at<double>(0,2) = sin(UAV_ROLL);

		m4.at<double>(1,0) = 0;
		m4.at<double>(1,1) = 1;
		m4.at<double>(1,2) = 0;

		m4.at<double>(2,0) = -sin(UAV_ROLL);
		m4.at<double>(2,1) = 0;
		m4.at<double>(2,2) = cos(UAV_ROLL);

		// ·É»úº½Ïò½Ç
		cv::Mat m5(3,3,CV_64FC1);
		m5.at<double>(0,0) = cos(UAV_YAW);
		m5.at<double>(0,1) = sin(UAV_YAW);
		m5.at<double>(0,2) = 0;

		m5.at<double>(1,0) = -sin(UAV_YAW);
		m5.at<double>(1,1) = cos(UAV_YAW);
		m5.at<double>(1,2) = 0;

		m5.at<double>(2,0) = 0;
		m5.at<double>(2,1) = 0;
		m5.at<double>(2,2) = 1;


		// ¾ØÕó³Ë·¨
		cv::Mat m54321;
		cv::gemm(m2, m1, 1, cv::noArray(), 0, m54321);
		cv::gemm(m3, m54321, 1, cv::noArray(), 0, m54321);
		cv::gemm(m4, m54321, 1, cv::noArray(), 0, m54321);
		cv::gemm(m5, m54321, 1, cv::noArray(), 0, m54321);


		// È¡³öM54321ÖеÄÖµ
		double x1 = m54321.at<double>(0,0);
		double x2 = m54321.at<double>(0,1);
		double x3 = m54321.at<double>(0,2);

		double x4 = m54321.at<double>(1,0);
		double x5 = m54321.at<double>(1,1);
		double x6 = m54321.at<double>(1,2);

		double x7 = m54321.at<double>(2,0);
		double x8 = m54321.at<double>(2,1);
		double x9 = m54321.at<double>(2,2);

		// ×ø±êת»»
		double x = 0, y = 0;

		// ÏñƽÃæ×ø±êϵ
		x = +pt_x - imgWidth / 2;
		y = -pt_y + imgHeight / 2;  

		// ת»»ÎªÎïÀí×ø±ê µ¥Î» Ã×
		x = x * pixelSize;
		y = y * pixelSize;

		// ·ÖĸÁãÖµÅжÏ
		if (x7 * x + x8 * y - x9 * f == 0)
		{
			return false;
		}

		// ¹²Ïß·½³ÌÄ£ÐÍ
		double XA = -(x1 * x + x2 * y - x3 * f) / (x7 * x + x8 * y - x9 * f) * height;
		double YA = -(x4 * x + x5 * y - x6 * f) / (x7 * x + x8 * y - x9 * f) * height;

		// ¼ÆË㷽λ½ÇºÍ¾àÀë
		double distance = sqrt(XA * XA + YA * YA);
		double yaw = getYaw(XA, YA);

		// ¼ÆËãͼÏñÖÐÐĵ㾭γ¶È
		CalculatePtCoordinate(Lon, Lat, BaseLon, BaseLat, yaw, distance, 3); // WGS84×ø±êϵ

		// Ä¿±ê¸ß¶È¼ÆËã
		H = uavGroundHeight;

		return true;
	}
	catch(cv::Exception &e)
	{
		e.msg.c_str();
		return false;
	}
	catch(...)
	{
		return false;
	}

	return true;
}


// ³ä·ÖÀûÓü¤¹â£¬Ìá¸ßµ¥µã¶¨Î»¾«¶È
void CalLonLatOffset(double &LonOffset, double &LatOffset, const struQB_FJ *frame)
{
	try
	{
		// ÖÃÁã
		LonOffset = 0;
		LatOffset = 0;

		// »ù´¡Ìõ¼þÅжÏ
		if (frame == NULL || frame->bValid == false)
		{
			return;
		}

		// Èç¹ûÎÞ¼¤¹â Ö±½Ó·µ»Ø
		if ((frame->ZH_LaserOn == true && frame->ZH_LaserDist > 0 && frame->ZH_LaserDist < 65535) == false)
		{
			return;
		}

		// ¼¤¹â²âÁ¿¼ÆËã½á¹û
		double LaserLon = 0.0;
		double LaserLat = 0.0;
		double LaserH = 0.0;

		// ÎÞ¼¤¹â²âÁ¿¼ÆËã½á¹û
		double Lon = 0.0;
		double Lat = 0.0;
		double H = 0.0;

		if (true == CalImgCenterCoordinate(LaserLon, LaserLat, LaserH, frame) && 
			true == CalAnyPtCoordinateWithoutErrorCompensate(Lon, Lat, H, frame, frame->ZH_ImgWidth / 2, frame->ZH_ImgWidth / 2))
		{
			LonOffset = LaserLon - Lon;
			LatOffset = LaserLat - Lat;
		}
	}
	catch(cv::Exception &e)
	{
		e.msg.c_str();
		return;
	}
	catch(...)
	{
		return;
	}
}

// ¼ÆËãͼÏñÖÐÐĵ㾭γ¶È×ø±ê
bool CalImgCenterCoordinate(double &Lon, double &Lat, double &H, const struQB_FJ *frame)
{
	try
	{
		// »ù´¡Ìõ¼þÅжÏ
		if (frame == NULL || frame->bValid == FALSE)
		{
			return false;
		}

		// Åжϸ´½ÓÊý¾ÝÓÐЧÐÔ
		double uavLon = frame->UAV_GPS_Lon;
		double uavLat = frame->UAV_GPS_Lat;
		double uavAlt = frame->UAV_GPS_Alt;

		double uavGroundHeight = frame->UAV_GroundHeight; 

		double uavYaw = frame->UAV_Yaw;
		double uavPitch = frame->UAV_Pitch;
		double uavRoll = frame->UAV_Roll;

		double zhAz = frame->ZH_Azimuth;
		double zhPitch = frame->ZH_Pitch;

		// 2. Ô½½çÅжÏ
		if (abs(uavLon) > 180) // ¾­¶È
		{
			return false;
		}

		if (abs(uavLat) > 90)  // γ¶È
		{
			return false;
		}

		if (uavAlt < 0 || uavAlt > 10000) // ¸ß¶È
		{
			return false;
		}

		if (uavGroundHeight < 0 || (uavAlt - uavGroundHeight < 30))
		{
			return false;
		}

		if (abs(uavYaw) > 360) // ·É»úº½Ïò½Ç
		{
			return false;
		}

		if (abs(uavPitch) > 30) // ·É»ú¸©Ñö½Ç
		{
			return false;
		}

		if (abs(uavRoll) > 30)  // ·É»ú¹öת½Ç
		{
			return false;
		}

		if (abs(zhAz) > 180) // Ôغɷ½Î»½Ç
		{
			return false;
		}

		if (zhPitch > -15 || zhPitch < -110) 
		{
			return false;
		}


		// ·É»ú
		double UAV_ROLL  = uavRoll;  // ·É»ú¹öת½Ç  £¨½Ç¶È£©
		double UAV_PITCH = uavPitch; // ·É»ú¸©Ñö½Ç  £¨½Ç¶È£©
		double UAV_YAW   = uavYaw;   // ·É»úº½Ïò½Ç  £¨½Ç¶È£©

		// ÔغÉ
		double ZH_PITCH = zhPitch; // ÔØºÉ¸ßµÍ½Ç  £¨½Ç¶È£©
		double ZH_YAW   = zhAz;    // Ôغɷ½Î»½Ç  £¨½Ç¶È£©

		// Ôغɸ©Ñö½ÇÐÞÕý
		ZH_PITCH = ZH_PITCH + 90; // [0 90]

		// ¸÷¸ö½Çת»»Îª»¡¶È
		ZH_PITCH = ZH_PITCH / 180 * CV_PI;   // 1
		ZH_YAW   =   ZH_YAW / 180 * CV_PI;   // 2

		UAV_PITCH = UAV_PITCH / 180 * CV_PI; // 3
		UAV_ROLL  = UAV_ROLL  / 180 * CV_PI; // 4
		UAV_YAW   = UAV_YAW   / 180 * CV_PI; // 5


		double BaseLon = uavLon; // ¾­¶È
		double BaseLat = uavLat; // ¾­¶È

		double height = uavAlt - uavGroundHeight; // Ïà¶Ô¸ß¶È = ·ÉÐÐ¸ß¶È - µØÃæ¸ß³Ì

		bool bLaser = frame->ZH_LaserOn;
		double LaserDistance = frame->ZH_LaserDist;

		// ¼¤¹â²â¾àÓÐЧÐÔÅжÏ
		if (bLaser == true && LaserDistance < 100)
		{
			bLaser = false;
		}

		// ¼ÆËãÐýת¾ØÕó 
		// x1  x2  x3
		// x4  x5  x6
		// x7  x8  x9

		// Ôغɸ©Ñö½Ç
		cv::Mat m1(3,3,CV_64FC1);
		m1.at<double>(0,0) = 1;
		m1.at<double>(0,1) = 0;
		m1.at<double>(0,2) = 0;

		m1.at<double>(1,0) = 0;
		m1.at<double>(1,1) = cos(ZH_PITCH);
		m1.at<double>(1,2) = -sin(ZH_PITCH);

		m1.at<double>(2,0) = 0;
		m1.at<double>(2,1) = sin(ZH_PITCH);
		m1.at<double>(2,2) = cos(ZH_PITCH);

		// Ôغɷ½Î»½Ç
		cv::Mat m2(3,3,CV_64FC1);
		m2.at<double>(0,0) = cos(ZH_YAW);
		m2.at<double>(0,1) = sin(ZH_YAW);
		m2.at<double>(0,2) = 0;

		m2.at<double>(1,0) = -sin(ZH_YAW);
		m2.at<double>(1,1) = cos(ZH_YAW);
		m2.at<double>(1,2) = 0;

		m2.at<double>(2,0) = 0;
		m2.at<double>(2,1) = 0;
		m2.at<double>(2,2) = 1;

		// ·É»ú¸©Ñö½Ç
		cv::Mat m3(3,3,CV_64FC1);
		m3.at<double>(0,0) = 1;
		m3.at<double>(0,1) = 0;
		m3.at<double>(0,2) = 0;

		m3.at<double>(1,0) = 0;
		m3.at<double>(1,1) = cos(UAV_PITCH);
		m3.at<double>(1,2) = -sin(UAV_PITCH);

		m3.at<double>(2,0) = 0;
		m3.at<double>(2,1) = sin(UAV_PITCH);
		m3.at<double>(2,2) = cos(UAV_PITCH);

		// ·É»ú¹öת½Ç
		cv::Mat m4(3,3,CV_64FC1);
		m4.at<double>(0,0) = cos(UAV_ROLL);
		m4.at<double>(0,1) = 0;
		m4.at<double>(0,2) = sin(UAV_ROLL);

		m4.at<double>(1,0) = 0;
		m4.at<double>(1,1) = 1;
		m4.at<double>(1,2) = 0;

		m4.at<double>(2,0) = -sin(UAV_ROLL);
		m4.at<double>(2,1) = 0;
		m4.at<double>(2,2) = cos(UAV_ROLL);

		// ·É»úº½Ïò½Ç
		cv::Mat m5(3,3,CV_64FC1);
		m5.at<double>(0,0) = cos(UAV_YAW);
		m5.at<double>(0,1) = sin(UAV_YAW);
		m5.at<double>(0,2) = 0;

		m5.at<double>(1,0) = -sin(UAV_YAW);
		m5.at<double>(1,1) = cos(UAV_YAW);
		m5.at<double>(1,2) = 0;

		m5.at<double>(2,0) = 0;
		m5.at<double>(2,1) = 0;
		m5.at<double>(2,2) = 1;


		// ¾ØÕó³Ë·¨
		cv::Mat m54321;
		cv::gemm(m2, m1, 1, cv::noArray(), 0, m54321);
		cv::gemm(m3, m54321, 1, cv::noArray(), 0, m54321);
		cv::gemm(m4, m54321, 1, cv::noArray(), 0, m54321);
		cv::gemm(m5, m54321, 1, cv::noArray(), 0, m54321);


		// È¡³öM54321ÖеÄÖµ
		double x1 = m54321.at<double>(0,0);
		double x2 = m54321.at<double>(0,1);
		double x3 = m54321.at<double>(0,2);

		double x4 = m54321.at<double>(1,0);
		double x5 = m54321.at<double>(1,1);
		double x6 = m54321.at<double>(1,2);

		double x7 = m54321.at<double>(2,0);
		double x8 = m54321.at<double>(2,1);
		double x9 = m54321.at<double>(2,2);


		// Èç¹ûÓм¤¹â²â¾à ÒÔ¼¤¹â²â¾àΪ׼
		if (bLaser == true && LaserDistance < 65535) // 65535 = 0xFFFF ÎÞЧֵ
		{
			height = LaserDistance * x9;
		}

		double XA = -(x3) / (x9) * height;
		double YA = -(x6) / (x9) * height;

		// Êä³öºÏ³Éº½Ïò½Ç Yaw 
		double PayLoad_Yaw = 0;
		PayLoad_Yaw = getYaw(XA, YA);

		// ¸ßµÍ½Ç ºÏ³É
		double PayLoad_Pitch = acos(x9) * 180 / CV_PI;   

		// Èý½ÇÐεױß
		double distance = height * tan(PayLoad_Pitch / 180.0 * CV_PI);

		// Èç¹ûÓм¤¹â²â¾à ÒÔ¼¤¹â²â¾àΪ׼
		if (bLaser == true && LaserDistance < 65535)
		{
			// ¸ù¾Ý¼¤¹â¾àÀëºÍºÏ³É½Ç¶È ¼ÆËãƽÃæͶӰ¾àÀë
			distance = LaserDistance * sin(PayLoad_Pitch / 180.0 * CV_PI);
		}

		//¹¦ÄÜ£º¼ÆËãÄ¿±êµãµÄ¾­Î³¶È
		CalculatePtCoordinate(Lon, Lat, BaseLon, BaseLat, PayLoad_Yaw, distance, 3); // WGS84×ø±êϵ

		// Ä¿±ê¸ß¶È¼ÆËã
		H = uavGroundHeight;  

		// Èç¹ûÓм¤¹â²â¾à 
		if (bLaser == true && LaserDistance < 65535)
		{
			H = uavAlt - LaserDistance * cos(PayLoad_Pitch * CV_PI / 180);
		}
	}
	catch(cv::Exception &e)
	{
		e.msg.c_str();
		return false;
	}
	catch(...)
	{
		return false;
	}

	return true;
}

//¹¦ÄÜ£º¼ÆËãͼÏñÖÐÈÎÒâµã¾­Î³¶È×ø±ê
//ÊäÈ룺	1. frame  ÊäÈ븴½ÓÊý¾Ý
//          2. pt_x£º ÁÐ
//          3. pt_y£º ÐÐ
//      
//Êä³ö£º	1. Lon     ¾­¶È
//          2. Lat£º   γ¶È
//          3. ·µ»ØÖµ£º´¦Àí³É¹¦·µ»Øtrue£¬Ê§°Ü·µ»Øfalse
bool CalAnyPtCoordinate(double &Lon, double &Lat, double &H, const struQB_FJ *frame, int pt_x, int pt_y)
{
	try
	{
		// »ù´¡Ìõ¼þÅжÏ
		if (frame == NULL || frame->bValid == FALSE)
		{
			return false;
		}

		// Åжϸ´½ÓÊý¾ÝÓÐЧÐÔ
		double uavLon = frame->UAV_GPS_Lon;
		double uavLat = frame->UAV_GPS_Lat;
		double uavAlt = frame->UAV_GPS_Alt;

		double uavGroundHeight = frame->UAV_GroundHeight; 

		double uavYaw = frame->UAV_Yaw;
		double uavPitch = frame->UAV_Pitch;
		double uavRoll = frame->UAV_Roll;

		double zhAz = frame->ZH_Azimuth;
		double zhPitch = frame->ZH_Pitch;

		// 2. Ô½½çÅжÏ
		if (abs(uavLon) > 180) // ¾­¶È
		{
			return false;
		}

		if (abs(uavLat) > 90)  // γ¶È
		{
			return false;
		}

		if (uavAlt < 0 || uavAlt > 10000) // ¸ß¶È
		{
			return false;
		}

		if (uavGroundHeight < 0 || (uavAlt - uavGroundHeight < 30))
		{
			return false;
		}

		if (abs(uavYaw) > 360) // ·É»úº½Ïò½Ç
		{
			return false;
		}

		if (abs(uavPitch) > 30) // ·É»ú¸©Ñö½Ç
		{
			return false;
		}

		if (abs(uavRoll) > 30)  // ·É»ú¹öת½Ç
		{
			return false;
		}

		if (abs(zhAz) > 180) // Ôغɷ½Î»½Ç
		{
			return false;
		}

		if (zhPitch > -15 || zhPitch < -110) 
		{
			return false;
		}

		// ͼÏñ³ß´ç
		int imgWidth = frame->ZH_ImgWidth;
		int imgHeight = frame->ZH_ImgHeight;

		// ÅжÏ
		if (imgWidth <= 0 || imgHeight <= 0)
		{
			return false;
		}

		// ÊäÈëλÖÃÅжÏ
		if (pt_x < 0 || pt_x >= imgWidth)
		{
			return false;
		}

		if (pt_y < 0 || pt_y >= imgHeight)
		{
			return false;
		}

		// ½¹¾à
		double f = frame->ZH_FocalLen / 1000;  // Ã×
		if (f <= 0)
		{
			return false;
		}

		// ÏñÔª³ß´ç Ã×
		double pixelSize = frame->ZH_PixelSize;
		if (pixelSize <= 0)
		{
			return false;
		}

		// ·É»ú
		double UAV_ROLL  = uavRoll;  // ·É»ú¹öת½Ç  £¨½Ç¶È£©
		double UAV_PITCH = uavPitch; // ·É»ú¸©Ñö½Ç  £¨½Ç¶È£©
		double UAV_YAW   = uavYaw;   // ·É»úº½Ïò½Ç  £¨½Ç¶È£©

		// ÔغÉ
		double ZH_PITCH = zhPitch; // ÔØºÉ¸ßµÍ½Ç  £¨½Ç¶È£©
		double ZH_YAW   = zhAz;    // Ôغɷ½Î»½Ç  £¨½Ç¶È£©

		// Ôغɸ©Ñö½ÇÐÞÕý
		ZH_PITCH = ZH_PITCH + 90; // [0 90]

		// ¸÷¸ö½Çת»»Îª»¡¶È
		ZH_PITCH = ZH_PITCH / 180 * CV_PI;   // 1
		ZH_YAW   =   ZH_YAW / 180 * CV_PI;   // 2

		UAV_PITCH = UAV_PITCH / 180 * CV_PI; // 3
		UAV_ROLL  = UAV_ROLL  / 180 * CV_PI; // 4
		UAV_YAW   = UAV_YAW   / 180 * CV_PI; // 5


		double BaseLon = uavLon; // ¾­¶È
		double BaseLat = uavLat; // ¾­¶È

		double height = uavAlt - uavGroundHeight; // Ïà¶Ô¸ß¶È = ·ÉÐÐ¸ß¶È - µØÃæ¸ß³Ì

		bool bLaser = frame->ZH_LaserOn;
		double LaserDistance = frame->ZH_LaserDist;

		// ¼¤¹â²â¾àÓÐЧÐÔÅжÏ
		if (bLaser == true && LaserDistance < 100)
		{
			bLaser = false;
		}

		// ¼ÆËãÐýת¾ØÕó 
		// x1  x2  x3
		// x4  x5  x6
		// x7  x8  x9

		// Ôغɸ©Ñö½Ç
		cv::Mat m1(3,3,CV_64FC1);
		m1.at<double>(0,0) = 1;
		m1.at<double>(0,1) = 0;
		m1.at<double>(0,2) = 0;

		m1.at<double>(1,0) = 0;
		m1.at<double>(1,1) = cos(ZH_PITCH);
		m1.at<double>(1,2) = -sin(ZH_PITCH);

		m1.at<double>(2,0) = 0;
		m1.at<double>(2,1) = sin(ZH_PITCH);
		m1.at<double>(2,2) = cos(ZH_PITCH);

		// Ôغɷ½Î»½Ç
		cv::Mat m2(3,3,CV_64FC1);
		m2.at<double>(0,0) = cos(ZH_YAW);
		m2.at<double>(0,1) = sin(ZH_YAW);
		m2.at<double>(0,2) = 0;

		m2.at<double>(1,0) = -sin(ZH_YAW);
		m2.at<double>(1,1) = cos(ZH_YAW);
		m2.at<double>(1,2) = 0;

		m2.at<double>(2,0) = 0;
		m2.at<double>(2,1) = 0;
		m2.at<double>(2,2) = 1;

		// ·É»ú¸©Ñö½Ç
		cv::Mat m3(3,3,CV_64FC1);
		m3.at<double>(0,0) = 1;
		m3.at<double>(0,1) = 0;
		m3.at<double>(0,2) = 0;

		m3.at<double>(1,0) = 0;
		m3.at<double>(1,1) = cos(UAV_PITCH);
		m3.at<double>(1,2) = -sin(UAV_PITCH);

		m3.at<double>(2,0) = 0;
		m3.at<double>(2,1) = sin(UAV_PITCH);
		m3.at<double>(2,2) = cos(UAV_PITCH);

		// ·É»ú¹öת½Ç
		cv::Mat m4(3,3,CV_64FC1);
		m4.at<double>(0,0) = cos(UAV_ROLL);
		m4.at<double>(0,1) = 0;
		m4.at<double>(0,2) = sin(UAV_ROLL);

		m4.at<double>(1,0) = 0;
		m4.at<double>(1,1) = 1;
		m4.at<double>(1,2) = 0;

		m4.at<double>(2,0) = -sin(UAV_ROLL);
		m4.at<double>(2,1) = 0;
		m4.at<double>(2,2) = cos(UAV_ROLL);

		// ·É»úº½Ïò½Ç
		cv::Mat m5(3,3,CV_64FC1);
		m5.at<double>(0,0) = cos(UAV_YAW);
		m5.at<double>(0,1) = sin(UAV_YAW);
		m5.at<double>(0,2) = 0;

		m5.at<double>(1,0) = -sin(UAV_YAW);
		m5.at<double>(1,1) = cos(UAV_YAW);
		m5.at<double>(1,2) = 0;

		m5.at<double>(2,0) = 0;
		m5.at<double>(2,1) = 0;
		m5.at<double>(2,2) = 1;


		// ¾ØÕó³Ë·¨
		cv::Mat m54321;
		cv::gemm(m2, m1, 1, cv::noArray(), 0, m54321);
		cv::gemm(m3, m54321, 1, cv::noArray(), 0, m54321);
		cv::gemm(m4, m54321, 1, cv::noArray(), 0, m54321);
		cv::gemm(m5, m54321, 1, cv::noArray(), 0, m54321);


		// È¡³öM54321ÖеÄÖµ
		double x1 = m54321.at<double>(0,0);
		double x2 = m54321.at<double>(0,1);
		double x3 = m54321.at<double>(0,2);

		double x4 = m54321.at<double>(1,0);
		double x5 = m54321.at<double>(1,1);
		double x6 = m54321.at<double>(1,2);

		double x7 = m54321.at<double>(2,0);
		double x8 = m54321.at<double>(2,1);
		double x9 = m54321.at<double>(2,2);

		// ×ø±êת»»
		double x = 0, y = 0;

		// ÏñƽÃæ×ø±êϵ
		x = +pt_x - imgWidth / 2;
		y = -pt_y + imgHeight / 2;  

		// ת»»ÎªÎïÀí×ø±ê µ¥Î» Ã×
		x = x * pixelSize;
		y = y * pixelSize;

		// ·ÖĸÁãÖµÅжÏ
		if (x7 * x + x8 * y - x9 * f == 0)
		{
			return false;
		}

		// ¹²Ïß·½³ÌÄ£ÐÍ
		double XA = -(x1 * x + x2 * y - x3 * f) / (x7 * x + x8 * y - x9 * f) * height;
		double YA = -(x4 * x + x5 * y - x6 * f) / (x7 * x + x8 * y - x9 * f) * height;

		// ¼ÆË㷽λ½ÇºÍ¾àÀë
		double distance = sqrt(XA * XA + YA * YA);
		double yaw = getYaw(XA, YA);

		// ¼ÆËãͼÏñÖÐÐĵ㾭γ¶È
		CalculatePtCoordinate(Lon, Lat, BaseLon, BaseLat, yaw, distance, 3); // WGS84×ø±êϵ

		// ³ä·ÖÀûÓü¤¹â¶¨Î»Ïà¶Ô׼ȷ£¬¼ÆËãÆ«ÒÆÁ¿£¬×÷²¹³¥
		double Lonoffset = 0.0;
		double Latoffset = 0.0;
		CalLonLatOffset(Lonoffset, Latoffset, frame);

		// ÐÞÕýÆ«ÒÆÁ¿
		Lon += Lonoffset;
		Lat += Latoffset;

		// Ä¿±ê¸ß¶È¼ÆËã
		H = uavGroundHeight;

		return true;
	}
	catch(cv::Exception &e)
	{
		e.msg.c_str();
		return false;
	}
	catch(...)
	{
		return false;
	}

	return true;
}

// ¹¦ÄÜ£ºÊÍ·ÅÇ鱨Êý¾Ý
void ReleaseQBData(QBStru *pQBData)
{
	// Ö¸ÕëΪ¿ÕÅжÏ
	if (pQBData != NULL)
	{
		// srcImg
		delete [] pQBData->image.srcImg.buff;

		// dstImg
		delete [] pQBData->image.dstImg.buff;

		// demImg
		delete [] pQBData->image.geoImg.buff;

		// ÇåÁã
		memset(pQBData, 0, sizeof(QBStru));
	}
}

// ¹¦ÄÜ£º¿Ë¡ͼÏñÊý¾Ý
void CloneImgStru(const ImgStru *src, ImgStru *dst)
{
	try
	{
		if(src == NULL || dst == NULL)
		{
			return;
		}

		if (src->ImgWidth > 0 && src->ImgHeight > 0 && (src->bitcount == 24 || src->bitcount == 8) && src->buff != NULL)
		{
			// ͼÏñÊôÐÔ¸´ÖÆ
			dst->ImgWidth = src->ImgWidth;
			dst->ImgHeight = src->ImgHeight;
			dst->bitcount = src->bitcount;
			dst->BoundingBox = src->BoundingBox;

			// Êý¾Ý¸´ÖÆ
			SAFE_DELETE_ARRAY(dst->buff);
			dst->buff = NULL;

			// ²½³¤£ºÃ¿ÐÐÏñËØËùÕ¼×Ö½ÚÊý£¬±ØÐëÀ©Õ¹³É4µÄ±¶Êý 
			int lineByte = (dst->ImgWidth * dst->bitcount / 8);

			// λͼÊý¾Ý»º³åÇøµÄ´óС£¬¼´Í¼Ïñ´óС
			int imgBufSize = dst->ImgHeight * lineByte;

			// ·ÖÅäÄÚ´æ¿Õ¼ä
			if (imgBufSize > 0)
			{
				dst->buff = new BYTE[imgBufSize];

				if (dst->buff != NULL)
				{
					memcpy(dst->buff, src->buff, static_cast<size_t>(imgBufSize)); // ͼÏñÊý¾Ý¸´ÖÆ
				}
			}

			dst->BoundingBox = src->BoundingBox;
		}
	}
	catch(cv::Exception &e)
	{
		e.msg;
		return;
	}
	catch(...)
	{
		return;
	}
}

void CloneQBData(const QBStru *src, QBStru *dst)
{
	if(src == NULL || dst == NULL)
	{
		return;
	}

	// Èç¹ûsrcÊý¾Ý¾ùÎÞЧ£¬Ö±½Ó·µ»Ø
	if (src->image.bValid == false)
	{
		return;
	}

	//	dst->frameAll = src->frameAll;
	dst->framePart = src->framePart; 


	if (src->image.bValid == true)
	{
		dst->image.bValid = true;
		CloneImgStru(&src->image.srcImg, &dst->image.srcImg);
		CloneImgStru(&src->image.dstImg, &dst->image.dstImg);
		CloneImgStru(&src->image.geoImg, &dst->image.geoImg);
	}
}

BYTE Buffer[512000];

void GetH264VideoDataFromNet()
{
	// ʵʱ״̬
	if (g_bWorkMode == false)
	{
		BYTE Buffer[512000];
		sockaddr_in loc;
		while (true && g_socketFH96Data != INVALID_SOCKET)
		{
			// ½ÓÊÕÊý¾Ý
			int len = sizeof(SOCKADDR);
			int ret = recvfrom(g_socketFH96Data, 
				(char* )&Buffer, 
				512000, 
				0, 
				(struct sockaddr*)&loc, 
				&len);
			if (ret != SOCKET_ERROR)
			{
				if (g_CH96_h264decoder != nullptr)
				{
					g_CH96_h264decoder->decoding(Buffer, ret);
				}
				// ´¦ÀíÊý¾Ý
				//SocketReceiveH264VideoData(Buffer, ret);
				/*if (g_tcpStarted)
				{
					g_TCPTrans2Server.WriteBuffer(Buffer, ret);
				}*/
				if (g_udpStarted)
				{
					int iTimes = ret/512;
					for (int i = 0; i < iTimes; i++)
					{
						g_UDPTrans2Server.WriteBuffer(Buffer + i * 512, 512);
					}
					if (ret%512 > 0)
					{
						g_UDPTrans2Server.WriteBuffer(Buffer + iTimes * 512, ret%512);
					}
					
					//g_UDPTrans2Server.WriteBuffer(Buffer, ret);
				}
			}
		}
	}
}

void SocketReceiveH264VideoData(const void* buffer, int length)
{
	if (buffer == nullptr || length < 1) 
	{
		return;
	}
	else
	{
		// ´æÈëÄÚ´æ³Ø¡ª¡ª¶àÏ߳̿ª¹Ø£¬ÓÃÓÚProcessData
		WaitForSingleObject(g_hMutex_ProcessDataH264, INFINITE);
		try
		{
			if (g_bWorkMode == false) // ʵʱģʽÏÂдÈëÎļþ
			{
				//дÈë±¾µØÎļþ
				if (g_pFileSaveVideoH264Data != NULL) 
				{
					fwrite(buffer, static_cast<size_t>(1), length, g_pFileSaveVideoH264Data); // дÊý¾Ý
				}
			}

			// Åж¨Êý¾Ý
			if (length + g_validDataLenSocket < g_MaxBufferLenSocket) //³õʼg_validDataLenSocketΪ0£¬¼´ÎªÈç¹ûÊÕÊýµÄlengthСÓÚ×î´ó×Ö³¤£¬Ôò¡­¡­
			{
				memcpy(g_receiveDataBufferSocket + g_validDataLenSocket, buffer, length); //½«bufferÄÚÈÝ¿½±´µ½g_receiveDataBufferSocketÑÓ³¤g_validDataLenSocket¸ö×Ö½ÚÖ®ºóµÄg_receiveDataBufferSocketÖÐ
				g_validDataLenSocket = length + g_validDataLenSocket; //ÓÐЧ×Ö³¤ÖÃΪÊÕÊý×Ö³¤
			}
			else
			{
				if (length <= g_MaxBufferLenSocket)  // Ð޸ģ¨ÔöÌí£©
				{
					// ÇåÀí»º´æ
					g_validDataLenSocket = 0;

					// ¼Ç¼±¾´ÎÊý¾Ý
					memcpy(g_receiveDataBufferSocket + g_validDataLenSocket, buffer, length);
					g_validDataLenSocket = length + g_validDataLenSocket;
				}
			}
			ReleaseMutex(g_hMutex_ProcessDataH264);
		}
		catch(...)
		{
			ReleaseMutex(g_hMutex_ProcessDataH264);
		}
	}
}

void ProcessH264VideoData()
{
	return;
	while(g_bProcessDataSocketFlag)
	{
		try
		{
			WaitForSingleObject(g_hMutex_ProcessDataH264, INFINITE);
			// ÊÇ·ñ³É¹¦½«Êý¾Ý¸´ÖƵ½ÁÙʱλÖ㬱íÕ÷±äÁ¿
			//bool bHaveRelease = false;
			// ×îС֡³¤
			if (g_validDataLenSocket > 0)
			{
				//unsigned char midBuffer[1024];

				//// Êý¾Ý¸´ÖÆ
				//memcpy(midBuffer, g_receiveDataBufferSocket, 1024);
				//// ¸üÐÂÄÚ´æ
				//unsigned int frameLen = 256;
				//g_validDataLenSocket = g_validDataLenSocket - frameLen;
				//memcpy(g_receiveDataBuffer_middleSocket, g_receiveDataBufferSocket + frameLen, g_validDataLenSocket);
				//memcpy(g_receiveDataBufferSocket, g_receiveDataBuffer_middleSocket, g_validDataLenSocket);
				ReleaseMutex(g_hMutex_ProcessDataH264);
				// Êý¾Ý³É¹¦¸´ÖƵ½ÁÙʱλÖÃ
				//bHaveRelease = true;
				// ½âÂë
				if (g_CH96_h264decoder != nullptr)
				{
					g_CH96_h264decoder->decoding(g_receiveDataBufferSocket, g_validDataLenSocket);
				}
			}

			/*if (bHaveRelease == false)
			{
				ReleaseMutex(g_hMutex_ProcessDataH264);
			}*/
		}
		catch(...)
		{
			
		}
	}
}

//¹¦ÄÜ£º´®¿ÚÊý×Ö»»Ëãµ½±ê×¼µÄ´®¿ÚÊä³ö
//ÊäÈ룺´®¿ÚÊýÖµportID
//Êä³ö£º´®¿ÚÊý×éport
void GetSerialPortNumber(char *port, const CString portID)
{	

	CString strSerialPortNumber = portID;
	strSerialPortNumber.Delete( 0, 4 );
	int     ptNumber = atoi( strSerialPortNumber );

	CString str = _T( "" );
	str.Format( "COM%d", ptNumber );

	memcpy( port, str.GetBuffer( 255 ), str.GetLength() );
	str.ReleaseBuffer();
}