private void ParseStatusData_daoHangKuaiSu(byte[] buffer, byte canId, UInt16 frameNO)
{
if (buffer.Length < Marshal.SizeOf(typeof(DAOHANGSHUJU_KuaiSu)) + CRCLENGTH)
{
return;
}
using (MemoryStream stream = new MemoryStream(buffer))
{
using (BinaryReader br = new BinaryReader(stream))
{
DAOHANGSHUJU_KuaiSu sObject = new DAOHANGSHUJU_KuaiSu
{
daoHangXiTongShiJian = br.ReadUInt32(), // 导航系统时间
jingDu = br.ReadInt32(), // 经度（组合结果）当量：1e-7
weiDu = br.ReadInt32(), // 纬度（组合结果）当量：1e-7
haiBaGaoDu = br.ReadInt32(), // 海拔高度（组合结果）当量：1e-7
dongXiangSuDu = br.ReadInt32(), // 东向速度（组合结果）当量：1e-7
beiXiangSuDu = br.ReadInt32(), // 北向速度（组合结果）当量：1e-7
tianXiangSuDu = br.ReadInt32(), // 天向速度（组合结果）当量：1e-7
GNSSTime = br.ReadUInt32(), // GNSS时间 单位s,UTC秒部
fuYangJiao = br.ReadSingle(), // 俯仰角
gunZhuanJiao = br.ReadSingle(), // 滚转角
pianHangJiao = br.ReadSingle(), // 偏航角
// 上5ms速度
tuoLuoShuJu_X = br.ReadSingle(), // 陀螺X数据
tuoLuoShuJu_Y = br.ReadSingle(), // 陀螺Y数据
tuoLuoShuJu_Z = br.ReadSingle(), // 陀螺Z数据
// 上5ms加速度
jiaSuDuJiShuJu_X = br.ReadSingle(), // 加速度计X数据
jiaSuDuJiShuJu_Y = br.ReadSingle(), // 加速度计Y数据
jiaSuDuJiShuJu_Z = br.ReadSingle(), // 加速度计Z数据
// 本5ms速度
tuoLuoShuJu_X2 = br.ReadSingle(), // 陀螺X数据2
tuoLuoShuJu_Y2 = br.ReadSingle(), // 陀螺Y数据2
tuoLuoShuJu_Z2 = br.ReadSingle(), // 陀螺Z数据2
// 本5ms加速度
jiaSuDuJiShuJu_X2 = br.ReadSingle(), // 加速度计X数据2
jiaSuDuJiShuJu_Y2 = br.ReadSingle(), // 加速度计Y数据2
jiaSuDuJiShuJu_Z2 = br.ReadSingle(), // 加速度计Z数据2
zhuangTaiBiaoZhiWei = br.ReadByte(), // 状态标志位
tuoLuoGuZhangBiaoZhi = br.ReadByte(), // 陀螺故障标志
};
if ((canId == frameType_daoHangKuaiSu_Tou || canId == frameType_daoHangKuaiSu_Ti) && FlyTime != 0 && sObject.GNSSTime != 0)
{
try
{
algorithm.CalcResultYc(sObject.weiDu * Math.Pow(10, -7), sObject.jingDu * Math.Pow(10, -7),
sObject.haiBaGaoDu * Math.Pow(10, -2), sObject.dongXiangSuDu * Math.Pow(10, -2), sObject.beiXiangSuDu * Math.Pow(10, -2),
sObject.tianXiangSuDu * Math.Pow(10, -2), mainForm.constLaunchFsx, Config.GetInstance().placementHeight,
out FallPoint fallPoint, out double fallTime, out double distance, out double x, out double y, out double z,
out double vx, out double vy, out double vz);
S_OBJECT obj = new S_OBJECT
{
time = (int)(sObject.GNSSTime) - FlyStartTime,
X = x,
Y = y,
Z = z,
VX = vx,
VY = vy,
VZ = vz
};
GetMinMax(ref pos, obj.time, out MinMaxValue minMaxValue);
obj.MinX = minMaxValue.MinX;
obj.MaxX = minMaxValue.MaxX;
obj.MinY = minMaxValue.MinY;
obj.MaxY = minMaxValue.MaxY;
obj.MinZ = minMaxValue.MinZ;
obj.MaxZ = minMaxValue.MaxZ;
obj.MinVx = minMaxValue.MinVx;
obj.MaxVx = minMaxValue.MaxVx;
obj.MinVy = minMaxValue.MinVy;
obj.MaxVy = minMaxValue.MaxVy;
obj.MinVz = minMaxValue.MinVz;
obj.MaxVz = minMaxValue.MaxVz;
if (canId == frameType_daoHangKuaiSu_Tou)
{
//弹头
obj.suit = 1;
YcMessage msg = new YcMessage
{
sObject = obj,
fallPoint = fallPoint,
fallTime = fallTime,
distance = distance
};
IntPtr ptr = Marshal.AllocHGlobal(Marshal.SizeOf <YcMessage>());
Marshal.StructureToPtr(msg, ptr, true);
PostMessage(mainFormHandle, MainForm.WM_YC_I, 0, ptr);
}
else
{
//弹体
obj.suit = 2;
YcMessage msg = new YcMessage
{
sObject = obj,
fallPoint = fallPoint,
fallTime = fallTime,
distance = distance
};
IntPtr ptr = Marshal.AllocHGlobal(Marshal.SizeOf <YcMessage>());
Marshal.StructureToPtr(msg, ptr, true);
PostMessage(mainFormHandle, MainForm.WM_YC_II, 0, ptr);
}
}
catch (Exception) { }
}
}
}
}
protected override void DefWndProc(ref Message m)
{
IntPtr ptr = m.LParam;
switch (m.Msg)
{
case WM_RADAR_DATA:
if (Config.GetInstance().source == 0)
{
S_OBJECT sObject = Marshal.PtrToStructure <S_OBJECT>(ptr);
historyData.AddObject(sObject);
FallPoint fallPoint = null;
double distance = 0;
double fallTime = 0;
try
{
algorithm.CalcResultLd(Config.GetInstance().longitudeInit, sObject.X, sObject.Y, sObject.Z,
sObject.VX, sObject.VY, sObject.VZ, constLaunchFsx,
Config.GetInstance().placementHeight, out fallPoint, out fallTime, out distance);
CheckFallPoint(fallPoint, fallTime, 1);
historyData.AddFallPoint(fallPoint);
}
catch (Exception) { }
AddDisplayData(sObject.time, sObject.X, sObject.Y, sObject.Z, sObject.VX, sObject.VY, sObject.VZ,
sObject.MinX, sObject.MaxX, sObject.MinY, sObject.MaxY, sObject.MinZ, sObject.MaxZ,
sObject.MinVx, sObject.MaxVx, sObject.MinVy, sObject.MaxVy, sObject.MinVz, sObject.MaxVz,
fallPoint, fallTime, distance, 1);
CheckPosition(sObject.X, sObject.Y, sObject.Z, sObject.MinX, sObject.MaxX, sObject.MinY, sObject.MaxY, sObject.MinZ, sObject.MaxZ, 1);
CheckSpeed(sObject.VX, sObject.VY, sObject.VZ, sObject.MinVx, sObject.MaxVx, sObject.MinVy, sObject.MaxVy, sObject.MinVz, sObject.MaxVz, 1);
}
Marshal.FreeHGlobal(ptr);
break;
case WM_RADAR_DATA_COMMING:
recvRaderNetworkDataTime = DateTime.Now;
picRadarNetwork.Image = greenLedImage;
break;
case WM_TELEMETRY_DATA_COMMING:
recvTelemetryNetworkDataTime = DateTime.Now;
picTelemetryNetwork.Image = greenLedImage;
break;
case WM_T0:
editT0.Enabled = btnStartT0.Enabled = false;
break;
case WM_YC_I:
if (Config.GetInstance().source == 1)
{
YcMessage msg = Marshal.PtrToStructure <YcMessage>(ptr);
historyData.AddObject(msg.sObject);
FallPoint fallPoint = msg.fallPoint;
double fallTime = msg.fallTime;
double distance = msg.distance;
historyData.AddFallPoint(fallPoint);
AddDisplayData(msg.sObject.time, msg.sObject.X, msg.sObject.Y, msg.sObject.Z, msg.sObject.VX, msg.sObject.VY, msg.sObject.VZ,
msg.sObject.MinX, msg.sObject.MaxX, msg.sObject.MinY, msg.sObject.MaxY, msg.sObject.MinZ, msg.sObject.MaxZ,
msg.sObject.MinVx, msg.sObject.MaxVx, msg.sObject.MinVy, msg.sObject.MaxVy, msg.sObject.MinVz, msg.sObject.MaxVz,
fallPoint, fallTime, distance, 1);
CheckFallPoint(fallPoint, fallTime, 1);
CheckPosition(msg.sObject.X, msg.sObject.Y, msg.sObject.Z, msg.sObject.MinX, msg.sObject.MaxX, msg.sObject.MinY, msg.sObject.MaxY, msg.sObject.MinZ, msg.sObject.MaxZ, 1);
CheckSpeed(msg.sObject.VX, msg.sObject.VY, msg.sObject.VZ, msg.sObject.MinVx, msg.sObject.MaxVx, msg.sObject.MinVy, msg.sObject.MaxVy, msg.sObject.MinVz, msg.sObject.MaxVz, 1);
}
Marshal.FreeHGlobal(ptr);
break;
case WM_YC_II:
if (Config.GetInstance().source == 1)
{
YcMessage msg = Marshal.PtrToStructure <YcMessage>(ptr);
historyData.AddObject(msg.sObject);
FallPoint fallPoint = msg.fallPoint;
double fallTime = msg.fallTime;
double distance = msg.distance;
historyData.AddFallPoint(fallPoint);
AddDisplayData(msg.sObject.time, msg.sObject.X, msg.sObject.Y, msg.sObject.Z, msg.sObject.VX, msg.sObject.VY, msg.sObject.VZ,
msg.sObject.MinX, msg.sObject.MaxX, msg.sObject.MinY, msg.sObject.MaxY, msg.sObject.MinZ, msg.sObject.MaxZ,
msg.sObject.MinVx, msg.sObject.MaxVx, msg.sObject.MinVy, msg.sObject.MaxVy, msg.sObject.MinVz, msg.sObject.MaxVz,
fallPoint, fallTime, distance, 2);
CheckFallPoint(fallPoint, fallTime, 2);
CheckPosition(msg.sObject.X, msg.sObject.Y, msg.sObject.Z, msg.sObject.MinX, msg.sObject.MaxX, msg.sObject.MinY, msg.sObject.MaxY, msg.sObject.MinZ, msg.sObject.MaxZ, 2);
CheckSpeed(msg.sObject.VX, msg.sObject.VY, msg.sObject.VZ, msg.sObject.MinVx, msg.sObject.MaxVx, msg.sObject.MinVy, msg.sObject.MaxVy, msg.sObject.MinVz, msg.sObject.MaxVz, 2);
}
Marshal.FreeHGlobal(ptr);
break;
default:
base.DefWndProc(ref m);
break;
}
}
