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

Last change on this file since 1034 was 1029, checked in by mervart, 16 years ago

Back to status of date 20080807

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