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;
}
/*
import sbp_piksi
link.link.send_message(sbp_piksi.SETTINGS, 'uart_ftdi\0mode\0SBP\0')
import sbp_piksi
self.link.send_message(sbp_piksi.SETTINGS, 'uart_ftdi\0baudrate\0%s\0' % ('1000000'.encode('ascii')))
import sbp_piksi
self.link.send_message(sbp_piksi.SETTINGS, 'uart_uarta\0sbp_message_mask\0%s\0' % ('65535'.encode('ascii')))
import sbp_piksi
self.link.send_message(sbp_piksi.RESET, '')
*/
static void Main(string[] args)
{
Console.Clear();
if (args.Length < 3)
{
Console.WriteLine("Piksi v0.1.4.1 beta By Michael Oborne");
Console.WriteLine("Copyright Michael Oborne 2015");
Console.WriteLine("Usage: program.exe outputformat source destination");
Console.WriteLine("outputformat = rtcm, sbp, trimble, rinex");
Console.WriteLine("rtcm = read sbp from source and output rtcm to destination");
Console.WriteLine("sbp = read rtcm from source and output sbp to destination");
Console.WriteLine("trimble = read sbp from source and output trimble rt17 to destination");
Console.WriteLine("rinex = read sbp from source file and output rinex to destination file (Files only)");
Console.WriteLine();
Console.WriteLine("Port 9876 streams all received piksi data to it");
Console.WriteLine();
Console.WriteLine("source/destination can be 'COM? 115200' or 'tcp://*****:*****@host/source' or 'tcp://0.0.0.0:989'");
Console.ReadLine();
return;
}
string outmode = args[0];
// rinex output
if (outmode.ToLower() == "rinex")
{
using (rinexoutput = new StreamWriter(args[2]))
{
using (rinexoutput2 = new StreamWriter(args[2].Replace(".obs","") + ".relay.obs"))
{
string header = @" 3.02 OBSERVATION DATA M: Mixed RINEX VERSION / TYPE
MARKER NAME
MARKER NUMBER
MARKER TYPE
OBSERVER / AGENCY
REC # / TYPE / VERS
ANT # / TYPE
0.0000 0.0000 0.0000 APPROX POSITION XYZ
0.0000 0.0000 0.0000 ANTENNA: DELTA H/E/N
G 4 C1C L1C D1C S1C SYS / # / OBS TYPES
G SYS / PHASE SHIFT
END OF HEADER ";
rinexoutput.WriteLine(header);
rinexoutput2.WriteLine(header);
piksi.ObsMessage += pkrinex_ObsMessage;
piksi.consoleoutput = false;
DateTime reporttime = DateTime.MinValue;
if (args[1].ToLower().EndsWith("json"))
{
using (StreamReader sr = new StreamReader(new BufferedStream(File.OpenRead(args[1]), 1024 * 1024 * 4)))
{
long length = sr.BaseStream.Length;
while (!sr.EndOfStream)
{
double percent = (sr.BaseStream.Position / (double)length) * 100.0;
if (reporttime.Second != DateTime.Now.Second)
{
Console.WriteLine(percent);
reporttime = DateTime.Now;
}
string line = sr.ReadLine();
var item = JSON.getpacket(line);
piksi.ProcessMessage(item);
}
}
}
else
{
using (BinaryReader br = new BinaryReader(new BufferedStream(File.OpenRead(args[1]), 1024 * 1024 * 4)))
{
long length = br.BaseStream.Length;
while (br.BaseStream.Position < length)
{
double percent = (br.BaseStream.Position / (double)length) * 100.0;
if (reporttime.Second != DateTime.Now.Second)
{
Console.WriteLine(percent);
reporttime = DateTime.Now;
}
piksi.read(br.ReadByte());
}
}
}
}
}
return;
}
rawpiksi.Start();
rawpiksi.BeginAcceptTcpClient(rawpiksiAcceptCallBack, rawpiksi);
// process other commands
int nextarg = 2;
string port = args[1];
// input
if (port.ToLower().Contains("tcp://0.0.0.0"))
{
inputstream = new TCPServer(int.Parse(port.Split(':')[2]));
nextarg = 2;
}
else if (port.ToLower().Contains("tcp://"))
{
inputstream = new TCPClient(port.ToLower().Replace("tcp://",""), int.Parse(port.Split(':')[2]));
nextarg = 2;
}
else if (port.ToLower().Contains("ntrip://"))
{
inputstream = new NTRIP(port.Replace("ntrip://", ""));
nextarg = 2;
}
else
{
int baudrate = int.Parse(args[2]);
inputstream = new SerialPort(port,baudrate);
nextarg = 3;
}
try
{
inputstream.Open();
}
catch
{
Console.WriteLine("Failed to open input");
return;
}
string portout = args[nextarg];
// output
if (portout.ToLower().Contains("tcp://0.0.0.0"))
{
deststream = new TCPServer(int.Parse(portout.Split(':')[2]));
nextarg = 2;
}
else if (portout.ToLower().Contains("tcp://"))
{
deststream = new TCPClient(portout.ToLower().Replace("tcp://", ""), int.Parse(portout.Split(':')[2]));
nextarg++;
}
else if (portout.ToLower().Contains("ntrip://"))
{
deststream = new NTRIP(portout.Replace("ntrip://", ""));
nextarg++;
}
else
{
int baudrate = int.Parse(args[nextarg + 1]);
deststream = new SerialPort(portout, baudrate);
nextarg = nextarg + 2;
}
System.Threading.Thread th = new System.Threading.Thread(UI);
//th.Start();
try
{
deststream.Open();
}
catch
{
Console.WriteLine("Failed to open output");
return;
}
if (outmode.ToLower() == "trimble")
{
piksi.ObsMessage += pktrimble_ObsMessage;
piksi.EphMessage += pktrimble_EphMessage;
DateTime ephdeadline = DateTime.Now.AddSeconds(12);
while (true)
{
while (inputstream.dataToRead)
{
piksi.read((byte)inputstream.ReadByte());
}
if (ephdeadline < DateTime.Now)
{
string[] files = Directory.GetFiles(".", "*.eph");
foreach (var ephfile in files)
{
var info = new FileInfo(ephfile);
// check age
if (info.LastWriteTime.AddHours(4) < DateTime.Now)
continue;
byte[] ephbytes = File.ReadAllBytes(ephfile);
piksi.piksimsg msg = new piksi.piksimsg();
msg.payload = ephbytes;
pktrimble_EphMessage(msg, null);
}
ephdeadline = DateTime.Now.AddSeconds(30);
}
System.Threading.Thread.Sleep(5);
}
}
// sbp to rtcm
if (outmode.ToLower() == "rtcm")
{
piksi.ObsMessage += pkrtcm_ObsMessage;
piksi.EphMessage += pkrtcm_EphMessage;
while (true)
{
while (inputstream.dataToRead)
{
piksi.read((byte)inputstream.ReadByte());
}
System.Threading.Thread.Sleep(5);
}
}
if (outmode.ToLower() == "sbp")
{
rtcm.ObsMessage += rtcm_ObsMessage;
rtcm.BasePosMessage += rtcm_BasePosMessage;
while (true)
{
while (inputstream.dataToRead)
{
rtcm.Read((byte)inputstream.ReadByte());
}
while (deststream.dataToRead)
{
byte[] buffer = new byte[1024];
int read = deststream.Read(buffer,0,buffer.Length);
if (rawpiksiclient != null && rawpiksiclient.Connected)
{
try
{
rawpiksiclient.Client.Send(buffer, read, SocketFlags.None);
//piksi.read((byte)deststream.ReadByte());
if (rawpiksiclient.Client.Available > 0)
{
read = rawpiksiclient.Client.Receive(buffer, buffer.Length, SocketFlags.None);
deststream.Write(buffer, 0, read);
}
}
catch
{
}
}
}
System.Threading.Thread.Sleep(5);
}
}
Console.ReadLine();
}