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) { }
}
}
}
}
public void AddObject(S_OBJECT sObject)
{
listSObject.Add(sObject);
}
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;
}
}
private void ParseRadarData(byte[] buffer)
{
String errMsg;
if (!CheckPacket(buffer, out errMsg))
{
Logger.GetInstance().Log(Logger.LOG_LEVEL.LOG_ERROR, "数据包错误:" + errMsg);
return;
}
PostMessage(mainFormHandle, MainForm.WM_RADAR_DATA_COMMING, 0, IntPtr.Zero);
if (Config.GetInstance().source != 0)
{
return;
}
using (MemoryStream stream = new MemoryStream(buffer))
{
using (BinaryReader br = new BinaryReader(stream))
{
PACK_HEAD packHead = new PACK_HEAD
{
Station = br.ReadByte(),
Type = br.ReadByte()
};
if (!Config.GetInstance().stationId.Equals(String.Empty) && packHead.Station != int.Parse(Config.GetInstance().stationId))
{
return;
}
if (packHead.Type == 0x20)
{
S_HEAD sHead = new S_HEAD
{
Len = br.ReadUInt16(),
Time = br.ReadInt32(),
SrcId = br.ReadByte(),
SrcType = br.ReadByte(),
CS = br.ReadByte(),
CT = br.ReadByte(),
FF = br.ReadByte(),
Num = br.ReadByte(),
C = br.ReadByte(),
S = br.ReadBytes(1)
};
if (Config.GetInstance().source == 0)
{
if (isPrintRaderLog == false)
{
Logger.GetInstance().Log(Logger.LOG_LEVEL.LOG_INFO, String.Format("收到第一帧雷测数据,Type={0}，Time={1}ms",
packHead.Type, sHead.Time));
isPrintRaderLog = true;
}
if (T0 == -1)
{
if (isStartGetT0 && sHead.Time >= T0Delay)
{
T0 = sHead.Time - T0Delay;
isStartGetT0 = false;
Logger.GetInstance().Log(Logger.LOG_LEVEL.LOG_INFO, String.Format("手动计算T0结果:{0}", T0));
PostMessage(mainFormHandle, MainForm.WM_T0, 0, IntPtr.Zero);
}
else
{
return;
}
}
}
while (stream.Position <= stream.Length - 90)
{
S_OBJECT sObject = new S_OBJECT
{
time = sHead.Time - T0,
ObjectId = br.ReadUInt16(),
A = br.ReadDouble(),
E = br.ReadDouble(),
R = br.ReadDouble(),
v = br.ReadDouble(),
X = br.ReadDouble(),
Y = br.ReadDouble(),
Z = br.ReadDouble(),
VX = br.ReadDouble(),
VY = br.ReadDouble(),
VZ = br.ReadDouble(),
BD = br.ReadByte(),
SS = br.ReadByte(),
VF = br.ReadByte(),
Reserve = br.ReadBytes(5),
};
GetMinMax(ref pos, sObject.time, out MinMaxValue minMaxValue);
sObject.MinX = minMaxValue.MinX;
sObject.MaxX = minMaxValue.MaxX;
sObject.MinY = minMaxValue.MinY;
sObject.MaxY = minMaxValue.MaxY;
sObject.MinZ = minMaxValue.MinZ;
sObject.MaxZ = minMaxValue.MaxZ;
sObject.MinVx = minMaxValue.MinVx;
sObject.MaxVx = minMaxValue.MaxVx;
sObject.MinVy = minMaxValue.MinVy;
sObject.MaxVy = minMaxValue.MaxVy;
sObject.MinVz = minMaxValue.MinVz;
sObject.MaxVz = minMaxValue.MaxVz;
sObject.suit = 1;
sObjectListSuit1.Add(sObject);
if (sObjectListSuit1.Count > 25)
{
sObjectListSuit1.RemoveAt(0);
}
if (sObjectListSuit1.Count == 25)
{
double[] timeArray = new double[25];
double[] xArray = new double[25];
double[] yArray = new double[25];
double[] zArray = new double[25];
for (int i = 0; i < 25; ++i)
{
timeArray[i] = sObjectListSuit1[i].time;
xArray[i] = sObjectListSuit1[i].X;
yArray[i] = sObjectListSuit1[i].Y;
zArray[i] = sObjectListSuit1[i].Z;
}
S_OBJECT obj = sObjectListSuit1[12];
obj.VX = Algorithm.GetSpeed(timeArray, xArray, obj.time);
obj.VY = Algorithm.GetSpeed(timeArray, yArray, obj.time);
obj.VZ = Algorithm.GetSpeed(timeArray, zArray, obj.time);
IntPtr ptr = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(S_OBJECT)));
Marshal.StructureToPtr(obj, ptr, true);
PostMessage(mainFormHandle, MainForm.WM_RADAR_DATA, 0, ptr);
}
}
}
else if (packHead.Type == 0x50)
{
S_HEAD sHead = new S_HEAD
{
Len = br.ReadUInt16(),
Time = br.ReadInt32(),
SrcId = br.ReadByte(),
SrcType = br.ReadByte(),
CS = br.ReadByte(),
CT = br.ReadByte(),
FF = br.ReadByte(),
Num = br.ReadByte(),
C = br.ReadByte(),
S = br.ReadBytes(1)
};
if (Config.GetInstance().source == 0)
{
if (isPrintRaderLog == false)
{
Logger.GetInstance().Log(Logger.LOG_LEVEL.LOG_INFO, String.Format("收到第一帧雷测数据,Type={0}，Time={1}ms",
packHead.Type, sHead.Time));
isPrintRaderLog = true;
}
recvRaderT0 = DateTime.Now;
if (T0 == -1 && telemetryFlyTime != -1 && recvRaderT0 != DateTime.MinValue && recvTelemetryFlyTime != DateTime.MinValue &&
recvRaderT0 >= recvTelemetryFlyTime)
{
if (sHead.Time > (int)(recvRaderT0 - recvTelemetryFlyTime).TotalMilliseconds - telemetryFlyTime)
{
T0 = sHead.Time - (int)(recvRaderT0 - recvTelemetryFlyTime).TotalMilliseconds - telemetryFlyTime;
Logger.GetInstance().Log(Logger.LOG_LEVEL.LOG_INFO, String.Format("收到雷测T0帧，T0={0}ms", T0));
PostMessage(mainFormHandle, MainForm.WM_T0, 0, IntPtr.Zero);
}
}
}
}
}
}
}
