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 { }
}
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 { }
}