#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 #include #include #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((((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(guideAzimuth); //方位角 g_Guide_Pitch = static_cast(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(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(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(0, 0) = 1; m3.at(0, 1) = 0; m3.at(0, 2) = 0; m3.at(1, 0) = 0; m3.at(1, 1) = cos(uavPitch); m3.at(1, 2) = -sin(uavPitch); m3.at(2, 0) = 0; m3.at(2, 1) = sin(uavPitch); m3.at(2, 2) = cos(uavPitch); // 飞机滚转角 cv::Mat m4(3, 3, CV_64FC1); m4.at(0, 0) = cos(uavRoll); m4.at(0, 1) = 0; m4.at(0, 2) = sin(uavRoll); m4.at(1, 0) = 0; m4.at(1, 1) = 1; m4.at(1, 2) = 0; m4.at(2, 0) = -sin(uavRoll); m4.at(2, 1) = 0; m4.at(2, 2) = cos(uavRoll); // 飞机航向角 cv::Mat m5(3, 3, CV_64FC1); m5.at(0, 0) = cos(uavYaw); m5.at(0, 1) = sin(uavYaw); m5.at(0, 2) = 0; m5.at(1, 0) = -sin(uavYaw); m5.at(1, 1) = cos(uavYaw); m5.at(1, 2) = 0; m5.at(2, 0) = 0; m5.at(2, 1) = 0; m5.at(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(0, 0); double a2 = m543.at(0, 1); double a3 = m543.at(0, 2); double a4 = m543.at(1, 0); double a5 = m543.at(1, 1); double a6 = m543.at(1, 2); double a7 = m543.at(2, 0); double a8 = m543.at(2, 1); double a9 = m543.at(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(0,0) = 1; m1.at(0,1) = 0; m1.at(0,2) = 0; m1.at(1,0) = 0; m1.at(1,1) = cos(ZH_PITCH); m1.at(1,2) = -sin(ZH_PITCH); m1.at(2,0) = 0; m1.at(2,1) = sin(ZH_PITCH); m1.at(2,2) = cos(ZH_PITCH); // 载荷方位角 cv::Mat m2(3,3,CV_64FC1); m2.at(0,0) = cos(ZH_YAW); m2.at(0,1) = sin(ZH_YAW); m2.at(0,2) = 0; m2.at(1,0) = -sin(ZH_YAW); m2.at(1,1) = cos(ZH_YAW); m2.at(1,2) = 0; m2.at(2,0) = 0; m2.at(2,1) = 0; m2.at(2,2) = 1; // 飞机俯仰角 cv::Mat m3(3,3,CV_64FC1); m3.at(0,0) = 1; m3.at(0,1) = 0; m3.at(0,2) = 0; m3.at(1,0) = 0; m3.at(1,1) = cos(UAV_PITCH); m3.at(1,2) = -sin(UAV_PITCH); m3.at(2,0) = 0; m3.at(2,1) = sin(UAV_PITCH); m3.at(2,2) = cos(UAV_PITCH); // 飞机滚转角 cv::Mat m4(3,3,CV_64FC1); m4.at(0,0) = cos(UAV_ROLL); m4.at(0,1) = 0; m4.at(0,2) = sin(UAV_ROLL); m4.at(1,0) = 0; m4.at(1,1) = 1; m4.at(1,2) = 0; m4.at(2,0) = -sin(UAV_ROLL); m4.at(2,1) = 0; m4.at(2,2) = cos(UAV_ROLL); // 飞机航向角 cv::Mat m5(3,3,CV_64FC1); m5.at(0,0) = cos(UAV_YAW); m5.at(0,1) = sin(UAV_YAW); m5.at(0,2) = 0; m5.at(1,0) = -sin(UAV_YAW); m5.at(1,1) = cos(UAV_YAW); m5.at(1,2) = 0; m5.at(2,0) = 0; m5.at(2,1) = 0; m5.at(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(0,0); double x2 = m54321.at(0,1); double x3 = m54321.at(0,2); double x4 = m54321.at(1,0); double x5 = m54321.at(1,1); double x6 = m54321.at(1,2); double x7 = m54321.at(2,0); double x8 = m54321.at(2,1); double x9 = m54321.at(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(0,0) = 1; m1.at(0,1) = 0; m1.at(0,2) = 0; m1.at(1,0) = 0; m1.at(1,1) = cos(ZH_PITCH); m1.at(1,2) = -sin(ZH_PITCH); m1.at(2,0) = 0; m1.at(2,1) = sin(ZH_PITCH); m1.at(2,2) = cos(ZH_PITCH); // 载荷方位角 cv::Mat m2(3,3,CV_64FC1); m2.at(0,0) = cos(ZH_YAW); m2.at(0,1) = sin(ZH_YAW); m2.at(0,2) = 0; m2.at(1,0) = -sin(ZH_YAW); m2.at(1,1) = cos(ZH_YAW); m2.at(1,2) = 0; m2.at(2,0) = 0; m2.at(2,1) = 0; m2.at(2,2) = 1; // 飞机俯仰角 cv::Mat m3(3,3,CV_64FC1); m3.at(0,0) = 1; m3.at(0,1) = 0; m3.at(0,2) = 0; m3.at(1,0) = 0; m3.at(1,1) = cos(UAV_PITCH); m3.at(1,2) = -sin(UAV_PITCH); m3.at(2,0) = 0; m3.at(2,1) = sin(UAV_PITCH); m3.at(2,2) = cos(UAV_PITCH); // 飞机滚转角 cv::Mat m4(3,3,CV_64FC1); m4.at(0,0) = cos(UAV_ROLL); m4.at(0,1) = 0; m4.at(0,2) = sin(UAV_ROLL); m4.at(1,0) = 0; m4.at(1,1) = 1; m4.at(1,2) = 0; m4.at(2,0) = -sin(UAV_ROLL); m4.at(2,1) = 0; m4.at(2,2) = cos(UAV_ROLL); // 飞机航向角 cv::Mat m5(3,3,CV_64FC1); m5.at(0,0) = cos(UAV_YAW); m5.at(0,1) = sin(UAV_YAW); m5.at(0,2) = 0; m5.at(1,0) = -sin(UAV_YAW); m5.at(1,1) = cos(UAV_YAW); m5.at(1,2) = 0; m5.at(2,0) = 0; m5.at(2,1) = 0; m5.at(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(0,0); double x2 = m54321.at(0,1); double x3 = m54321.at(0,2); double x4 = m54321.at(1,0); double x5 = m54321.at(1,1); double x6 = m54321.at(1,2); double x7 = m54321.at(2,0); double x8 = m54321.at(2,1); double x9 = m54321.at(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(0,0) = 1; m1.at(0,1) = 0; m1.at(0,2) = 0; m1.at(1,0) = 0; m1.at(1,1) = cos(ZH_PITCH); m1.at(1,2) = -sin(ZH_PITCH); m1.at(2,0) = 0; m1.at(2,1) = sin(ZH_PITCH); m1.at(2,2) = cos(ZH_PITCH); // 载荷方位角 cv::Mat m2(3,3,CV_64FC1); m2.at(0,0) = cos(ZH_YAW); m2.at(0,1) = sin(ZH_YAW); m2.at(0,2) = 0; m2.at(1,0) = -sin(ZH_YAW); m2.at(1,1) = cos(ZH_YAW); m2.at(1,2) = 0; m2.at(2,0) = 0; m2.at(2,1) = 0; m2.at(2,2) = 1; // 飞机俯仰角 cv::Mat m3(3,3,CV_64FC1); m3.at(0,0) = 1; m3.at(0,1) = 0; m3.at(0,2) = 0; m3.at(1,0) = 0; m3.at(1,1) = cos(UAV_PITCH); m3.at(1,2) = -sin(UAV_PITCH); m3.at(2,0) = 0; m3.at(2,1) = sin(UAV_PITCH); m3.at(2,2) = cos(UAV_PITCH); // 飞机滚转角 cv::Mat m4(3,3,CV_64FC1); m4.at(0,0) = cos(UAV_ROLL); m4.at(0,1) = 0; m4.at(0,2) = sin(UAV_ROLL); m4.at(1,0) = 0; m4.at(1,1) = 1; m4.at(1,2) = 0; m4.at(2,0) = -sin(UAV_ROLL); m4.at(2,1) = 0; m4.at(2,2) = cos(UAV_ROLL); // 飞机航向角 cv::Mat m5(3,3,CV_64FC1); m5.at(0,0) = cos(UAV_YAW); m5.at(0,1) = sin(UAV_YAW); m5.at(0,2) = 0; m5.at(1,0) = -sin(UAV_YAW); m5.at(1,1) = cos(UAV_YAW); m5.at(1,2) = 0; m5.at(2,0) = 0; m5.at(2,1) = 0; m5.at(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(0,0); double x2 = m54321.at(0,1); double x3 = m54321.at(0,2); double x4 = m54321.at(1,0); double x5 = m54321.at(1,1); double x6 = m54321.at(1,2); double x7 = m54321.at(2,0); double x8 = m54321.at(2,1); double x9 = m54321.at(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(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(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(); }