Context Navigation

source: ntrip/trunk/BNC/src/RTCM3/RTCM3Decoder.cpp@ 6813

Last change on this file since 6813 was 6812, checked in by stoecker, 10 years ago
integrate RTCM3 parsing into BNC and directly fill target structures, add doxygen documentation
File size: 45.3 KB

Line
1	// Part of BNC, a utility for retrieving decoding and
2	// converting GNSS data streams from NTRIP broadcasters.
3	//
4	// Copyright (C) 2007
5	// German Federal Agency for Cartography and Geodesy (BKG)
6	// http://www.bkg.bund.de
7	// Czech Technical University Prague, Department of Geodesy
8	// http://www.fsv.cvut.cz
9	//
10	// Email: euref-ip@bkg.bund.de
11	//
12	// This program is free software; you can redistribute it and/or
13	// modify it under the terms of the GNU General Public License
14	// as published by the Free Software Foundation, version 2.
15	//
16	// This program is distributed in the hope that it will be useful,
17	// but WITHOUT ANY WARRANTY; without even the implied warranty of
18	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19	// GNU General Public License for more details.
20	//
21	// You should have received a copy of the GNU General Public License
22	// along with this program; if not, write to the Free Software
23	// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
24
25	/* -------------------------------------------------------------------------
26	* BKG NTRIP Client
27	* -------------------------------------------------------------------------
28	*
29	* Class: RTCM3Decoder
30	*
31	* Purpose: RTCM3 Decoder
32	*
33	* Author: L. Mervart
34	*
35	* Created: 24-Aug-2006
36	*
37	* Changes:
38	*
39	* -----------------------------------------------------------------------*/
40
41	#include <iostream>
42	#include <iomanip>
43	#include <sstream>
44	#include <math.h>
45	#include <string.h>
46
47	#include "bits.h"
48	#include "gnss.h"
49	#include "RTCM3Decoder.h"
50	#include "rtcm_utils.h"
51	#include "bncconst.h"
52	#include "bnccore.h"
53	#include "bncutils.h"
54	#include "bncsettings.h"
55
56	using namespace std;
57
58	// Error Handling
59	////////////////////////////////////////////////////////////////////////////
60	void RTCM3Error(const char*, ...) {
61	}
62
63	// Constructor
64	////////////////////////////////////////////////////////////////////////////
65	RTCM3Decoder::RTCM3Decoder(const QString& staID, bncRawFile* rawFile) :
66	GPSDecoder() {
67
68	_staID = staID;
69	_rawFile = rawFile;
70
71	connect(this, SIGNAL(newGPSEph(t_ephGPS)), BNC_CORE, SLOT(slotNewGPSEph(t_ephGPS)));
72	connect(this, SIGNAL(newGlonassEph(t_ephGlo)), BNC_CORE, SLOT(slotNewGlonassEph(t_ephGlo)));
73	connect(this, SIGNAL(newGalileoEph(t_ephGal)), BNC_CORE, SLOT(slotNewGalileoEph(t_ephGal)));
74	connect(this, SIGNAL(newSBASEph(t_ephSBAS)), BNC_CORE, SLOT(slotNewSBASEph(t_ephSBAS)));
75	connect(this, SIGNAL(newBDSEph(t_ephBDS)), BNC_CORE, SLOT(slotNewBDSEph(t_ephBDS)));
76
77	_MessageSize = _SkipBytes = _BlockSize = _NeedBytes = 0;
78	}
79
80	// Destructor
81	////////////////////////////////////////////////////////////////////////////
82	RTCM3Decoder::~RTCM3Decoder() {
83	QMapIterator<QByteArray, RTCM3coDecoder*> it(_coDecoders);
84	while(it.hasNext())
85	{
86	it.next();
87	delete it.value();
88	}
89	}
90
91	//
92	////////////////////////////////////////////////////////////////////////////
93	bool RTCM3Decoder::DecodeRTCM3GPS(unsigned char* data, int size)
94	{
95	bool decoded = false;
96	bncTime CurrentObsTime;
97	int i, numsats, syncf, type;
98	uint64_t numbits = 0, bitfield = 0;
99
100	data += 3; /* header */
101	size -= 6; /* header + crc */
102
103	GETBITS(type, 12)
104	SKIPBITS(12) /* id */
105	GETBITS(i,30)
106
107	CurrentObsTime.set(i);
108	if(_CurrentTime.valid() && CurrentObsTime != _CurrentTime)
109	{
110	decoded = true;
111	_obsList = _CurrentObsList;
112	_CurrentObsList.clear();
113	}
114	_CurrentTime = CurrentObsTime;
115
116	GETBITS(syncf,1) /* sync */
117	GETBITS(numsats,5)
118	SKIPBITS(4) /* smind, smint */
119
120	while(numsats--)
121	{
122	int sv, code, l1range, amb=0;
123	t_satObs CurrentObs;
124	CurrentObs._time = CurrentObsTime;
125
126	GETBITS(sv, 6)
127	if(sv < 40)
128	CurrentObs._prn.set('G', sv);
129	else
130	CurrentObs._prn.set('S', sv-20);
131
132	t_frqObs *frqObs = new t_frqObs;
133	/* L1 */
134	GETBITS(code, 1);
135	frqObs->_rnxType2ch = code ? "1W" : "1C";
136	GETBITS(l1range, 24);
137	GETBITSSIGN(i, 20);
138	if((i&((1<<20)-1)) != 0x80000)
139	{
140	frqObs->_code = l1range*0.02;
141	frqObs->_phase = (l1range0.02+i0.0005)/GPS_WAVELENGTH_L1;
142	frqObs->_codeValid = frqObs->_phaseValid = true;
143	}
144	GETBITS(i, 7);
145	frqObs->_slipCounter = i;
146	if(type == 1002 \|\| type == 1004)
147	{
148	GETBITS(amb,8);
149	if(amb)
150	{
151	frqObs->_code += amb*299792.458;
152	frqObs->_phase += (amb*299792.458)/GPS_WAVELENGTH_L1;
153	}
154	GETBITS(i, 8);
155	if(i)
156	{
157	frqObs->_snr = i*0.25;
158	frqObs->_snrValid = true;
159	}
160	}
161	CurrentObs._obs.push_back(frqObs);
162	if(type == 1003 \|\| type == 1004)
163	{
164	frqObs = new t_frqObs;
165	/* L2 */
166	GETBITS(code,2);
167	switch(code)
168	{
169	case 3: frqObs->_rnxType2ch = "2W"; /* or "2Y"? */ break;
170	case 2: frqObs->_rnxType2ch = "2W"; break;
171	case 1: frqObs->_rnxType2ch = "2P"; break;
172	case 0: frqObs->_rnxType2ch = "2X"; /* or "2S" or "2L"? */ break;
173	}
174	GETBITSSIGN(i,14);
175	if((i&((1<<14)-1)) != 0x2000)
176	{
177	frqObs->_code = l1range0.02+i0.02+amb*299792.458;
178	frqObs->_codeValid = true;
179	}
180	GETBITSSIGN(i,20);
181	if((i&((1<<20)-1)) != 0x80000)
182	{
183	frqObs->_phase = (l1range0.02+i0.0005+amb*299792.458)/GPS_WAVELENGTH_L2;
184	frqObs->_phaseValid = true;
185	}
186	GETBITS(i,7);
187	frqObs->_slipCounter = i;
188	if(type == 1004)
189	{
190	GETBITS(i, 8);
191	if(i)
192	{
193	frqObs->_snr = i*0.25;
194	frqObs->_snrValid = true;
195	}
196	}
197	CurrentObs._obs.push_back(frqObs);
198	}
199	_CurrentObsList.push_back(CurrentObs);
200	}
201	if(!syncf)
202	{
203	decoded = true;
204	_obsList = _CurrentObsList;
205	_CurrentTime.reset();
206	_CurrentObsList.clear();
207	}
208	return decoded;
209	}
210
211	#define RTCM3_MSM_NUMSIG 32
212	#define RTCM3_MSM_NUMSAT 64
213	#define RTCM3_MSM_NUMCELLS 96 /* arbitrary limit */
214
215	/**
216	* Frequency numbers of GLONASS with an offset of 100 to detect unset values.
217	* Gets filled by ephemeris and data blocks and shared between different streams.
218	*/
219	static int GLOFreq[RTCM3_MSM_NUMSAT];
220
221	/*
222	* Storage structure to store frequency and RINEX ID assignment for MSM
223	* message */
224	struct CodeData {
225	double wl;
226	const char code; / currently unused */
227	};
228
229	/** MSM signal types for GPS and SBAS */
230	static struct CodeData gps[RTCM3_MSM_NUMSIG] =
231	{
232	{0.0,0},
233	{GPS_WAVELENGTH_L1,"1C"},
234	{GPS_WAVELENGTH_L1,"1P"},
235	{GPS_WAVELENGTH_L1,"1W"},
236	{0.0,0}/{GPS_WAVELENGTH_L1,"1Y"}/,
237	{0.0,0},
238	{0.0,0},
239	{GPS_WAVELENGTH_L2,"2C"},
240	{GPS_WAVELENGTH_L2,"2P"},
241	{GPS_WAVELENGTH_L2,"2W"},
242	{0.0,0}/{GPS_WAVELENGTH_L2,"2Y"}/,
243	{0.0,0},
244	{0.0,0},
245	{0.0,0},
246	{GPS_WAVELENGTH_L2,"2S"},
247	{GPS_WAVELENGTH_L2,"2L"},
248	{GPS_WAVELENGTH_L2,"2X"},
249	{0.0,0},
250	{0.0,0},
251	{0.0,0},
252	{0.0,0},
253	{GPS_WAVELENGTH_L5,"5I"},
254	{GPS_WAVELENGTH_L5,"5Q"},
255	{GPS_WAVELENGTH_L5,"5X"},
256	{0.0,0},
257	{0.0,0},
258	{0.0,0},
259	{0.0,0},
260	{0.0,0},
261	{GPS_WAVELENGTH_L1,"1S"},
262	{GPS_WAVELENGTH_L1,"1L"},
263	{GPS_WAVELENGTH_L1,"1X"}
264	};
265
266	/**
267	* MSM signal types for GLONASS
268	*
269	* NOTE: Uses 0.0, 1.0 for wavelength as sat index dependence is done later!
270	*/
271	static struct CodeData glo[RTCM3_MSM_NUMSIG] =
272	{
273	{0.0,0},
274	{0.0,"1C"},
275	{0.0,"1P"},
276	{0.0,0},
277	{0.0,0},
278	{0.0,0},
279	{0.0,0},
280	{1.0,"2C"},
281	{1.0,"2P"},
282	{0.0,0},
283	{0.0,0},
284	{0.0,0},
285	{0.0,0},
286	{0.0,0},
287	{0.0,0},
288	{0.0,0},
289	{0.0,0},
290	{0.0,0},
291	{0.0,0},
292	{0.0,0},
293	{0.0,0},
294	{0.0,0},
295	{0.0,0},
296	{0.0,0},
297	{0.0,0},
298	{0.0,0},
299	{0.0,0},
300	{0.0,0},
301	{0.0,0},
302	{0.0,0},
303	{0.0,0},
304	{0.0,0}
305	};
306
307	/** MSM signal types for Galileo */
308	static struct CodeData gal[RTCM3_MSM_NUMSIG] =
309	{
310	{0.0,0},
311	{GAL_WAVELENGTH_E1,"1C"},
312	{GAL_WAVELENGTH_E1,"1A"},
313	{GAL_WAVELENGTH_E1,"1B"},
314	{GAL_WAVELENGTH_E1,"1X"},
315	{GAL_WAVELENGTH_E1,"1Z"},
316	{0.0,0},
317	{GAL_WAVELENGTH_E6,"6C"},
318	{GAL_WAVELENGTH_E6,"6A"},
319	{GAL_WAVELENGTH_E6,"6B"},
320	{GAL_WAVELENGTH_E6,"6X"},
321	{GAL_WAVELENGTH_E6,"6Z"},
322	{0.0,0},
323	{GAL_WAVELENGTH_E5B,"7I"},
324	{GAL_WAVELENGTH_E5B,"7Q"},
325	{GAL_WAVELENGTH_E5B,"7X"},
326	{0.0,0},
327	{GAL_WAVELENGTH_E5AB,"8I"},
328	{GAL_WAVELENGTH_E5AB,"8Q"},
329	{GAL_WAVELENGTH_E5AB,"8X"},
330	{0.0,0},
331	{GAL_WAVELENGTH_E5A,"5I"},
332	{GAL_WAVELENGTH_E5A,"5Q"},
333	{GAL_WAVELENGTH_E5A,"5X"},
334	{0.0,0},
335	{0.0,0},
336	{0.0,0},
337	{0.0,0},
338	{0.0,0},
339	{0.0,0},
340	{0.0,0},
341	{0.0,0},
342	};
343
344	/** MSM signal types for QZSS */
345	static struct CodeData qzss[RTCM3_MSM_NUMSIG] =
346	{
347	{0.0,0},
348	{GPS_WAVELENGTH_L1,"1C"},
349	{0.0,0},
350	{0.0,0},
351	{0.0,0},
352	{GPS_WAVELENGTH_L1,"1Z"},
353	{0.0,0},
354	{0.0,0},
355	{QZSS_WAVELENGTH_LEX,"6S"},
356	{QZSS_WAVELENGTH_LEX,"6L"},
357	{QZSS_WAVELENGTH_LEX,"6X"},
358	{0.0,0},
359	{0.0,0},
360	{0.0,0},
361	{GPS_WAVELENGTH_L2,"2S"},
362	{GPS_WAVELENGTH_L2,"2L"},
363	{GPS_WAVELENGTH_L2,"2X"},
364	{0.0,0},
365	{0.0,0},
366	{0.0,0},
367	{0.0,0},
368	{GPS_WAVELENGTH_L5,"5I"},
369	{GPS_WAVELENGTH_L5,"5Q"},
370	{GPS_WAVELENGTH_L5,"5X"},
371	{0.0,0},
372	{0.0,0},
373	{0.0,0},
374	{0.0,0},
375	{0.0,0},
376	{GPS_WAVELENGTH_L1,"1D"},
377	{GPS_WAVELENGTH_L1,"1P"},
378	{GPS_WAVELENGTH_L1,"1X"}
379	};
380
381	/** MSM signal types for Beidou/BDS */
382	static struct CodeData bds[RTCM3_MSM_NUMSIG] =
383	{
384	{0.0,0},
385	{BDS_WAVELENGTH_B1,"1I"},
386	{0.0,0},
387	{0.0,0},
388	{0.0,0},
389	{0.0,0},
390	{0.0,0},
391	{BDS_WAVELENGTH_B3,"6I"},
392	{0.0,0},
393	{0.0,0},
394	{0.0,0},
395	{0.0,0},
396	{0.0,0},
397	{BDS_WAVELENGTH_B2,"7I"},
398	{0.0,0},
399	{0.0,0},
400	{0.0,0},
401	{0.0,0},
402	{0.0,0},
403	{0.0,0},
404	{0.0,0},
405	{0.0,0},
406	{0.0,0},
407	{0.0,0},
408	{0.0,0},
409	{0.0,0},
410	{0.0,0},
411	{0.0,0},
412	{0.0,0},
413	{0.0,0},
414	{0.0,0},
415	{0.0,0},
416	};
417
418	#define UINT64(c) c ## ULL
419
420	//
421	////////////////////////////////////////////////////////////////////////////
422	bool RTCM3Decoder::DecodeRTCM3MSM(unsigned char* data, int size)
423	{
424	bool decoded = false;
425	int type, syncf, i;
426	uint64_t numbits = 0, bitfield = 0;
427
428	data += 3; /* header */
429	size -= 6; /* header + crc */
430
431	GETBITS(type, 12)
432	SKIPBITS(12) /* id */
433	char sys;
434	if(type >= 1121)
435	sys = 'C';
436	else if(type >= 1111)
437	sys = 'J';
438	else if(type >= 1101)
439	sys = 'S';
440	else if(type >= 1091)
441	sys = 'E';
442	else if(type >= 1081)
443	sys = 'R';
444	else
445	sys = 'G';
446
447	bncTime CurrentObsTime;
448	if(sys == 'C') /* BDS */
449	{
450	GETBITS(i,30)
451	CurrentObsTime.setBDS(i);
452	}
453	else if(sys == 'R') /* GLONASS */
454	{
455	SKIPBITS(3)
456	GETBITS(i,27) /* tk */
457	CurrentObsTime.setTk(i);
458	}
459	else /* GPS style date */
460	{
461	GETBITS(i,30)
462	CurrentObsTime.set(i);
463	}
464	if(_CurrentTime.valid() && CurrentObsTime != _CurrentTime)
465	{
466	decoded = true;
467	_obsList = _CurrentObsList;
468	_CurrentObsList.clear();
469	}
470	_CurrentTime = CurrentObsTime;
471
472	GETBITS(syncf, 1)
473	/**
474	* Ignore unknown types except for sync flag
475	*
476	* We actually support types 1-3 in following code, but as they are missing
477	* the full cycles and can't be used later we skip interpretation here already.
478	*/
479	if(type <= 1130 && (type % 10) >= 4 && (type % 10) <= 7)
480	{
481	int sigmask, numsat = 0, numsig = 0;
482	uint64_t satmask, cellmask, ui;
483	double rrmod[RTCM3_MSM_NUMSAT];
484	int rrint[RTCM3_MSM_NUMSAT], rdop[RTCM3_MSM_NUMSAT],
485	extsat[RTCM3_MSM_NUMSAT];
486	int ll[RTCM3_MSM_NUMCELLS]/, hc[RTCM3_MSM_NUMCELLS]/;
487	double cnr[RTCM3_MSM_NUMCELLS];
488	double cp[RTCM3_MSM_NUMCELLS], psr[RTCM3_MSM_NUMCELLS],
489	dop[RTCM3_MSM_NUMCELLS];
490
491	SKIPBITS(3+7+2+2+1+3)
492	GETBITS64(satmask, RTCM3_MSM_NUMSAT)
493
494	/* http://gurmeetsingh.wordpress.com/2008/08/05/fast-bit-counting-routines/ */
495	for(ui = satmask; ui; ui &= (ui - 1) /* remove rightmost bit */)
496	++numsat;
497	GETBITS(sigmask, RTCM3_MSM_NUMSIG)
498	for(i = sigmask; i; i &= (i - 1) /* remove rightmost bit */)
499	++numsig;
500	for(i = 0; i < RTCM3_MSM_NUMSAT; ++i)
501	extsat[i] = 15;
502
503	i = numsat*numsig;
504	GETBITS64(cellmask, (unsigned)i)
505
506	switch(type % 10)
507	{
508	case 1: case 2: case 3:
509	/* partial data, already skipped above, but implemented for future expansion ! */
510	for(int j = numsat; j--;)
511	GETFLOAT(rrmod[j], 10, 1.0/1024.0)
512	break;
513	case 4: case 6:
514	for(int j = numsat; j--;)
515	GETBITS(rrint[j], 8)
516	for(int j = numsat; j--;)
517	GETFLOAT(rrmod[j], 10, 1.0/1024.0)
518	break;
519	case 5: case 7:
520	for(int j = numsat; j--;)
521	GETBITS(rrint[j], 8)
522	for(int j = numsat; j--;)
523	GETBITS(extsat[j], 4)
524	for(int j = numsat; j--;)
525	GETFLOAT(rrmod[j], 10, 1.0/1024.0)
526	for(int j = numsat; j--;)
527	GETBITSSIGN(rdop[j], 14)
528	break;
529	}
530
531	int numcells = numsat*numsig;
532	/** Drop anything which exceeds our cell limit. Increase limit definition
533	* when that happens. */
534	if(numcells <= RTCM3_MSM_NUMCELLS)
535	{
536	switch(type % 10)
537	{
538	case 1:
539	for(int count = numcells; count--;)
540	if(cellmask & (UINT64(1)<<count))
541	GETFLOATSIGN(psr[count], 15, 1.0/(1<<24))
542	break;
543	case 2:
544	for(int count = numcells; count--;)
545	if(cellmask & (UINT64(1)<<count))
546	GETFLOATSIGN(cp[count], 22, 1.0/(1<<29))
547	for(int count = numcells; count--;)
548	if(cellmask & (UINT64(1)<<count))
549	GETBITS(ll[count], 4)
550	for(int count = numcells; count--;)
551	if(cellmask & (UINT64(1)<<count))
552	SKIPBITS(1)/GETBITS(hc[count], 1)/
553	break;
554	case 3:
555	for(int count = numcells; count--;)
556	if(cellmask & (UINT64(1)<<count))
557	GETFLOATSIGN(psr[count], 15, 1.0/(1<<24))
558	for(int count = numcells; count--;)
559	if(cellmask & (UINT64(1)<<count))
560	GETFLOATSIGN(cp[count], 22, 1.0/(1<<29))
561	for(int count = numcells; count--;)
562	if(cellmask & (UINT64(1)<<count))
563	GETBITS(ll[count], 4)
564	for(int count = numcells; count--;)
565	if(cellmask & (UINT64(1)<<count))
566	SKIPBITS(1)/GETBITS(hc[count], 1)/
567	break;
568	case 4:
569	for(int count = numcells; count--;)
570	if(cellmask & (UINT64(1)<<count))
571	GETFLOATSIGN(psr[count], 15, 1.0/(1<<24))
572	for(int count = numcells; count--;)
573	if(cellmask & (UINT64(1)<<count))
574	GETFLOATSIGN(cp[count], 22, 1.0/(1<<29))
575	for(int count = numcells; count--;)
576	if(cellmask & (UINT64(1)<<count))
577	GETBITS(ll[count], 4)
578	for(int count = numcells; count--;)
579	if(cellmask & (UINT64(1)<<count))
580	SKIPBITS(1)/GETBITS(hc[count], 1)/
581	for(int count = numcells; count--;)
582	if(cellmask & (UINT64(1)<<count))
583	GETBITS(cnr[count], 6)
584	break;
585	case 5:
586	for(int count = numcells; count--;)
587	if(cellmask & (UINT64(1)<<count))
588	GETFLOATSIGN(psr[count], 15, 1.0/(1<<24))
589	for(int count = numcells; count--;)
590	if(cellmask & (UINT64(1)<<count))
591	GETFLOATSIGN(cp[count], 22, 1.0/(1<<29))
592	for(int count = numcells; count--;)
593	if(cellmask & (UINT64(1)<<count))
594	GETBITS(ll[count], 4)
595	for(int count = numcells; count--;)
596	if(cellmask & (UINT64(1)<<count))
597	SKIPBITS(1)/GETBITS(hc[count], 1)/
598	for(int count = numcells; count--;)
599	if(cellmask & (UINT64(1)<<count))
600	GETFLOAT(cnr[count], 6, 1.0)
601	for(int count = numcells; count--;)
602	if(cellmask & (UINT64(1)<<count))
603	GETFLOATSIGN(dop[count], 15, 0.0001)
604	break;
605	case 6:
606	for(int count = numcells; count--;)
607	if(cellmask & (UINT64(1)<<count))
608	GETFLOATSIGN(psr[count], 20, 1.0/(1<<29))
609	for(int count = numcells; count--;)
610	if(cellmask & (UINT64(1)<<count))
611	GETFLOATSIGN(cp[count], 24, 1.0/(1U<<31))
612	for(int count = numcells; count--;)
613	if(cellmask & (UINT64(1)<<count))
614	GETBITS(ll[count], 10)
615	for(int count = numcells; count--;)
616	if(cellmask & (UINT64(1)<<count))
617	SKIPBITS(1)/GETBITS(hc[count], 1)/
618	for(int count = numcells; count--;)
619	if(cellmask & (UINT64(1)<<count))
620	GETFLOAT(cnr[count], 10, 1.0/(1<<4))
621	break;
622	case 7:
623	for(int count = numcells; count--;)
624	if(cellmask & (UINT64(1)<<count))
625	GETFLOATSIGN(psr[count], 20, 1.0/(1<<29))
626	for(int count = numcells; count--;)
627	if(cellmask & (UINT64(1)<<count))
628	GETFLOATSIGN(cp[count], 24, 1.0/(1U<<31))
629	for(int count = numcells; count--;)
630	if(cellmask & (UINT64(1)<<count))
631	GETBITS(ll[count], 10)
632	for(int count = numcells; count--;)
633	if(cellmask & (UINT64(1)<<count))
634	SKIPBITS(1)/GETBITS(hc[count], 1)/
635	for(int count = numcells; count--;)
636	if(cellmask & (UINT64(1)<<count))
637	GETFLOAT(cnr[count], 10, 1.0/(1<<4))
638	for(int count = numcells; count--;)
639	if(cellmask & (UINT64(1)<<count))
640	GETFLOATSIGN(dop[count], 15, 0.0001)
641	break;
642	}
643	i = RTCM3_MSM_NUMSAT;
644	int j = -1;
645	t_satObs CurrentObs;
646	for(int count = numcells; count--;)
647	{
648	while(j >= 0 && !(sigmask&(1<<--j)))
649	;
650	if(j < 0)
651	{
652	while(!(satmask&(UINT64(1)<<(--i)))) /* next satellite */
653	;
654	if(CurrentObs._obs.size() > 0)
655	_CurrentObsList.push_back(CurrentObs);
656	CurrentObs.clear();
657	CurrentObs._time = CurrentObsTime;
658	if(sys == 'S')
659	CurrentObs._prn.set(sys, 20-1+RTCM3_MSM_NUMSAT-i);
660	else
661	CurrentObs._prn.set(sys, RTCM3_MSM_NUMSAT-i);
662	j = RTCM3_MSM_NUMSIG;
663	while(!(sigmask&(1<<--j)))
664	;
665	--numsat;
666	}
667	if(cellmask & (UINT64(1)<<count))
668	{
669	struct CodeData cd = {0.0,0};
670	switch(sys)
671	{
672	case 'J':
673	cd = qzss[RTCM3_MSM_NUMSIG-j-1];
674	break;
675	case 'C':
676	cd = bds[RTCM3_MSM_NUMSIG-j-1];
677	break;
678	case 'G': case 'S':
679	cd = gps[RTCM3_MSM_NUMSIG-j-1];
680	break;
681	case 'R':
682	cd = glo[RTCM3_MSM_NUMSIG-j-1];
683	{
684	int k = GLOFreq[RTCM3_MSM_NUMSAT-i-1];
685	if(extsat[numsat] < 14)
686	{
687	k = GLOFreq[RTCM3_MSM_NUMSAT-i-1] = 100+extsat[numsat]-7;
688	}
689	if(k)
690	cd.wl = (cd.wl == 0.0 ? GLO_WAVELENGTH_L1(k-100) : GLO_WAVELENGTH_L2(k-100));
691	else
692	cd.code = 0;
693	}
694	break;
695	case 'E':
696	cd = gal[RTCM3_MSM_NUMSIG-j-1];
697	break;
698	}
699	if(cd.code)
700	{
701	t_frqObs *frqObs = new t_frqObs;
702	frqObs->_rnxType2ch = cd.code;
703
704	switch(type % 10)
705	{
706	case 1:
707	if(psr[count] > -1.0/(1<<10))
708	{
709	frqObs->_code = psr[count]*LIGHTSPEED/1000.0
710	+(rrmod[numsat])*LIGHTSPEED/1000.0;
711	frqObs->_codeValid = true;
712	}
713	break;
714	case 2:
715	if(cp[count] > -1.0/(1<<8))
716	{
717	frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
718	+(rrmod[numsat])*LIGHTSPEED/1000.0/cd.wl;
719	frqObs->_phaseValid = true;
720	frqObs->_slipCounter = ll[count];
721	}
722	break;
723	case 3:
724	if(psr[count] > -1.0/(1<<10))
725	{
726	frqObs->_code = psr[count]*LIGHTSPEED/1000.0
727	+(rrmod[numsat])*LIGHTSPEED/1000.0;
728	frqObs->_codeValid = true;
729	}
730
731	if(cp[count] > -1.0/(1<<8))
732	{
733	frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
734	+rrmod[numsat]*LIGHTSPEED/1000.0/cd.wl;
735	frqObs->_phaseValid = true;
736	frqObs->_slipCounter = ll[count];
737	}
738	break;
739	case 4:
740	if(psr[count] > -1.0/(1<<10))
741	{
742	frqObs->_code = psr[count]*LIGHTSPEED/1000.0
743	+(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0;
744	frqObs->_codeValid = true;
745	}
746
747	if(cp[count] > -1.0/(1<<8))
748	{
749	frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
750	+(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl;
751	frqObs->_phaseValid = true;
752	frqObs->_slipCounter = ll[count];
753	}
754
755	frqObs->_snr = cnr[count];
756	frqObs->_snrValid = true;
757	break;
758	case 5:
759	if(psr[count] > -1.0/(1<<10))
760	{
761	frqObs->_code = psr[count]*LIGHTSPEED/1000.0
762	+(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0;
763	frqObs->_codeValid = true;
764	}
765
766	if(cp[count] > -1.0/(1<<8))
767	{
768	frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
769	+(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl;
770	frqObs->_phaseValid = true;
771	frqObs->_slipCounter = ll[count];
772	}
773
774	frqObs->_snr = cnr[count];
775	frqObs->_snrValid = true;
776
777	if(dop[count] > -1.6384)
778	{
779	frqObs->_doppler = -(dop[count]+rdop[numsat])/cd.wl;
780	frqObs->_dopplerValid = true;
781	}
782	break;
783	case 6:
784	if(psr[count] > -1.0/(1<<10))
785	{
786	frqObs->_code = psr[count]*LIGHTSPEED/1000.0
787	+(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0;
788	frqObs->_codeValid = true;
789	}
790
791	if(cp[count] > -1.0/(1<<8))
792	{
793	frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
794	+(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl;
795	frqObs->_phaseValid = true;
796	frqObs->_slipCounter = ll[count];
797	}
798
799	frqObs->_snr = cnr[count];
800	frqObs->_snrValid = true;
801	break;
802	case 7:
803	if(psr[count] > -1.0/(1<<10))
804	{
805	frqObs->_code = psr[count]*LIGHTSPEED/1000.0
806	+(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0;
807	frqObs->_codeValid = true;
808	}
809
810	if(cp[count] > -1.0/(1<<8))
811	{
812	frqObs->_phase = cp[count]*LIGHTSPEED/1000.0/cd.wl
813	+(rrmod[numsat]+rrint[numsat])*LIGHTSPEED/1000.0/cd.wl;
814	frqObs->_phaseValid = true;
815	frqObs->_slipCounter = ll[count];
816	}
817
818	frqObs->_snr = cnr[count];
819	frqObs->_snrValid = true;
820
821	if(dop[count] > -1.6384)
822	{
823	frqObs->_doppler = -(dop[count]+rdop[numsat])/cd.wl;
824	frqObs->_dopplerValid = true;
825	}
826	break;
827	}
828	CurrentObs._obs.push_back(frqObs);
829	}
830	}
831	}
832	if(CurrentObs._obs.size() > 0)
833	_CurrentObsList.push_back(CurrentObs);
834	}
835	}
836	else if((type % 10) < 3)
837	{
838	emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")
839	.arg(_staID).arg(type).toAscii(), true));
840	}
841	if(!syncf)
842	{
843	decoded = true;
844	_obsList = _CurrentObsList;
845	_CurrentTime.reset();
846	_CurrentObsList.clear();
847	}
848	return decoded;
849	}
850
851	//
852	////////////////////////////////////////////////////////////////////////////
853	bool RTCM3Decoder::DecodeRTCM3GLONASS(unsigned char* data, int size)
854	{
855	bool decoded = false;
856	bncTime CurrentObsTime;
857	int i, numsats, syncf, type;
858	uint64_t numbits = 0, bitfield = 0;
859
860	data += 3; /* header */
861	size -= 6; /* header + crc */
862
863	GETBITS(type, 12)
864	SKIPBITS(12) /* id */
865	GETBITS(i,27) /* tk */
866
867	CurrentObsTime.setTk(i);
868	if(_CurrentTime.valid() && CurrentObsTime != _CurrentTime)
869	{
870	decoded = true;
871	_obsList = _CurrentObsList;
872	_CurrentObsList.clear();
873	}
874	_CurrentTime = CurrentObsTime;
875
876	GETBITS(syncf,1) /* sync */
877	GETBITS(numsats,5)
878	SKIPBITS(4) /* smind, smint */
879
880	while(numsats--)
881	{
882	int sv, code, l1range, amb=0, freq;
883	t_satObs CurrentObs;
884	CurrentObs._time = CurrentObsTime;
885
886	GETBITS(sv, 6)
887	CurrentObs._prn.set('R', sv);
888	GETBITS(code, 1)
889	GETBITS(freq, 5)
890	GLOFreq[sv-1] = 100+freq-7; /* store frequency for other users (MSM) */
891
892	t_frqObs *frqObs = new t_frqObs;
893	/* L1 */
894	frqObs->_rnxType2ch = code ? "1P" : "1C";
895	GETBITS(l1range, 25);
896	GETBITSSIGN(i, 20);
897	if((i&((1<<20)-1)) != 0x80000)
898	{
899	frqObs->_code = l1range*0.02;
900	frqObs->_phase = (l1range0.02+i0.0005)/GLO_WAVELENGTH_L1(freq-7);
901	frqObs->_codeValid = frqObs->_phaseValid = true;
902	}
903	GETBITS(i, 7);
904	frqObs->_slipCounter = i;
905	if(type == 1010 \|\| type == 1012)
906	{
907	GETBITS(amb,7);
908	if(amb)
909	{
910	frqObs->_code += amb*599584.916;
911	frqObs->_phase += (amb*599584.916)/GLO_WAVELENGTH_L1(freq-7);
912	}
913	GETBITS(i, 8);
914	if(i)
915	{
916	frqObs->_snr = i*0.25;
917	frqObs->_snrValid = true;
918	}
919	}
920	CurrentObs._obs.push_back(frqObs);
921	if(type == 1011 \|\| type == 1012)
922	{
923	frqObs = new t_frqObs;
924	/* L2 */
925	GETBITS(code,2);
926	switch(code)
927	{
928	case 3: frqObs->_rnxType2ch = "2P"; break;
929	case 2: frqObs->_rnxType2ch = "2P"; break;
930	case 1: frqObs->_rnxType2ch = "2P"; break;
931	case 0: frqObs->_rnxType2ch = "2C"; break;
932	}
933	GETBITSSIGN(i,14);
934	if((i&((1<<14)-1)) != 0x2000)
935	{
936	frqObs->_code = l1range0.02+i0.02+amb*599584.916;
937	frqObs->_codeValid = true;
938	}
939	GETBITSSIGN(i,20);
940	if((i&((1<<20)-1)) != 0x80000)
941	{
942	frqObs->_phase = (l1range0.02+i0.0005+amb*599584.916)/GLO_WAVELENGTH_L2(freq-7);
943	frqObs->_phaseValid = true;
944	}
945	GETBITS(i,7);
946	frqObs->_slipCounter = i;
947	if(type == 1012)
948	{
949	GETBITS(i, 8);
950	if(i)
951	{
952	frqObs->_snr = i*0.25;
953	frqObs->_snrValid = true;
954	}
955	}
956	CurrentObs._obs.push_back(frqObs);
957	}
958	_CurrentObsList.push_back(CurrentObs);
959	}
960	if(!syncf)
961	{
962	decoded = true;
963	_obsList = _CurrentObsList;
964	_CurrentTime.reset();
965	_CurrentObsList.clear();
966	}
967	return decoded;
968	}
969
970	//
971	////////////////////////////////////////////////////////////////////////////
972	bool RTCM3Decoder::DecodeGPSEphemeris(unsigned char* data, int size)
973	{
974	bool decoded = false;
975
976	if(size == 67)
977	{
978	t_ephGPS eph;
979	int i, week;
980	uint64_t numbits = 0, bitfield = 0;
981
982	data += 3; /* header */
983	size -= 6; /* header + crc */
984	SKIPBITS(12)
985
986	eph._receptDateTime = currentDateAndTimeGPS();
987
988	GETBITS(i, 6)
989	eph._prn.set('G', i);
990	GETBITS(week, 10)
991	week += 1024;
992	GETBITS(i, 4)
993	eph._ura = accuracyFromIndex(i, eph.type());
994	GETBITS(eph._L2Codes, 2)
995	GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
996	GETBITS(eph._IODE, 8)
997	GETBITS(i, 16)
998	i <<= 4;
999	eph._TOC.set(i*1000);
1000	GETFLOATSIGN(eph._clock_driftrate, 8, 1.0/(double)(1<<30)/(double)(1<<25))
1001	GETFLOATSIGN(eph._clock_drift, 16, 1.0/(double)(1<<30)/(double)(1<<13))
1002	GETFLOATSIGN(eph._clock_bias, 22, 1.0/(double)(1<<30)/(double)(1<<1))
1003	GETBITS(eph._IODC, 10)
1004	GETFLOATSIGN(eph._Crs, 16, 1.0/(double)(1<<5))
1005	GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1006	GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1007	GETFLOATSIGN(eph._Cuc, 16, 1.0/(double)(1<<29))
1008	GETFLOAT(eph._e, 32, 1.0/(double)(1<<30)/(double)(1<<3))
1009	GETFLOATSIGN(eph._Cus, 16, 1.0/(double)(1<<29))
1010	GETFLOAT(eph._sqrt_A, 32, 1.0/(double)(1<<19))
1011	GETBITS(i, 16)
1012	i <<= 4;
1013	eph._TOEsec = i;
1014	bncTime t;
1015	t.set(i*1000);
1016	eph._TOEweek = t.gpsw();
1017	/* week from HOW, differs from TOC, TOE week, we use adapted value instead */
1018	if(eph._TOEweek > week + 1 \|\| eph._TOEweek < week-1) /* invalid week */
1019	return false;
1020	GETFLOATSIGN(eph._Cic, 16, 1.0/(double)(1<<29))
1021	GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1022	GETFLOATSIGN(eph._Cis, 16, 1.0/(double)(1<<29))
1023	GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1024	GETFLOATSIGN(eph._Crc, 16, 1.0/(double)(1<<5))
1025	GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1026	GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1027	GETFLOATSIGN(eph._TGD, 8, 1.0/(double)(1<<30)/(double)(1<<1))
1028	GETBITS(eph._health, 6)
1029	GETBITS(eph._L2PFlag, 1)
1030	GETBITS(eph._fitInterval, 1)
1031	eph._TOT = 0.9999e9;
1032
1033	emit newGPSEph(eph);
1034	decoded = true;
1035	}
1036	return decoded;
1037	}
1038
1039	//
1040	////////////////////////////////////////////////////////////////////////////
1041	bool RTCM3Decoder::DecodeGLONASSEphemeris(unsigned char* data, int size)
1042	{
1043	bool decoded = false;
1044
1045	if(size == 51)
1046	{
1047	t_ephGlo eph;
1048	int sv, i, tk;
1049	uint64_t numbits = 0, bitfield = 0;
1050
1051	data += 3; /* header */
1052	size -= 6; /* header + crc */
1053	SKIPBITS(12)
1054
1055	eph._receptDateTime = currentDateAndTimeGPS();
1056
1057	GETBITS(sv, 6)
1058	eph._prn.set('R', sv);
1059
1060	GETBITS(i, 5)
1061	eph._frequency_number = i-7;
1062	GLOFreq[sv-1] = 100+i-7; /* store frequency for other users (MSM) */
1063
1064	SKIPBITS(4) /* almanac healthy, almanac health ok, P1 */
1065	GETBITS(i, 5)
1066	tk = i6060;
1067	GETBITS(i, 6)
1068	tk += i*60;
1069	GETBITS(i, 1)
1070	tk += i*30;
1071	eph._tki = tk < 36060 ? tk-36060+86400 : tk-36060;
1072	GETBITS(eph._health, 1)
1073	SKIPBITS(1) /* P2 */
1074	GETBITS(i, 7)
1075	eph._TOC.setTk(i15601000); / tb */
1076
1077	GETFLOATSIGNM(eph._x_velocity, 24, 1.0/(double)(1<<20))
1078	GETFLOATSIGNM(eph._x_pos, 27, 1.0/(double)(1<<11))
1079	GETFLOATSIGNM(eph._x_acceleration, 5, 1.0/(double)(1<<30))
1080	GETFLOATSIGNM(eph._y_velocity, 24, 1.0/(double)(1<<20))
1081	GETFLOATSIGNM(eph._y_pos, 27, 1.0/(double)(1<<11))
1082	GETFLOATSIGNM(eph._y_acceleration, 5, 1.0/(double)(1<<30))
1083	GETFLOATSIGNM(eph._z_velocity, 24, 1.0/(double)(1<<20))
1084	GETFLOATSIGNM(eph._z_pos, 27, 1.0/(double)(1<<11))
1085	GETFLOATSIGNM(eph._z_acceleration, 5, 1.0/(double)(1<<30))
1086	SKIPBITS(1) /* P3 */
1087	GETFLOATSIGNM(eph._gamma, 11, 1.0/(double)(1<<30)/(double)(1<<10))
1088	SKIPBITS(3) /* GLONASS-M P, GLONASS-M ln (third string) */
1089	GETFLOATSIGNM(eph._tau, 22, 1.0/(double)(1<<30)) /* GLONASS tau n(tb) */
1090	SKIPBITS(5) /* GLONASS-M delta tau n(tb) */
1091	GETBITS(eph._E, 5)
1092	/* GETBITS(i, 1) / * GLONASS-M P4 */
1093	/* GETBITS(i, 4) / * GLONASS-M Ft */
1094	/* GETBITS(i, 11) / * GLONASS-M Nt */
1095	/* GETBITS(i, 2) / * GLONASS-M M */
1096	/* GETBITS(i, 1) / * GLONASS-M The Availability of Additional Data */
1097	/* GETBITS(i, 11) / * GLONASS-M Na */
1098	/* GETFLOATSIGNM(i, 32, 1.0/(double)(1<<30)/(double)(1<<1)) / * GLONASS tau c */
1099	/* GETBITS(i, 5) / * GLONASS-M N4 */
1100	/* GETFLOATSIGNM(i, 22, 1.0/(double)(1<<30)) / * GLONASS-M tau GPS */
1101	/* GETBITS(i, 1) / * GLONASS-M ln (fifth string) */
1102
1103	unsigned year, month, day;
1104	eph._TOC.civil_date(year, month, day);
1105	eph._gps_utc = gnumleap(year, month, day);
1106	eph._tt = eph._TOC;
1107
1108	eph._xv(1) = eph._x_pos * 1.e3;
1109	eph._xv(2) = eph._y_pos * 1.e3;
1110	eph._xv(3) = eph._z_pos * 1.e3;
1111	eph._xv(4) = eph._x_velocity * 1.e3;
1112	eph._xv(5) = eph._y_velocity * 1.e3;
1113	eph._xv(6) = eph._z_velocity * 1.e3;
1114
1115	emit newGlonassEph(eph);
1116	decoded = true;
1117	}
1118	return decoded;
1119	}
1120
1121	//
1122	////////////////////////////////////////////////////////////////////////////
1123	bool RTCM3Decoder::DecodeQZSSEphemeris(unsigned char* data, int size)
1124	{
1125	bool decoded = false;
1126
1127	if(size == 67)
1128	{
1129	t_ephGPS eph;
1130	int i, week;
1131	uint64_t numbits = 0, bitfield = 0;
1132
1133	data += 3; /* header */
1134	size -= 6; /* header + crc */
1135	SKIPBITS(12)
1136
1137	eph._receptDateTime = currentDateAndTimeGPS();
1138
1139	GETBITS(i, 4)
1140	eph._prn.set('J', i);
1141
1142	GETBITS(i, 16)
1143	i <<= 4;
1144	eph._TOC.set(i*1000);
1145
1146	GETFLOATSIGN(eph._clock_driftrate, 8, 1.0/(double)(1<<30)/(double)(1<<25))
1147	GETFLOATSIGN(eph._clock_drift, 16, 1.0/(double)(1<<30)/(double)(1<<13))
1148	GETFLOATSIGN(eph._clock_bias, 22, 1.0/(double)(1<<30)/(double)(1<<1))
1149	GETBITS(eph._IODE, 8)
1150	GETFLOATSIGN(eph._Crs, 16, 1.0/(double)(1<<5))
1151	GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1152	GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1153	GETFLOATSIGN(eph._Cuc, 16, 1.0/(double)(1<<29))
1154	GETFLOAT(eph._e, 32, 1.0/(double)(1<<30)/(double)(1<<3))
1155	GETFLOATSIGN(eph._Cus, 16, 1.0/(double)(1<<29))
1156	GETFLOAT(eph._sqrt_A, 32, 1.0/(double)(1<<19))
1157	GETBITS(i, 16)
1158	i <<= 4;
1159	eph._TOEsec = i;
1160	bncTime t;
1161	t.set(i);
1162
1163	GETFLOATSIGN(eph._Cic, 16, 1.0/(double)(1<<29))
1164	GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1165	GETFLOATSIGN(eph._Cis, 16, 1.0/(double)(1<<29))
1166	GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1167	GETFLOATSIGN(eph._Crc, 16, 1.0/(double)(1<<5))
1168	GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1169	GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1170	GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1171	GETBITS(eph._L2Codes, 2)
1172	GETBITS(week, 10)
1173	week += 1024;
1174	eph._TOEweek = t.gpsw();
1175	/* week from HOW, differs from TOC, TOE week, we use adapted value instead */
1176	if(eph._TOEweek > week + 1 \|\| eph._TOEweek < week-1) /* invalid week */
1177	return false;
1178
1179	GETBITS(i, 4)
1180	if(i <= 6)
1181	eph._ura = ceil(10.0*pow(2.0, 1.0+i/2.0))/10.0;
1182	else
1183	eph._ura = ceil(10.0*pow(2.0, i/2.0))/10.0;
1184	GETBITS(eph._health, 6)
1185	GETFLOATSIGN(eph._TGD, 8, 1.0/(double)(1<<30)/(double)(1<<1))
1186	GETBITS(eph._IODC, 10)
1187	GETBITS(eph._fitInterval, 1)
1188	eph._TOT = 0.9999e9;
1189	eph._L2PFlag = 0; /* does not exist for QZSS */
1190
1191	emit newGPSEph(eph);
1192	decoded = true;
1193	}
1194	return decoded;
1195	}
1196
1197	//
1198	////////////////////////////////////////////////////////////////////////////
1199	bool RTCM3Decoder::DecodeSBASEphemeris(unsigned char* data, int size)
1200	{
1201	bool decoded = false;
1202
1203	if(size == 35)
1204	{
1205	t_ephSBAS eph;
1206	int i;
1207	uint64_t numbits = 0, bitfield = 0;
1208
1209	data += 3; /* header */
1210	size -= 6; /* header + crc */
1211	SKIPBITS(12)
1212
1213	eph._receptDateTime = currentDateAndTimeGPS();
1214
1215	GETBITS(i, 6)
1216	eph._prn.set('S', 20+i);
1217	GETBITS(eph._IODN, 8)
1218	GETBITS(i, 13)
1219	i <<= 4;
1220	eph._TOC.setTOD(i*1000);
1221	GETBITS(i, 4)
1222	eph._ura = accuracyFromIndex(i, eph.type());
1223	GETFLOATSIGN(eph._x_pos, 30, 0.08)
1224	GETFLOATSIGN(eph._y_pos, 30, 0.08)
1225	GETFLOATSIGN(eph._z_pos, 25, 0.4)
1226	GETFLOATSIGN(eph._x_velocity, 17, 0.000625)
1227	GETFLOATSIGN(eph._y_velocity, 17, 0.000625)
1228	GETFLOATSIGN(eph._z_velocity, 18, 0.004)
1229	GETFLOATSIGN(eph._x_acceleration, 10, 0.0000125)
1230	GETFLOATSIGN(eph._y_acceleration, 10, 0.0000125)
1231	GETFLOATSIGN(eph._z_acceleration, 10, 0.0000625)
1232	GETFLOATSIGN(eph._agf0, 12, 1.0/(1<<30)/(1<<1))
1233	GETFLOATSIGN(eph._agf1, 8, 1.0/(1<<30)/(1<<10))
1234
1235	eph._TOW = 0.9999E9;
1236	eph._health = 0;
1237
1238	emit newSBASEph(eph);
1239	decoded = true;
1240	}
1241	return decoded;
1242	}
1243
1244	//
1245	////////////////////////////////////////////////////////////////////////////
1246	bool RTCM3Decoder::DecodeGalileoEphemeris(unsigned char* data, int size)
1247	{
1248	bool decoded = false;
1249	uint64_t numbits = 0, bitfield = 0;
1250	int i;
1251
1252	data += 3; /* header */
1253	size -= 6; /* header + crc */
1254	GETBITS(i, 12)
1255
1256	if((i == 1046 && size == 61) \|\| (i == 1045 && size == 60))
1257	{
1258	t_ephGal eph;
1259
1260	eph._receptDateTime = currentDateAndTimeGPS();
1261
1262	eph._inav = (i == 1046);
1263	eph._fnav = (i == 1045);
1264	GETBITS(i, 6)
1265	eph._prn.set('E', i, eph._inav ? 1 : 0);
1266
1267	GETBITS(eph._TOEweek, 12)
1268	GETBITS(eph._IODnav, 10)
1269	GETBITS(i, 8)
1270	eph._SISA = accuracyFromIndex(i, eph.type());
1271	GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1272	GETBITSFACTOR(i, 14, 60)
1273	eph._TOC.set(eph._TOEweek, i);
1274	GETFLOATSIGN(eph._clock_driftrate, 6, 1.0/(double)(1<<30)/(double)(1<<29))
1275	GETFLOATSIGN(eph._clock_drift, 21, 1.0/(double)(1<<30)/(double)(1<<16))
1276	GETFLOATSIGN(eph._clock_bias, 31, 1.0/(double)(1<<30)/(double)(1<<4))
1277	GETFLOATSIGN(eph._Crs, 16, 1.0/(double)(1<<5))
1278	GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1279	GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1280	GETFLOATSIGN(eph._Cuc, 16, 1.0/(double)(1<<29))
1281	GETFLOAT(eph._e, 32, 1.0/(double)(1<<30)/(double)(1<<3))
1282	GETFLOATSIGN(eph._Cus, 16, 1.0/(double)(1<<29))
1283	GETFLOAT(eph._sqrt_A, 32, 1.0/(double)(1<<19))
1284	GETBITSFACTOR(eph._TOEsec, 14, 60)
1285	/* FIXME: overwrite value, copied from old code */
1286	eph._TOEsec = eph._TOC.gpssec();
1287	GETFLOATSIGN(eph._Cic, 16, 1.0/(double)(1<<29))
1288	GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1289	GETFLOATSIGN(eph._Cis, 16, 1.0/(double)(1<<29))
1290	GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1291	GETFLOATSIGN(eph._Crc, 16, 1.0/(double)(1<<5))
1292	GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1293	GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1294	GETFLOATSIGN(eph._BGD_1_5A, 10, 1.0/(double)(1<<30)/(double)(1<<2))
1295	if(eph._inav)
1296	{
1297	/* set usused F/NAV values */
1298	eph._E5aHS = 0.0;
1299	eph._e5aDataInValid = false;
1300
1301	GETFLOATSIGN(eph._BGD_1_5B, 10, 1.0/(double)(1<<30)/(double)(1<<2))
1302	GETBITS(eph._E5bHS, 2)
1303	GETBITS(eph._e5bDataInValid, 1)
1304	GETBITS(eph._E1_bHS, 2)
1305	GETBITS(eph._e1DataInValid, 1)
1306	}
1307	else
1308	{
1309	/* set usused I/NAV values */
1310	eph._BGD_1_5B = 0.0;
1311	eph._E5bHS = 0.0;
1312	eph._E1_bHS = 0.0;
1313	eph._e1DataInValid = false;
1314	eph._e5bDataInValid = false;
1315
1316	GETBITS(eph._E5aHS, 2)
1317	GETBITS(eph._e5aDataInValid, 1)
1318	}
1319	eph._TOT = 0.9999e9;
1320
1321	emit newGalileoEph(eph);
1322	decoded = true;
1323	}
1324	return decoded;
1325	}
1326
1327	//
1328	////////////////////////////////////////////////////////////////////////////
1329	bool RTCM3Decoder::DecodeBDSEphemeris(unsigned char* data, int size)
1330	{
1331	bool decoded = false;
1332
1333	if(size == 70)
1334	{
1335	t_ephBDS eph;
1336	int i;
1337	uint64_t numbits = 0, bitfield = 0;
1338
1339	data += 3; /* header */
1340	size -= 6; /* header + crc */
1341	SKIPBITS(12)
1342
1343	eph._receptDateTime = currentDateAndTimeGPS();
1344
1345	GETBITS(i, 6)
1346	eph._prn.set('C', i);
1347
1348	SKIPBITS(13) /* week */
1349	GETBITS(i, 4)
1350	eph._URA = accuracyFromIndex(i, eph.type());
1351	GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1352	GETBITS(eph._AODE, 5)
1353	GETBITS(i, 17)
1354	i <<= 3;
1355	eph._TOC.setBDS(i*1000);
1356	GETFLOATSIGN(eph._clock_driftrate, 11, 1.0/(double)(1<<30)/(double)(1<<30)/(double)(1<<6))
1357	GETFLOATSIGN(eph._clock_drift, 22, 1.0/(double)(1<<30)/(double)(1<<20))
1358	GETFLOATSIGN(eph._clock_bias, 24, 1.0/(double)(1<<30)/(double)(1<<3))
1359	GETBITS(eph._AODC, 5)
1360	GETFLOATSIGN(eph._Crs, 18, 1.0/(double)(1<<6))
1361	GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1362	GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1363	GETFLOATSIGN(eph._Cuc, 18, 1.0/(double)(1<<30)/(double)(1<<1))
1364	GETFLOAT(eph._e, 32, 1.0/(double)(1<<30)/(double)(1<<3))
1365	GETFLOATSIGN(eph._Cus, 18, 1.0/(double)(1<<30)/(double)(1<<1))
1366	GETFLOAT(eph._sqrt_A, 32, 1.0/(double)(1<<19))
1367	GETBITS(i, 17)
1368	i <<= 3;
1369	eph._TOEsec = i;
1370	eph._TOE.setBDS(i*1000);
1371	GETFLOATSIGN(eph._Cic, 18, 1.0/(double)(1<<30)/(double)(1<<1))
1372	GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1373	GETFLOATSIGN(eph._Cis, 18, 1.0/(double)(1<<30)/(double)(1<<1))
1374	GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1375	GETFLOATSIGN(eph._Crc, 18, 1.0/(double)(1<<6))
1376	GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1377	GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1378	GETFLOATSIGN(eph._TGD1, 10, 0.0000000001)
1379	GETFLOATSIGN(eph._TGD2, 10, 0.0000000001)
1380	GETBITS(eph._SatH1, 1)
1381
1382	eph._TOW = 0.9999E9;
1383	emit newBDSEph(eph);
1384	decoded = true;
1385	}
1386	return decoded;
1387	}
1388
1389	//
1390	////////////////////////////////////////////////////////////////////////////
1391	bool RTCM3Decoder::DecodeAntenna(unsigned char* data, int size)
1392	{
1393	char *antenna;
1394	int antnum;
1395	uint64_t numbits = 0, bitfield = 0;
1396
1397	data += 3; /* header */
1398	size -= 6; /* header + crc */
1399
1400	SKIPBITS(12)
1401	GETSTRING(antnum,antenna)
1402	_antType.push_back(antenna);
1403
1404	return true;
1405	}
1406
1407	//
1408	////////////////////////////////////////////////////////////////////////////
1409	bool RTCM3Decoder::DecodeAntennaPosition(unsigned char* data, int size)
1410	{
1411	int type;
1412	uint64_t numbits = 0, bitfield = 0;
1413	double x, y, z;
1414
1415	data += 3; /* header */
1416	size -= 6; /* header + crc */
1417
1418	GETBITS(type, 12)
1419	_antList.push_back(t_antInfo());
1420	_antList.back().type = t_antInfo::ARP;
1421	SKIPBITS(22)
1422	GETBITSSIGN(x, 38)
1423	_antList.back().xx = x * 1e-4;
1424	SKIPBITS(2)
1425	GETBITSSIGN(y, 38)
1426	_antList.back().yy = y * 1e-4;
1427	SKIPBITS(2)
1428	GETBITSSIGN(z, 38)
1429	_antList.back().zz = z * 1e-4;
1430	if(type == 1006)
1431	{
1432	double h;
1433	GETBITS(h, 16)
1434	_antList.back().height = h * 1e-4;
1435	_antList.back().height_f = true;
1436	}
1437	_antList.back().message = type;
1438
1439	return true;
1440	}
1441
1442	//
1443	////////////////////////////////////////////////////////////////////////////
1444	t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg)
1445	{
1446	bool decoded = false;
1447
1448	errmsg.clear();
1449
1450	while(bufLen && _MessageSize < sizeof(_Message))
1451	{
1452	int l = sizeof(_Message) - _MessageSize;
1453	if(l > bufLen)
1454	l = bufLen;
1455	memcpy(_Message+_MessageSize, buffer, l);
1456	_MessageSize += l;
1457	bufLen -= l;
1458	buffer += l;
1459	int id;
1460	while((id = GetMessage()))
1461	{
1462	/* reset station ID for file loading as it can change */
1463	if(_rawFile)
1464	_staID = _rawFile->staID();
1465	/* store the id into the list of loaded blocks */
1466	_typeList.push_back(id);
1467
1468	/* Clock and orbit data handled in another function, already pass the
1469	* extracted data block. That does no harm, as it anyway skip everything
1470	* else. */
1471	if((id >= 1057 && id <= 1068) \|\| (id >= 1240 && id <= 1263))
1472	{
1473	if (!_coDecoders.contains(_staID.toAscii()))
1474	_coDecoders[_staID.toAscii()] = new RTCM3coDecoder(_staID);
1475	RTCM3coDecoder* coDecoder = _coDecoders[_staID.toAscii()];
1476	if(coDecoder->Decode(reinterpret_cast<char *>(_Message), _BlockSize,
1477	errmsg) == success)
1478	{
1479	decoded = true;
1480	}
1481	}
1482	else if(id >= 1070 && id <= 1229) /* MSM */
1483	{
1484	if(DecodeRTCM3MSM(_Message, _BlockSize))
1485	decoded = true;
1486	}
1487	else
1488	{
1489	switch(id)
1490	{
1491	case 1001: case 1003:
1492	emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")
1493	.arg(_staID).arg(id).toAscii(), true));
1494	break; /* no use decoding partial data ATM, remove break when data can be used */
1495	case 1002: case 1004:
1496	if(DecodeRTCM3GPS(_Message, _BlockSize))
1497	decoded = true;
1498	break;
1499	case 1009: case 1011:
1500	emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")
1501	.arg(_staID).arg(id).toAscii(), true));
1502	break; /* no use decoding partial data ATM, remove break when data can be used */
1503	case 1010: case 1012:
1504	if(DecodeRTCM3GLONASS(_Message, _BlockSize))
1505	decoded = true;
1506	break;
1507	case 1019:
1508	if(DecodeGPSEphemeris(_Message, _BlockSize))
1509	decoded = true;
1510	break;
1511	case 1020:
1512	if(DecodeGLONASSEphemeris(_Message, _BlockSize))
1513	decoded = true;
1514	break;
1515	case 1043:
1516	if(DecodeSBASEphemeris(_Message, _BlockSize))
1517	decoded = true;
1518	break;
1519	case 1044:
1520	if(DecodeQZSSEphemeris(_Message, _BlockSize))
1521	decoded = true;
1522	break;
1523	case 1045: case 1046:
1524	if(DecodeGalileoEphemeris(_Message, _BlockSize))
1525	decoded = true;
1526	break;
1527	case RTCM3ID_BDS:
1528	if(DecodeBDSEphemeris(_Message, _BlockSize))
1529	decoded = true;
1530	break;
1531	case 1007: case 1008: case 1033:
1532	DecodeAntenna(_Message, _BlockSize);
1533	break;
1534	case 1005: case 1006:
1535	DecodeAntennaPosition(_Message, _BlockSize);
1536	break;
1537	}
1538	}
1539	}
1540	}
1541	return decoded ? success : failure;
1542	};
1543
1544	//
1545	////////////////////////////////////////////////////////////////////////////
1546	uint32_t RTCM3Decoder::CRC24(long size, const unsigned char *buf)
1547	{
1548	uint32_t crc = 0;
1549	int i;
1550
1551	while(size--)
1552	{
1553	crc ^= (*buf++) << (16);
1554	for(i = 0; i < 8; i++)
1555	{
1556	crc <<= 1;
1557	if(crc & 0x1000000)
1558	crc ^= 0x01864cfb;
1559	}
1560	}
1561	return crc;
1562	}
1563
1564	//
1565	////////////////////////////////////////////////////////////////////////////
1566	int RTCM3Decoder::GetMessage(void)
1567	{
1568	unsigned char m, e;
1569	int i;
1570
1571	m = _Message+_SkipBytes;
1572	e = _Message+_MessageSize;
1573	_NeedBytes = _SkipBytes = 0;
1574	while(e-m >= 3)
1575	{
1576	if(m[0] == 0xD3)
1577	{
1578	_BlockSize = ((m[1]&3)<<8)\|m[2];
1579	if(e-m >= static_cast<int>(_BlockSize+6))
1580	{
1581	if(static_cast<uint32_t>((m[3+_BlockSize]<<16)\|(m[3+_BlockSize+1]<<8)
1582	\|(m[3+_BlockSize+2])) == CRC24(_BlockSize+3, m))
1583	{
1584	_BlockSize +=6;
1585	_SkipBytes = _BlockSize;
1586	break;
1587	}
1588	else
1589	++m;
1590	}
1591	else
1592	{
1593	_NeedBytes = _BlockSize;
1594	break;
1595	}
1596	}
1597	else
1598	++m;
1599	}
1600	if(e-m < 3)
1601	_NeedBytes = 3;
1602
1603	/* copy buffer to front */
1604	i = m - _Message;
1605	if(i && m < e)
1606	memmove(_Message, m, static_cast<size_t>(_MessageSize-i));
1607	_MessageSize -= i;
1608
1609	return !_NeedBytes ? ((_Message[3]<<4)\|(_Message[4]>>4)) : 0;
1610	}
1611
1612	// Time of Corrections
1613	//////////////////////////////////////////////////////////////////////////////
1614	int RTCM3Decoder::corrGPSEpochTime() const {
1615	return _coDecoders.size() > 0 ? _coDecoders.begin().value()->corrGPSEpochTime() : -1;
1616	}

Note: See TracBrowser for help on using the repository browser.

Download in other formats: