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