コード例 #1
0
        static void pkrtcm_ObsMessage(object sender, EventArgs e)
        {
            piksi.piksimsg msg = (piksi.piksimsg)sender;

            var hdr = msg.payload.ByteArrayToStructure <piksi.msg_obs_header_t>(0);

            // relay packet
            if (msg.sender == 0)
            {
                return;
            }

            // total is number of packets
            int total = hdr.seq >> piksi.MSG_OBS_HEADER_SEQ_SHIFT;
            // this is packet count number
            int count = hdr.seq & piksi.MSG_OBS_HEADER_SEQ_MASK;

            int lenhdr = Marshal.SizeOf(hdr);

            int lenobs = Marshal.SizeOf(new piksi.packed_obs_content_t());

            int obscount = (msg.length - lenhdr) / lenobs;

            int linebase = (count > 0) ? 8 : 0;

            RTCM3.type1002 t1002 = new RTCM3.type1002();

            for (int a = 0; a < obscount; a++)
            {
                var ob = msg.payload.ByteArrayToStructure <piksi.packed_obs_content_t>(lenhdr + a * lenobs);

                double[] sat_pos = new double[3];
                double[] sat_vel = new double[3];
                double   clock_err = 0, clock_err_rate = 0;

                piksi.gps_time_t tt = new piksi.gps_time_t()
                {
                    tow = hdr.t.tow / 1000.0, wn = hdr.t.wn
                };

                piksi.eph[ob.sid + 1].calc_sat_pos(sat_pos, sat_vel, ref clock_err, ref clock_err_rate, tt);

                double[] e1      = new double[3];
                double   geodist = global::piksi.piksi.geodistnosagnac(new double[] { sat_pos[0], sat_pos[1], sat_pos[2] }, new double[] { piksi.lastpos[0], piksi.lastpos[1], piksi.lastpos[2] }, ref e1);

                double geodist2 = global::piksi.piksi.geodist(new double[] { sat_pos[0], sat_pos[1], sat_pos[2] }, new double[] { piksi.lastpos[0], piksi.lastpos[1], piksi.lastpos[2] }, ref e1);

                if (a == 2 && Graph.instance != null && !Graph.instance.IsDisposed)
                {
                    Graph.instance.Invoke((Action) delegate()
                    {
                        try
                        {
                            Graph.instance.zedGraphControl1.GraphPane.Title.Text =
                                (ob.sid + 1).ToString();
                        }
                        catch {
                        }
                    });

                    double wl = CLIGHT / 1.57542E9;

                    double newpr = (ob.P / piksi.MSG_OBS_P_MULTIPLIER) + piksi.lastpos[3];
                    double newcp = -(ob.L.i + (ob.L.f / 256.0)) * wl;// +(piksi.clockdrift.linearRegression(0));

                    Graph.instance.AddData(1, tt.tow, lastpr - newpr);
                    Graph.instance.AddData(2, tt.tow, lastcp - newcp);
                    Graph.instance.AddData(3, tt.tow, lastgeodist - geodist);

                    //Graph.instance.AddData(5, tt.tow, -(lastclockbias - piksi.lastpos[3]));

                    lastpr        = newpr;
                    lastcp        = newcp;
                    lastgeodist   = geodist;
                    lastgeodist2  = geodist2;
                    lastclockbias = piksi.lastpos[3];
                }

                if (a == 1 && Graph.instance != null)
                {
                    //Graph.instance.AddData(3, ob.P / piksi.MSG_OBS_P_MULTIPLIER);
                    //Graph.instance.AddData(4, -(ob.L.Li + (ob.L.Lf / 256.0)));
                }

                if (a == 2 && Graph.instance != null)
                {
                    //Graph.instance.AddData(5, ob.P / piksi.MSG_OBS_P_MULTIPLIER);
                    //Graph.instance.AddData(6, -(ob.L.Li + (ob.L.Lf / 256.0)));
                }

                RTCM3.ob rtcmob = new RTCM3.ob();

                rtcmob.prn  = (byte)(ob.sid + 1);
                rtcmob.snr  = (byte)(ob.cn0 / piksi.MSG_OBS_SNR_MULTIPLIER);
                rtcmob.pr   = (ob.P / piksi.MSG_OBS_P_MULTIPLIER) + piksi.lastpos[3];
                rtcmob.cp   = -(ob.L.i + (ob.L.f / 256.0));
                rtcmob.week = hdr.t.wn;
                rtcmob.tow  = hdr.t.tow;

                if (lockcount[rtcmob.prn] == ob.@lock)
                {
                    rtcmob.raw.lock1 = 127;
                }

                lockcount[rtcmob.prn] = ob.@lock;

                t1002.obs.Add(rtcmob);
            }

            byte[] rtcmpacket = rtcm.gen_rtcm(t1002);

            try
            {
                if (deststream != null)
                {
                    deststream.Write(rtcmpacket, 0, rtcmpacket.Length);
                }
            }
            catch { }
        }
コード例 #2
0
ファイル: Program.cs プロジェクト: DroneBuster/piksi
        static void pkrtcm_ObsMessage(object sender, EventArgs e)
        {
            piksi.piksimsg msg = (piksi.piksimsg)sender;

            var hdr = msg.payload.ByteArrayToStructure<piksi.msg_obs_header_t>(0);

            // relay packet
            if (msg.sender == 0)
                return;

            // total is number of packets
            int total = hdr.seq >> piksi.MSG_OBS_HEADER_SEQ_SHIFT;
            // this is packet count number
            int count = hdr.seq & piksi.MSG_OBS_HEADER_SEQ_MASK;

            int lenhdr = Marshal.SizeOf(hdr);

            int lenobs = Marshal.SizeOf(new piksi.packed_obs_content_t());

            int obscount = (msg.length - lenhdr) / lenobs;

            int linebase = (count > 0) ? 8 : 0;

            RTCM3.type1002 t1002 = new RTCM3.type1002();

            for (int a = 0; a < obscount; a++)
            {
                var ob = msg.payload.ByteArrayToStructure<piksi.packed_obs_content_t>(lenhdr + a * lenobs);

                double[] sat_pos = new double[3];
                double[] sat_vel = new double[3];
                double clock_err = 0, clock_err_rate = 0;

                piksi.gps_time_t tt = new piksi.gps_time_t() { tow = hdr.t.tow / 1000.0, wn = hdr.t.wn };

                piksi.eph[ob.sid + 1].calc_sat_pos(sat_pos, sat_vel, ref clock_err, ref clock_err_rate, tt);

                double[] e1 = new double[3];
                double geodist = global::piksi.piksi.geodistnosagnac(new double[] { sat_pos[0], sat_pos[1], sat_pos[2] }, new double[] { piksi.lastpos[0], piksi.lastpos[1], piksi.lastpos[2] }, ref e1);

                double geodist2 = global::piksi.piksi.geodist(new double[] { sat_pos[0], sat_pos[1], sat_pos[2] }, new double[] { piksi.lastpos[0], piksi.lastpos[1], piksi.lastpos[2] }, ref e1);

                if (a == 2 && Graph.instance != null && !Graph.instance.IsDisposed)
                {
                    Graph.instance.Invoke((Action) delegate()
                    {
                        try
                        {
                            Graph.instance.zedGraphControl1.GraphPane.Title.Text =
                                (ob.sid+1).ToString();
                        }
                        catch {
                        }
                    });

                    double wl = CLIGHT / 1.57542E9;

                    double newpr = (ob.P/piksi.MSG_OBS_P_MULTIPLIER) + piksi.lastpos[3];
                    double newcp = -(ob.L.i + (ob.L.f / 256.0)) * wl;// +(piksi.clockdrift.linearRegression(0));

                    Graph.instance.AddData(1, tt.tow, lastpr - newpr);
                    Graph.instance.AddData(2, tt.tow, lastcp - newcp);
                    Graph.instance.AddData(3, tt.tow, lastgeodist - geodist);

                    //Graph.instance.AddData(5, tt.tow, -(lastclockbias - piksi.lastpos[3]));

                    lastpr = newpr;
                    lastcp = newcp;
                    lastgeodist = geodist;
                    lastgeodist2 = geodist2;
                    lastclockbias = piksi.lastpos[3];
                }

                if (a == 1 && Graph.instance != null)
                {
                    //Graph.instance.AddData(3, ob.P / piksi.MSG_OBS_P_MULTIPLIER);
                    //Graph.instance.AddData(4, -(ob.L.Li + (ob.L.Lf / 256.0)));
                }

                if (a == 2 && Graph.instance != null)
                {
                    //Graph.instance.AddData(5, ob.P / piksi.MSG_OBS_P_MULTIPLIER);
                    //Graph.instance.AddData(6, -(ob.L.Li + (ob.L.Lf / 256.0)));
                }

                RTCM3.ob rtcmob = new RTCM3.ob();

                rtcmob.prn = (byte)(ob.sid+1);
                rtcmob.snr = (byte)(ob.cn0 / piksi.MSG_OBS_SNR_MULTIPLIER);
                rtcmob.pr = (ob.P / piksi.MSG_OBS_P_MULTIPLIER) +piksi.lastpos[3];
                rtcmob.cp = -(ob.L.i + (ob.L.f / 256.0));
                rtcmob.week = hdr.t.wn;
                rtcmob.tow = hdr.t.tow;

                if (lockcount[rtcmob.prn] == ob.@lock)
                {
                    rtcmob.raw.lock1 = 127;
                }

                lockcount[rtcmob.prn] = ob.@lock;

                t1002.obs.Add(rtcmob);
            }

            byte[] rtcmpacket = rtcm.gen_rtcm(t1002);

            try
            {
                if (deststream != null)
                    deststream.Write(rtcmpacket, 0, rtcmpacket.Length);
            }
            catch { }
        }