public byte[] gen_rtcm(type1002 obs)
{
byte[] buffer = new u8[300];
uint len = 0;
uint i = 0;
uint nbit = 0;
uint nbyte = 0;
rtcmpreamble pre = new rtcmpreamble();
pre.Write(buffer);
rtcmheader head = new rtcmheader();
head.messageno = 1002;
head.nsat = (byte)obs.obs.Count;
head.epoch = (u32)obs.obs[0].tow;
head.Write(buffer);
nbit = obs.Write(buffer);
/* padding to align 8 bit boundary */
for (i = nbit; (i % 8) > 0; i++)
{
setbitu(buffer, i, 1, 0);
}
/* message length (header+data) (bytes) */
if ((len = i / 8) >= 3 + 1024)
{
/*trace(2,"generate rtcm 3 message length error len=%d\n",rtcm->len-3);*/
nbit = len = 0;
return(null);
}
/* message length without header and parity */
setbitu(buffer, 14, 10, len - 3);
/* crc-24q */
uint crc = crc24q(buffer, len, 0);
setbitu(buffer, i, 24, crc);
/* length total (bytes) */
nbyte = len + 3;
Array.Resize <byte>(ref buffer, (int)nbyte);
return(buffer);
}
public int Read(byte data)
{
switch (step)
{
default:
case 0:
if (data == RTCM3PREAMB)
{
step = 1;
buffer[0] = data;
}
break;
case 1:
buffer[1] = data;
step++;
break;
case 2:
buffer[2] = data;
step++;
pre = new rtcmpreamble();
pre.Read(buffer);
payloadlen = pre.length;
msglencount = 0;
// reset on oversize packet
if (payloadlen > buffer.Length)
step = 0;
break;
case 3:
if (msglencount < (payloadlen))
{
buffer[msglencount + 3] = data;
msglencount++;
}
else
{
step++;
goto case 4;
}
break;
case 4:
buffer[payloadlen + 3] = data;
step++;
break;
case 5:
buffer[payloadlen + 3 + 1] = data;
step++;
break;
case 6:
buffer[payloadlen + 3 + 2] = data;
payloadlen = payloadlen + 3;
u32 crc = crc24.crc24q(buffer, payloadlen, 0);
u32 crcpacket = getbitu(buffer, payloadlen * 8, 24);
if (crc == crcpacket)
{
rtcmheader head = new rtcmheader();
head.Read(buffer);
step = 0;
return head.messageno;
}
step = 0;
break;
}
return -1;
}
public int Read(byte data)
{
switch (step)
{
default:
case 0:
if (data == RTCM3PREAMB)
{
step = 1;
buffer[0] = data;
}
break;
case 1:
buffer[1] = data;
step++;
break;
case 2:
buffer[2] = data;
step++;
pre = new rtcmpreamble();
pre.Read(buffer);
payloadlen = pre.length;
msglencount = 0;
// reset on oversize packet
if (payloadlen > buffer.Length)
{
step = 0;
}
break;
case 3:
if (msglencount < (payloadlen))
{
buffer[msglencount + 3] = data;
msglencount++;
}
else
{
step++;
goto case 4;
}
break;
case 4:
buffer[payloadlen + 3] = data;
step++;
break;
case 5:
buffer[payloadlen + 3 + 1] = data;
step++;
break;
case 6:
buffer[payloadlen + 3 + 2] = data;
payloadlen = payloadlen + 3;
u32 crc = crc24.crc24q(buffer, payloadlen, 0);
u32 crcpacket = getbitu(buffer, payloadlen * 8, 24);
if (crc == crcpacket)
{
rtcmheader head = new rtcmheader();
head.Read(buffer);
step = 0;
return(head.messageno);
}
step = 0;
break;
}
return(-1);
}
public byte[] gen_rtcm(type1002 obs)
{
byte[] buffer = new u8[300];
uint len = 0;
uint i = 0;
uint nbit = 0;
uint nbyte = 0;
rtcmpreamble pre = new rtcmpreamble();
pre.Write(buffer);
rtcmheader head = new rtcmheader();
head.messageno = 1002;
head.nsat = (byte)obs.obs.Count;
head.epoch = (u32)obs.obs[0].tow;
head.Write(buffer);
nbit = obs.Write(buffer);
/* padding to align 8 bit boundary */
for (i = nbit; (i % 8) > 0; i++)
setbitu(buffer, i, 1, 0);
/* message length (header+data) (bytes) */
if ((len = i / 8) >= 3 + 1024)
{
/*trace(2,"generate rtcm 3 message length error len=%d\n",rtcm->len-3);*/
nbit = len = 0;
return null;
}
/* message length without header and parity */
setbitu(buffer, 14, 10, len - 3);
/* crc-24q */
uint crc = crc24q(buffer, len, 0);
setbitu(buffer, i, 24, crc);
/* length total (bytes) */
nbyte = len + 3;
Array.Resize<byte>(ref buffer, (int)nbyte);
return buffer;
}
public void Read(byte data)
{
switch (step)
{
case 0:
if (data == RTCM3PREAMB)
{
step++;
packet[0] = data;
}
break;
case 1:
packet[1] = data;
step++;
break;
case 2:
packet[2] = data;
step++;
pre = new rtcmpreamble();
pre.Read(packet);
len = pre.length;
a = 0;
// reset on oversize packet
if (len > packet.Length)
step = 0;
break;
case 3:
if (a < (len))
{
packet[a + 3] = data;
a++;
}
else
{
step++;
goto case 4;
}
break;
case 4:
packet[len + 3] = data;
step++;
break;
case 5:
packet[len + 3 + 1] = data;
step++;
break;
case 6:
packet[len + 3 + 2] = data;
len = len + 3;
u32 crc = crc24q(packet, len, 0);
u32 crcpacket = getbitu(packet, len * 8, 24);
//Console.WriteLine(crc.ToString("X") + " " + crcpacket.ToString("X"));
if (crc == crcpacket)
{
rtcmheader head = new rtcmheader();
head.Read(packet);
if (head.messageno == 1002)
{
type1002 tp = new type1002();
tp.Read(packet);
/*
byte[] test = new byte[300];
pre.Write(test);
head.Write(test);
tp.Write(test);
uint rem = tp.nbits % 8;
uint len2 = (tp.nbits + rem) / 8;
uint crc2 = crc24q(test, len, 0);
setbitu(test, tp.nbits + rem, 24, crc2);
*/
if (ObsMessage != null)
ObsMessage(tp.obs, null);
}
if (head.messageno == 1004)
{
type1004 tp = new type1004();
tp.Read(packet);
if (ObsMessage != null)
ObsMessage(tp.obs, null);
}
if (head.messageno == 1005)
{
type1005 tp = new type1005();
tp.Read(packet);
if (BasePosMessage != null)
BasePosMessage(tp, null);
}
if (head.messageno == 1006)
{
type1006 tp = new type1006();
tp.Read(packet);
if (BasePosMessage != null)
BasePosMessage(tp, null);
}
if (head.messageno == 1019)
{
type1019 tp = new type1019();
tp.Read(packet);
if (EphMessage != null)
EphMessage(tp, null);
}
}
step = 0;
break;
}
}
public int Read(byte data)
{
switch (step)
{
default:
case 0:
if (data == RTCM3PREAMB)
{
step = 1;
packet[0] = data;
}
break;
case 1:
packet[1] = data;
step++;
break;
case 2:
packet[2] = data;
step++;
pre = new rtcmpreamble();
pre.Read(packet);
len = pre.length;
a = 0;
// reset on oversize packet
if (len > packet.Length)
step = 0;
break;
case 3:
if (a < (len))
{
packet[a + 3] = data;
a++;
}
else
{
step++;
goto case 4;
}
break;
case 4:
packet[len + 3] = data;
step++;
break;
case 5:
packet[len + 3 + 1] = data;
step++;
break;
case 6:
packet[len + 3 + 2] = data;
len = len + 3;
u32 crc = crc24.crc24q(packet, len, 0);
u32 crcpacket = getbitu(packet, len * 8, 24);
if (crc == crcpacket)
{
rtcmheader head = new rtcmheader();
head.Read(packet);
step = 0;
return head.messageno;
}
step = 0;
break;
}
return -1;
}
public int Read(byte data)
{
switch (step)
{
default:
case 0:
if (data == RTCM3PREAMB)
{
step = 1;
packet[0] = data;
}
break;
case 1:
packet[1] = data;
step++;
break;
case 2:
packet[2] = data;
step++;
pre = new rtcmpreamble();
pre.Read(packet);
len = pre.length;
a = 0;
// reset on oversize packet
if (len > packet.Length)
{
step = 0;
}
break;
case 3:
if (a < (len))
{
packet[a + 3] = data;
a++;
}
else
{
step++;
goto case 4;
}
break;
case 4:
packet[len + 3] = data;
step++;
break;
case 5:
packet[len + 3 + 1] = data;
step++;
break;
case 6:
packet[len + 3 + 2] = data;
len = len + 3;
u32 crc = crc24.crc24q(packet, len, 0);
u32 crcpacket = getbitu(packet, len * 8, 24);
if (crc == crcpacket)
{
rtcmheader head = new rtcmheader();
head.Read(packet);
step = 0;
return(head.messageno);
}
step = 0;
break;
}
return(-1);
}
public void Read(byte data)
{
switch (step)
{
case 0:
if (data == RTCM3PREAMB)
{
step++;
packet[0] = data;
}
break;
case 1:
packet[1] = data;
step++;
break;
case 2:
packet[2] = data;
step++;
pre = new rtcmpreamble();
pre.Read(packet);
len = pre.length;
a = 0;
// reset on oversize packet
if (len > packet.Length)
{
step = 0;
}
break;
case 3:
if (a < (len))
{
packet[a + 3] = data;
a++;
}
else
{
step++;
goto case 4;
}
break;
case 4:
packet[len + 3] = data;
step++;
break;
case 5:
packet[len + 3 + 1] = data;
step++;
break;
case 6:
packet[len + 3 + 2] = data;
len = len + 3;
u32 crc = crc24q(packet, len, 0);
u32 crcpacket = getbitu(packet, len * 8, 24);
//Console.WriteLine(crc.ToString("X") + " " + crcpacket.ToString("X"));
if (crc == crcpacket)
{
rtcmheader head = new rtcmheader();
head.Read(packet);
if (head.messageno == 1002)
{
type1002 tp = new type1002();
tp.Read(packet);
/*
* byte[] test = new byte[300];
*
* pre.Write(test);
* head.Write(test);
* tp.Write(test);
*
* uint rem = tp.nbits % 8;
* uint len2 = (tp.nbits + rem) / 8;
*
* uint crc2 = crc24q(test, len, 0);
* setbitu(test, tp.nbits + rem, 24, crc2);
*/
if (ObsMessage != null)
{
ObsMessage(tp.obs, null);
}
}
if (head.messageno == 1004)
{
type1004 tp = new type1004();
tp.Read(packet);
if (ObsMessage != null)
{
ObsMessage(tp.obs, null);
}
}
if (head.messageno == 1005)
{
type1005 tp = new type1005();
tp.Read(packet);
if (BasePosMessage != null)
{
BasePosMessage(tp, null);
}
}
if (head.messageno == 1006)
{
type1006 tp = new type1006();
tp.Read(packet);
if (BasePosMessage != null)
{
BasePosMessage(tp, null);
}
}
if (head.messageno == 1019)
{
type1019 tp = new type1019();
tp.Read(packet);
if (EphMessage != null)
{
EphMessage(tp, null);
}
}
}
step = 0;
break;
}
}