Exemplo n.º 1
0
        static 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.payload  = 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);

            //foreach (var ch in allbytes)
            {
                // pk.read(ch);
            }

            //Console.ReadLine();
        }
Exemplo 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.payload = 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;
        }
Exemplo n.º 3
0
        public void read(byte data)
        {
            switch (state)
            {
                case 0:
                    if (data == 0x55)
                    {
                        state++;
                        msg = new piksimsg();
                        msg.preamble = data;
                        crc = new Crc16Ccitt(InitialCrcValue.Zeros);
                        crcpacket = (ushort)InitialCrcValue.Zeros;
                    }
                    else
                    {

                    }
                    break;
                case 1:
                    msg.msg_type = (u16)(data);
                    crcpacket = crc.Accumulate(data, crcpacket);
                    state++;
                    break;
                case 2:
                    msg.msg_type = (u16)(msg.msg_type + (data << 8));
                    crcpacket = crc.Accumulate(data, crcpacket);
                    state++;
                    break;
                case 3:
                    msg.sender = (u16)(data);
                    crcpacket = crc.Accumulate(data, crcpacket);
                    state++;
                    break;
                case 4:
                    msg.sender = (u16)(msg.sender + (data << 8));
                    crcpacket = crc.Accumulate(data, crcpacket);
                    state++;
                    break;
                case 5:
                    msg.length = data;
                    crcpacket = crc.Accumulate(data, crcpacket);
                    msg.payload = new u8[msg.length];
                    length = 0;
                    state++;
                    break;
                case 6:
                    if (length == msg.length)
                    {
                        state++;
                        goto case 7;
                    }
                    else
                    {
                        msg.payload[length] = data;
                        crcpacket = crc.Accumulate(data, crcpacket);
                        length++;
                    }
                    break;
                case 7:
                    msg.crc = (u16)(data);
                    state++;
                    break;
                case 8:
                    msg.crc = (u16)(msg.crc + (data << 8));
                    state = 0;

                    if (msg.crc == crcpacket)
                    {
                        ProcessMessage(msg);
                    }
                    else
                    {
                        Console.SetCursorPosition(0, 6);
                        Console.WriteLine(DateTime.Now + " sbp crc fail");
                    }
                    break;
            }
        }
Exemplo n.º 4
0
        static 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.payload = 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);

            //foreach (var ch in allbytes)
            {
               // pk.read(ch);
            }

            //Console.ReadLine();
        }