//发给移动站数据 void rtcm_ObsMessage(object sender, EventArgs e) { List<RTCM3.ob> msg = (List<RTCM3.ob>)sender; if (msg.Count == 0) return; byte total = 1; byte count = 0; piksi.observation_header_t head = new piksi.observation_header_t(); head.t.wn = (ushort)(msg[0].week); head.t.tow = (uint)((msg[0].tow * piksi.MSG_OBS_TOW_MULTIPLIER)); double soln_freq = 10; double obs_output_divisor = 2; double epoch_count = (head.t.tow / piksi.MSG_OBS_TOW_MULTIPLIER) * (soln_freq / obs_output_divisor); double checkleft = Math.Abs(epoch_count - Math.Round(epoch_count)); Console.WriteLine(head.t.tow + " " + checkleft.ToString("0.000") + " " + epoch_count.ToString("0.000") + " " + Math.Round(epoch_count) + " > " + 1e-3); // rounding - should not need this, but testing against a ublox requires some "lieing" //head.t.tow = (uint)(Math.Round((decimal)(head.t.tow / 1000.0)) * (decimal)1000.0); List<piksi.packed_obs_content_t> obs = new List<piksi.packed_obs_content_t>(); foreach (var item in msg) { item.cp *= -1; piksi.packed_obs_content_t ob = new piksi.packed_obs_content_t(); ob.sid = (byte)(item.prn - 1); ob.P = (uint)(item.pr * piksi.MSG_OBS_P_MULTIPLIER); ob.L.i = (int)item.cp; ob.L.f = (byte)((item.cp - ob.L.i) * 256.0); ob.cn0 = (byte)(item.snr * piksi.MSG_OBS_SNR_MULTIPLIER); obs.Add(ob); } head.n_obs = (byte)((total << piksi.MSG_OBS_HEADER_SEQ_SHIFT) | (count & piksi.MSG_OBS_HEADER_SEQ_MASK)); //create piksi packet piksi.piksimsg msgpreamble = new piksi.piksimsg(); int lenpre = Marshal.SizeOf(msgpreamble) - 1; // 8 int lenhdr = Marshal.SizeOf(head); int lenobs = Marshal.SizeOf(new piksi.packed_obs_content_t()); byte[] allbytes = new byte[lenpre + lenhdr + lenobs * obs.Count]; msgpreamble.crc = 0x1234; msgpreamble.preamble = 0x55; msgpreamble.msg_type = (ushort)piksi.MSG.SBP_MSG_OBS; msgpreamble.sender = 1; msgpreamble.length = (byte)(obs.Count * lenobs + lenhdr); msgpreamble.payloads = new byte[msgpreamble.length]; int payloadcount = (lenpre - 2) + lenhdr; // exclude checksum foreach (var ob in obs) { byte[] obbytes = StaticUtils.StructureToByteArray(ob); Array.Copy(obbytes, 0, allbytes, payloadcount, obbytes.Length); payloadcount += lenobs; } byte[] preamblebytes = StaticUtils.StructureToByteArray(msgpreamble); Array.Copy(preamblebytes, 0, allbytes, 0, preamblebytes.Length - 2); byte[] headbytes = StaticUtils.StructureToByteArray(head); Array.Copy(headbytes, 0, allbytes, lenpre - 2, headbytes.Length); Crc16Ccitt crc = new Crc16Ccitt(InitialCrcValue.Zeros); ushort crcpacket = 0; for (int i = 1; i < (allbytes.Length - 2); i++) { crcpacket = crc.Accumulate(allbytes[i], crcpacket); } allbytes[allbytes.Length - 2] = (byte)(crcpacket & 0xff); allbytes[allbytes.Length - 1] = (byte)((crcpacket >> 8) & 0xff); //Console.WriteLine(); //deststream.Write(allbytes, 0, allbytes.Length); GCSMainForm.comPort.InjectGpsData(allbytes, (byte)Math.Min(allbytes.Length, 110));// (byte)allbytes.Length); }
public byte[] GeneratePacket(object indata, MSG msgtype) { byte[] data = new byte[0]; if (indata == null) { } else if (indata is string) { data = ASCIIEncoding.ASCII.GetBytes((string)indata); } else if (indata.GetType().IsValueType) { data = StaticUtils.StructureToByteArray(indata); } byte[] packet = new u8[data.Length + 6 + 2]; piksi.piksimsg msgpreamble = new piksi.piksimsg(); msgpreamble.crc = 0x1234; msgpreamble.preamble = 0x55; msgpreamble.msg_type = (ushort)msgtype; msgpreamble.sender = 1; msgpreamble.length = (byte)(data.Length); msgpreamble.payloads = new byte[msgpreamble.length]; byte[] preamblebytes = StaticUtils.StructureToByteArray(msgpreamble); Array.Copy(preamblebytes, 0, packet, 0, preamblebytes.Length - 2); Array.Copy(data, 0, packet, 6, data.Length); Crc16Ccitt crc = new Crc16Ccitt(InitialCrcValue.Zeros); ushort crcpacket = 0; for (int i = 1; i < (packet.Length - 2); i++) { crcpacket = crc.Accumulate(packet[i], crcpacket); } packet[packet.Length - 2] = (byte)(crcpacket & 0xff); packet[packet.Length - 1] = (byte)((crcpacket >> 8) & 0xff); return packet; }