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