source: ntrip/trunk/BNC/RTCM/RTCM2.cpp@ 1105

Last change on this file since 1105 was 1105, checked in by weber, 14 years ago

* empty log message *

File size: 35.3 KB
Line 
1//------------------------------------------------------------------------------
2//
3// RTCM2.cpp
4//
5// Purpose:
6//
7// Module for extraction of RTCM2 messages
8//
9// References:
10//
11// RTCM 10402.3 Recommended Standards for Differential GNSS (Global
12// Navigation Satellite Systems) Service; RTCM Paper 136-2001/SC104-STD,
13// Version 2.3, 20 Aug. 2001; Radio Technical Commission For Maritime
14// Services, Alexandria, Virgina (2001).
15// ICD-GPS-200; Navstar GPS Space Segment / Navigation User Interfaces;
16// Revison C; 25 Sept. 1997; Arinc Research Corp., El Segundo (1997).
17// Jensen M.; RTCM2ASC Documentation;
18// URL http://kom.aau.dk/~borre/masters/receiver/rtcm2asc.htm;
19// last accessed 17 Sep. 2006
20// Sager J.; Decoder for RTCM SC-104 data from a DGPS beacon receiver;
21// URL http://www.wsrcc.com/wolfgang/ftp/rtcm-0.3.tar.gz;
22// last accessed 17 Sep. 2006
23//
24// Notes:
25//
26// - The host computer is assumed to use little endian (Intel) byte order
27//
28// Last modified:
29//
30// 2006/09/17 OMO Created
31// 2006/09/19 OMO Fixed getHeader() methods
32// 2006/09/21 OMO Reduced phase ambiguity to 2^23 cycles
33// 2006/10/05 OMO Specified const'ness of various member functions
34// 2006/10/13 LMV Fixed resolvedPhase to handle missing C1 range
35// 2006/10/14 LMV Fixed loop cunter in ThirtyBitWord
36// 2006/10/14 LMV Exception handling
37// 2006/10/17 OMO Removed obsolete check of multiple message indicator
38// 2006/10/17 OMO Fixed parity handling
39// 2006/10/18 OMO Improved screening of bad data in RTCM2_Obs::extract
40// 2006/11/25 OMO Revised check for presence of GLONASS data
41// 2007/05/25 GW Round time tag to 100 ms
42// 2007/12/11 AHA Changed handling of C/A- and P-Code on L1
43// 2007/12/13 AHA Changed epoch comparison in packet extraction
44// 2008/03/01 OMO Compilation flag for epoch rounding
45// 2008/03/04 AHA Fixed problems with PRN 32
46// 2008/03/05 AHA Implemeted fix for Trimble 4000SSI receivers
47// 2008/03/07 AHA Major revision of input buffer handling
48// 2008/03/07 AHA Removed unnecessary failure flag
49// 2008/03/10 AHA Corrected extraction of antenna serial number
50// 2008/03/10 AHA Corrected buffer length check in getPacket()
51// 2008/03/11 AHA isGPS-flag in RTCM2_Obs is now set to false on clear()
52// 2008/03/14 AHA Added checks for data consistency in extraction routines
53// 2008/09/01 AHA Harmonization with newest BNC version
54//
55// (c) DLR/GSOC
56//
57//------------------------------------------------------------------------------
58
59#include <bitset>
60#include <cmath>
61#include <fstream>
62#include <iomanip>
63#include <iostream>
64#include <string>
65#include <vector>
66
67#include "RTCM2.h"
68
69// Activate (1) or deactivate (0) debug output for tracing parity errors and
70// undersized packets in get(Unsigned)Bits
71
72#define DEBUG 0
73
74// Activate (1) or deactivate (0) rounding of measurement epochs to 100ms
75//
76// Note: A need to round the measurement epoch to integer tenths of a second was
77// noted by BKG in the processing of RTCM2 data from various receivers in NTRIP
78// real-time networks. It is unclear at present, whether this is due to an
79// improper implementation of the RTCM2 standard in the respective receivers
80// or an unclear formulation of the standard.
81
82#define ROUND_EPOCH 1
83
84// Fix for data streams originating from TRIMBLE_4000SSI receivers.
85// GPS PRN32 is erroneously flagged as GLONASS satellite in the C/A
86// pseudorange messages. We therefore use a majority voting to
87// determine the true constellation for this message.
88// This fix is only required for Trimble4000SSI receivers but can also
89// be used with all other known receivers.
90
91#define FIX_TRIMBLE_4000SSI 1
92
93using namespace std;
94
95
96// GPS constants
97
98const double c_light = 299792458.0; // Speed of light [m/s]; IAU 1976
99const double f_L1 = 1575.42e6; // L1 frequency [Hz] (10.23MHz*154)
100const double f_L2 = 1227.60e6; // L2 frequency [Hz] (10.23MHz*120)
101
102const double lambda_L1 = c_light/f_L1; // L1 wavelength [m] (0.1903m)
103const double lambda_L2 = c_light/f_L2; // L2 wavelength [m]
104
105//
106// Bits for message availability checks
107//
108
109const int bit_L1rngGPS = 0;
110const int bit_L2rngGPS = 1;
111const int bit_L1cphGPS = 2;
112const int bit_L2cphGPS = 3;
113const int bit_L1rngGLO = 4;
114const int bit_L2rngGLO = 5;
115const int bit_L1cphGLO = 6;
116const int bit_L2cphGLO = 7;
117
118
119//
120// namespace rtcm2
121//
122
123namespace rtcm2 {
124
125//------------------------------------------------------------------------------
126//
127// class ThirtyBitWord (implementation)
128//
129// Purpose:
130//
131// Handling of RTCM2 30bit words
132//
133//------------------------------------------------------------------------------
134
135// Constructor
136
137ThirtyBitWord::ThirtyBitWord() : W(0) {
138};
139
140// Clear entire 30-bit word and 2-bit parity from previous word
141
142void ThirtyBitWord::clear() {
143 W = 0;
144};
145
146// Parity check
147
148bool ThirtyBitWord::validParity() const {
149
150 // Parity stuff
151
152 static const unsigned int PARITY_25 = 0xBB1F3480;
153 static const unsigned int PARITY_26 = 0x5D8F9A40;
154 static const unsigned int PARITY_27 = 0xAEC7CD00;
155 static const unsigned int PARITY_28 = 0x5763E680;
156 static const unsigned int PARITY_29 = 0x6BB1F340;
157 static const unsigned int PARITY_30 = 0x8B7A89C0;
158
159 // Look-up table for parity of eight bit bytes
160 // (parity=0 if the number of 0s and 1s is equal, else parity=1)
161 static unsigned char byteParity[] = {
162 0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,
163 1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,
164 1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,
165 0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,
166 1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,
167 0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,
168 0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,
169 1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0
170 };
171
172 // Local variables
173
174 unsigned int t, w, p;
175
176 // The sign of the data is determined by the D30* parity bit
177 // of the previous data word. If D30* is set, invert the data
178 // bits D01..D24 to obtain the d01..d24 (but leave all other
179 // bits untouched).
180
181 w = W;
182 if ( w & 0x40000000 ) w ^= 0x3FFFFFC0;
183
184 // Compute the parity of the sign corrected data bits d01..d24
185 // as described in the ICD-GPS-200
186
187 t = w & PARITY_25;
188 p = ( byteParity[t &0xff] ^ byteParity[(t>> 8)&0xff] ^
189 byteParity[(t>>16)&0xff] ^ byteParity[(t>>24) ] );
190
191 t = w & PARITY_26;
192 p = (p<<1) |
193 ( byteParity[t &0xff] ^ byteParity[(t>> 8)&0xff] ^
194 byteParity[(t>>16)&0xff] ^ byteParity[(t>>24) ] );
195
196 t = w & PARITY_27;
197 p = (p<<1) |
198 ( byteParity[t &0xff] ^ byteParity[(t>> 8)&0xff] ^
199 byteParity[(t>>16)&0xff] ^ byteParity[(t>>24) ] );
200
201 t = w & PARITY_28;
202 p = (p<<1) |
203 ( byteParity[t &0xff] ^ byteParity[(t>> 8)&0xff] ^
204 byteParity[(t>>16)&0xff] ^ byteParity[(t>>24) ] );
205
206 t = w & PARITY_29;
207 p = (p<<1) |
208 ( byteParity[t &0xff] ^ byteParity[(t>> 8)&0xff] ^
209 byteParity[(t>>16)&0xff] ^ byteParity[(t>>24) ] );
210
211 t = w & PARITY_30;
212 p = (p<<1) |
213 ( byteParity[t &0xff] ^ byteParity[(t>> 8)&0xff] ^
214 byteParity[(t>>16)&0xff] ^ byteParity[(t>>24) ] );
215
216 return ( (W & 0x3f) == p);
217
218};
219
220
221// Check preamble
222
223bool ThirtyBitWord::isHeader() const {
224
225 const unsigned char Preamble = 0x66;
226
227 unsigned char b = (value()>>22) & 0xFF;
228
229 return ( b==Preamble );
230
231};
232
233
234// Return entire 32-bit (current word and previous parity)
235
236unsigned int ThirtyBitWord::all() const {
237 return W;
238};
239
240
241// Return sign-corrected 30-bit (or zero if parity mismatch)
242
243unsigned int ThirtyBitWord::value() const {
244
245 unsigned int w = W;
246
247 if (validParity()) {
248 // Return data and current parity bits. Invert data bits if D30*
249 // is set and discard old parity bits.
250 if ( w & 0x40000000 ) w ^= 0x3FFFFFC0;
251 return (w & 0x3FFFFFFF);
252 }
253 else {
254 // Error; invalid parity
255 return 0;
256 };
257
258};
259
260
261// Append a byte with six data bits
262
263void ThirtyBitWord::append(unsigned char b) {
264
265 // Look up table for swap (left-right) of 6 data bits
266 static const unsigned char
267 swap[] = {
268 0,32,16,48, 8,40,24,56, 4,36,20,52,12,44,28,60,
269 2,34,18,50,10,42,26,58, 6,38,22,54,14,46,30,62,
270 1,33,17,49, 9,41,25,57, 5,37,21,53,13,45,29,61,
271 3,35,19,51,11,43,27,59, 7,39,23,55,15,47,31,63
272 };
273
274 // Bits 7 and 6 (of 0..7) must be "01" for valid data bytes
275 if ( (b & 0x40) != 0x40 ) {
276 // We simply skip the invalid input byte and leave the word unchanged
277#if (DEBUG>0)
278 cerr << "Error in append()" << bitset<32>(all()) << endl;
279#endif
280 return;
281 };
282
283 // Swap bits 0..5 to restore proper bit order for 30bit words
284 b = swap[ b & 0x3f];
285
286 // Fill word
287 W = ( (W <<6) | (b & 0x3f) ) ;
288
289};
290
291
292// Get next 30bit word from string
293
294void ThirtyBitWord::get(const string& buf) {
295
296 // Check if string is long enough
297
298 if (buf.size()<5) {
299 // Ignore; users should avoid this case prior to calling get()
300
301#if ( DEBUG > 0 )
302 cerr << "Error in get(): packet too short (" << buf.size() <<")" << endl;
303#endif
304
305 return;
306 };
307
308 // Process 5 bytes
309
310 for (int i=0; i<5; i++) append(buf[i]);
311
312#if (DEBUG>0)
313 if (!validParity()) {
314 cerr << "Parity error in get()"
315 << bitset<32>(all()) << endl;
316 };
317#endif
318
319};
320
321// Get next 30bit word from file
322
323void ThirtyBitWord::get(istream& inp) {
324
325 unsigned char b;
326
327 for (int i=0; i<5; i++) {
328 inp >> b;
329 if (inp.fail()) { clear(); return; };
330 append(b);
331 };
332
333#if (DEBUG>0)
334 if (!validParity()) {
335 cerr << "Parity error in get()"
336 << bitset<32>(all()) << endl;
337 };
338#endif
339
340};
341
342// Get next header word from string
343
344void ThirtyBitWord::getHeader(string& buf) {
345
346 const unsigned int wordLen = 5; // Number of bytes representing a 30-bit word
347 const unsigned int spare = 1; // Number of spare words for resync of parity
348 // (same value as inRTCM2packet::getPacket())
349 unsigned int i;
350
351 i=0;
352 // append spare word (to get correct parity) and first consecutive word
353 while (i<(spare+1)*wordLen) {
354 // Process byte
355 append(buf[i]);
356 // Increment count
357 i++;
358 };
359
360 // start searching for preamble in first word after spare word
361 while (!isHeader() && i<buf.size() ) {
362 // Process byte
363 append(buf[i]);
364 // Increment count
365 i++;
366 };
367
368 // Remove processed bytes from buffer. Retain also the previous word to
369 // allow a resync if getHeader() is called repeatedly on the same buffer.
370 if (i>=(1+spare)*wordLen) buf.erase(0,i-(1+spare)*wordLen);
371
372#if (DEBUG>0)
373 if (!validParity()) {
374 cerr << "Parity error in getHeader()"
375 << bitset<32>(all()) << endl;
376 };
377#endif
378
379};
380
381// Get next header word from file
382
383void ThirtyBitWord::getHeader(istream& inp) {
384
385 unsigned char b;
386 unsigned int i;
387
388 i=0;
389 while ( !isHeader() || i<5 ) {
390 inp >> b;
391 if (inp.fail()) { clear(); return; };
392 append(b); i++;
393 };
394
395#if (DEBUG>0)
396 if (!validParity()) {
397 cerr << "Parity error in getHeader()"
398 << bitset<32>(all()) << endl;
399 };
400#endif
401
402};
403
404
405//------------------------------------------------------------------------------
406//
407// RTCM2packet (class implementation)
408//
409// Purpose:
410//
411// A class for handling RTCM2 data packets
412//
413//------------------------------------------------------------------------------
414
415// Constructor
416
417RTCM2packet::RTCM2packet() {
418 clear();
419};
420
421// Initialization
422
423void RTCM2packet::clear() {
424
425 W.clear();
426
427 H1=0;
428 H2=0;
429
430 DW.resize(0,0);
431
432};
433
434// Complete packet, valid parity
435
436bool RTCM2packet::valid() const {
437
438 // The methods for creating a packet (get,">>") ensure
439 // that a packet has a consistent number of data words
440 // and a valid parity in all header and data words.
441 // Therefore a packet is either empty or valid.
442
443 return (H1!=0);
444
445};
446
447
448//
449// Gets the next packet from the buffer
450//
451
452void RTCM2packet::getPacket(std::string& buf) {
453
454 const int wordLen = 5; // Number of bytes representing a 30-bit word
455 const int spare = 1; // Number of spare words for resync of parity
456 // (same value as used in ThirtyBitWord::getHeader)
457 unsigned int n;
458
459 // Does the package content at least spare bytes and first header byte?
460 if (buf.size()<(spare+1)*wordLen) {
461 clear();
462 return;
463 };
464
465 // Try to read a full packet. Processed bytes are removed from the input
466 // buffer except for the latest spare*wordLen bytes to restore the parity
467 // bytes upon subseqeunt calls of getPacket().
468
469 // Locate and read the first header word
470 W.getHeader(buf);
471 if (!W.isHeader()) {
472 // No header found; try again next time. buf retains only the spare
473 // words. The packet contents is cleared to indicate an unsuccessful
474 // termination of getPacket().
475 clear();
476
477#if ( DEBUG > 0 )
478 cerr << "Error in getPacket(): W.isHeader() = false for H1" << endl;
479#endif
480
481 return;
482 };
483 H1 = W.value();
484
485 // Do we have enough bytes to read the next word? If not, the packet
486 // contents is cleared to indicate an unsuccessful termination. The
487 // previously read spare and header bytes are retained in the buffer
488 // for use in the next call of getPacket().
489 if (buf.size()<(spare+2)*wordLen) {
490 clear();
491
492#if ( DEBUG > 0 )
493 cerr << "Error in getPacket(): buffer too short for complete H2" << endl;
494#endif
495
496 return;
497 };
498
499 // Read the second header word
500 W.get(buf.substr((spare+1)*wordLen,buf.size()-(spare+1)*wordLen));
501 H2 = W.value();
502 if (!W.validParity()) {
503 // Invalid H2 word; delete first buffer byte and try to resynch next time.
504 // The packet contents is cleared to indicate an unsuccessful termination.
505 clear();
506 buf.erase(0,1);
507
508#if ( DEBUG > 0 )
509 cerr << "Error in getPacket(): W.validParity() = false for H2" << endl;
510#endif
511
512 return;
513 };
514
515 n = nDataWords();
516
517 // Do we have enough bytes to read the next word? If not, the packet
518 // contents is cleared to indicate an unsuccessful termination. The
519 // previously read spare and header bytes are retained in the buffer
520 // for use in the next call of getPacket().
521 if (buf.size()<(spare+2+n)*wordLen) {
522 clear();
523
524#if ( DEBUG > 0 )
525 cerr << "Error in getPacket(): buffer too short for complete " << n
526 << " DWs" << endl;
527#endif
528
529 return;
530 };
531
532 DW.resize(n);
533 for (unsigned int i=0; i<n; i++) {
534 W.get(buf.substr((spare+2+i)*wordLen,buf.size()-(spare+2+i)*wordLen));
535 DW[i] = W.value();
536 if (!W.validParity()) {
537 // Invalid data word; delete first byte and try to resynch next time.
538 // The packet contents is cleared to indicate an unsuccessful termination.
539 clear();
540 buf.erase(0,1);
541
542#if ( DEBUG > 0 )
543 cerr << "Error in getPacket(): W.validParity() = false for DW"
544 << i << endl;
545#endif
546
547 return;
548 };
549 };
550
551 // Successful packet extraction; delete total number of message bytes
552 // from buffer.
553 // Note: a total of "spare" words remain in the buffer to enable a
554 // parity resynchronization when searching the next header.
555
556 buf.erase(0,(n+2)*wordLen);
557
558 return;
559
560};
561
562
563//
564// Gets the next packet from the input stream
565//
566
567void RTCM2packet::getPacket(std::istream& inp) {
568
569 int n;
570
571 W.getHeader(inp);
572 H1 = W.value();
573 if (inp.fail() || !W.isHeader()) { clear(); return; }
574
575 W.get(inp);
576 H2 = W.value();
577 if (inp.fail() || !W.validParity()) { clear(); return; }
578
579 n = nDataWords();
580 DW.resize(n);
581 for (int i=0; i<n; i++) {
582 W.get(inp);
583 DW[i] = W.value();
584 if (inp.fail() || !W.validParity()) { clear(); return; }
585 };
586
587 return;
588
589};
590
591//
592// Input operator
593//
594// Reads an RTCM2 packet from the input stream.
595//
596
597istream& operator >> (istream& is, RTCM2packet& p) {
598
599 p.getPacket(is);
600
601 return is;
602
603};
604
605// Access methods
606
607unsigned int RTCM2packet::header1() const {
608 return H1;
609};
610
611unsigned int RTCM2packet::header2() const {
612 return H2;
613};
614
615unsigned int RTCM2packet::dataWord(int i) const {
616 if ( (unsigned int)i < DW.size() ) {
617 return DW[i];
618 }
619 else {
620 return 0;
621 }
622};
623
624unsigned int RTCM2packet::msgType() const {
625 return ( H1>>16 & 0x003F );
626};
627
628unsigned int RTCM2packet::stationID() const {
629 return ( H1>> 6 & 0x03FF );
630};
631
632unsigned int RTCM2packet::modZCount() const {
633 return ( H2>>17 & 0x01FFF );
634};
635
636unsigned int RTCM2packet::seqNumber() const {
637 return ( H2>>14 & 0x0007 );
638};
639
640unsigned int RTCM2packet::nDataWords() const {
641 return ( H2>> 9 & 0x001F );
642};
643
644unsigned int RTCM2packet::staHealth() const {
645 return ( H2>> 6 & 0x0003 );
646};
647
648
649//
650// Get unsigned bit field
651//
652// Bits are numbered from left (msb) to right (lsb) starting at bit 0
653//
654
655unsigned int RTCM2packet::getUnsignedBits ( unsigned int start,
656 unsigned int n ) const {
657
658 unsigned int iFirst = start/24; // Index of first data word
659 unsigned int iLast = (start+n-1)/24; // Index of last data word
660 unsigned int bitField = 0;
661 unsigned int tmp;
662
663 // Checks
664
665 if (n>32) {
666 throw("Error: can't handle >32 bits in RTCM2packet::getUnsignedBits");
667 };
668
669 if ( 24*DW.size() < start+n-1 ) {
670#if (DEBUG>0)
671 cerr << "Debug output RTCM2packet::getUnsignedBits" << endl
672 << " P.msgType: " << setw(5) << msgType() << endl
673 << " P.nDataWords: " << setw(5) << nDataWords() << endl
674 << " start: " << setw(5) << start << endl
675 << " n: " << setw(5) << n << endl
676 << " P.H1: " << setw(5) << bitset<32>(H1) << endl
677 << " P.H2: " << setw(5) << bitset<32>(H2) << endl
678 << endl
679 << flush;
680#endif
681 throw("Error: Packet too short in RTCM2packet::getUnsignedBits");
682 }
683
684 // Handle initial data word
685 // Get all data bits. Strip parity and unwanted leading bits.
686 // Store result in 24 lsb bits of tmp.
687
688 tmp = (DW[iFirst]>>6) & 0xFFFFFF;
689 tmp = ( ( tmp << start%24) & 0xFFFFFF ) >> start%24 ;
690
691 // Handle central data word
692
693 if ( iFirst<iLast ) {
694 bitField = tmp;
695 for (unsigned int iWord=iFirst+1; iWord<iLast; iWord++) {
696 tmp = (DW[iWord]>>6) & 0xFFFFFF;
697 bitField = (bitField << 24) | tmp;
698 };
699 tmp = (DW[iLast]>>6) & 0xFFFFFF;
700 };
701
702 // Handle last data word
703
704 tmp = tmp >> (23-(start+n-1)%24);
705 bitField = (bitField << ((start+n-1)%24+1)) | tmp;
706
707 // Done
708
709 return bitField;
710
711};
712
713//
714// Get signed bit field
715//
716// Bits are numbered from left (msb) to right (lsb) starting at bit 0
717//
718
719int RTCM2packet::getBits ( unsigned int start,
720 unsigned int n ) const {
721
722
723 // Checks
724
725 if (n>32) {
726 throw("Error: can't handle >32 bits in RTCM2packet::getBits");
727 };
728
729 if ( 24*DW.size() < start+n-1 ) {
730#if (DEBUG>0)
731 cerr << "Debug output RTCM2packet::getUnsignedBits" << endl
732 << " P.msgType: " << setw(5) << msgType() << endl
733 << " P.nDataWords: " << setw(5) << nDataWords() << endl
734 << " start: " << setw(5) << start << endl
735 << " n: " << setw(5) << n << endl
736 << " P.H1: " << setw(5) << bitset<32>(H1) << endl
737 << " P.H2: " << setw(5) << bitset<32>(H2) << endl
738 << endl
739 << flush;
740#endif
741 throw("Error: Packet too short in RTCM2packet::getBits");
742 }
743
744 return ((int)(getUnsignedBits(start,n)<<(32-n))>>(32-n));
745
746};
747
748
749//------------------------------------------------------------------------------
750//
751// RTCM2_03 (class implementation)
752//
753// Purpose:
754//
755// A class for handling RTCM 2 GPS Reference Station Parameters messages
756//
757//------------------------------------------------------------------------------
758
759// Constructor
760RTCM2_03::RTCM2_03(){
761 validMsg = false;
762 x = 0.0;
763 y = 0.0;
764 z=0.0;
765};
766
767void RTCM2_03::extract(const RTCM2packet& P) {
768
769 // Check validity, packet type and number of data words
770
771 validMsg = (P.valid());
772 if (!validMsg) return;
773
774 validMsg = (P.ID()==03);
775 if (!validMsg) return;
776
777 validMsg = (P.nDataWords()==4);
778 if (!validMsg) return;
779
780 // Antenna reference point coordinates
781
782 x = P.getBits( 0,32)*0.01; // X [m]
783 y = P.getBits(32,32)*0.01; // Y [m]
784 z = P.getBits(64,32)*0.01; // Z [m]
785
786};
787
788//------------------------------------------------------------------------------
789//
790// RTCM2_23 (class implementation)
791//
792// Purpose:
793//
794// A class for handling RTCM 2 Antenna Type Definition messages
795//
796//------------------------------------------------------------------------------
797
798void RTCM2_23::extract(const RTCM2packet& P) {
799
800 unsigned int nad, nas;
801
802 const unsigned int nF1 = 8; // bits in first field (R,AF,SF,NAD)
803 const unsigned int nF2 =16; // bits in second field (SETUP ID,R,NAS)
804 const unsigned int nBits=24; // data bits in 30bit word
805
806 // Check validity, packet type and number of data words
807
808 validMsg = (P.valid());
809 if (!validMsg) return;
810
811 validMsg = (P.ID()==23);
812 if (!validMsg) return;
813
814 // Check number of data words (can nad be read in?)
815
816 validMsg = (P.nDataWords()>=1);
817 if (!validMsg){
818 cerr << "RTCM2_23::extract: P.nDataWords()>=1" << endl;
819 return;
820 }
821
822 // Antenna descriptor
823 antType = "";
824 nad = P.getUnsignedBits(3,5);
825
826 // Check number of data words (can antenna description be read in?)
827 validMsg = ( P.nDataWords() >=
828 (unsigned int)ceil((nF1+nad*8)/(double)nBits) );
829
830 if (!validMsg) return;
831
832 for (unsigned int i=0;i<nad;i++)
833 antType += (char)P.getUnsignedBits(nF1+i*8,8);
834
835 // Optional antenna serial numbers
836 if (P.getUnsignedBits(2,1)==1) {
837
838 // Check number of data words (can nas be read in?)
839
840 validMsg = ( P.nDataWords() >=
841 (unsigned int)ceil((nF1+nad*8+nF2)/(double)nBits) );
842 if (!validMsg) return;
843
844 nas = P.getUnsignedBits(19+8*nad,5);
845
846 // Check number of data words (can antenna serial number be read in?)
847
848 validMsg = ( P.nDataWords() >=
849 (unsigned int)ceil((nF1+nad*8+nF2+nas*8)/(double)nBits) );
850 if (!validMsg) return;
851
852 antSN = "";
853 for (unsigned int i=0;i<nas;i++)
854 antSN += (char)P.getUnsignedBits(nF1+8*nad+nF2+i*8,8);
855 };
856
857};
858
859
860//------------------------------------------------------------------------------
861//
862// RTCM2_24 (class implementation)
863//
864// Purpose:
865//
866// A class for handling RTCM 2 Reference Station Antenna
867// Reference Point Parameter messages
868//
869//------------------------------------------------------------------------------
870
871void RTCM2_24::extract(const RTCM2packet& P) {
872
873 double dx,dy,dz;
874
875 // Check validity, packet type and number of data words
876
877 validMsg = (P.valid());
878 if (!validMsg) return;
879
880 validMsg = (P.ID()==24);
881 if (!validMsg) return;
882
883 validMsg = (P.nDataWords()==6);
884 if (!validMsg) return;
885
886 // System indicator
887
888 isGPS = (P.getUnsignedBits(118,1)==0);
889 isGLONASS = (P.getUnsignedBits(118,1)==1);
890
891 // Antenna reference point coordinates
892
893 x = 64.0*P.getBits( 0,32);
894 y = 64.0*P.getBits(40,32);
895 z = 64.0*P.getBits(80,32);
896 dx = P.getUnsignedBits( 32,6);
897 dy = P.getUnsignedBits( 72,6);
898 dz = P.getUnsignedBits(112,6);
899 x = 0.0001*( x + (x<0? -dx:+dx) );
900 y = 0.0001*( y + (y<0? -dy:+dy) );
901 z = 0.0001*( z + (z<0? -dz:+dz) );
902
903 // Antenna Height
904
905 if (P.getUnsignedBits(119,1)==1) {
906 h= P.getUnsignedBits(120,18)*0.0001;
907 };
908
909
910};
911
912
913//------------------------------------------------------------------------------
914//
915// RTCM2_Obs (class definition)
916//
917// Purpose:
918//
919// A class for handling blocks of RTCM2 18 & 19 packets that need to be
920// combined to get a complete set of measurements
921//
922// Notes:
923//
924// The class collects L1/L2 code and phase measurements for GPS and GLONASS.
925// Since the Multiple Message Indicator is inconsistently handled by various
926// receivers we simply require code and phase on L1 and L2 for a complete
927// set ob observations at a given epoch. GLONASS observations are optional,
928// but all four types (code+phase,L1+L2) must be provided, if at least one
929// is given. Also, the GLONASS message must follow the corresponding GPS
930// message.
931//
932//------------------------------------------------------------------------------
933
934// Constructor
935
936RTCM2_Obs::RTCM2_Obs() {
937
938 clear();
939
940};
941
942// Reset entire block
943
944void RTCM2_Obs::clear() {
945
946 GPSonly = true;
947
948 secs=0.0; // Seconds of hour (GPS time)
949 nSat=0; // Number of space vehicles
950 PRN.resize(0); // space vehicles
951 rng_C1.resize(0); // Pseudorange [m]
952 rng_P1.resize(0); // Pseudorange [m]
953 rng_P2.resize(0); // Pseudorange [m]
954 cph_L1.resize(0); // Carrier phase [m]
955 cph_L2.resize(0); // Carrier phase [m]
956 slip_L1.resize(0); // Slip counter
957 slip_L2.resize(0); // Slip counter
958
959 availability.reset(); // Message status flags
960
961};
962
963// Availability checks
964
965bool RTCM2_Obs::anyGPS() const {
966
967 return availability.test(bit_L1rngGPS) ||
968 availability.test(bit_L2rngGPS) ||
969 availability.test(bit_L1cphGPS) ||
970 availability.test(bit_L2cphGPS);
971
972};
973
974bool RTCM2_Obs::anyGLONASS() const {
975
976 return availability.test(bit_L1rngGLO) ||
977 availability.test(bit_L2rngGLO) ||
978 availability.test(bit_L1cphGLO) ||
979 availability.test(bit_L2cphGLO);
980
981};
982
983bool RTCM2_Obs::allGPS() const {
984
985 return availability.test(bit_L1rngGPS) &&
986 availability.test(bit_L2rngGPS) &&
987 availability.test(bit_L1cphGPS) &&
988 availability.test(bit_L2cphGPS);
989
990};
991
992bool RTCM2_Obs::allGLONASS() const {
993
994 return availability.test(bit_L1rngGLO) &&
995 availability.test(bit_L2rngGLO) &&
996 availability.test(bit_L1cphGLO) &&
997 availability.test(bit_L2cphGLO);
998
999};
1000
1001// Validity
1002
1003bool RTCM2_Obs::valid() const {
1004
1005 return ( allGPS() && ( GPSonly || allGLONASS() ) );
1006
1007};
1008
1009
1010//
1011// Extract RTCM2 18 & 19 messages and store relevant data for future use
1012//
1013
1014void RTCM2_Obs::extract(const RTCM2packet& P) {
1015
1016 bool isGPS,isCAcode,isL1,isOth;
1017 int NSat,idx;
1018 int sid,prn,slip_cnt;
1019 double t,rng,cph;
1020
1021 // Check validity and packet type
1022
1023 if ( ! ( P.valid() &&
1024 (P.ID()==18 || P.ID()==19) ) ) return;
1025
1026 // Check number of data words, message starts with 1 DW for epoch, then each
1027 // satellite brings 2 DW,
1028 // Do not start decoding if less than 3 DW are in package
1029
1030 if ( P.nDataWords()<3 ) {
1031#if ( DEBUG > 0 )
1032 cerr << "Error in RTCM2_Obs::extract(): less than 3 DW ("
1033 << P.nDataWords() << ") detected" << endl;
1034#endif
1035
1036 return;
1037 };
1038
1039 // Check if number of data words is odd number
1040
1041 if ( P.nDataWords()%2==0 ){
1042#if ( DEBUG > 0 )
1043 cerr << "Error in RTCM2_Obs::extract(): odd number of DW ("
1044 << P.nDataWords() << ") detected" << endl;
1045#endif
1046
1047 return;
1048 };
1049
1050 // Clear previous data if block was already complete
1051
1052 if (valid()) clear();
1053
1054 // Process carrier phase message
1055
1056 if ( P.ID()==18 ) {
1057
1058 // Number of satellites in current message
1059 NSat = (P.nDataWords()-1)/2;
1060
1061 // Current epoch (mod 3600 sec)
1062 t = 0.6*P.modZCount()
1063 + P.getUnsignedBits(4,20)*1.0e-6;
1064
1065#if (ROUND_EPOCH==1)
1066 // SC-104 V2.3 4-42 Note 1 4. Assume measurements at hard edges
1067 // of receiver clock with minimum divisions of 10ms
1068 // and clock error less then recommended 1.1ms
1069 // Hence, round time tag to 100 ms
1070 t = floor(t*100.0+0.5)/100.0;
1071#endif
1072
1073 // Frequency (exit if neither L1 nor L2)
1074 isL1 = ( P.getUnsignedBits(0,1)==0 );
1075 isOth = ( P.getUnsignedBits(1,1)==1 );
1076 if (isOth) return;
1077
1078 // Constellation (for first satellite in message)
1079 isGPS = ( P.getUnsignedBits(26,1)==0 );
1080 GPSonly = GPSonly && isGPS;
1081
1082 // Multiple Message Indicator (only checked for first satellite)
1083 // pendingMsg = ( P.getUnsignedBits(24,1)==1 );
1084
1085 // Handle epoch: store epoch of first GPS message and
1086 // check consistency of subsequent messages. GLONASS time tags
1087 // are different and have to be ignored
1088 if (isGPS) {
1089 if ( nSat==0 ) {
1090 secs = t; // Store epoch
1091 }
1092// else if (t!=secs) {
1093 else if (abs(t-secs)>1e-6) {
1094 clear(); secs = t; // Clear all data, then store epoch
1095 };
1096 };
1097
1098 // Discard GLONASS observations if no prior GPS observations
1099 // are available
1100 if (!isGPS && !anyGPS() ) return;
1101
1102 // Set availability flags
1103
1104 if ( isL1 && isGPS) availability.set(bit_L1cphGPS);
1105 if (!isL1 && isGPS) availability.set(bit_L2cphGPS);
1106 if ( isL1 && !isGPS) availability.set(bit_L1cphGLO);
1107 if (!isL1 && !isGPS) availability.set(bit_L2cphGLO);
1108
1109#if ( DEBUG > 0 )
1110 cerr << "RTCM2_Obs::extract(): availability "
1111 << bitset<8>(availability) << endl;
1112#endif
1113
1114
1115 // Process all satellites
1116
1117 for (int iSat=0;iSat<NSat;iSat++){
1118
1119 // Code type
1120 isCAcode = ( P.getUnsignedBits(iSat*48+25,1)==0 );
1121
1122 // Satellite
1123 sid = P.getUnsignedBits(iSat*48+27,5);
1124 if (sid==0) sid=32;
1125
1126 prn = (isGPS? sid : sid+200 );
1127
1128 // Carrier phase measurement (mod 2^23 [cy]; sign matched to range)
1129 cph = -P.getBits(iSat*48+40,32)/256.0;
1130
1131 // Slip counter
1132 slip_cnt = P.getUnsignedBits(iSat*48+35,5);
1133
1134 // Is this a new PRN?
1135 idx=-1;
1136 for (unsigned int i=0;i<PRN.size();i++) {
1137 if (PRN[i]==prn) { idx=i; break; };
1138 };
1139 if (idx==-1) {
1140 // Insert new sat at end of list
1141 nSat++; idx = nSat-1;
1142 PRN.push_back(prn);
1143 rng_C1.push_back(0.0);
1144 rng_P1.push_back(0.0);
1145 rng_P2.push_back(0.0);
1146 cph_L1.push_back(0.0);
1147 cph_L2.push_back(0.0);
1148 slip_L1.push_back(-1);
1149 slip_L2.push_back(-1);
1150 };
1151
1152 // Store measurement
1153 if (isL1) {
1154 cph_L1[idx] = cph;
1155 slip_L1[idx] = slip_cnt;
1156 }
1157 else {
1158 cph_L2[idx] = cph;
1159 slip_L2[idx] = slip_cnt;
1160 };
1161
1162 };
1163
1164 };
1165
1166
1167 // Process pseudorange message
1168
1169 if ( P.ID()==19 ) {
1170
1171 // Number of satellites in current message
1172 NSat = (P.nDataWords()-1)/2;
1173
1174 // Current epoch (mod 3600 sec)
1175 t = 0.6*P.modZCount()
1176 + P.getUnsignedBits(4,20)*1.0e-6;
1177
1178#if (ROUND_EPOCH==1)
1179 // SC-104 V2.3 4-42 Note 1 4. Assume measurements at hard edges
1180 // of receiver clock with minimum divisions of 10ms
1181 // and clock error less then recommended 1.1ms
1182 // Hence, round time tag to 100 ms
1183 t = floor(t*100.0+0.5)/100.0;
1184#endif
1185
1186 // Frequency (exit if neither L1 nor L2)
1187 isL1 = ( P.getUnsignedBits(0,1)==0 );
1188 isOth = ( P.getUnsignedBits(1,1)==1 );
1189 if (isOth) return;
1190
1191#if (FIX_TRIMBLE_4000SSI==1)
1192 // Fix for data streams originating from TRIMBLE_4000SSI receivers.
1193 // GPS PRN32 is erroneously flagged as GLONASS satellite in the C/A
1194 // pseudorange messages. We therefore use a majority voting to
1195 // determine the true constellation for this message.
1196 // This fix is only required for Trimble4000SSI receivers but can also
1197 // be used with all other known receivers.
1198 int nGPS=0;
1199 for(int iSat=0; iSat<NSat; iSat++){
1200 // Constellation (for each satellite in message)
1201 isGPS = ( P.getUnsignedBits(iSat*48+26,1)==0 );
1202 if(isGPS) nGPS++;
1203 };
1204 isGPS = (2*nGPS>NSat);
1205#else
1206 // Constellation (for first satellite in message)
1207 isGPS = ( P.getUnsignedBits(26,1)==0 );
1208#endif
1209 GPSonly = GPSonly && isGPS;
1210
1211 // Multiple Message Indicator (only checked for first satellite)
1212 // pendingMsg = ( P.getUnsignedBits(24,1)==1 );
1213
1214 // Handle epoch: store epoch of first GPS message and
1215 // check consistency of subsequent messages. GLONASS time tags
1216 // are different and have to be ignored
1217 if (isGPS) {
1218 if ( nSat==0 ) {
1219 secs = t; // Store epoch
1220 }
1221// else if (t!=secs) {
1222 else if (abs(t-secs)>1e-6) {
1223 clear(); secs = t; // Clear all data, then store epoch
1224 };
1225 };
1226
1227 // Discard GLONASS observations if no prior GPS observations
1228 // are available
1229 if (!isGPS && !anyGPS() ) return;
1230
1231 // Set availability flags
1232 if ( isL1 && isGPS) availability.set(bit_L1rngGPS);
1233 if (!isL1 && isGPS) availability.set(bit_L2rngGPS);
1234 if ( isL1 && !isGPS) availability.set(bit_L1rngGLO);
1235 if (!isL1 && !isGPS) availability.set(bit_L2rngGLO);
1236
1237#if ( DEBUG > 0 )
1238 cerr << "RTCM2_Obs::extract(): availability "
1239 << bitset<8>(availability) << endl;
1240#endif
1241
1242 // Process all satellites
1243
1244 for (int iSat=0;iSat<NSat;iSat++){
1245
1246 // Code type
1247 isCAcode = ( P.getUnsignedBits(iSat*48+25,1)==0 );
1248
1249 // Satellite
1250 sid = P.getUnsignedBits(iSat*48+27,5);
1251 if (sid==0) sid=32;
1252 prn = (isGPS? sid : sid+200 );
1253
1254 // Pseudorange measurement [m]
1255 rng = P.getUnsignedBits(iSat*48+40,32)*0.02;
1256
1257 // Is this a new PRN?
1258 idx=-1;
1259 for (unsigned int i=0;i<PRN.size();i++) {
1260 if (PRN[i]==prn) { idx=i; break; };
1261 };
1262 if (idx==-1) {
1263 // Insert new sat at end of list
1264 nSat++; idx = nSat-1;
1265 PRN.push_back(prn);
1266 rng_C1.push_back(0.0);
1267 rng_P1.push_back(0.0);
1268 rng_P2.push_back(0.0);
1269 cph_L1.push_back(0.0);
1270 cph_L2.push_back(0.0);
1271 slip_L1.push_back(-1);
1272 slip_L2.push_back(-1);
1273 };
1274
1275 // Store measurement
1276 if (isL1) {
1277 if (isCAcode) {
1278 rng_C1[idx] = rng;
1279 }
1280 else {
1281 rng_P1[idx] = rng;
1282 }
1283 }
1284 else {
1285 rng_P2[idx] = rng;
1286 };
1287
1288 };
1289
1290 };
1291
1292};
1293
1294//
1295// Resolution of 2^24 cy carrier phase ambiguity
1296// caused by 32-bit data field restrictions
1297//
1298// Note: the RTCM standard specifies an ambiguity of +/-2^23 cy.
1299// However, numerous receivers generate data in the +/-2^22 cy range.
1300// A reduced ambiguity of 2^23 cy appears compatible with both cases.
1301//
1302
1303double RTCM2_Obs::resolvedPhase_L1(int i) const {
1304
1305//const double ambig = pow(2.0,24); // as per RTCM2 spec
1306 const double ambig = pow(2.0,23); // used by many receivers
1307
1308 double rng;
1309 double n;
1310
1311 if (!valid() || i<0 || i>nSat-1) return 0.0;
1312
1313 rng = rng_C1[i];
1314 if (rng==0.0) rng = rng_P1[i];
1315 if (rng==0.0) return 0.0;
1316
1317 n = floor( (rng/lambda_L1-cph_L1[i]) / ambig + 0.5 );
1318
1319 return cph_L1[i] + n*ambig;
1320
1321};
1322
1323double RTCM2_Obs::resolvedPhase_L2(int i) const {
1324
1325//const double ambig = pow(2.0,24); // as per RTCM2 spec
1326 const double ambig = pow(2.0,23); // used by many receivers
1327
1328 double rng;
1329 double n;
1330
1331 if (!valid() || i<0 || i>nSat-1) return 0.0;
1332
1333 rng = rng_C1[i];
1334 if (rng==0.0) rng = rng_P1[i];
1335 if (rng==0.0) return 0.0;
1336
1337 n = floor( (rng/lambda_L2-cph_L2[i]) / ambig + 0.5 );
1338
1339 return cph_L2[i] + n*ambig;
1340
1341};
1342
1343//
1344// Resolution of epoch using reference date (GPS week and secs)
1345//
1346
1347void RTCM2_Obs::resolveEpoch (int refWeek, double refSecs,
1348 int& epochWeek, double& epochSecs ) const {
1349
1350 const double secsPerWeek = 604800.0;
1351
1352 epochWeek = refWeek;
1353 epochSecs = secs + 3600.0*(floor((refSecs-secs)/3600.0+0.5));
1354
1355 if (epochSecs<0 ) { epochWeek--; epochSecs+=secsPerWeek; };
1356 if (epochSecs>secsPerWeek) { epochWeek++; epochSecs-=secsPerWeek; };
1357
1358};
1359
1360}; // End of namespace rtcm2
Note: See TracBrowser for help on using the repository browser.