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