Esempio n. 1
0
        //发给移动站数据
        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);
        }
Esempio n. 2
0
        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;
        }