public static void writeTrimble15(IStreamExtra outputto, int week, uint tow)
{
MemoryStream dat = new MemoryStream();
BinaryWriter bw = new BinaryWriter(dat);
bw.Write((byte)0x02);
bw.Write((byte)0x00);
bw.Write((byte)tcomType.RET_TIME);
bw.Write((byte)20); // 20
reverseEndian(bw, ((int)(tow / 1000)).ToString().PadLeft(7, '0')); // tow
reverseEndian(bw, convert(week.ToString(), 4)); // week
reverseEndian(bw, convert("16", 2)); // leap seconds
reverseEndian(bw, convert("0000", 4)); // tz offset
reverseEndian(bw, convert("UTC", 3)); // tz
// exclude startchar
bw.Seek(1, SeekOrigin.Begin);
int num = 0;
while (dat.Position < dat.Length)
{
byte num2 = (byte)dat.ReadByte();
num = (byte)(num + num2);
}
byte[] data = dat.ToArray();
outputto.Write(data, 0, (int)data.Length);
CtcomTail tail = new CtcomTail();
tail.checksum = (byte)(num);
tail.writeToStream(outputto);
}
public static void writeTrimbleR17(IStreamExtra outputto, double towinms, List<piksi.msg_obs_content_t> obs)
{
CdatType17 type = new CdatType17
{
tmeasRCVR = towinms,
clockOffset = 0,
nMeasurementSets = 0
};
type.svMeasResize(obs.Count);
for (int i = 0; i < obs.Count; i++)
{
{
double[] azel = new double[2] { 0, 0 };
try
{
// satazel(myposllh, raw2.satxyzscale[raw.data[i].SV], ref azel);
if (azel[0] < 0)
azel[0] = 0;
if (azel[1] < 0)
azel[1] = 0;
}
catch { }
type.svMeas[type.nMeasurementSets].prn = (byte)(obs[i].prn + 1);
type.svMeas[type.nMeasurementSets].elevationDeg = (short)((azel[1]));//Convert.ToInt16(this.m_measurement[i].header.elevation);
type.svMeas[type.nMeasurementSets].azimuthDeg = (short)((azel[0]));//Convert.ToInt16(this.m_measurement[i].header.azimuth);
type.svMeas[type.nMeasurementSets].flags1 = 0; // 249
type.svMeas[type.nMeasurementSets].flags2 = 0; // 4
type.svMeas[type.nMeasurementSets].flagStatus = 0;
// l1 only
type.svMeas[type.nMeasurementSets].snCountsL1 = obs[i].snr / piksi.MSG_OBS_SNR_MULTIPLIER;
type.svMeas[type.nMeasurementSets].pseudorange = obs[i].P / piksi.MSG_OBS_P_MULTIPLIER;
type.svMeas[type.nMeasurementSets].phaseL1 = (obs[i].L.Li + (obs[i].L.Lf / 256.0));
// type.svMeas[type.nMeasurementSets].doppler = (type.svMeas[type.nMeasurementSets].phaseL1 - cpold[obs[i].prn]);
cpold[obs[i].prn] = type.svMeas[type.nMeasurementSets].phaseL1;
type.svMeas[type.nMeasurementSets].lidcpr = 0.0;
CdatType17.measT st1 = type.svMeas[type.nMeasurementSets];
st1.flags1 = (byte)(st1.flags1 | 0x40); // l1 data valid
CdatType17.measT st2 = type.svMeas[type.nMeasurementSets];
st2.flags1 = (byte)(st2.flags1 | 0x80); // new positon computed
if (lockcount[obs[i].prn] == obs[i].lock_counter)
{
st2.flags1 = (byte)(st2.flags1 | 0x10); // l1 phase valid
st2.flags1 = (byte)(st2.flags1 | 0x8); // L1 Phase Lock Point
}
lockcount[obs[i].prn] = obs[i].lock_counter;
type.svMeas[type.nMeasurementSets].auxPseudorange = 0.0;
CdatType17.measT st21 = type.svMeas[type.nMeasurementSets];
st21.flagStatus = (byte)(st21.flagStatus | 1);
CdatType17.measT st22 = type.svMeas[type.nMeasurementSets];
st22.flagStatus = (byte)(st22.flagStatus | 2);
type.nMeasurementSets = (byte)(type.nMeasurementSets + 1);
}
}
MemoryStream dat = new MemoryStream();
writeToStream(new BinaryWriter(dat), type);
CtcomTail tail = new CtcomTail();
byte packetsize = 244;
int totalleft = (int)dat.Length;
int loops = (totalleft / packetsize) + 1;
byte sending = 0;
int pageno = 1;
for (int a = 0; a < loops; a++)
{
//trace(3, "%d/%d\n", pageno, loops);
if (totalleft > packetsize)
{
sending = packetsize;
}
else
{
sending = (byte)totalleft;
}
outputto.Write((byte)0x02); // stx
outputto.Write((byte)0x00); // status
outputto.Write((byte)tcomType.RET_RAWDATA); // type
outputto.Write((byte)(sending + 4)); // 4 extra for 4 ytes below
outputto.Write((byte)0);// record type
byte page = Convert.ToByte((int)((pageno << 4) & 240));
page = (byte)(page | Convert.ToByte((int)(loops & 15)));
outputto.Write((byte)page);// page
outputto.Write((byte)trimblerawreply); // replyno
outputto.Write((byte)0); // flags - 0 = expanded and no realtimedata
byte[] packet = dat.ToArray();
outputto.Write(packet, ((int)dat.Length - totalleft), sending);
// single bytes only
tail.checksum = computeChecksum((byte)0x00, (byte)tcomType.RET_RAWDATA, (byte)(sending), (byte)0, (byte)page, (byte)trimblerawreply, (byte)0, dat, ((int)dat.Length - totalleft));
tail.writeToStream(outputto);
pageno++;
totalleft -= sending;
}
trimblerawreply++;
}
public static void writeTrimble55_1(IStreamExtra outputto, piksi.ephemeris_t es, int prn)
{
MemoryStream dat = new MemoryStream();
BinaryWriter bw = new BinaryWriter(dat);
bw.Write((byte)0x02);
bw.Write((byte)0x00);
bw.Write((byte)tcomType.RET_SVDATA);
bw.Write((byte)(180 - 4)); // 4 for header
reverseEndian(bw, (byte)0x1); // Data type
reverseEndian(bw, (byte)prn);
//int tow = (int)time2gpst(es.ttr, ref weekwork);
//int toc = (int)time2gpst(es.toc, ref weekwork);
//int toe = (int)time2gpst(es.toe, ref weekwork);
//if (toe < tow)
{
// weekwork--;
}
//int iodc =
reverseEndian(bw, (ushort)(es.toe.wn));
reverseEndian(bw, (ushort)0);//es.iodc);
reverseEndian(bw, (byte)0); // res
reverseEndian(bw, (byte)0);//es.iode);
reverseEndian(bw, (int)es.toc.tow);
reverseEndian(bw, (int)es.toc.tow);
reverseEndian(bw, (int)es.toe.tow);
reverseEndian(bw, (double)es.tgd); // check scale ex -1.72294676303864e-8
reverseEndian(bw, (double)es.af2);
reverseEndian(bw, (double)es.af1);
reverseEndian(bw, (double)es.af0);
reverseEndian(bw, (double)es.crs);
reverseEndian(bw, (double)es.dn / SC2RAD);
reverseEndian(bw, (double)es.m0 / SC2RAD);
reverseEndian(bw, (double)es.cuc / SC2RAD);
reverseEndian(bw, (double)es.ecc);
reverseEndian(bw, (double)es.cus / SC2RAD);
reverseEndian(bw, (double)es.sqrta); // checkme
reverseEndian(bw, (double)es.cic / SC2RAD);
reverseEndian(bw, (double)es.omega0 / SC2RAD);
reverseEndian(bw, (double)es.cis / SC2RAD);
reverseEndian(bw, (double)es.inc / SC2RAD);
reverseEndian(bw, (double)es.crc);
reverseEndian(bw, (double)es.w / SC2RAD);
reverseEndian(bw, (double)es.omegadot / SC2RAD);
reverseEndian(bw, (double)es.inc_dot / SC2RAD);
uint flags = 0;
flags = flags | (uint)(0 & 1);
flags = flags | (uint)((1 & 3) << 1);
flags = flags | (uint)((1) << 3);
flags = flags | (uint)(((es.healthy-1) & 63) << 4);
flags = flags | (uint)(((uint)0 & 1) << 10);
flags = flags | (uint)((0 & 15) << 11);
//flags = flags | (uint)((raw.alm[sat - 1].svconf & 7) << 16);
flags = flags | (uint)((0) << 19);
reverseEndian(bw, (int)flags);
// exclude startchar
bw.Seek(1, SeekOrigin.Begin);
int num = 0;
while (dat.Position < dat.Length)
{
byte num2 = (byte)dat.ReadByte();
num = (byte)(num + num2);
}
outputto.Write(dat.ToArray(), 0, (int)dat.Length);
CtcomTail tail = new CtcomTail();
tail.checksum = (byte)num;
tail.writeToStream(outputto);
}
public static void writeTrimble15(IStreamExtra outputto, int week, uint tow)
{
MemoryStream dat = new MemoryStream();
BinaryWriter bw = new BinaryWriter(dat);
bw.Write((byte)0x02);
bw.Write((byte)0x00);
bw.Write((byte)tcomType.RET_TIME);
bw.Write((byte)20); // 20
reverseEndian(bw, ((int)(tow / 1000)).ToString().PadLeft(7, '0')); // tow
reverseEndian(bw, convert(week.ToString(), 4)); // week
reverseEndian(bw, convert("16", 2)); // leap seconds
reverseEndian(bw, convert("0000", 4)); // tz offset
reverseEndian(bw, convert("UTC", 3)); // tz
// exclude startchar
bw.Seek(1, SeekOrigin.Begin);
int num = 0;
while (dat.Position < dat.Length)
{
byte num2 = (byte)dat.ReadByte();
num = (byte)(num + num2);
}
byte[] data = dat.ToArray();
outputto.Write(data, 0, (int)data.Length);
CtcomTail tail = new CtcomTail();
tail.checksum = (byte)(num);
tail.writeToStream(outputto);
}
const double SC2RAD = 3.1415926535898; /* semi-circle to radian (IS-GPS) */
public static void writeTrimble55_1(IStreamExtra outputto, piksi.ephemeris_t es, int prn)
{
MemoryStream dat = new MemoryStream();
BinaryWriter bw = new BinaryWriter(dat);
bw.Write((byte)0x02);
bw.Write((byte)0x00);
bw.Write((byte)tcomType.RET_SVDATA);
bw.Write((byte)(180 - 4)); // 4 for header
reverseEndian(bw, (byte)0x1); // Data type
reverseEndian(bw, (byte)prn);
//int tow = (int)time2gpst(es.ttr, ref weekwork);
//int toc = (int)time2gpst(es.toc, ref weekwork);
//int toe = (int)time2gpst(es.toe, ref weekwork);
//if (toe < tow)
{
// weekwork--;
}
//int iodc =
reverseEndian(bw, (ushort)(es.toe.wn));
reverseEndian(bw, (ushort)0); //es.iodc);
reverseEndian(bw, (byte)0); // res
reverseEndian(bw, (byte)0); //es.iode);
reverseEndian(bw, (int)es.toc.tow);
reverseEndian(bw, (int)es.toc.tow);
reverseEndian(bw, (int)es.toe.tow);
reverseEndian(bw, (double)es.tgd); // check scale ex -1.72294676303864e-8
reverseEndian(bw, (double)es.af2);
reverseEndian(bw, (double)es.af1);
reverseEndian(bw, (double)es.af0);
reverseEndian(bw, (double)es.crs);
reverseEndian(bw, (double)es.dn / SC2RAD);
reverseEndian(bw, (double)es.m0 / SC2RAD);
reverseEndian(bw, (double)es.cuc / SC2RAD);
reverseEndian(bw, (double)es.ecc);
reverseEndian(bw, (double)es.cus / SC2RAD);
reverseEndian(bw, (double)es.sqrta); // checkme
reverseEndian(bw, (double)es.cic / SC2RAD);
reverseEndian(bw, (double)es.omega0 / SC2RAD);
reverseEndian(bw, (double)es.cis / SC2RAD);
reverseEndian(bw, (double)es.inc / SC2RAD);
reverseEndian(bw, (double)es.crc);
reverseEndian(bw, (double)es.w / SC2RAD);
reverseEndian(bw, (double)es.omegadot / SC2RAD);
reverseEndian(bw, (double)es.inc_dot / SC2RAD);
uint flags = 0;
flags = flags | (uint)(0 & 1);
flags = flags | (uint)((1 & 3) << 1);
flags = flags | (uint)((1) << 3);
flags = flags | (uint)(((es.healthy - 1) & 63) << 4);
flags = flags | (uint)(((uint)0 & 1) << 10);
flags = flags | (uint)((0 & 15) << 11);
//flags = flags | (uint)((raw.alm[sat - 1].svconf & 7) << 16);
flags = flags | (uint)((0) << 19);
reverseEndian(bw, (int)flags);
// exclude startchar
bw.Seek(1, SeekOrigin.Begin);
int num = 0;
while (dat.Position < dat.Length)
{
byte num2 = (byte)dat.ReadByte();
num = (byte)(num + num2);
}
outputto.Write(dat.ToArray(), 0, (int)dat.Length);
CtcomTail tail = new CtcomTail();
tail.checksum = (byte)num;
tail.writeToStream(outputto);
}
public static void writeTrimbleR17(IStreamExtra outputto, double towinms, List <piksi.msg_obs_content_t> obs)
{
CdatType17 type = new CdatType17
{
tmeasRCVR = towinms,
clockOffset = 0,
nMeasurementSets = 0
};
type.svMeasResize(obs.Count);
for (int i = 0; i < obs.Count; i++)
{
{
double[] azel = new double[2] {
0, 0
};
try
{
// satazel(myposllh, raw2.satxyzscale[raw.data[i].SV], ref azel);
if (azel[0] < 0)
{
azel[0] = 0;
}
if (azel[1] < 0)
{
azel[1] = 0;
}
}
catch { }
type.svMeas[type.nMeasurementSets].prn = (byte)(obs[i].prn + 1);
type.svMeas[type.nMeasurementSets].elevationDeg = (short)((azel[1])); //Convert.ToInt16(this.m_measurement[i].header.elevation);
type.svMeas[type.nMeasurementSets].azimuthDeg = (short)((azel[0])); //Convert.ToInt16(this.m_measurement[i].header.azimuth);
type.svMeas[type.nMeasurementSets].flags1 = 0; // 249
type.svMeas[type.nMeasurementSets].flags2 = 0; // 4
type.svMeas[type.nMeasurementSets].flagStatus = 0;
// l1 only
type.svMeas[type.nMeasurementSets].snCountsL1 = obs[i].snr / piksi.MSG_OBS_SNR_MULTIPLIER;
type.svMeas[type.nMeasurementSets].pseudorange = obs[i].P / piksi.MSG_OBS_P_MULTIPLIER;
type.svMeas[type.nMeasurementSets].phaseL1 = (obs[i].L.Li + (obs[i].L.Lf / 256.0));
// type.svMeas[type.nMeasurementSets].doppler = (type.svMeas[type.nMeasurementSets].phaseL1 - cpold[obs[i].prn]);
cpold[obs[i].prn] = type.svMeas[type.nMeasurementSets].phaseL1;
type.svMeas[type.nMeasurementSets].lidcpr = 0.0;
CdatType17.measT st1 = type.svMeas[type.nMeasurementSets];
st1.flags1 = (byte)(st1.flags1 | 0x40); // l1 data valid
CdatType17.measT st2 = type.svMeas[type.nMeasurementSets];
st2.flags1 = (byte)(st2.flags1 | 0x80); // new positon computed
if (lockcount[obs[i].prn] == obs[i].lock_counter)
{
st2.flags1 = (byte)(st2.flags1 | 0x10); // l1 phase valid
st2.flags1 = (byte)(st2.flags1 | 0x8); // L1 Phase Lock Point
}
lockcount[obs[i].prn] = obs[i].lock_counter;
type.svMeas[type.nMeasurementSets].auxPseudorange = 0.0;
CdatType17.measT st21 = type.svMeas[type.nMeasurementSets];
st21.flagStatus = (byte)(st21.flagStatus | 1);
CdatType17.measT st22 = type.svMeas[type.nMeasurementSets];
st22.flagStatus = (byte)(st22.flagStatus | 2);
type.nMeasurementSets = (byte)(type.nMeasurementSets + 1);
}
}
MemoryStream dat = new MemoryStream();
writeToStream(new BinaryWriter(dat), type);
CtcomTail tail = new CtcomTail();
byte packetsize = 244;
int totalleft = (int)dat.Length;
int loops = (totalleft / packetsize) + 1;
byte sending = 0;
int pageno = 1;
for (int a = 0; a < loops; a++)
{
//trace(3, "%d/%d\n", pageno, loops);
if (totalleft > packetsize)
{
sending = packetsize;
}
else
{
sending = (byte)totalleft;
}
outputto.Write((byte)0x02); // stx
outputto.Write((byte)0x00); // status
outputto.Write((byte)tcomType.RET_RAWDATA); // type
outputto.Write((byte)(sending + 4)); // 4 extra for 4 ytes below
outputto.Write((byte)0); // record type
byte page = Convert.ToByte((int)((pageno << 4) & 240));
page = (byte)(page | Convert.ToByte((int)(loops & 15)));
outputto.Write((byte)page); // page
outputto.Write((byte)trimblerawreply); // replyno
outputto.Write((byte)0); // flags - 0 = expanded and no realtimedata
byte[] packet = dat.ToArray();
outputto.Write(packet, ((int)dat.Length - totalleft), sending);
// single bytes only
tail.checksum = computeChecksum((byte)0x00, (byte)tcomType.RET_RAWDATA, (byte)(sending), (byte)0, (byte)page, (byte)trimblerawreply, (byte)0, dat, ((int)dat.Length - totalleft));
tail.writeToStream(outputto);
pageno++;
totalleft -= sending;
}
trimblerawreply++;
}
