bool RawRtcm23::GetMeasurementTime(Frame& f) { if (HourOfWeek < 0 || Week < 0) return Error("Haven't received a time tag yet\n"); // Get the time from the record int zcount = f.GetField(2, 1, 13); int GNSST = f.GetField(3, 5, 24); double ETOM = GNSST/1000000.0 + zcount*.6; debug("RawRtcm23::GetMeasurementTime zcount=%d GNSST=%d ETOM=%.6f\n",zcount,GNSST,ETOM); double ReceiverTime = round(ETOM, .01); // Epochs are at most 100 hz // If we moved backwards, then we missed a clock record if (ReceiverTime < PreviousReceiverTime) { debug("RawRtcm23 - Clock moved backwards prev=%.2f\n", PreviousReceiverTime); HourOfWeek++; if (HourOfWeek >= 7*24) { HourOfWeek = 0; Week++; } } PreviousReceiverTime = ReceiverTime; // calculate the time of week double Tow = HourOfWeek*3600.0 + ReceiverTime; if (EpochTow == -1) EpochTow = Tow; else if (EpochTow != Tow) return Error("Measurement time changed within an epoch"); return OK; }
bool RawRtcm23::ProcessTimeTag(Frame& f) { Week = f.GetField(3, 1, 10); HourOfWeek = f.GetField(3, 11, 18); int Leapsec = f.GetField(3, 19, 24); debug("ProcessTimeTag: Week=%d HourOfWeek=%d LeapSec=%d\n", Week, HourOfWeek, LeapSec); return OK; }
bool RawRtcm23::ProcessEphemeris(Frame& f) { // Figure out which satellite int svid = f.GetField(31, 17, 21); if (svid == 0) svid = 32; int s = SvidToSat(svid); // Make sure we have an xmitted ephemeris (we do, but check anyway) EphemerisXmit& e = *dynamic_cast<EphemerisXmit*>( &(*this)[s] ); if (&e == NULL) return OK; // update the ephemeris with the new frame EphemerisXmitRaw r; f.ToRaw(r); return e.FromRaw(r); }
bool RawRtcm23::ProcessCarrierPhase(Frame& f) { // Get the time of measurement if (GetMeasurementTime(f) != OK) return OK; // We are only interested in L1 for now if (f.GetField(3, 1, 2) != 0) return OK; // Decide how many observables int n = (f.NrWords-3)/2; // Get the data for each observation for (int i=0; i<n; i++) { // We want C/A GPS measurements only (for now) if (f.GetField(4+2*i, 2, 3) != 0) continue; // which satellite? int svid = f.GetField(4+2*i, 4, 8); int Sat = SvidToSat(svid); if (Sat == -1) return Error("RawRtcm23: didn't recognize svid=%d\n", svid); HasPhase[Sat] = true; // Phase int64 phase = (f.GetSigned(4+2*i, 17, 24)<<24) | f.GetField(5+2*i, 1, 24); obs[Sat].Phase = phase / -256.0; obs[Sat].Doppler = 0; // TODO: Handle phase rollover. // for now, it will be caught as an outlier // Quality uint32 DataQual = f.GetField(4+2*i, 9, 11); // Slipped? int32 LossCount = f.GetField(4+2*i, 12, 16); obs[Sat].Slip = (LossCount == CarrierLossCount[Sat]); if (obs[Sat].Slip) CarrierLossCount[Sat] = LossCount; // More? MoreToCome = (f.GetField(4+2*i, 1, 1) != 0); debug("RawRtcm23: Sat=%d Phase=%.3f MoreToComm=%d\n", Sat,obs[Sat].Phase, MoreToCome); } return OK; }
bool RawRtcm23::ProcessPseudorange(Frame& f) { // Get the time of measurement if (GetMeasurementTime(f) != OK) return Error(); // We are only interested in L1 for now if (f.GetField(3, 1, 2) != 00) return OK; // Decide how many observables int n = (f.NrWords-3)/2; // Get the data for each observation for (int i=0; i<n; i++) { // We want C/A GPS measurements only (for now) if (f.GetField(4+2*i, 2, 3) != 0) continue; // which satellite? int svid = f.GetField(4+2*i, 4, 8); if (svid == 0) svid = 32; int Sat = SvidToSat(svid); obs[Sat].Valid = true; // Pseudorange uint32 pr = (f.GetField(4+2*i, 17, 24)<<24) | f.GetField(5+2*i, 1, 24); obs[Sat].PR = pr / 50.0; debug("RawRtcm23::ProcessPseudorange s=%d PR=%.3f pr=0x%08x\n", Sat, obs[Sat].PR, pr); // Quality int DataQual = f.GetField(4+2*i, 9, 12); int Multipath = f.GetField(4+2*i, 13, 16); obs[Sat].SNR = DataQual; // For now. We need to do better. // More? MoreToCome = (f.GetField(4+2*i, 1, 1) != 0); debug("RawRtcm23: Sat=%d PR=%.3f MoreToComm=%d\n", Sat,obs[Sat].PR, MoreToCome); } return OK; }
bool RawRtcm23::ProcessAntennaRef(Frame& f) { int64 x = ((int64)f.GetSigned(3, 1, 24)<<14) | f.GetField(4, 1, 14); int64 y = ((int64)f.GetSigned(4, 17, 24)<<30) | (f.GetField(5, 1, 24)<<6) | f.GetField(6,1,6); int64 z = ((int64)f.GetSigned(6, 9, 24)<<22) | f.GetField(7, 1, 22); int64 height = 0; if (f.GetField(7, 24, 24) != 0) height = f.GetField(8, 1, 18); Pos = Position(x/10000.0, y/10000.0, z/10000.0); AntennaHeight = height/10000.0; debug("ProcessAntennaRef: x=%.3f y=%.3f z=%.3f h=%.3f\n", Pos.x, Pos.y, Pos.z, AntennaHeight); return OK; }
bool RawRtcm23::NextEpoch() { // Clear out our observations for (int s=0; s<MaxSats; s++) obs[s].Valid = HasPhase[s] = false; MoreToCome = true; EpochTow = -1; // repeat until we get measurements without the "more" bits while (MoreToCome) { // Read a frame Frame f; bool slip; if (In.ReadFrame(f, slip) != OK) return Error(); // Get the header information int type = f.GetField(1, 9, 14); if (type == 0) type = 64; int StationId = f.GetField(1, 15, 24); int zcount = f.GetField(2, 1, 13); int seqnr = f.GetField(2, 14, 16); int StationHealth = f.GetField(2, 22, 24); debug("Rtcm23::NextEpoch - type=%d zcount=%d StationId=%d Health=%d seqnr=%d\n", type, zcount, StationId, StationHealth, seqnr); // Update the sequence number slip |= (SequenceNr != seqnr); SequenceNr = (seqnr + 1) & 0x7; // if our communications slipped, ... // TODO. What should we do? We may be missing frames. // don't need to slip the phases since the slip counts handle it. // We should make sure the current epoch is consistent. // Perhaps keep epoch time for each code+phase. // Process the frame according to its type if (type == 14) ProcessTimeTag(f); else if (type == 24) ProcessAntennaRef(f); else if (type == 23) ProcessAntennaTypeDef(f); else if (type == 19) ProcessPseudorange(f); else if (type == 18) ProcessCarrierPhase(f); else if (type == 17) ProcessEphemeris(f); } // valid observations have both code and phase. (NOTE: CHANGE!!!) for (int s=0; s<MaxSats; s++) obs[s].Valid &= HasPhase[s]; // Normalize the results GpsTime = ConvertGpsTime(Week, EpochTow); AdjustToHz(); return OK; }