static void writeToStreammeasT(BinaryWriter outStream, CdatType17.measT thiss)
{
reverseEndian(outStream, thiss.prn);
reverseEndian(outStream, thiss.flags1);
reverseEndian(outStream, thiss.flags2);
reverseEndian(outStream, thiss.flagStatus);
reverseEndian(outStream, thiss.elevationDeg);
reverseEndian(outStream, thiss.azimuthDeg);
if ((thiss.flags1 & 64) != 0)
{
reverseEndian(outStream, thiss.snCountsL1);
reverseEndian(outStream, thiss.pseudorange);
reverseEndian(outStream, thiss.phaseL1);
reverseEndian(outStream, thiss.doppler);
reverseEndian(outStream, thiss.lidcpr);
}
if ((thiss.flags1 & 1) != 0)
{
reverseEndian(outStream, thiss.snCountsL2);
reverseEndian(outStream, thiss.phaseL2);
reverseEndian(outStream, thiss.pCodeL2minusCcodeL1);
if ((thiss.flags2 & 0x20) != 0)
{
reverseEndian(outStream, thiss.auxPseudorange);
}
}
}
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++;
}
