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++; }