CaoYuhui
/
FH98ViewOnPlaneSoftware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
NewDCCtrl
main
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'c7e5a61e34'
${ noResults }
FH98ViewOnPlaneSoftware/Src/CH91PayloadSoftware/Global.cpp

2802 lines
67 KiB
C++
Raw Normal View History Unescape Escape

first commit
2 years ago
#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У<43><D0A3>
//struct struCHECK
//{
// struCHECK()
// {
// C0 = C1 = 0;
// };
//
// BYTE C0;
// BYTE C1;
//};
const UINT16 Crc_Table[256] = // X16+X12+X5+1 1021<32><31>ʽ<EFBFBD><CABD> 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); // <20><>8λ<38><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݴ<EFBFBD>CRC <20>ĸ<EFBFBD>8 λ
crc <<= 8; // <20><><EFBFBD><EFBFBD> 8 λ
crc ^= Crc_Table[temp ^ *ptr]; // <20><><EFBFBD>ֽں͵<DABA>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>XOR <20>ٲ<EFBFBD><D9B2><EFBFBD>
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;
}
// <20><>ȡָ<C8A1><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD>ռ<EFBFBD>
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;
}
// <20><><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>Ŀ¼
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;
}
//<2F><><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD>в<EFBFBD><D0B2><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>
bool SearchDirectory(const CString &strDir)
{
CFileFind fileFind;
if (fileFind.FindFile(strDir))
{
fileFind.Close();
return true;
}
else
{
fileFind.Close();
return false;
}
}
// <20><>ȡ<EFBFBD>ɼ<EFBFBD><C9BC>⡢<EFBFBD><E2A1A2><EFBFBD><EFBFBD>֡<EFBFBD>ĵ<EFBFBD>֡<EFBFBD><D6A1><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>
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)); //һ<><D2BB>unsigned int<6E><74>4<EFBFBD><34><EFBFBD>ֽڣ<D6BD><DAA3><EFBFBD>ͷ*<2A><><EFBFBD><EFBFBD>ȡֵ<C8A1><D6B5><EFBFBD><EFBFBD>˼
return val;
}
// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƭ֡<C6AC>ĵ<EFBFBD>֡<EFBFBD><D6A1><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD>
unsigned int getFrameLenDC(BYTE B0, BYTE B1)
{
BYTE a[2];
a[0] = B1;
a[1] = B0;
wchar_t val = *((wchar_t*)(a)); //һ<><D2BB>wchar_t<5F><74>2<EFBFBD><32><EFBFBD>ֽڣ<D6BD><DAA3><EFBFBD>ͷ*<2A><><EFBFBD><EFBFBD>ȡֵ<C8A1><D6B5><EFBFBD><EFBFBD>˼
return val;
}
// <20><>ȡͼ<C8A1><CDBC>˫<EFBFBD><CBAB><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>
void CALLBACK GetEOImgCenterOffset(CPoint offset, int flag)
{
g_payloadCtrlProtocol = 11;
g_TrackOffset = offset;
}
// <20>Ҽ<EFBFBD><D2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⣬ת<E2A3AC><D7AA>Ϊ<EFBFBD>ֶ<EFBFBD><D6B6><EFBFBD><EFBFBD><EFBFBD>
void CALLBACK GetEOIRImgRightButtonDown(int type)
{
// <20>л<EFBFBD>Ϊ<EFBFBD>ֶ<EFBFBD><D6B6><EFBFBD><EFBFBD><EFBFBD>
if (type == 1)
{
g_payloadCtrlProtocol = 10;
}
// ֹͣ<CDA3><D6B9>Ӧ<EFBFBD>ӳ<EFBFBD><D3B3><EFBFBD>С
else if (type == 2)
{
g_payloadCtrlProtocol = 0;
}
}
// ͨ<><CDA8>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD>
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));
}
}
//<2F><>ͼģ<CDBC><C4A3><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
void CALLBACK GetLeadLonLat(double Lon, double Lat, double Alt)
{
//<2F>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD>ͼģ<CDBC><C4A3><EFBFBD>ϵľ<CFB5>γ<EFBFBD>Ȼ<EFBFBD>ȡ<EFBFBD>õ<EFBFBD><C3B5>ĸ߳<C4B8>ֵ
CString rootDir = GetSoftwareCurrentDirectory() + "\\ElevationData";
float fElevator = 0;
bool ret = GetDem( fElevator, float(Lon), float(Lat), rootDir.GetBuffer(0));//<2F><>γ<EFBFBD>߶ȶ<DFB6><C8B6><EFBFBD><EFBFBD>Ի<EFBFBD><D4BB><EFBFBD><EFBFBD><EFBFBD>
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)
{
// <20>˴<EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD>91<39><31>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD>ʴ˴<CAB4><CBB4><EFBFBD>λ<EFBFBD><CEBB>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD><D3A6>91Э<31><D0AD>ͳһ
g_Guide_Azimuth = -static_cast<float>(guideAzimuth); //<2F><>λ<EFBFBD><CEBB>
g_Guide_Pitch = static_cast<float>(guidePitch); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ <20><><EFBFBD>µ<EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
::SendMessageA(g_CH96CtrlDlgHwnd, WM_SHOW_MAP_GUIDE_POS, WPARAM(0), LPARAM(0));
}
}
// <20>ط<EFBFBD>ģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void ReplayInputData(const void* buffer, int length)
{
// <20>ط<EFBFBD>ģʽ<C4A3>ж<EFBFBD>
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++)
{
20240130 增加IMP显示和控制界面和串口通信
1 year ago
// g_UDPTrans2Server.WriteBuffer(Buffer + i * 512, 512);
first commit
2 years ago
}
if (length%512 > 0)
{
20240130 增加IMP显示和控制界面和串口通信
1 year ago
// g_UDPTrans2Server.WriteBuffer(Buffer + iTimes * 512, length%512);
first commit
2 years ago
}
//g_UDPTrans2Server.WriteBuffer(buffer, length);
}
}
}
//UDP <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>غɣ<D8BA>
void CALLBACK UDPRevTgtInfo(const void* buffer, int length, UINT32 data)
{
if (g_bWorkMode == false) // ʵʱģʽ
{
if (buffer == nullptr || length < 1)
{
return;
}
else
{
try
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
memcpy(&tgtRecvInfo, buffer, length);
::PostMessageA(g_MapGuideHwnd, WM_SHOW_QB_TARGET, 0, 0);
}
catch(...)
{
}
}
}
}
20240130 增加IMP显示和控制界面和串口通信
1 year ago
// L<><4C><EFBFBD>鲥<EFBFBD><E9B2A5><EFBFBD><EFBFBD>
first commit
2 years ago
void GetFH96DataFromNet()
{
// ʵʱ״̬
if (g_bWorkMode == false)
{
BYTE Buffer[512000];
sockaddr_in loc;
while (true && g_socketFH96Data != INVALID_SOCKET)
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int len = sizeof(SOCKADDR);
int ret = recvfrom(g_socketFH96Data,
(char* )&Buffer,
512000,
0,
(struct sockaddr*)&loc,
&len);
if (ret != SOCKET_ERROR)
{
20240130 增加IMP显示和控制界面和串口通信
1 year ago
//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);
// }
first commit
2 years ago
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SocketReceiveFH96Data(Buffer, ret);
/*if (g_tcpStarted)
{
g_TCPTrans2Server.WriteBuffer(Buffer, ret);
}*/
20240130 增加IMP显示和控制界面和串口通信
1 year ago
first commit
2 years ago
}
}
}
}
void SocketReceiveFH96Data(const void* buffer, int length)
{
if (buffer == nullptr || length < 1)
{
return;
}
else
{
// <20><><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4>ء<EFBFBD><D8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߳̿<DFB3><CCBF>أ<EFBFBD><D8A3><EFBFBD><EFBFBD><EFBFBD>ProcessData
WaitForSingleObject(g_hMutex_ProcessDataCH96, INFINITE);
try
{
if (g_bWorkMode == false) // ʵʱģʽ<C4A3><CABD>д<EFBFBD><D0B4><EFBFBD>ļ<EFBFBD>
{
//д<>뱾<EFBFBD><EBB1BE><EFBFBD>ļ<EFBFBD>
if (g_pFileSaveCH96txData != NULL)
{
fwrite(buffer, static_cast<size_t>(1), length, g_pFileSaveCH96txData); // д<><D0B4><EFBFBD><EFBFBD>
// <20><><EFBFBD>ƶ<EFBFBD><C6B6>ն˷<D5B6><CBB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
20240130 增加IMP显示和控制界面和串口通信
1 year ago
// g_UdpSend2Mobile.WriteBuffer(buffer, length);
first commit
2 years ago
}
}
// <20>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD>
if (length + g_validDataLen < g_MaxBufferLen) //<2F><>ʼg_validDataLenΪ0<CEAA><30><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>lengthС<68><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֳ<EFBFBD><D6B3><EFBFBD><EFBFBD>򡭡<EFBFBD>
{
memcpy(g_receiveDataBuffer + g_validDataLen, buffer, length); //<2F><>buffer<65><72><EFBFBD>ݿ<EFBFBD><DDBF><EFBFBD><EFBFBD><EFBFBD>g_receiveDataBuffer<65>ӳ<EFBFBD>g_validDataLen<65><6E><EFBFBD>ֽ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD><EFBFBD>g_receiveDataBuffer<65><72>
g_validDataLen = length + g_validDataLen; //<2F><>Ч<EFBFBD>ֳ<EFBFBD><D6B3><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD>ֳ<EFBFBD>
}
else
{
if (length <= g_MaxBufferLen) // <20>޸ģ<DEB8><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
g_validDataLen = 0;
// <20><>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
memcpy(g_receiveDataBuffer + g_validDataLen, buffer, length);
g_validDataLen = length + g_validDataLen;
}
}
ReleaseMutex(g_hMutex_ProcessDataCH96);
}
catch(...)
{
ReleaseMutex(g_hMutex_ProcessDataCH96);
}
}
}
// h264<36><34><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
void CH96_h264DecodeResultCallbackFun(unsigned char* data,int width,int height)
{
if (data == nullptr || width != 1920 || height != 1080)
{
return;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ýϵͣ<CFB5><CDA3>򡭡<EFBFBD>
if (g_bGoodComputer == FALSE)
{
// Ӳ<><D3B2><EFBFBD><EFBFBD>ֱ<EFBFBD>ӽ<EFBFBD><D3BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><CABE><EFBFBD>˿ؼ<CBBF><D8BC>ϣ<EFBFBD><CFA3>˴<EFBFBD><CBB4><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD><D0B1><EFBFBD>
memcpy(g_ImgforLowComputer.data, data, 1080*1920*3);
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ýϸߣ<CFB8><DFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD>ʾ<EFBFBD><CABE>ʽ<EFBFBD><CABD><EFBFBD>д<EFBFBD><D0B4><EFBFBD>
else
{
// <20>ɼ<EFBFBD><C9BC><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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);
// <20>Ƿ<EFBFBD><C7B7>ɹ<EFBFBD><C9B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݸ<EFBFBD><DDB8>Ƶ<EFBFBD><C6B5><EFBFBD>ʱλ<CAB1>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
bool bHaveRelease = false;
// <20><>С֡<D0A1><D6A1>
if (g_validDataLen >= 512)
{
// <20><>֡ͷ
int _head = 0; // head <20><>0<EFBFBD><30>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>
bool _bfindhead = false;
int SetMaxLoc = g_validDataLen - 256; // <20><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>β256<35><36><EFBFBD>ֽڴ<D6BD>
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; // <20><><EFBFBD><EFBFBD>forѭ<72><D1AD>
}
} // for ѭ<><D1AD><EFBFBD><EFBFBD>֡ͷ
// <20><><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>֡ͷ
if (_bfindhead == true)
{
// <20><><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD>
memcpy(midBuffer, g_receiveDataBuffer + _head, 256);
// <20><><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD>
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);
// <20><><EFBFBD>ݳɹ<DDB3><C9B9><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5><EFBFBD>ʱλ<CAB1><CEBB>
bHaveRelease = true;
// <20><><EFBFBD><EFBFBD>ң<EFBFBD><D2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (midBuffer[4] == 0x2A)
{
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
else if (midBuffer[4] == 0x50)
{
// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD>ֻ<EFBFBD><D6BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>9-158<35>ֽڣ<D6BD><DAA3><EFBFBD>150<35>ֽڣ<D6BD>
WaitForSingleObject(g_hMutex_SaveFJData, INFINITE);
memcpy(&g_CH96FJData, midBuffer + 8, 150);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
TransCH96FJData(g_CH96TransFJData, g_CH96FJData);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD>鱨<EFBFBD><E9B1A8><EFBFBD><EFBFBD><EFBFBD>ø<EFBFBD><C3B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
TransCH96FJDatatoQBData(g_CH96QBData, g_CH96TransFJData);
ReleaseMutex(g_hMutex_SaveFJData);
// <20>鲥ҳ<E9B2A5><D2B3><EFBFBD><EFBFBD>ʾFJ<46><4A><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
::PostMessageA(g_MulticastSocketReceiveDlgHwnd, WN_SHOW_CH96_FJDATA, 0, 0);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//::PostMessageA(g_mainDlgHwnd, WM_SHOW_CH96_INFO, 0, 0);
// <20><>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
::PostMessageA(g_QBTargetTrackFunctionsHwnd, WM_SHOW_LASERDIS_QBFUNCTIONSDLG, 0, 0);
// <20><><EFBFBD>µ<EFBFBD>ͼ״̬
::PostMessageA(g_MapGuideHwnd, WM_UPDATA_GOOGLEMAP, 0, 0);
}
// ͼ<><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
else if (midBuffer[4] == 0xD5)
{
// <20><><EFBFBD><EFBFBD>
if (g_CH96_h264decoder != nullptr)
{
g_CH96_h264decoder->decoding(midBuffer + 8, 248);
20240130 增加IMP显示和控制界面和串口通信
1 year ago
g_UDPTrans2Server.WriteBuffer(midBuffer + 8, 248);
first commit
2 years ago
}
/*
if(g_tcpStarted==true){
memcpy(g_tcpData+26,midBuffer + 8, 248);
//֡<><D6A1><EFBFBD><EFBFBD>
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);
}
*/
// <20>洢<EFBFBD><E6B4A2><EFBFBD><EFBFBD>
// д<>뱾<EFBFBD><EBB1BE><EFBFBD>ļ<EFBFBD>
if (g_pFileSaveVideoDataCH96 != NULL && g_bVideoCapture == TRUE)
{
fwrite(midBuffer + 8, static_cast<size_t>(1), 248, g_pFileSaveVideoDataCH96); // д<><D0B4><EFBFBD><EFBFBD>
}
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
else
{
// do nothing
}
}
// û<>ҵ<EFBFBD>֡ͷ
else
{
// head֮ǰ<D6AE><C7B0>ɾ<EFBFBD><C9BE>
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);
}
}
}
// <20><>ȡCH96<39><36><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void TransCH96FJData(Data96TXD &dst, Protocal96TXD &src)
{
dst.head[0] = 0xEE; // ֡ͷ
dst.head[1] = 0x16;
// <20>غ<EFBFBD><D8BA><EFBFBD><EFBFBD><EFBFBD>
dst.mem1 = 0x00; // Ԥ<><D4A4>
dst.SwitchCmd = src.SwitchCmd; // <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
dst.Status0 = src.Status0; // ״̬<D7B4><CCAC>0
dst.Status1 = src.Status1; // ״̬<D7B4><CCAC>1
dst.EOFocus = (float)(src.EOFocus * 0.1); // EO<45><4F><EFBFBD><EFBFBD>mm
dst.Status2 = src.Status2; // ״̬<D7B4><CCAC>2
dst.FrameCount = src.FrameCount; // ʱ<><CAB1>
dst.dx = (int)src.dx; // <20><><EFBFBD><EFBFBD><EFBFBD>Ѱ<EFBFBD><D1B0><EFBFBD>
dst.dy = (int)src.dy; // <20><>λ<EFBFBD>Ѱ<EFBFBD><D1B0><EFBFBD>
dst.IRFocus = (float)(src.IRFocus * 0.1); // IR<49><52><EFBFBD><EFBFBD>mm
dst.LaserDis = (int)src.LaserDis; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
dst.LaserState = src.LaserState; // <20><><EFBFBD><EFBFBD>״̬
dst.None_1 = src.None_1; // <20><>
dst.None_2 = src.None_2; // <20><>
dst.ZH_Azimuth = (float)(src.ZH_Azimuth * 0.01); // <20>غɸ<D8BA><C9B8><EFBFBD><EFBFBD><EFBFBD>
dst.ZH_Pitch = (float)(src.ZH_Pitch * 0.01); // <20>غɷ<D8BA>λ<EFBFBD><CEBB>
dst.PixelSize = (float)(src.PixelSize*1e-3F); // <20><>Ԫ<EFBFBD>ߴ<EFBFBD>
// <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD>
dst.PlaneID = (int)src.PlaneID; // <20>ɻ<EFBFBD>ID
dst.Pitch = (float)(src.Pitch * 0.01); // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.Roll = (float)(src.Roll * 0.01F); // <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA>
dst.Yaw = (float)(src.Yaw * 0.01F); // <20>ɻ<EFBFBD><C9BB><EFBFBD>λ<EFBFBD><CEBB>
dst.Satelite_lat = (double)(src.Satelite_lat * 1e-7); // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD>˾<EFBFBD><CBBE><EFBFBD>
dst.Satelite_lon = (double)(src.Satelite_lon * 1e-7); // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD>γ<EFBFBD><CEB3>
dst.Satelite_height = (double)(src.Satelite_height * 0.01); // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD>˸߶<CBB8>
dst.Satelite_Ground_Speed = (float)(src.Satelite_Ground_Speed * 0.01); // <20>ɻ<EFBFBD>GPS<50><53><EFBFBD><EFBFBD>
dst.Satelite_Ground_Speed_Dir = (float)(src.Satelite_Ground_Speed_Dir * 0.01); // <20>ɻ<EFBFBD>GPS<50><53><EFBFBD>ٷ<EFBFBD><D9B7><EFBFBD>
dst.Satelite_Sky_Speed = (float)(src.Satelite_Sky_Speed * 0.01); // <20>ɻ<EFBFBD>GPS<50><53><EFBFBD><EFBFBD>
dst.MTI_Pitch = (float)(src.MTI_Pitch*0.01); // MTI<54><49><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.MTI_Roll = (float)(src.MTI_Roll*0.01); // MTI<54><49>ת<EFBFBD><D7AA>
dst.MTI_Yaw = (float)(src.MTI_Yaw*0.01); // MTI<54><49><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.MTI_lat = (double)(src.MTI_lat * 1e-7); // MTI<54><49><EFBFBD><EFBFBD>
dst.MTI_lon = (double)(src.MTI_lon * 1e-7); // MTIγ<49><CEB3>
dst.MTI_height = (double)(src.MTI_height * 0.2 - 500); // MTI<54>߶<EFBFBD>
dst.MTI_Ground_Speed = (float)(src.MTI_Ground_Speed * 0.01); // MTI<54><49><EFBFBD><EFBFBD>
dst.MTI_Ground_Speed_Dir = (float)(src.MTI_Ground_Speed_Dir * 0.01); // MTI<54><49><EFBFBD>ٷ<EFBFBD><D9B7><EFBFBD>
dst.MTI_Sky_Speed = (float)(src.MTI_Sky_Speed * 0.01); // MTI<54><49><EFBFBD><EFBFBD>
dst.UTC_Second = (int)(src.UTC_Second * 0.001); //UTC<54><43><EFBFBD><EFBFBD>
dst.UTC_Year = (int)(src.UTC_Year); // UTC<54><43>
dst.UTC_Month = (int)(src.UTC_Month); // UTC<54><43>
dst.UTC_Day = (int)(src.UTC_Day); // UTC<54><43>
dst.NavigationState = src.NavigationState; // <20><><EFBFBD><EFBFBD>״̬
dst.PressureHEG = (float)(src.PressureHEG * 0.2 - 500); // <20><>ѹ<EFBFBD>߶<EFBFBD>
dst.AirSpeed = (float)(src.AirSpeed * 0.01); // <20><><EFBFBD><EFBFBD>
dst.Engine_Speed = (int)(src.Engine_Speed); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
dst.Left_Tmp = (int)(src.Left_Tmp * 2); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.Right_Tmp = (int)(src.Right_Tmp * 2); // <20>Ҹ<EFBFBD><D2B8><EFBFBD>
dst.Pitch_Install_Err = (float)(src.Pitch_Install_Err * 0.02); // <20>غɸ<D8BA><C9B8><EFBFBD><EFBFBD><EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>
dst.Roll_Install_Err = (float)(src.Roll_Install_Err * 0.02); // <20>غɷ<D8BA>λ<EFBFBD><CEBB>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>
dst.Yaw_Install_Err = (float)(src.Yaw_Install_Err * 0.02); // <20>غ<EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD>
dst.mem3 = src.mem3; // Ԥ<><D4A4>
dst.CRC = src.CRC; // CRCУ<43><D0A3>
}
void TransCH96FJDatatoQBData(struQB_FJ &QB_FJ, Data96TXD &CH96FJ)
{
QB_FJ.bValid = true; // <20>ṹ<EFBFBD><E1B9B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>Ч<EFBFBD><D0A7> TRUE<55><45><EFBFBD><EFBFBD>Ч FALSE<53><45><EFBFBD><EFBFBD>Ч
// <20>غ<EFBFBD><D8BA><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
QB_FJ.ZH_StablePlatformWorkState = 0x01; // <20>ȶ<EFBFBD>ƽ̨<C6BD><CCA8><EFBFBD><EFBFBD>״̬<D7B4><CCAC>000<30><30><EFBFBD>ֶ<EFBFBD><D6B6><EFBFBD><EFBFBD><EFBFBD> 001<30><31><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD> 010<31><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 011<31><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 100<30><30>ɨ<EFBFBD><C9A8> 101<30><31>ɲ<EFBFBD><C9B2> 110<31><30><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC> 111<31><31>ƫ<EFBFBD>ĸ<EFBFBD><C4B8><EFBFBD>
QB_FJ.ZH_TrackState = 0x11; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<D7B4><CCAC>000<30><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 001<30><31><EFBFBD>쳣 010<31><30>Ŀ<EFBFBD>궪ʧ 011<31><31><EFBFBD><EFBFBD><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD> 100<30><30><EFBFBD><EFBFBD><EFBFBD>ĺ<EFBFBD> 101<30><31><EFBFBD><EFBFBD><EFBFBD>İ<EFBFBD>
QB_FJ.ZH_Pitch = CH96FJ.ZH_Pitch; // <20>غɸ<D8BA><C9B8><EFBFBD><EFBFBD><EFBFBD>
QB_FJ.ZH_Azimuth = -CH96FJ.ZH_Azimuth; // <20>غɷ<D8BA>λ<EFBFBD><CEBB>
QB_FJ.ZH_LaserOn = !(CH96FJ.LaserState & 0x01); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>࿪<EFBFBD>أ<EFBFBD>trueΪ<65><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λΪ<CEBB><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>࿪<EFBFBD><E0BFAA>λ<EFBFBD>ã<EFBFBD>
QB_FJ.ZH_LaserDist = CH96FJ.LaserDis; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
QB_FJ.ZH_FocalLen = CH96FJ.EOFocus; //EO<45><4F><EFBFBD><EFBFBD> mm
// QB_FJ.ZH_IRFocalLen = CH96FJ.IRFocus; //IR<49><52><EFBFBD><EFBFBD> mm
QB_FJ.ZH_DeltaX = CH96FJ.dx; //<2F><><EFBFBD><EFBFBD><EFBFBD>Ѱ<EFBFBD><D1B0><EFBFBD>
QB_FJ.ZH_DeltaY = CH96FJ.dy; //<2F><>λ<EFBFBD>Ѱ<EFBFBD><D1B0><EFBFBD>
QB_FJ.ZH_PixelSize = CH96FJ.PixelSize * 1e-6; //<2F><>Ԫ<EFBFBD>ߴ<EFBFBD><DFB4>ɼ<EFBFBD><C9BC><EFBFBD>2.8um<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>15um<EFBFBD><EFBFBD>30um
QB_FJ.ZH_ImgWidth = 1920; // ͼ<><CDBC><EFBFBD><EFBFBD>
QB_FJ.ZH_ImgHeight = 1080; // ͼ<><CDBC><EFBFBD><EFBFBD>
// <20>ɻ<EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>̬GPS
QB_FJ.UAV_GPS_Lon = CH96FJ.Satelite_lon; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD>
QB_FJ.UAV_GPS_Lat = CH96FJ.Satelite_lat; // <20><><EFBFBD>˻<EFBFBD>γ<EFBFBD><CEB3>
QB_FJ.UAV_GPS_Alt = float(CH96FJ.Satelite_height); // <20><><EFBFBD>˻<EFBFBD><CBBB>߶<EFBFBD>
QB_FJ.UAV_GPS_HSpeed = CH96FJ.Satelite_Ground_Speed; // ˮƽ<CBAE>ٶ<EFBFBD>
QB_FJ.UAV_Pitch = CH96FJ.Pitch; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
QB_FJ.UAV_Yaw = CH96FJ.Yaw; // <20><><EFBFBD><EFBFBD><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
QB_FJ.UAV_Roll = CH96FJ.Roll; // <20><>ת<EFBFBD><D7AA>
QB_FJ.UAV_GroundHeight = 1280; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD>ӵ<EFBFBD><D3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߶ȣ<DFB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ30<33>׸߳<D7B8>
// GPSʱ<53><CAB1>
QB_FJ.UAV_GPS_Year = CH96FJ.UTC_Year; // <20><><EFBFBD><EFBFBD>1<EFBFBD><31>
QB_FJ.UAV_GPS_Month = CH96FJ.UTC_Month; // <20><><EFBFBD><EFBFBD>1<EFBFBD><31>
QB_FJ.UAV_GPS_Day = CH96FJ.UTC_Day; // <20><><EFBFBD><EFBFBD>1<EFBFBD><31>
QB_FJ.UAV_GPS_UTC = CH96FJ.UTC_Second; // <20><><EFBFBD><EFBFBD>1ʱ<31><CAB1><EFBFBD><EFBFBD>
}
// λ<><CEBB><EFBFBD><EFBFBD>Ϣ<EFBFBD>ṹ<EFBFBD><E1B9B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
// <20><>ȡCRCУ<43><D0A3>λ <20><><EFBFBD><EFBFBD>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;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽṹ<DDBD><E1B9B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void TransFJStruct( struTranslateProtocal_FJ &dst, const struProtocal_FJ &src)
{
// <20>ж<EFBFBD>src<72><63>Ч<EFBFBD><D0A7>
if (src.Head[0] == 0xEB && src.Head[1] == 0x90)
{
dst.bValid = TRUE;
// ת<><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD>غ<EFBFBD>ʶ<EFBFBD><CAB6><EFBFBD><EFBFBD>
dst.ZH_Type = src.ZH_Type;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>
dst.ZH_CmdType = src.ZH_CmdType;
//<2F><><EFBFBD><EFBFBD><EFBFBD>غɹ<D8BA><C9B9><EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD>ƽ̨<C6BD><CCA8><EFBFBD><EFBFBD>״̬
dst.ZH_StablePlatformWorkState = (src.ZH_WorkState & 0xE0)>>5;
//<2F><><EFBFBD><EFBFBD><EFBFBD>غɹ<D8BA><C9B9><EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.ZH_LaserRangeWorkState = (src.ZH_WorkState & 0x1C)>>2;
// <20>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>࿪<EFBFBD><E0BFAA>
if (dst.ZH_LaserRangeWorkState == 0x04)
{
dst.ZH_LaserOn = true;
}
else
{
dst.ZH_LaserOn = false;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD>غɹ<D8BA><C9B9><EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ
dst.ZH_ElevatorWorkState = src.ZH_WorkState & 0x03;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<D7B4><CCAC>0
dst.ZH_CtrlState0 = src.ZH_CtrlState0;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<D7B4><CCAC>1
dst.ZH_CtrlState1 = src.ZH_CtrlState1;
//<2F><><EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD>״̬<D7B4><CCAC>
dst.ZH_ServoState = src.ZH_ServoState;
//<2F><><EFBFBD><EFBFBD><EFBFBD>ɼ<EFBFBD><C9BC><EFBFBD>״̬<D7B4><CCAC>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
dst.ZH_EOCameraWorkState = (src.ZH_EOState0 & 0x80) >> 7;
//<2F><><EFBFBD><EFBFBD><EFBFBD>ɼ<EFBFBD><C9BC><EFBFBD>״̬<D7B4><CCAC>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD>ɼ<EFBFBD><C9BC><EFBFBD>ͼ<EFBFBD><CDBC>״̬
dst.ZH_EOCameraImgState = (src.ZH_EOState0 & 0x40) >> 6;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<D7B4>֡<EFBFBD><D6A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
dst.ZH_IRCameraWorkState = (src.ZH_IRState & 0x80) >> 7;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<D7B4>֡<EFBFBD><D6A1><EFBFBD>ͼ<EFBFBD><CDBC>״̬
dst.ZH_IRCameraImgState = (src.ZH_IRState & 0x40) >> 6;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<D7B4>֡<EFBFBD><D6A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ䱶
dst.ZH_IRCameraDigitalDouble = (src.ZH_IRState & 0x08) >> 3;
dst.ZH_SMExposalTime = src.ZH_IRState * 20;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<D7B4>֡<EFBFBD><D6A1><EFBFBD><EFBFBD>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.ZH_EOTrackState_Control = (src.ZH_TrackState & 0x80) >> 7;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<D7B4>֡<EFBFBD><D6A1><EFBFBD><EFBFBD>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
dst.ZH_EOTrackState = (src.ZH_TrackState & 0x70) >> 4;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<D7B4>֡<EFBFBD><D6A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.ZH_IRTrackState_Control = (src.ZH_TrackState & 0x08) >> 3;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<D7B4>֡<EFBFBD><D6A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
dst.ZH_IRTrackState = (src.ZH_TrackState & 0x07);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<D7B4><CCAC>
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);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
dst.ZH_LaserDist = (unsigned int)src.ZH_LaserDist;
//<2F>ɼ<EFBFBD><C9BC>⽹<EFBFBD><E2BDB9>ֵ
dst.ZH_EOFocalLen = (float)(src.ZH_EOFocalLen * 0.01);
//<2F><><EFBFBD>⽹<EFBFBD><E2BDB9>ֵ
dst.ZH_IRFocalLen = (float)(src.ZH_IRFocalLen * 0.01);
dst.ZH_SMSerialNumber = src.ZH_IRFocalLen; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƭ<EFBFBD><C6AC><EFBFBD><EFBFBD>
// <20><>Ԫ<EFBFBD>ߴ<EFBFBD> <20>̶<EFBFBD>ֵ
dst.ZH_EOPixelSize = 4.5e-6;
dst.ZH_IRPixelSize = 15e-6;
dst.ZH_SMPixelSize = 2.8e-6; // <20><>ʱ
//ͼ<><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.ZH_ImgType = src.ZH_ImgType;
//ͼ<><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.ZH_ImgFlow = src.ZH_ImgFlow;
dst.ZH_SMOverlapRatio = float(src.ZH_SMOverlapRatio * 0.5);
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD>ݡ<EFBFBD><DDA1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
//<2F><><EFBFBD>˻<EFBFBD><CBBB>ͺ<EFBFBD>
dst.UAV_Type = src.UAV_Type;
//<2F>ɻ<EFBFBD>ID
dst.UAV_ID = src.UAV_ID;
//<2F><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD>״̬
for (int i = 0; i<10; i++)
{
dst.UAV_States[i] = src.UAV_States[i];
}
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD><EFBFBD>ǡ<EFBFBD><C7A1><EFBFBD><EFBFBD><EFBFBD>180~<7E><>180<38><30>
dst.UAV_GyroPitch = (float)(src.UAV_GyroPitch * 0.01);
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD>ݹ<EFBFBD>ת<EFBFBD>ǡ<EFBFBD><C7A1><EFBFBD><EFBFBD><EFBFBD>180<38><30>~<7E><>180<38><30>
dst.UAV_GyroRoll = (float)(src.UAV_GyroRoll * 0.01);
//<2F><>ѹ<EFBFBD>߶<EFBFBD>
dst.UAV_AirPresAlt = (float)(src.UAV_AirPresAlt * 0.2 - 500.0);
//<2F>ź<EFBFBD><C5BA><EFBFBD><EFBFBD>ǡ<EFBFBD><C7A1><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>0~360<36><30>
dst.UAV_MagHeading = (float)(src.UAV_MagHeading * 0.1);
//<2F><><EFBFBD><EFBFBD>1<EFBFBD><31>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>״̬
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;
//<2F><><EFBFBD><EFBFBD>1UTC<54><43><EFBFBD><EFBFBD>
dst.GPS_LR_UTC = (unsigned int)(src.GPS_LR_UTC * 0.001);
//<2F><><EFBFBD><EFBFBD>1UTC<54><43>
dst.GPS_LR_Day = (unsigned int)src.GPS_LR_Day;
//<2F><><EFBFBD><EFBFBD>1UTC<54><43>
dst.GPS_LR_Month = (unsigned int)src.GPS_LR_Month;
// <20><><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (dst.GPS_LR_Month > 48)
{
dst.GPS_LR_Month -= 48;
}
//<2F><><EFBFBD><EFBFBD>1UTC<54><43>
dst.GPS_LR_Year = (unsigned int)(src.GPS_LR_Year + 2000);
//<2F><><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD><D6BD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>ֽ<EFBFBD><D6BD>ں<EFBFBD><DABA><EFBFBD>
dst.GPS_LR_Lon = (float)(src.GPS_LR_Lon * 1.0 / 10000000.0);
//<2F><><EFBFBD><EFBFBD>1γ<31>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD>γΪ<CEB3><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>γΪ<CEB3><CEAA>
dst.GPS_LR_Lat = (float)(src.GPS_LR_Lat * 1.0 / 10000000.0);
//<2F><><EFBFBD><EFBFBD>1<EFBFBD>߶<EFBFBD>
dst.GPS_LR_Alt = (float)(src.GPS_LR_Alt * 0.2 - 500.0);
//<2F><><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD>ٷ<EFBFBD><D9B7>򡪡<EFBFBD><F2A1AAA1><EFBFBD>ƫ<EFBFBD><C6AB>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>0~360<36><30>
dst.GPS_LR_VHeading = (float)(src.GPS_LR_VHeading * 0.01);
//<2F><><EFBFBD><EFBFBD>1ˮƽ<CBAE>ٶ<EFBFBD>
dst.GPS_LR_HoriSpeed = (float)(src.GPS_LR_HoriSpeed * 0.01);
//<2F><><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD>١<EFBFBD><D9A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¸<EFBFBD>
dst.GPS_LR_VertSpeed = (float)(src.GPS_LR_VertSpeed * 0.01);
//<2F><><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD>ǡ<EFBFBD><C7A1><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>0~360<36><30>
dst.GPS_LR_Heading = (float)(src.GPS_LR_Heading * 0.01);
//<2F><><EFBFBD><EFBFBD>1<EFBFBD><31>ת<EFBFBD><D7AA>
dst.GPS_LR_Roll = (float)(src.GPS_LR_Roll * 0.01);
//<2F><><EFBFBD><EFBFBD>2<EFBFBD><32>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>״̬
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;
//<2F><><EFBFBD><EFBFBD>2UTC<54><43><EFBFBD><EFBFBD>
dst.GPS_FB_UTC = (unsigned int)(src.GPS_FB_UTC * 0.001);
//<2F><><EFBFBD><EFBFBD>2UTC<54><43>
dst.GPS_FB_Day = (unsigned int)src.GPS_FB_Day;
//<2F><><EFBFBD><EFBFBD>2UTC<54><43>
dst.GPS_FB_Month = (unsigned int)src.GPS_FB_Month;
// <20><><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (dst.GPS_FB_Month > 48)
{
dst.GPS_FB_Month -= 48;
}
//<2F><><EFBFBD><EFBFBD>2UTC<54><43>
dst.GPS_FB_Year = (unsigned int)(src.GPS_FB_Year + 2000);
//<2F><><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>
dst.GPS_FB_Lon = (float)(src.GPS_FB_Lon * 1.0 / 10000000.0);
//<2F><><EFBFBD><EFBFBD>2γ<32>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD>γΪ<CEB3><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>γΪ<CEB3><CEAA>
dst.GPS_FB_Lat = (float)(src.GPS_FB_Lat * 1.0 / 10000000.0);
//<2F><><EFBFBD><EFBFBD>2<EFBFBD>߶<EFBFBD>
dst.GPS_FB_Alt = (float)(src.GPS_FB_Alt * 0.2 - 500.0);
//<2F><><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD>ٷ<EFBFBD><D9B7>򡪡<EFBFBD><F2A1AAA1><EFBFBD>ƫ<EFBFBD><C6AB>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>0~360<36><30>
dst.GPS_FB_VHeading = (float)(src.GPS_FB_VHeading * 0.01);
//<2F><><EFBFBD><EFBFBD>2ˮƽ<CBAE>ٶ<EFBFBD>
dst.GPS_FB_HoriSpeed = (float)(src.GPS_FB_HoriSpeed * 0.01);
//<2F><><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD>١<EFBFBD><D9A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¸<EFBFBD>
dst.GPS_FB_VertSpeed = (float)(src.GPS_FB_VertSpeed * 0.01);
//<2F><><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD>ǡ<EFBFBD><C7A1><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>0~360<36><30>
dst.GPS_FB_Heading = (float)(src.GPS_FB_Heading * 0.01);
//<2F><><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD>ǡ<EFBFBD><C7A1><EFBFBD>̧ͷ<CCA7><CDB7><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>180~<7E><>180<38><30>
dst.GPS_FB_Pitch = (float)(src.GPS_FB_Pitch * 0.01);
}
else
{
dst.bValid = FALSE;
}
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void PushToFrameDataDeque(const struQB_FJ &TgtLocStru)
{
// <20>̻߳<DFB3><CCBB><EFBFBD>
WaitForSingleObject(g_hMutex_TgtLoc, INFINITE);
if ( g_TgtLocStructDeque.size() < 2)
{
g_TgtLocStructDeque.push_back(TgtLocStru); // <20><><EFBFBD><EFBFBD>
}
else
{
while(g_TgtLocStructDeque.size() >= 2) // <20><><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>;<EFBFBD><CDBE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD>ڵ<EFBFBD>ǰ<EFBFBD><C7B0>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
// <20><><EFBFBD><EFBFBD>
g_TgtLocStructDeque.pop_front();
}
g_TgtLocStructDeque.push_back(TgtLocStru); // <20><><EFBFBD><EFBFBD>
}
ReleaseMutex(g_hMutex_TgtLoc);
}
// ת<>鱨<EFBFBD><E9B1A8>EO<45><4F><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void TransToEOQBFJ(struQB_FJ &dst, const struTranslateProtocal_FJ &src)
{
dst.bValid = true;
if (src.bValid == true)
{
// <20>غ<EFBFBD><D8BA><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.ZH_StablePlatformWorkState = src.ZH_StablePlatformWorkState; // ƽ̨״̬
dst.ZH_TrackState = src.ZH_EOTrackState; // <20><><EFBFBD><EFBFBD>״̬
dst.ZH_Pitch = src.ZH_Pitch; // <20>غɸ<D8BA><C9B8><EFBFBD><EFBFBD><EFBFBD>
dst.ZH_Azimuth = src.ZH_Azimuth; // <20>غɷ<D8BA>λ<EFBFBD><CEBB>
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; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>࿪<EFBFBD>أ<EFBFBD>trueΪ<65><CEAA>
dst.ZH_LaserDist = src.ZH_LaserDist; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
dst.ZH_FocalLen = src.ZH_EOFocalLen; //<2F><><EFBFBD><EFBFBD>
if (dst.ZH_FocalLen < 1e-6)
{
dst.bValid = false;
}
dst.ZH_DeltaX = src.ZH_DeltaX; //<2F><><EFBFBD><EFBFBD><EFBFBD>Ѱ<EFBFBD><D1B0><EFBFBD>
dst.ZH_DeltaY = src.ZH_DeltaY; //<2F><>λ<EFBFBD>Ѱ<EFBFBD><D1B0><EFBFBD>
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; // ͼ<><CDBC><EFBFBD><EFBFBD>
dst.ZH_ImgHeight = 1080; // ͼ<><CDBC><EFBFBD><EFBFBD>
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;
// <20><>λ״̬<D7B4><CCAC>
// 0x00 <20><><EFBFBD>㶨λ
// 0x02 <20>ز<EFBFBD><D8B2><EFBFBD>λ<EFBFBD><CEBB><EFBFBD>ֶ<EFBFBD>λ
// <20><><EFBFBD><EFBFBD>״̬<D7B4><CCAC>
// 0x00 <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (GPS_FB_LocState == 0x00 || GPS_FB_LocState == 0x02) // ǰ<><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
{
dst.UAV_GPS_Lon = src.GPS_FB_Lon; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_GPS_Lat = src.GPS_FB_Lat; // <20><><EFBFBD>˻<EFBFBD>γ<EFBFBD><CEB3>
dst.UAV_GPS_Alt = src.GPS_FB_Alt; // <20><><EFBFBD>˻<EFBFBD><CBBB>߶<EFBFBD>
dst.UAV_GPS_HSpeed = src.GPS_FB_HoriSpeed; // ˮƽ<CBAE>ٶ<EFBFBD>
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) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
{
dst.UAV_GPS_Lon = src.GPS_LR_Lon; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_GPS_Lat = src.GPS_LR_Lat; // <20><><EFBFBD>˻<EFBFBD>γ<EFBFBD><CEB3>
dst.UAV_GPS_Alt = src.GPS_LR_Alt; // <20><><EFBFBD>˻<EFBFBD><CBBB>߶<EFBFBD>
dst.UAV_GPS_HSpeed = src.GPS_LR_HoriSpeed; // ˮƽ<CBAE>ٶ<EFBFBD>
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; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_GPS_Lat = src.GPS_FB_Lat; // <20><><EFBFBD>˻<EFBFBD>γ<EFBFBD><CEB3>
dst.UAV_GPS_Alt = src.GPS_FB_Alt; // <20><><EFBFBD>˻<EFBFBD><CBBB>߶<EFBFBD>
dst.UAV_GPS_HSpeed = src.GPS_FB_HoriSpeed; // ˮƽ<CBAE>ٶ<EFBFBD>
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; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_GPS_Lat = src.GPS_LR_Lat; // <20><><EFBFBD>˻<EFBFBD>γ<EFBFBD><CEB3>
dst.UAV_GPS_Alt = src.GPS_LR_Alt; // <20><><EFBFBD>˻<EFBFBD><CBBB>߶<EFBFBD>
dst.UAV_GPS_HSpeed = src.GPS_LR_HoriSpeed; // ˮƽ<CBAE>ٶ<EFBFBD>
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 // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD>λ
{
dst.bValid = false;
}
// <20><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
if (GPS_LR_PosState == 0x00 && GPS_FB_PosState == 0x00)
{
dst.UAV_Yaw = src.GPS_LR_Heading; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_Roll = src.GPS_LR_Roll; // <20><>ת<EFBFBD><D7AA>
dst.UAV_Pitch = src.GPS_FB_Pitch; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
else
{
if (GPS_LR_PosState == 0x00)
{
// <20><><EFBFBD><EFBFBD>GPS<50>⣺<EFBFBD><E2A3BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+<2B><>ת<EFBFBD><D7AA>
dst.UAV_Yaw = src.GPS_LR_Heading; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_Roll = src.GPS_LR_Roll; // <20><>ת<EFBFBD><D7AA>
// <20><>ֱ<EFBFBD><D6B1><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_Pitch = src.UAV_GyroPitch;
}
else if (GPS_FB_PosState == 0x00)
{
// ǰ<><C7B0>GPS<50>⣺<EFBFBD><E2A3BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+<2B><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_Yaw = src.GPS_FB_Heading;
dst.UAV_Pitch = src.GPS_FB_Pitch;
// <20><>ֱ<EFBFBD><D6B1><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD>ת<EFBFBD><D7AA>
dst.UAV_Roll = src.UAV_GyroRoll;
}
else
{
// <20><><EFBFBD><EFBFBD> + <20>ź<EFBFBD><C5BA><EFBFBD>
dst.UAV_Yaw = src.UAV_MagHeading; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_Roll = src.UAV_GyroRoll; // <20><>ת<EFBFBD><D7AA>
dst.UAV_Pitch = src.UAV_GyroPitch;
}
}
}
}
// ת<>鱨<EFBFBD><E9B1A8>IR<49><52><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void TransToIRQBFJ(struQB_FJ &dst, const struTranslateProtocal_FJ &src)
{
dst.bValid = true;
if (src.bValid == true)
{
// <20>غ<EFBFBD><D8BA><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.ZH_StablePlatformWorkState = src.ZH_StablePlatformWorkState; // ƽ̨״̬
dst.ZH_TrackState = src.ZH_IRTrackState; // <20><><EFBFBD><EFBFBD>״̬
dst.ZH_Pitch = src.ZH_Pitch; // <20>غɸ<D8BA><C9B8><EFBFBD><EFBFBD><EFBFBD>
dst.ZH_Azimuth = src.ZH_Azimuth; // <20>غɷ<D8BA>λ<EFBFBD><CEBB>
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; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>࿪<EFBFBD>أ<EFBFBD>trueΪ<65><CEAA>
dst.ZH_LaserDist = src.ZH_LaserDist; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
dst.ZH_FocalLen = src.ZH_IRFocalLen; //<2F><><EFBFBD><EFBFBD>
// <20><>Ӧ<EFBFBD><D3A6><EFBFBD>ֱ䱶 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; //<2F><><EFBFBD><EFBFBD><EFBFBD>Ѱ<EFBFBD><D1B0><EFBFBD>
dst.ZH_DeltaY = src.ZH_DeltaY; //<2F><>λ<EFBFBD>Ѱ<EFBFBD><D1B0><EFBFBD>
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; // ͼ<><CDBC><EFBFBD><EFBFBD>
dst.ZH_ImgHeight = 576; // ͼ<><CDBC><EFBFBD><EFBFBD>
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;
// <20><>λ״̬<D7B4><CCAC>
// 0x00 <20><><EFBFBD>㶨λ
// 0x02 <20>ز<EFBFBD><D8B2><EFBFBD>λ<EFBFBD><CEBB><EFBFBD>ֶ<EFBFBD>λ
// <20><><EFBFBD><EFBFBD>״̬<D7B4><CCAC>
// 0x00 <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (GPS_FB_LocState == 0x00 || GPS_FB_LocState == 0x02) // ǰ<><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
{
dst.UAV_GPS_Lon = src.GPS_FB_Lon; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_GPS_Lat = src.GPS_FB_Lat; // <20><><EFBFBD>˻<EFBFBD>γ<EFBFBD><CEB3>
dst.UAV_GPS_Alt = src.GPS_FB_Alt; // <20><><EFBFBD>˻<EFBFBD><CBBB>߶<EFBFBD>
dst.UAV_GPS_HSpeed = src.GPS_FB_HoriSpeed; // ˮƽ<CBAE>ٶ<EFBFBD>
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) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
{
dst.UAV_GPS_Lon = src.GPS_LR_Lon; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_GPS_Lat = src.GPS_LR_Lat; // <20><><EFBFBD>˻<EFBFBD>γ<EFBFBD><CEB3>
dst.UAV_GPS_Alt = src.GPS_LR_Alt; // <20><><EFBFBD>˻<EFBFBD><CBBB>߶<EFBFBD>
dst.UAV_GPS_HSpeed = src.GPS_LR_HoriSpeed; // ˮƽ<CBAE>ٶ<EFBFBD>
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) // ǰ<><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
{
dst.UAV_GPS_Lon = src.GPS_FB_Lon; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_GPS_Lat = src.GPS_FB_Lat; // <20><><EFBFBD>˻<EFBFBD>γ<EFBFBD><CEB3>
dst.UAV_GPS_Alt = src.GPS_FB_Alt; // <20><><EFBFBD>˻<EFBFBD><CBBB>߶<EFBFBD>
dst.UAV_GPS_HSpeed = src.GPS_FB_HoriSpeed; // ˮƽ<CBAE>ٶ<EFBFBD>
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) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
{
dst.UAV_GPS_Lon = src.GPS_LR_Lon; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_GPS_Lat = src.GPS_LR_Lat; // <20><><EFBFBD>˻<EFBFBD>γ<EFBFBD><CEB3>
dst.UAV_GPS_Alt = src.GPS_LR_Alt; // <20><><EFBFBD>˻<EFBFBD><CBBB>߶<EFBFBD>
dst.UAV_GPS_HSpeed = src.GPS_LR_HoriSpeed; // ˮƽ<CBAE>ٶ<EFBFBD>
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 // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD>λ
{
dst.bValid = false;;
}
// <20><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
if (GPS_LR_PosState == 0x00 && GPS_FB_PosState == 0x00)
{
dst.UAV_Yaw = src.GPS_LR_Heading; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_Roll = src.GPS_LR_Roll; // <20><>ת<EFBFBD><D7AA>
dst.UAV_Pitch = src.GPS_FB_Pitch; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
else
{
if (GPS_LR_PosState == 0x00)
{
// <20><><EFBFBD><EFBFBD>GPS<50>⣺<EFBFBD><E2A3BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+<2B><>ת<EFBFBD><D7AA>
dst.UAV_Yaw = src.GPS_LR_Heading; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_Roll = src.GPS_LR_Roll; // <20><>ת<EFBFBD><D7AA>
// <20><>ֱ<EFBFBD><D6B1><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_Pitch = src.UAV_GyroPitch;
}
else if (GPS_FB_PosState == 0x00)
{
// ǰ<><C7B0>GPS<50>⣺<EFBFBD><E2A3BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+<2B><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_Yaw = src.GPS_FB_Heading;
dst.UAV_Pitch = src.GPS_FB_Pitch;
// <20><>ֱ<EFBFBD><D6B1><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD>ת<EFBFBD><D7AA>
dst.UAV_Roll = src.UAV_GyroRoll;
}
else
{
// <20><><EFBFBD><EFBFBD> + <20>ź<EFBFBD><C5BA><EFBFBD>
dst.UAV_Yaw = src.UAV_MagHeading; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
dst.UAV_Roll = src.UAV_GyroRoll; // <20><>ת<EFBFBD><D7AA>
dst.UAV_Pitch = src.UAV_GyroPitch;
}
}
}
}
// <20><><EFBFBD>ܣ<EFBFBD><DCA3>غɵ<D8BA><C9B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD>̬<EFBFBD><CCAC>()
// <20><><EFBFBD>룺
// 1. uavPos<6F><73><EFBFBD>ɻ<EFBFBD>λ<EFBFBD>ã<EFBFBD>double uavPos[GPS<50><53><EFBFBD>ȣ<EFBFBD>GPSγ<53>ȣ<EFBFBD>GPS<50>߶<EFBFBD>]
// 2. tgtPos<6F><73>Ŀ<EFBFBD><C4BF>λ<EFBFBD>ã<EFBFBD>double tgtPos[GPS<50><53><EFBFBD>ȣ<EFBFBD>GPSγ<53>ȣ<EFBFBD>GPS<50>߶<EFBFBD>]
// 3. uavAttitude: <20>ɻ<EFBFBD><C9BB><EFBFBD>̬<EFBFBD><CCAC>double uavAttitude[<5B><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ƫ<EFBFBD><C6AB>]
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// 1. loadPitch <20>غɸߵͽǣ<CDBD><C7A3><EFBFBD>λ<EFBFBD><CEBB>
// 2. loadAzimuth <20>غɷ<D8BA>λ<EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD>λ<EFBFBD><CEBB>
// <20><><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD><C9B9><EFBFBD><EFBFBD><EFBFBD>true,<2C><><EFBFBD>򷵻<EFBFBD>false<73><65>
bool CalLoadAttitudeForLoadGuide(double &loadPitch, double &loadAzimuth, const double *uavPos, const double *tgtPos, const double *uavAttitude)
{
const double g_PI = 3.1415926;
// <20><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
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;
}
// Ŀ<><C4BF> <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ھ<EFBFBD>γ<EFBFBD><CEB3>10<31>ȷ<EFBFBD>Χ<EFBFBD>ڣ<EFBFBD><DAA3>ҷɻ<D2B7><C9BB><EFBFBD>Ŀ<EFBFBD><C4BF>Ҫ<EFBFBD><D2AA>
if (abs(uavLon - tgtLon) > 10 || abs(uavLat - tgtLat) > 10 || uavAlt - tgtAlt < 0)
{
return false;
}
// <20><><EFBFBD><EFBFBD><EFBFBD>ɻ<EFBFBD><C9BB><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>
double uavTgtDist = 0;
CalculateTwoPtsSlopeDistance(uavTgtDist, uavPos, tgtPos, 3);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA>
if (uavTgtDist < 1)
{
return false;
}
// <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>
uavPitch = uavAttitude[0] / 180 * CV_PI;
uavRoll = uavAttitude[1] / 180 * CV_PI;
uavYaw = uavAttitude[2] / 180 * CV_PI;
// <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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);
// <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA>
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);
// <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
// <20><><EFBFBD><EFBFBD><EFBFBD>˷<EFBFBD>
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);
// <20><><EFBFBD>ڷɻ<DAB7>λ<EFBFBD>ú<EFBFBD>Ŀ<EFBFBD><C4BF>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>x3 x6 x9
double x3 = 0;
double x6 = 0;
double x9 = 0;
// <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ĸ߶<C4B8>
double height = uavAlt - tgtAlt;
x9 = height / uavTgtDist;
// <20><><EFBFBD><EFBFBD><EFBFBD>εױ<CEB5>
double distance = sqrt(pow(uavTgtDist, 2) - pow(height, 2));
// <20><>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڷɻ<DAB7><C9BB><EFBFBD><EFBFBD><EFBFBD>ͶӰ<CDB6>ķ<EFBFBD>λ<EFBFBD><CEBB>
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;
}
// <20><>ȡ<EFBFBD>ɻ<EFBFBD><C9BB><EFBFBD>̬
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)
{
// <20><>λ<EFBFBD>ж<EFBFBD>
if (GPS_FB_LocState == 0x00 || GPS_FB_LocState == 0x02) // ǰ<><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
{
UAVLon = src.GPS_LR_Lon; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD>
UAVLat = src.GPS_LR_Lat; // <20><><EFBFBD>˻<EFBFBD>γ<EFBFBD><CEB3>
UAVAlt = src.GPS_LR_Alt; // <20><><EFBFBD>˻<EFBFBD><CBBB>߶<EFBFBD>
}
else if (GPS_LR_LocState == 0x00 || GPS_LR_LocState == 0x02) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ
{
UAVLon = src.GPS_LR_Lon; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD>
UAVLat = src.GPS_LR_Lat; // <20><><EFBFBD>˻<EFBFBD>γ<EFBFBD><CEB3>
UAVAlt = src.GPS_LR_Alt; // <20><><EFBFBD>˻<EFBFBD><CBBB>߶<EFBFBD>
UAVYaw = src.GPS_LR_Heading; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
UAVRoll = src.GPS_LR_Roll; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD>ת<EFBFBD><D7AA>
}
else if (GPS_FB_LocState == 0x01 || GPS_FB_LocState == 0x03)
{
UAVLon = src.GPS_LR_Lon; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD>
UAVLat = src.GPS_LR_Lat; // <20><><EFBFBD>˻<EFBFBD>γ<EFBFBD><CEB3>
UAVAlt = src.GPS_LR_Alt; // <20><><EFBFBD>˻<EFBFBD><CBBB>߶<EFBFBD>
UAVYaw = src.GPS_LR_Heading; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
UAVRoll = src.GPS_LR_Roll; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD>ת<EFBFBD><D7AA>
}
else if (GPS_LR_LocState == 0x01 || GPS_LR_LocState == 0x03)
{
UAVLon = src.GPS_LR_Lon; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD>
UAVLat = src.GPS_LR_Lat; // <20><><EFBFBD>˻<EFBFBD>γ<EFBFBD><CEB3>
UAVAlt = src.GPS_LR_Alt; // <20><><EFBFBD>˻<EFBFBD><CBBB>߶<EFBFBD>
UAVYaw = src.GPS_LR_Heading; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
UAVRoll = src.GPS_LR_Roll; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD>ת<EFBFBD><D7AA>
}
else
{
return false;
}
// <20><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
if (GPS_LR_PosState == 0x00 && GPS_FB_PosState == 0x00)
{
UAVYaw = src.GPS_LR_Heading; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
UAVRoll = src.GPS_LR_Roll; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD>ת<EFBFBD><D7AA>
UAVPitch = src.GPS_FB_Pitch; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
else
{
if (GPS_LR_PosState == 0x00)
{
// <20><><EFBFBD><EFBFBD>GPS<50>⣺<EFBFBD><E2A3BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+<2B><>ת<EFBFBD><D7AA>
UAVYaw = src.GPS_LR_Heading; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
UAVRoll = src.GPS_LR_Roll; // <20><><EFBFBD>˻<EFBFBD><CBBB><EFBFBD>ת<EFBFBD><D7AA>
// <20><>ֱ<EFBFBD><D6B1><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD><EFBFBD><EFBFBD>
UAVPitch = src.UAV_GyroPitch;
}
else if (GPS_FB_PosState == 0x00)
{
// ǰ<><C7B0>GPS<50>⣺<EFBFBD><E2A3BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>+<2B><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
UAVYaw = src.GPS_FB_Heading;
UAVPitch = src.GPS_FB_Pitch;
// <20><>ֱ<EFBFBD><D6B1><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD>ת<EFBFBD><D7AA>
UAVRoll = src.UAV_GyroRoll;
}
else
{
// <20><><EFBFBD><EFBFBD> + <20>ź<EFBFBD><C5BA><EFBFBD>
UAVYaw = src.UAV_MagHeading; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
UAVRoll = src.UAV_GyroRoll; // <20><>ת<EFBFBD><D7AA>
UAVPitch = src.UAV_GyroPitch;
}
}
}
return true;
}
// <20><><EFBFBD>㷽λ<E3B7BD><CEBB>
double getYaw(double XA, double YA)
{
// <20><><EFBFBD><EFBFBD><EFBFBD>ϳɺ<CFB3><C9BA><EFBFBD><EFBFBD><EFBFBD> Yaw
double PayLoad_Yaw = 0;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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)
{
// <20><>1<EFBFBD><31><EFBFBD><EFBFBD>
tanYaw = XA / YA;
PayLoad_Yaw = atan(tanYaw) * 180 / CV_PI;
}
else if (YA < 0 && XA > 0)
{
// <20><>2<EFBFBD><32><EFBFBD><EFBFBD>
tanYaw = -XA / YA;
PayLoad_Yaw = atan(tanYaw) * 180 / CV_PI;
PayLoad_Yaw = 180 - PayLoad_Yaw;
}
else if (YA < 0 && XA < 0)
{
// <20><>3<EFBFBD><33><EFBFBD><EFBFBD>
tanYaw = XA / YA;
PayLoad_Yaw = atan(tanYaw) * 180 / CV_PI;
PayLoad_Yaw = -180 + PayLoad_Yaw;
}
else if (YA > 0 && XA < 0)
{
// <20><>4<EFBFBD><34><EFBFBD><EFBFBD>
tanYaw = -XA / YA;
PayLoad_Yaw = atan(tanYaw) * 180 / CV_PI;
PayLoad_Yaw = -PayLoad_Yaw;
}
}
return PayLoad_Yaw;
}
//<2F><><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㾭γ<E3BEAD><CEB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EEB2B9>
//<2F><><EFBFBD>룺 1. frame <20><><EFBFBD>븴<EFBFBD><EBB8B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// 2. pt_x<5F><78> <20><>
// 3. pt_y<5F><79> <20><>
//
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 1. Lon <20><><EFBFBD><EFBFBD>
// 2. Lat<61><74> γ<><CEB3>
// 3. <20><><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD><C9B9><EFBFBD><EFBFBD><EFBFBD>true<75><65>ʧ<EFBFBD>ܷ<EFBFBD><DCB7><EFBFBD>false
bool CalAnyPtCoordinateWithoutErrorCompensate(double &Lon, double &Lat, double &H, const struQB_FJ *frame, int pt_x, int pt_y)
{
try
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
if (frame == NULL || frame->bValid == FALSE)
{
return false;
}
// <20>жϸ<D0B6><CFB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>
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. Խ<><D4BD><EFBFBD>ж<EFBFBD>
if (abs(uavLon) > 180) // <20><><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavLat) > 90) // γ<><CEB3>
{
return false;
}
if (uavAlt < 0 || uavAlt > 10000) // <20>߶<EFBFBD>
{
return false;
}
if (uavGroundHeight < 0 || (uavAlt - uavGroundHeight < 30))
{
return false;
}
if (abs(uavYaw) > 360) // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavPitch) > 30) // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavRoll) > 30) // <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA>
{
return false;
}
if (abs(zhAz) > 180) // <20>غɷ<D8BA>λ<EFBFBD><CEBB>
{
return false;
}
if (zhPitch > -15 || zhPitch < -110)
{
return false;
}
// ͼ<><CDBC><EFBFBD>ߴ<EFBFBD>
int imgWidth = frame->ZH_ImgWidth;
int imgHeight = frame->ZH_ImgHeight;
// <20>ж<EFBFBD>
if (imgWidth <= 0 || imgHeight <= 0)
{
return false;
}
// <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD>ж<EFBFBD>
if (pt_x < 0 || pt_x >= imgWidth)
{
return false;
}
if (pt_y < 0 || pt_y >= imgHeight)
{
return false;
}
// <20><><EFBFBD><EFBFBD>
double f = frame->ZH_FocalLen / 1000; // <20><>
if (f <= 0)
{
return false;
}
// <20><>Ԫ<EFBFBD>ߴ<EFBFBD> <20><>
double pixelSize = frame->ZH_PixelSize;
if (pixelSize <= 0)
{
return false;
}
// <20>ɻ<EFBFBD>
double UAV_ROLL = uavRoll; // <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA> <20><><EFBFBD>Ƕȣ<C7B6>
double UAV_PITCH = uavPitch; // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>Ƕȣ<C7B6>
double UAV_YAW = uavYaw; // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>Ƕȣ<C7B6>
// <20>غ<EFBFBD>
double ZH_PITCH = zhPitch; // <20>غɸߵͽ<DFB5> <20><><EFBFBD>Ƕȣ<C7B6>
double ZH_YAW = zhAz; // <20>غɷ<D8BA>λ<EFBFBD><CEBB> <20><><EFBFBD>Ƕȣ<C7B6>
// <20>غɸ<D8BA><C9B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ZH_PITCH = ZH_PITCH + 90; // [0 90]
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
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; // <20><><EFBFBD><EFBFBD>
double BaseLat = uavLat; // <20><><EFBFBD><EFBFBD>
double height = uavAlt - uavGroundHeight; // <20><><EFBFBD>Ը߶<D4B8> = <20><><EFBFBD>и߶<D0B8> - <20><><EFBFBD><EFBFBD><EFBFBD>߳<EFBFBD>
bool bLaser = frame->ZH_LaserOn;
double LaserDistance = frame->ZH_LaserDist;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD>ж<EFBFBD>
if (bLaser == true && LaserDistance < 100)
{
return false;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
// x1 x2 x3
// x4 x5 x6
// x7 x8 x9
// <20>غɸ<D8BA><C9B8><EFBFBD><EFBFBD><EFBFBD>
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);
// <20>غɷ<D8BA>λ<EFBFBD><CEBB>
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;
// <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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);
// <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA>
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);
// <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
// <20><><EFBFBD><EFBFBD><EFBFBD>˷<EFBFBD>
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);
// ȡ<><C8A1>M54321<32>е<EFBFBD>ֵ
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);
// <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
double x = 0, y = 0;
// <20><>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
x = +pt_x - imgWidth / 2;
y = -pt_y + imgHeight / 2;
// ת<><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>λ <20><>
x = x * pixelSize;
y = y * pixelSize;
// <20><>ĸ<EFBFBD><C4B8>ֵ<EFBFBD>ж<EFBFBD>
if (x7 * x + x8 * y - x9 * f == 0)
{
return false;
}
// <20><><EFBFBD>߷<EFBFBD><DFB7><EFBFBD>ģ<EFBFBD><C4A3>
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;
// <20><><EFBFBD>㷽λ<E3B7BD>Ǻ;<C7BA><CDBE><EFBFBD>
double distance = sqrt(XA * XA + YA * YA);
double yaw = getYaw(XA, YA);
// <20><><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD>ĵ㾭γ<E3BEAD><CEB3>
CalculatePtCoordinate(Lon, Lat, BaseLon, BaseLat, yaw, distance, 3); // WGS84<38><34><EFBFBD><EFBFBD>ϵ
// Ŀ<><C4BF><EFBFBD>߶ȼ<DFB6><C8BC><EFBFBD>
H = uavGroundHeight;
return true;
}
catch(cv::Exception &e)
{
e.msg.c_str();
return false;
}
catch(...)
{
return false;
}
return true;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ü<EFBFBD><C3BC>⣬<EFBFBD><E2A3AC><EFBFBD>ߵ<EFBFBD><DFB5>㶨λ<E3B6A8><CEBB><EFBFBD><EFBFBD>
void CalLonLatOffset(double &LonOffset, double &LatOffset, const struQB_FJ *frame)
{
try
{
// <20><><EFBFBD><EFBFBD>
LonOffset = 0;
LatOffset = 0;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
if (frame == NULL || frame->bValid == false)
{
return;
}
// <20><><EFBFBD><EFBFBD><EFBFBD>޼<EFBFBD><DEBC><EFBFBD> ֱ<>ӷ<EFBFBD><D3B7><EFBFBD>
if ((frame->ZH_LaserOn == true && frame->ZH_LaserDist > 0 && frame->ZH_LaserDist < 65535) == false)
{
return;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double LaserLon = 0.0;
double LaserLat = 0.0;
double LaserH = 0.0;
// <20>޼<EFBFBD><DEBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
}
}
// <20><><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD>ĵ㾭γ<E3BEAD><CEB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
bool CalImgCenterCoordinate(double &Lon, double &Lat, double &H, const struQB_FJ *frame)
{
try
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
if (frame == NULL || frame->bValid == FALSE)
{
return false;
}
// <20>жϸ<D0B6><CFB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>
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. Խ<><D4BD><EFBFBD>ж<EFBFBD>
if (abs(uavLon) > 180) // <20><><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavLat) > 90) // γ<><CEB3>
{
return false;
}
if (uavAlt < 0 || uavAlt > 10000) // <20>߶<EFBFBD>
{
return false;
}
if (uavGroundHeight < 0 || (uavAlt - uavGroundHeight < 30))
{
return false;
}
if (abs(uavYaw) > 360) // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavPitch) > 30) // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavRoll) > 30) // <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA>
{
return false;
}
if (abs(zhAz) > 180) // <20>غɷ<D8BA>λ<EFBFBD><CEBB>
{
return false;
}
if (zhPitch > -15 || zhPitch < -110)
{
return false;
}
// <20>ɻ<EFBFBD>
double UAV_ROLL = uavRoll; // <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA> <20><><EFBFBD>Ƕȣ<C7B6>
double UAV_PITCH = uavPitch; // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>Ƕȣ<C7B6>
double UAV_YAW = uavYaw; // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>Ƕȣ<C7B6>
// <20>غ<EFBFBD>
double ZH_PITCH = zhPitch; // <20>غɸߵͽ<DFB5> <20><><EFBFBD>Ƕȣ<C7B6>
double ZH_YAW = zhAz; // <20>غɷ<D8BA>λ<EFBFBD><CEBB> <20><><EFBFBD>Ƕȣ<C7B6>
// <20>غɸ<D8BA><C9B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ZH_PITCH = ZH_PITCH + 90; // [0 90]
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
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; // <20><><EFBFBD><EFBFBD>
double BaseLat = uavLat; // <20><><EFBFBD><EFBFBD>
double height = uavAlt - uavGroundHeight; // <20><><EFBFBD>Ը߶<D4B8> = <20><><EFBFBD>и߶<D0B8> - <20><><EFBFBD><EFBFBD><EFBFBD>߳<EFBFBD>
bool bLaser = frame->ZH_LaserOn;
double LaserDistance = frame->ZH_LaserDist;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD>ж<EFBFBD>
if (bLaser == true && LaserDistance < 100)
{
bLaser = false;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
// x1 x2 x3
// x4 x5 x6
// x7 x8 x9
// <20>غɸ<D8BA><C9B8><EFBFBD><EFBFBD><EFBFBD>
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);
// <20>غɷ<D8BA>λ<EFBFBD><CEBB>
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;
// <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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);
// <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA>
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);
// <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
// <20><><EFBFBD><EFBFBD><EFBFBD>˷<EFBFBD>
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);
// ȡ<><C8A1>M54321<32>е<EFBFBD>ֵ
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);
// <20><><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ׼
if (bLaser == true && LaserDistance < 65535) // 65535 = 0xFFFF <20><>Чֵ
{
height = LaserDistance * x9;
}
double XA = -(x3) / (x9) * height;
double YA = -(x6) / (x9) * height;
// <20><><EFBFBD><EFBFBD><EFBFBD>ϳɺ<CFB3><C9BA><EFBFBD><EFBFBD><EFBFBD> Yaw
double PayLoad_Yaw = 0;
PayLoad_Yaw = getYaw(XA, YA);
// <20>ߵͽ<DFB5> <20>ϳ<EFBFBD>
double PayLoad_Pitch = acos(x9) * 180 / CV_PI;
// <20><><EFBFBD><EFBFBD><EFBFBD>εױ<CEB5>
double distance = height * tan(PayLoad_Pitch / 180.0 * CV_PI);
// <20><><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ׼
if (bLaser == true && LaserDistance < 65535)
{
// <20><><EFBFBD>ݼ<EFBFBD><DDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͺϳɽǶ<C9BD> <20><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>ͶӰ<CDB6><D3B0><EFBFBD><EFBFBD>
distance = LaserDistance * sin(PayLoad_Pitch / 180.0 * CV_PI);
}
//<2F><><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD>γ<EFBFBD><CEB3>
CalculatePtCoordinate(Lon, Lat, BaseLon, BaseLat, PayLoad_Yaw, distance, 3); // WGS84<38><34><EFBFBD><EFBFBD>ϵ
// Ŀ<><C4BF><EFBFBD>߶ȼ<DFB6><C8BC><EFBFBD>
H = uavGroundHeight;
// <20><><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
}
//<2F><><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㾭γ<E3BEAD><CEB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD>룺 1. frame <20><><EFBFBD>븴<EFBFBD><EBB8B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// 2. pt_x<5F><78> <20><>
// 3. pt_y<5F><79> <20><>
//
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 1. Lon <20><><EFBFBD><EFBFBD>
// 2. Lat<61><74> γ<><CEB3>
// 3. <20><><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɹ<EFBFBD><C9B9><EFBFBD><EFBFBD><EFBFBD>true<75><65>ʧ<EFBFBD>ܷ<EFBFBD><DCB7><EFBFBD>false
bool CalAnyPtCoordinate(double &Lon, double &Lat, double &H, const struQB_FJ *frame, int pt_x, int pt_y)
{
try
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
if (frame == NULL || frame->bValid == FALSE)
{
return false;
}
// <20>жϸ<D0B6><CFB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>
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. Խ<><D4BD><EFBFBD>ж<EFBFBD>
if (abs(uavLon) > 180) // <20><><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavLat) > 90) // γ<><CEB3>
{
return false;
}
if (uavAlt < 0 || uavAlt > 10000) // <20>߶<EFBFBD>
{
return false;
}
if (uavGroundHeight < 0 || (uavAlt - uavGroundHeight < 30))
{
return false;
}
if (abs(uavYaw) > 360) // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavPitch) > 30) // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
return false;
}
if (abs(uavRoll) > 30) // <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA>
{
return false;
}
if (abs(zhAz) > 180) // <20>غɷ<D8BA>λ<EFBFBD><CEBB>
{
return false;
}
if (zhPitch > -15 || zhPitch < -110)
{
return false;
}
// ͼ<><CDBC><EFBFBD>ߴ<EFBFBD>
int imgWidth = frame->ZH_ImgWidth;
int imgHeight = frame->ZH_ImgHeight;
// <20>ж<EFBFBD>
if (imgWidth <= 0 || imgHeight <= 0)
{
return false;
}
// <20><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD>ж<EFBFBD>
if (pt_x < 0 || pt_x >= imgWidth)
{
return false;
}
if (pt_y < 0 || pt_y >= imgHeight)
{
return false;
}
// <20><><EFBFBD><EFBFBD>
double f = frame->ZH_FocalLen / 1000; // <20><>
if (f <= 0)
{
return false;
}
// <20><>Ԫ<EFBFBD>ߴ<EFBFBD> <20><>
double pixelSize = frame->ZH_PixelSize;
if (pixelSize <= 0)
{
return false;
}
// <20>ɻ<EFBFBD>
double UAV_ROLL = uavRoll; // <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA> <20><><EFBFBD>Ƕȣ<C7B6>
double UAV_PITCH = uavPitch; // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>Ƕȣ<C7B6>
double UAV_YAW = uavYaw; // <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>Ƕȣ<C7B6>
// <20>غ<EFBFBD>
double ZH_PITCH = zhPitch; // <20>غɸߵͽ<DFB5> <20><><EFBFBD>Ƕȣ<C7B6>
double ZH_YAW = zhAz; // <20>غɷ<D8BA>λ<EFBFBD><CEBB> <20><><EFBFBD>Ƕȣ<C7B6>
// <20>غɸ<D8BA><C9B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ZH_PITCH = ZH_PITCH + 90; // [0 90]
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
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; // <20><><EFBFBD><EFBFBD>
double BaseLat = uavLat; // <20><><EFBFBD><EFBFBD>
double height = uavAlt - uavGroundHeight; // <20><><EFBFBD>Ը߶<D4B8> = <20><><EFBFBD>и߶<D0B8> - <20><><EFBFBD><EFBFBD><EFBFBD>߳<EFBFBD>
bool bLaser = frame->ZH_LaserOn;
double LaserDistance = frame->ZH_LaserDist;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD>ж<EFBFBD>
if (bLaser == true && LaserDistance < 100)
{
bLaser = false;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
// x1 x2 x3
// x4 x5 x6
// x7 x8 x9
// <20>غɸ<D8BA><C9B8><EFBFBD><EFBFBD><EFBFBD>
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);
// <20>غɷ<D8BA>λ<EFBFBD><CEBB>
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;
// <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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);
// <20>ɻ<EFBFBD><C9BB><EFBFBD>ת<EFBFBD><D7AA>
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);
// <20>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
// <20><><EFBFBD><EFBFBD><EFBFBD>˷<EFBFBD>
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);
// ȡ<><C8A1>M54321<32>е<EFBFBD>ֵ
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);
// <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
double x = 0, y = 0;
// <20><>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
x = +pt_x - imgWidth / 2;
y = -pt_y + imgHeight / 2;
// ת<><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>λ <20><>
x = x * pixelSize;
y = y * pixelSize;
// <20><>ĸ<EFBFBD><C4B8>ֵ<EFBFBD>ж<EFBFBD>
if (x7 * x + x8 * y - x9 * f == 0)
{
return false;
}
// <20><><EFBFBD>߷<EFBFBD><DFB7><EFBFBD>ģ<EFBFBD><C4A3>
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;
// <20><><EFBFBD>㷽λ<E3B7BD>Ǻ;<C7BA><CDBE><EFBFBD>
double distance = sqrt(XA * XA + YA * YA);
double yaw = getYaw(XA, YA);
// <20><><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD>ĵ㾭γ<E3BEAD><CEB3>
CalculatePtCoordinate(Lon, Lat, BaseLon, BaseLat, yaw, distance, 3); // WGS84<38><34><EFBFBD><EFBFBD>ϵ
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ü<EFBFBD><C3BC>ⶨλ<E2B6A8><CEBB><EFBFBD><EFBFBD>׼ȷ<D7BC><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double Lonoffset = 0.0;
double Latoffset = 0.0;
CalLonLatOffset(Lonoffset, Latoffset, frame);
// <20><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>
Lon += Lonoffset;
Lat += Latoffset;
// Ŀ<><C4BF><EFBFBD>߶ȼ<DFB6><C8BC><EFBFBD>
H = uavGroundHeight;
return true;
}
catch(cv::Exception &e)
{
e.msg.c_str();
return false;
}
catch(...)
{
return false;
}
return true;
}
// <20><><EFBFBD>ܣ<EFBFBD><DCA3>ͷ<EFBFBD><CDB7>鱨<EFBFBD><E9B1A8><EFBFBD><EFBFBD>
void ReleaseQBData(QBStru *pQBData)
{
// ָ<><D6B8>Ϊ<EFBFBD><CEAA><EFBFBD>ж<EFBFBD>
if (pQBData != NULL)
{
// srcImg
delete [] pQBData->image.srcImg.buff;
// dstImg
delete [] pQBData->image.dstImg.buff;
// demImg
delete [] pQBData->image.geoImg.buff;
// <20><><EFBFBD><EFBFBD>
memset(pQBData, 0, sizeof(QBStru));
}
}
// <20><><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD>¡ͼ<C2A1><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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)
{
// ͼ<><CDBC><EFBFBD><EFBFBD><EFBFBD>Ը<EFBFBD><D4B8><EFBFBD>
dst->ImgWidth = src->ImgWidth;
dst->ImgHeight = src->ImgHeight;
dst->bitcount = src->bitcount;
dst->BoundingBox = src->BoundingBox;
// <20><><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD>
SAFE_DELETE_ARRAY(dst->buff);
dst->buff = NULL;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ռ<EFBFBD>ֽ<EFBFBD><D6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>չ<EFBFBD><D5B9>4<EFBFBD>ı<EFBFBD><C4B1><EFBFBD>
int lineByte = (dst->ImgWidth * dst->bitcount / 8);
// λͼ<CEBB><CDBC><EFBFBD>ݻ<EFBFBD><DDBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĵ<EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD>С
int imgBufSize = dst->ImgHeight * lineByte;
// <20><><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4>ռ<EFBFBD>
if (imgBufSize > 0)
{
dst->buff = new BYTE[imgBufSize];
if (dst->buff != NULL)
{
memcpy(dst->buff, src->buff, static_cast<size_t>(imgBufSize)); // ͼ<><CDBC><EFBFBD><EFBFBD><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD>
}
}
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;
}
// <20><><EFBFBD><EFBFBD>src<72><63><EFBFBD>ݾ<EFBFBD><DDBE><EFBFBD>Ч<EFBFBD><D0A7>ֱ<EFBFBD>ӷ<EFBFBD><D3B7><EFBFBD>
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)
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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);
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//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
{
// <20><><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4>ء<EFBFBD><D8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߳̿<DFB3><CCBF>أ<EFBFBD><D8A3><EFBFBD><EFBFBD><EFBFBD>ProcessData
WaitForSingleObject(g_hMutex_ProcessDataH264, INFINITE);
try
{
if (g_bWorkMode == false) // ʵʱģʽ<C4A3><CABD>д<EFBFBD><D0B4><EFBFBD>ļ<EFBFBD>
{
//д<>뱾<EFBFBD><EBB1BE><EFBFBD>ļ<EFBFBD>
if (g_pFileSaveVideoH264Data != NULL)
{
fwrite(buffer, static_cast<size_t>(1), length, g_pFileSaveVideoH264Data); // д<><D0B4><EFBFBD><EFBFBD>
}
}
// <20>ж<EFBFBD><D0B6><EFBFBD><EFBFBD><EFBFBD>
if (length + g_validDataLenSocket < g_MaxBufferLenSocket) //<2F><>ʼg_validDataLenSocketΪ0<CEAA><30><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>lengthС<68><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֳ<EFBFBD><D6B3><EFBFBD><EFBFBD>򡭡<EFBFBD>
{
memcpy(g_receiveDataBufferSocket + g_validDataLenSocket, buffer, length); //<2F><>buffer<65><72><EFBFBD>ݿ<EFBFBD><DDBF><EFBFBD><EFBFBD><EFBFBD>g_receiveDataBufferSocket<65>ӳ<EFBFBD>g_validDataLenSocket<65><74><EFBFBD>ֽ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD><EFBFBD>g_receiveDataBufferSocket<65><74>
g_validDataLenSocket = length + g_validDataLenSocket; //<2F><>Ч<EFBFBD>ֳ<EFBFBD><D6B3><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD>ֳ<EFBFBD>
}
else
{
if (length <= g_MaxBufferLenSocket) // <20>޸ģ<DEB8><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
g_validDataLenSocket = 0;
// <20><>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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);
// <20>Ƿ<EFBFBD><C7B7>ɹ<EFBFBD><C9B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݸ<EFBFBD><DDB8>Ƶ<EFBFBD><C6B5><EFBFBD>ʱλ<CAB1>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//bool bHaveRelease = false;
// <20><>С֡<D0A1><D6A1>
if (g_validDataLenSocket > 0)
{
//unsigned char midBuffer[1024];
//// <20><><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD>
//memcpy(midBuffer, g_receiveDataBufferSocket, 1024);
//// <20><><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD>
//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);
// <20><><EFBFBD>ݳɹ<DDB3><C9B9><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5><EFBFBD>ʱλ<CAB1><CEBB>
//bHaveRelease = true;
// <20><><EFBFBD><EFBFBD>
if (g_CH96_h264decoder != nullptr)
{
g_CH96_h264decoder->decoding(g_receiveDataBufferSocket, g_validDataLenSocket);
}
}
/*if (bHaveRelease == false)
{
ReleaseMutex(g_hMutex_ProcessDataH264);
}*/
}
catch(...)
{
}
}
}
20240130 增加IMP显示和控制界面和串口通信
1 year ago
//<2F><><EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֻ<EFBFBD><D6BB>㵽<EFBFBD><E3B5BD>׼<EFBFBD>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD>룺<EFBFBD><EBA3BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵportID
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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();
}