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();
}
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;
}
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;
}
}
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();
}