#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(); }