Context Navigation

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

Last change on this file since 10536 was 10534, checked in by stuerze, 11 months ago
Service and RTCM CRS encoding and decoding as well as Helmert parameter decoding added + some re-organisation
File size: 67.0 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	#include "bnctime.h"
56	#include "crs.h"
57
58	using namespace std;
59
60	// Error Handling
61	////////////////////////////////////////////////////////////////////////////
62	void RTCM3Error(const char*, ...) {
63	}
64
65	// Constructor
66	////////////////////////////////////////////////////////////////////////////
67	RTCM3Decoder::RTCM3Decoder(const QString& staID, bncRawFile* rawFile) :
68	GPSDecoder() {
69
70	_staID = staID;
71	_rawFile = rawFile;
72
73	connect(this, SIGNAL(newGPSEph(t_ephGPS)), BNC_CORE,
74	SLOT(slotNewGPSEph(t_ephGPS)));
75	connect(this, SIGNAL(newGlonassEph(t_ephGlo)), BNC_CORE,
76	SLOT(slotNewGlonassEph(t_ephGlo)));
77	connect(this, SIGNAL(newGalileoEph(t_ephGal)), BNC_CORE,
78	SLOT(slotNewGalileoEph(t_ephGal)));
79	connect(this, SIGNAL(newSBASEph(t_ephSBAS)), BNC_CORE,
80	SLOT(slotNewSBASEph(t_ephSBAS)));
81	connect(this, SIGNAL(newBDSEph(t_ephBDS)), BNC_CORE,
82	SLOT(slotNewBDSEph(t_ephBDS)));
83
84	_MessageSize = _SkipBytes = _BlockSize = _NeedBytes = 0;
85	}
86
87	// Destructor
88	////////////////////////////////////////////////////////////////////////////
89	RTCM3Decoder::~RTCM3Decoder() {
90	QMapIterator<QByteArray, RTCM3coDecoder*> it(_coDecoders);
91	while (it.hasNext()) {
92	it.next();
93	delete it.value();
94	}
95	_coDecoders.clear();
96	}
97
98	//
99	////////////////////////////////////////////////////////////////////////////
100	bool RTCM3Decoder::DecodeRTCM3GPS(unsigned char* data, int size) {
101	bool decoded = false;
102	bncTime CurrentObsTime;
103	int i, numsats, syncf, type;
104	uint64_t numbits = 0, bitfield = 0;
105
106	data += 3; /* header */
107	size -= 6; /* header + crc */
108
109	GETBITS(type, 12)
110	SKIPBITS(12)
111	/* id */
112	GETBITS(i, 30)
113
114	CurrentObsTime.set(i);
115	if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) {
116	decoded = true;
117	_obsList.append(_CurrentObsList);
118	_CurrentObsList.clear();
119	}
120
121	_CurrentTime = CurrentObsTime;
122
123	GETBITS(syncf, 1)
124	/* sync */
125	GETBITS(numsats, 5)
126	SKIPBITS(4)
127	/* smind, smint */
128
129	while (numsats--) {
130	int sv, code, l1range, amb = 0;
131	t_satObs CurrentObs;
132	CurrentObs._time = CurrentObsTime;
133	CurrentObs._type = type;
134
135	GETBITS(sv, 6)
136	if (sv < 40)
137	CurrentObs._prn.set('G', sv);
138	else
139	CurrentObs._prn.set('S', sv - 20);
140
141	t_frqObs *frqObs = new t_frqObs;
142	/* L1 */
143	GETBITS(code, 1);
144	(code) ?
145	frqObs->_rnxType2ch.assign("1W") : frqObs->_rnxType2ch.assign("1C");
146	GETBITS(l1range, 24);
147	GETBITSSIGN(i, 20);
148	if ((i & ((1 << 20) - 1)) != 0x80000) {
149	frqObs->_code = l1range * 0.02;
150	frqObs->_phase = (l1range * 0.02 + i * 0.0005) / GPS_WAVELENGTH_L1;
151	frqObs->_codeValid = frqObs->_phaseValid = true;
152	}
153	GETBITS(frqObs->_lockTimeIndicator, 7);
154	frqObs->_lockTime = lti2sec(type, frqObs->_lockTimeIndicator);
155	frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0 && frqObs->_phaseValid);
156	if (type == 1002 \|\| type == 1004) {
157	GETBITS(amb, 8);
158	if (amb) {
159	frqObs->_code += amb * 299792.458;
160	frqObs->_phase += (amb * 299792.458) / GPS_WAVELENGTH_L1;
161	}
162	GETBITS(i, 8);
163	if (i) {
164	frqObs->_snr = i * 0.25;
165	frqObs->_snrValid = true;
166	}
167	}
168	CurrentObs._obs.push_back(frqObs);
169	if (type == 1003 \|\| type == 1004) {
170	frqObs = new t_frqObs;
171	/* L2 */
172	GETBITS(code, 2);
173	switch (code) {
174	case 3:
175	frqObs->_rnxType2ch.assign("2W"); /* or "2Y"? */
176	break;
177	case 2:
178	frqObs->_rnxType2ch.assign("2W");
179	break;
180	case 1:
181	frqObs->_rnxType2ch.assign("2P");
182	break;
183	case 0:
184	frqObs->_rnxType2ch.assign("2X"); /* or "2S" or "2L"? */
185	break;
186	}
187	GETBITSSIGN(i, 14);
188	if ((i & ((1 << 14) - 1)) != 0x2000) {
189	frqObs->_code = l1range * 0.02 + i * 0.02 + amb * 299792.458;
190	frqObs->_codeValid = true;
191	}
192	GETBITSSIGN(i, 20);
193	if ((i & ((1 << 20) - 1)) != 0x80000) {
194	frqObs->_phase = (l1range * 0.02 + i * 0.0005 + amb * 299792.458)
195	/ GPS_WAVELENGTH_L2;
196	frqObs->_phaseValid = true;
197	}
198	GETBITS(frqObs->_lockTimeIndicator, 7);
199	frqObs->_lockTime = lti2sec(type, frqObs->_lockTimeIndicator);
200	frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0 && frqObs->_phaseValid);
201	if (type == 1004) {
202	GETBITS(i, 8);
203	if (i) {
204	frqObs->_snr = i * 0.25;
205	frqObs->_snrValid = true;
206	}
207	}
208	CurrentObs._obs.push_back(frqObs);
209	}
210	_CurrentObsList.push_back(CurrentObs);
211	}
212
213	if (!syncf) {
214	decoded = true;
215	_obsList.append(_CurrentObsList);
216	_CurrentTime.reset();
217	_CurrentObsList.clear();
218	}
219	return decoded;
220	}
221
222	#define RTCM3_MSM_NUMSIG 32
223	#define RTCM3_MSM_NUMSAT 64
224	#define RTCM3_MSM_NUMCELLS 96 /* arbitrary limit */
225
226	/**
227	* Frequency numbers of GLONASS with an offset of 100 to detect unset values.
228	* Gets filled by ephemeris and data blocks and shared between different streams.
229	*/
230	static int GLOFreq[RTCM3_MSM_NUMSAT];
231
232	/*
233	* Storage structure to store frequency and RINEX ID assignment for MSM
234	* message */
235	struct CodeData {
236	double wl;
237	const char code; / currently unused */
238	};
239
240	/** MSM signal types for GPS and SBAS */
241	static struct CodeData gps[RTCM3_MSM_NUMSIG] = {
242	{0.0, 0},
243	{GPS_WAVELENGTH_L1, "1C"},
244	{GPS_WAVELENGTH_L1, "1P"},
245	{GPS_WAVELENGTH_L1, "1W"},
246	{0.0, 0},
247	{0.0, 0},
248	{0.0, 0},
249	{GPS_WAVELENGTH_L2, "2C"},
250	{GPS_WAVELENGTH_L2, "2P"},
251	{GPS_WAVELENGTH_L2, "2W"},
252	{0.0, 0},
253	{0.0, 0},
254	{0.0, 0},
255	{0.0, 0},
256	{GPS_WAVELENGTH_L2, "2S"},
257	{GPS_WAVELENGTH_L2, "2L"},
258	{GPS_WAVELENGTH_L2, "2X"},
259	{0.0, 0},
260	{0.0, 0},
261	{0.0, 0},
262	{0.0, 0},
263	{GPS_WAVELENGTH_L5, "5I"},
264	{GPS_WAVELENGTH_L5, "5Q"},
265	{GPS_WAVELENGTH_L5, "5X"},
266	{0.0, 0},
267	{0.0, 0},
268	{0.0, 0},
269	{0.0, 0},
270	{0.0, 0},
271	{GPS_WAVELENGTH_L1, "1S"},
272	{GPS_WAVELENGTH_L1, "1L"},
273	{GPS_WAVELENGTH_L1, "1X"}
274	};
275
276	/**
277	* MSM signal types for GLONASS
278	*
279	* NOTE: Uses 0.0, 1.0 for wavelength as sat index dependence is done later!
280	*/
281	static struct CodeData glo[RTCM3_MSM_NUMSIG] = {
282	{0.0, 0},
283	{0.0, "1C"},
284	{0.0, "1P"},
285	{0.0, 0},
286	{0.0, 0},
287	{0.0, 0},
288	{0.0, 0},
289	{1.0, "2C"},
290	{1.0, "2P"},
291	{GLO_WAVELENGTH_L1a, "4A"},
292	{GLO_WAVELENGTH_L1a, "4B"},
293	{GLO_WAVELENGTH_L1a, "4X"},
294	{GLO_WAVELENGTH_L2a, "6A"},
295	{GLO_WAVELENGTH_L2a, "6B"},
296	{GLO_WAVELENGTH_L2a, "6X"},
297	{GLO_WAVELENGTH_L3, "3I"},
298	{GLO_WAVELENGTH_L3, "3Q"},
299	{GLO_WAVELENGTH_L3, "3X"},
300	{0.0, 0},
301	{0.0, 0},
302	{0.0, 0},
303	{0.0, 0},
304	{0.0, 0},
305	{0.0, 0},
306	{0.0, 0},
307	{0.0, 0},
308	{0.0, 0},
309	{0.0, 0},
310	{0.0, 0},
311	{0.0, 0},
312	{0.0, 0},
313	{0.0, 0}
314	};
315
316	/** MSM signal types for Galileo */
317	static struct CodeData gal[RTCM3_MSM_NUMSIG] = {
318	{0.0, 0},
319	{GAL_WAVELENGTH_E1, "1C"},
320	{GAL_WAVELENGTH_E1, "1A"},
321	{GAL_WAVELENGTH_E1, "1B"},
322	{GAL_WAVELENGTH_E1, "1X"},
323	{GAL_WAVELENGTH_E1, "1Z"},
324	{0.0, 0},
325	{GAL_WAVELENGTH_E6, "6C"},
326	{GAL_WAVELENGTH_E6, "6A"},
327	{GAL_WAVELENGTH_E6, "6B"},
328	{GAL_WAVELENGTH_E6, "6X"},
329	{GAL_WAVELENGTH_E6, "6Z"},
330	{0.0, 0},
331	{GAL_WAVELENGTH_E5B, "7I"},
332	{GAL_WAVELENGTH_E5B, "7Q"},
333	{GAL_WAVELENGTH_E5B, "7X"},
334	{0.0, 0},
335	{GAL_WAVELENGTH_E5AB,"8I"},
336	{GAL_WAVELENGTH_E5AB,"8Q"},
337	{GAL_WAVELENGTH_E5AB,"8X"},
338	{0.0, 0},
339	{GAL_WAVELENGTH_E5A, "5I"},
340	{GAL_WAVELENGTH_E5A, "5Q"},
341	{GAL_WAVELENGTH_E5A, "5X"},
342	{0.0, 0},
343	{0.0, 0},
344	{0.0, 0},
345	{0.0, 0},
346	{0.0, 0},
347	{0.0, 0},
348	{0.0, 0},
349	{0.0, 0}
350	};
351
352	/** MSM signal types for QZSS */
353	static struct CodeData qzss[RTCM3_MSM_NUMSIG] = {
354	{0.0, 0},
355	{GPS_WAVELENGTH_L1, "1C"},
356	{0.0, 0},
357	{0.0, 0},
358	{0.0, 0},
359	{0.0, 0},
360	{0.0, 0},
361	{0.0, 0},
362	{QZSS_WAVELENGTH_L6, "6S"},
363	{QZSS_WAVELENGTH_L6, "6L"},
364	{QZSS_WAVELENGTH_L6, "6X"},
365	{0.0, 0},
366	{0.0, 0},
367	{0.0, 0},
368	{GPS_WAVELENGTH_L2, "2S"},
369	{GPS_WAVELENGTH_L2, "2L"},
370	{GPS_WAVELENGTH_L2, "2X"},
371	{0.0, 0},
372	{0.0, 0},
373	{0.0, 0},
374	{0.0, 0},
375	{GPS_WAVELENGTH_L5, "5I"},
376	{GPS_WAVELENGTH_L5, "5Q"},
377	{GPS_WAVELENGTH_L5, "5X"},
378	{0.0, 0},
379	{0.0, 0},
380	{0.0, 0},
381	{0.0, 0},
382	{0.0, 0},
383	{GPS_WAVELENGTH_L1, "1S"},
384	{GPS_WAVELENGTH_L1, "1L"},
385	{GPS_WAVELENGTH_L1, "1X"}
386	};
387
388	/** MSM signal types for Beidou/BDS */
389	static struct CodeData bds[RTCM3_MSM_NUMSIG] = {
390	{0.0, 0},
391	{BDS_WAVELENGTH_B1, "2I"},
392	{BDS_WAVELENGTH_B1, "2Q"},
393	{BDS_WAVELENGTH_B1, "2X"},
394	{0.0, 0},
395	{0.0, 0},
396	{0.0, 0},
397	{BDS_WAVELENGTH_B3, "6I"},
398	{BDS_WAVELENGTH_B3, "6Q"},
399	{BDS_WAVELENGTH_B3, "6X"},
400	{0.0, 0},
401	{0.0, 0},
402	{0.0, 0},
403	{BDS_WAVELENGTH_B2, "7I"},
404	{BDS_WAVELENGTH_B2, "7Q"},
405	{BDS_WAVELENGTH_B2, "7X"},
406	{0.0, 0},
407	{0.0, 0},
408	{0.0, 0},
409	{0.0, 0},
410	{0.0, 0},
411	{BDS_WAVELENGTH_B2a, "5D"},
412	{BDS_WAVELENGTH_B2a, "5P"},
413	{BDS_WAVELENGTH_B2a, "5X"},
414	{BDS_WAVELENGTH_B2b, "7D"},
415	{0.0, 0},
416	{0.0, 0},
417	{0.0, 0},
418	{0.0, 0},
419	{BDS_WAVELENGTH_B1C, "1D"},
420	{BDS_WAVELENGTH_B1C, "1P"},
421	{BDS_WAVELENGTH_B1C, "1X"}
422	};
423
424	/** MSM signal types for IRNSS */
425	static struct CodeData irn[RTCM3_MSM_NUMSIG] = {
426	{0.0, 0},
427	{0.0, 0},
428	{0.0, 0},
429	{0.0, 0},
430	{0.0, 0},
431	{0.0, 0},
432	{0.0, 0},
433	{IRNSS_WAVELENGTH_S, "9A"},
434	{0.0, 0},
435	{0.0, 0},
436	{0.0, 0},
437	{0.0, 0},
438	{0.0, 0},
439	{0.0, 0},
440	{0.0, 0},
441	{0.0, 0},
442	{0.0, 0},
443	{0.0, 0},
444	{0.0, 0},
445	{0.0, 0},
446	{0.0, 0},
447	{IRNSS_WAVELENGTH_L5, "5A"},
448	{0.0, 0},
449	{0.0, 0},
450	{0.0, 0},
451	{0.0, 0},
452	{0.0, 0},
453	{0.0, 0},
454	{0.0, 0},
455	{0.0, 0},
456	{0.0, 0},
457	{0.0, 0}
458	};
459
460	#define UINT64(c) c ## ULL
461
462	//
463	////////////////////////////////////////////////////////////////////////////
464	bool RTCM3Decoder::DecodeRTCM3MSM(unsigned char* data, int size) {
465	bool decoded = false;
466	int type, syncf, i;
467	uint64_t numbits = 0, bitfield = 0;
468
469	data += 3; /* header */
470	size -= 6; /* header + crc */
471
472	GETBITS(type, 12)
473	SKIPBITS(12)
474	/* id */
475	char sys;
476	if (type >= 1131 && type <= 1137) {
477	sys = 'I';
478	}
479	else if (type >= 1121 && type <= 1127) {
480	sys = 'C';
481	}
482	else if (type >= 1111 && type <= 1117) {
483	sys = 'J';
484	}
485	else if (type >= 1101 && type <= 1107) {
486	sys = 'S';
487	}
488	else if (type >= 1091 && type <= 1097) {
489	sys = 'E';
490	}
491	else if (type >= 1081 && type <= 1087) {
492	sys = 'R';
493	}
494	else if (type >= 1071 && type <= 1077) {
495	sys = 'G';
496	}
497	else {
498	return decoded; // false
499	}
500	bncTime CurrentObsTime;
501	if (sys == 'C') /* BDS */ {
502	GETBITS(i, 30)
503	CurrentObsTime.setBDS(i);
504	}
505	else if (sys == 'R') /* GLONASS */ {
506	SKIPBITS(3)
507	GETBITS(i, 27)
508	/* tk */
509	CurrentObsTime.setTk(i);
510	}
511	else /* GPS style date */ {
512	GETBITS(i, 30)
513	CurrentObsTime.set(i);
514	}
515	if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) {
516	decoded = true;
517	_obsList.append(_CurrentObsList);
518	_CurrentObsList.clear();
519	}
520	_CurrentTime = CurrentObsTime;
521
522	GETBITS(syncf, 1)
523	/**
524	* Ignore unknown types except for sync flag
525	*
526	* We actually support types 1-3 in following code, but as they are missing
527	* the full cycles and can't be used later we skip interpretation here already.
528	*/
529	if (type <= 1137 && (type % 10) >= 4 && (type % 10) <= 7) {
530	int sigmask, numsat = 0, numsig = 0;
531	uint64_t satmask, cellmask, ui;
532	// satellite data
533	double rrmod[RTCM3_MSM_NUMSAT]; // GNSS sat rough ranges modulo 1 millisecond
534	int rrint[RTCM3_MSM_NUMSAT]; // number of integer msecs in GNSS sat rough ranges
535	int rdop[RTCM3_MSM_NUMSAT]; // GNSS sat rough phase range rates
536	int extsat[RTCM3_MSM_NUMSAT];// extended sat info
537	// signal data
538	int ll[RTCM3_MSM_NUMCELLS]; // lock time indicator
539	/int hc[RTCM3_MSM_NUMCELLS];/ // half cycle ambiguity indicator
540	double cnr[RTCM3_MSM_NUMCELLS]; // signal cnr
541	double cp[RTCM3_MSM_NUMCELLS]; // fine phase range data
542	double psr[RTCM3_MSM_NUMCELLS]; // fine psr
543	double dop[RTCM3_MSM_NUMCELLS]; // fine phase range rates
544
545	SKIPBITS(3 + 7 + 2 + 2 + 1 + 3)
546	GETBITS64(satmask, RTCM3_MSM_NUMSAT)
547
548	/* http://gurmeetsingh.wordpress.com/2008/08/05/fast-bit-counting-routines/ */
549	for (ui = satmask; ui; ui &= (ui - 1) /* remove rightmost bit */)
550	++numsat;
551	GETBITS(sigmask, RTCM3_MSM_NUMSIG)
552	for (i = sigmask; i; i &= (i - 1) /* remove rightmost bit */)
553	++numsig;
554	for (i = 0; i < RTCM3_MSM_NUMSAT; ++i)
555	extsat[i] = 15;
556
557	i = numsat * numsig;
558	GETBITS64(cellmask, (unsigned )i)
559	// satellite data
560	switch (type % 10) {
561	case 1:
562	case 2:
563	case 3:
564	/* partial data, already skipped above, but implemented for future expansion ! */
565	for (int j = numsat; j--;)
566	GETFLOAT(rrmod[j], 10, 1.0 / 1024.0)
567	break;
568	case 4:
569	case 6:
570	for (int j = numsat; j--;)
571	GETBITS(rrint[j], 8)
572	for (int j = numsat; j--;)
573	GETFLOAT(rrmod[j], 10, 1.0 / 1024.0)
574	break;
575	case 5:
576	case 7:
577	for (int j = numsat; j--;)
578	GETBITS(rrint[j], 8)
579	for (int j = numsat; j--;)
580	GETBITS(extsat[j], 4)
581	for (int j = numsat; j--;)
582	GETFLOAT(rrmod[j], 10, 1.0 / 1024.0)
583	for (int j = numsat; j--;)
584	GETBITSSIGN(rdop[j], 14)
585	break;
586	}
587	// signal data
588	int numcells = numsat * numsig;
589	/** Drop anything which exceeds our cell limit. Increase limit definition
590	* when that happens. */
591	if (numcells <= RTCM3_MSM_NUMCELLS) {
592	switch (type % 10) {
593	case 1:
594	for (int count = numcells; count--;)
595	if (cellmask & (UINT64(1) << count))
596	GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24))
597	break;
598	case 2:
599	for (int count = numcells; count--;)
600	if (cellmask & (UINT64(1) << count))
601	GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29))
602	for (int count = numcells; count--;)
603	if (cellmask & (UINT64(1) << count))
604	GETBITS(ll[count], 4)
605	for (int count = numcells; count--;)
606	if (cellmask & (UINT64(1) << count))
607	SKIPBITS(1)/GETBITS(hc[count], 1)/
608	break;
609	case 3:
610	for (int count = numcells; count--;)
611	if (cellmask & (UINT64(1) << count))
612	GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24))
613	for (int count = numcells; count--;)
614	if (cellmask & (UINT64(1) << count))
615	GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29))
616	for (int count = numcells; count--;)
617	if (cellmask & (UINT64(1) << count))
618	GETBITS(ll[count], 4)
619	for (int count = numcells; count--;)
620	if (cellmask & (UINT64(1) << count))
621	SKIPBITS(1)/GETBITS(hc[count], 1)/
622	break;
623	case 4:
624	for (int count = numcells; count--;)
625	if (cellmask & (UINT64(1) << count))
626	GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24))
627	for (int count = numcells; count--;)
628	if (cellmask & (UINT64(1) << count))
629	GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29))
630	for (int count = numcells; count--;)
631	if (cellmask & (UINT64(1) << count))
632	GETBITS(ll[count], 4)
633	for (int count = numcells; count--;)
634	if (cellmask & (UINT64(1) << count))
635	SKIPBITS(1)/GETBITS(hc[count], 1)/
636	for (int count = numcells; count--;)
637	if (cellmask & (UINT64(1) << count))
638	GETBITS(cnr[count], 6)
639	break;
640	case 5:
641	for (int count = numcells; count--;)
642	if (cellmask & (UINT64(1) << count))
643	GETFLOATSIGN(psr[count], 15, 1.0 / (1 << 24))
644	for (int count = numcells; count--;)
645	if (cellmask & (UINT64(1) << count))
646	GETFLOATSIGN(cp[count], 22, 1.0 / (1 << 29))
647	for (int count = numcells; count--;)
648	if (cellmask & (UINT64(1) << count))
649	GETBITS(ll[count], 4)
650	for (int count = numcells; count--;)
651	if (cellmask & (UINT64(1) << count))
652	SKIPBITS(1)/GETBITS(hc[count], 1)/
653	for (int count = numcells; count--;)
654	if (cellmask & (UINT64(1) << count))
655	GETFLOAT(cnr[count], 6, 1.0)
656	for (int count = numcells; count--;)
657	if (cellmask & (UINT64(1) << count))
658	GETFLOATSIGN(dop[count], 15, 0.0001)
659	break;
660	case 6:
661	for (int count = numcells; count--;)
662	if (cellmask & (UINT64(1) << count))
663	GETFLOATSIGN(psr[count], 20, 1.0 / (1 << 29))
664	for (int count = numcells; count--;)
665	if (cellmask & (UINT64(1) << count))
666	GETFLOATSIGN(cp[count], 24, 1.0 / (1U << 31))
667	for (int count = numcells; count--;)
668	if (cellmask & (UINT64(1) << count))
669	GETBITS(ll[count], 10)
670	for (int count = numcells; count--;)
671	if (cellmask & (UINT64(1) << count))
672	SKIPBITS(1)/GETBITS(hc[count], 1)/
673	for (int count = numcells; count--;)
674	if (cellmask & (UINT64(1) << count))
675	GETFLOAT(cnr[count], 10, 1.0 / (1 << 4))
676	break;
677	case 7:
678	for (int count = numcells; count--;)
679	if (cellmask & (UINT64(1) << count))
680	GETFLOATSIGN(psr[count], 20, 1.0 / (1 << 29))
681	for (int count = numcells; count--;)
682	if (cellmask & (UINT64(1) << count))
683	GETFLOATSIGN(cp[count], 24, 1.0 / (1U << 31))
684	for (int count = numcells; count--;)
685	if (cellmask & (UINT64(1) << count))
686	GETBITS(ll[count], 10)
687	for (int count = numcells; count--;)
688	if (cellmask & (UINT64(1) << count))
689	SKIPBITS(1)/GETBITS(hc[count], 1)/
690	for (int count = numcells; count--;)
691	if (cellmask & (UINT64(1) << count))
692	GETFLOAT(cnr[count], 10, 1.0 / (1 << 4))
693	for (int count = numcells; count--;)
694	if (cellmask & (UINT64(1) << count))
695	GETFLOATSIGN(dop[count], 15, 0.0001)
696	break;
697	}
698	i = RTCM3_MSM_NUMSAT;
699	int j = -1;
700	t_satObs CurrentObs;
701	for (int count = numcells; count--;) {
702	while (j >= 0 && !(sigmask & (1 << --j)))
703	;
704	if (j < 0) {
705	while (!(satmask & (UINT64(1) << (--i))))
706	/* next satellite */
707	;
708	if (CurrentObs._obs.size() > 0)
709	_CurrentObsList.push_back(CurrentObs);
710	CurrentObs.clear();
711	CurrentObs._time = CurrentObsTime;
712	CurrentObs._type = type;
713	if (sys == 'S')
714	CurrentObs._prn.set(sys, 20 - 1 + RTCM3_MSM_NUMSAT - i);
715	else
716	CurrentObs._prn.set(sys, RTCM3_MSM_NUMSAT - i);
717	j = RTCM3_MSM_NUMSIG;
718	while (!(sigmask & (1 << --j)))
719	;
720	--numsat;
721	}
722	if (cellmask & (UINT64(1) << count)) {
723	struct CodeData cd = {0.0, 0};
724	switch (sys) {
725	case 'J':
726	cd = qzss[RTCM3_MSM_NUMSIG - j - 1];
727	break;
728	case 'C':
729	cd = bds[RTCM3_MSM_NUMSIG - j - 1];
730	break;
731	case 'G':
732	case 'S':
733	cd = gps[RTCM3_MSM_NUMSIG - j - 1];
734	break;
735	case 'R':
736	cd = glo[RTCM3_MSM_NUMSIG - j - 1];
737	{
738	int k = GLOFreq[RTCM3_MSM_NUMSAT - i - 1];
739	if (extsat[numsat] < 14) { // channel number is available as extended info for MSM5/7
740	k = GLOFreq[RTCM3_MSM_NUMSAT - i - 1] = 100 + extsat[numsat] - 7;
741	}
742	if (k) {
743	if (cd.wl == 0.0) {
744	cd.wl = GLO_WAVELENGTH_L1(k - 100);
745	}
746	else if (cd.wl == 1.0) {
747	cd.wl = GLO_WAVELENGTH_L2(k - 100);
748	}
749	}
750	else if (!k && cd.wl <= 1) {
751	cd.code = 0;
752	}
753	}
754	break;
755	case 'E':
756	cd = gal[RTCM3_MSM_NUMSIG - j - 1];
757	break;
758	case 'I':
759	cd = irn[RTCM3_MSM_NUMSIG - j - 1];
760	break;
761	}
762	if (cd.code) {
763	t_frqObs *frqObs = new t_frqObs;
764	frqObs->_rnxType2ch.assign(cd.code);
765
766	switch (type % 10) {
767	case 1:
768	if (psr[count] > -1.0 / (1 << 10)) {
769	frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
770	+ (rrmod[numsat]) * LIGHTSPEED / 1000.0;
771	frqObs->_codeValid = true;
772	}
773	break;
774	case 2:
775	if (cp[count] > -1.0 / (1 << 8)) {
776	frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
777	+ (rrmod[numsat]) * LIGHTSPEED / 1000.0 / cd.wl;
778	frqObs->_phaseValid = true;
779	frqObs->_lockTime = lti2sec(type,ll[count]);
780	frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
781	frqObs->_lockTimeIndicator = ll[count];
782	}
783	break;
784	case 3:
785	if (psr[count] > -1.0 / (1 << 10)) {
786	frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
787	+ (rrmod[numsat]) * LIGHTSPEED / 1000.0;
788	frqObs->_codeValid = true;
789	}
790	if (cp[count] > -1.0 / (1 << 8)) {
791	frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
792	+ rrmod[numsat] * LIGHTSPEED / 1000.0 / cd.wl;
793	frqObs->_phaseValid = true;
794	frqObs->_lockTime = lti2sec(type,ll[count]);
795	frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
796	frqObs->_lockTimeIndicator = ll[count];
797	}
798	break;
799	case 4:
800	if (psr[count] > -1.0 / (1 << 10)) {
801	frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
802	+ (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;
803	frqObs->_codeValid = true;
804	}
805	if (cp[count] > -1.0 / (1 << 8)) {
806	frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
807	+ (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 / cd.wl;
808	frqObs->_phaseValid = true;
809	frqObs->_lockTime = lti2sec(type,ll[count]);
810	frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
811	frqObs->_lockTimeIndicator = ll[count];
812	}
813	frqObs->_snr = cnr[count];
814	frqObs->_snrValid = true;
815	break;
816	case 5:
817	if (psr[count] > -1.0 / (1 << 10)) {
818	frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
819	+ (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;
820	frqObs->_codeValid = true;
821	}
822	if (cp[count] > -1.0 / (1 << 8)) {
823	frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
824	+ (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 / cd.wl;
825	frqObs->_phaseValid = true;
826	frqObs->_lockTime = lti2sec(type,ll[count]);
827	frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
828	frqObs->_lockTimeIndicator = ll[count];
829	}
830	frqObs->_snr = cnr[count];
831	frqObs->_snrValid = true;
832	if (dop[count] > -1.6384) {
833	frqObs->_doppler = -(dop[count] + rdop[numsat]) / cd.wl;
834	frqObs->_dopplerValid = true;
835	}
836	break;
837	case 6:
838	if (psr[count] > -1.0 / (1 << 10)) {
839	frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
840	+ (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;
841	frqObs->_codeValid = true;
842	}
843	if (cp[count] > -1.0 / (1 << 8)) {
844	frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
845	+ (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 / cd.wl;
846	frqObs->_phaseValid = true;
847	frqObs->_lockTime = lti2sec(type,ll[count]);
848	frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
849	frqObs->_lockTimeIndicator = ll[count];
850	}
851
852	frqObs->_snr = cnr[count];
853	frqObs->_snrValid = true;
854	break;
855	case 7:
856	if (psr[count] > -1.0 / (1 << 10)) {
857	frqObs->_code = psr[count] * LIGHTSPEED / 1000.0
858	+ (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0;
859	frqObs->_codeValid = true;
860	}
861	if (cp[count] > -1.0 / (1 << 8)) {
862	frqObs->_phase = cp[count] * LIGHTSPEED / 1000.0 / cd.wl
863	+ (rrmod[numsat] + rrint[numsat]) * LIGHTSPEED / 1000.0 / cd.wl;
864	frqObs->_phaseValid = true;
865	frqObs->_lockTime = lti2sec(type,ll[count]);
866	frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0);
867	frqObs->_lockTimeIndicator = ll[count];
868	}
869
870	frqObs->_snr = cnr[count];
871	frqObs->_snrValid = true;
872
873	if (dop[count] > -1.6384) {
874	frqObs->_doppler = -(dop[count] + rdop[numsat]) / cd.wl;
875	frqObs->_dopplerValid = true;
876	}
877	break;
878	}
879	CurrentObs._obs.push_back(frqObs);
880	}
881	}
882	}
883	if (CurrentObs._obs.size() > 0) {
884	_CurrentObsList.push_back(CurrentObs);
885	}
886	}
887	}
888	else if ((type % 10) < 4) {
889	#ifdef BNC_DEBUG_OBS
890	emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")
891	.arg(_staID).arg(type).toLatin1(), true));
892	#endif
893	}
894	if (!syncf) {
895	decoded = true;
896	_obsList.append(_CurrentObsList);
897	_CurrentTime.reset();
898	_CurrentObsList.clear();
899	}
900	return decoded;
901	}
902
903	//
904	////////////////////////////////////////////////////////////////////////////
905	bool RTCM3Decoder::DecodeRTCM3GLONASS(unsigned char* data, int size) {
906	bool decoded = false;
907	bncTime CurrentObsTime;
908	int i, numsats, syncf, type;
909	uint64_t numbits = 0, bitfield = 0;
910
911	data += 3; /* header */
912	size -= 6; /* header + crc */
913
914	GETBITS(type, 12)
915	SKIPBITS(12)
916	/* id */
917	GETBITS(i, 27)
918	/* tk */
919
920	CurrentObsTime.setTk(i);
921	if (_CurrentTime.valid() && CurrentObsTime != _CurrentTime) {
922	decoded = true;
923	_obsList.append(_CurrentObsList);
924	_CurrentObsList.clear();
925	}
926	_CurrentTime = CurrentObsTime;
927
928	GETBITS(syncf, 1)
929	/* sync */
930	GETBITS(numsats, 5)
931	SKIPBITS(4)
932	/* smind, smint */
933
934	while (numsats--) {
935	int sv, code, l1range, amb = 0, freq;
936	t_satObs CurrentObs;
937	CurrentObs._time = CurrentObsTime;
938	CurrentObs._type = type;
939
940	GETBITS(sv, 6)
941	CurrentObs._prn.set('R', sv);
942	GETBITS(code, 1)
943	GETBITS(freq, 5)
944	GLOFreq[sv - 1] = 100 + freq - 7; /* store frequency for other users (MSM) */
945
946	t_frqObs *frqObs = new t_frqObs;
947	/* L1 */
948	(code) ?
949	frqObs->_rnxType2ch.assign("1P") : frqObs->_rnxType2ch.assign("1C");
950	GETBITS(l1range, 25);
951	GETBITSSIGN(i, 20);
952	if ((i & ((1 << 20) - 1)) != 0x80000) {
953	frqObs->_code = l1range * 0.02;
954	frqObs->_phase = (l1range * 0.02 + i * 0.0005) / GLO_WAVELENGTH_L1(freq - 7);
955	frqObs->_codeValid = frqObs->_phaseValid = true;
956	}
957	GETBITS(frqObs->_lockTimeIndicator, 7);
958	frqObs->_lockTime = lti2sec(type, frqObs->_lockTimeIndicator);
959	frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0 && frqObs->_phaseValid);
960	if (type == 1010 \|\| type == 1012) {
961	GETBITS(amb, 7);
962	if (amb) {
963	frqObs->_code += amb * 599584.916;
964	frqObs->_phase += (amb * 599584.916) / GLO_WAVELENGTH_L1(freq - 7);
965	}
966	GETBITS(i, 8);
967	if (i) {
968	frqObs->_snr = i * 0.25;
969	frqObs->_snrValid = true;
970	}
971	}
972	CurrentObs._obs.push_back(frqObs);
973	if (type == 1011 \|\| type == 1012) {
974	frqObs = new t_frqObs;
975	/* L2 */
976	GETBITS(code, 2);
977	switch (code) {
978	case 3:
979	frqObs->_rnxType2ch.assign("2P");
980	break;
981	case 2:
982	frqObs->_rnxType2ch.assign("2P");
983	break;
984	case 1:
985	frqObs->_rnxType2ch.assign("2P");
986	break;
987	case 0:
988	frqObs->_rnxType2ch.assign("2C");
989	break;
990	}
991	GETBITSSIGN(i, 14);
992	if ((i & ((1 << 14) - 1)) != 0x2000) {
993	frqObs->_code = l1range * 0.02 + i * 0.02 + amb * 599584.916;
994	frqObs->_codeValid = true;
995	}
996	GETBITSSIGN(i, 20);
997	if ((i & ((1 << 20) - 1)) != 0x80000) {
998	frqObs->_phase = (l1range * 0.02 + i * 0.0005 + amb * 599584.916)
999	/ GLO_WAVELENGTH_L2(freq - 7);
1000	frqObs->_phaseValid = true;
1001	}
1002	GETBITS(frqObs->_lockTimeIndicator, 7);
1003	frqObs->_lockTime = lti2sec(type, frqObs->_lockTimeIndicator);
1004	frqObs->_lockTimeValid = (frqObs->_lockTime >= 0.0 && frqObs->_phaseValid);
1005	if (type == 1012) {
1006	GETBITS(i, 8);
1007	if (i) {
1008	frqObs->_snr = i * 0.25;
1009	frqObs->_snrValid = true;
1010	}
1011	}
1012	CurrentObs._obs.push_back(frqObs);
1013	}
1014	_CurrentObsList.push_back(CurrentObs);
1015	}
1016	if (!syncf) {
1017	decoded = true;
1018	_obsList.append(_CurrentObsList);
1019	_CurrentTime.reset();
1020	_CurrentObsList.clear();
1021	}
1022	return decoded;
1023	}
1024
1025	//
1026	////////////////////////////////////////////////////////////////////////////
1027	bool RTCM3Decoder::DecodeGPSEphemeris(unsigned char* data, int size) {
1028	bool decoded = false;
1029
1030	if (size == 67) {
1031	t_ephGPS eph;
1032	int i, week;
1033	uint64_t numbits = 0, bitfield = 0;
1034	int fitIntervalFalg = 0;
1035
1036	data += 3; /* header */
1037	size -= 6; /* header + crc */
1038	SKIPBITS(12)
1039
1040	eph._receptDateTime = currentDateAndTimeGPS();
1041	eph._receptStaID = _staID;
1042
1043	GETBITS(i, 6)
1044	eph._prn.set('G', i);
1045	GETBITS(week, 10)
1046	GETBITS(i, 4)
1047	eph._ura = accuracyFromIndex(i, eph.type());
1048	GETBITS(eph._L2Codes, 2)
1049	GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1050	GETBITS(eph._IODE, 8)
1051	GETBITS(i, 16)
1052	i <<= 4;
1053	eph._TOC.set(i * 1000);
1054	GETFLOATSIGN(eph._clock_driftrate, 8, 1.0 / (double )(1 << 30) / (double )(1 << 25))
1055	GETFLOATSIGN(eph._clock_drift, 16, 1.0 / (double )(1 << 30) / (double )(1 << 13))
1056	GETFLOATSIGN(eph._clock_bias, 22, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1057	GETBITS(eph._IODC, 10)
1058	GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
1059	GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1060	GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1061	GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))
1062	GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
1063	GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))
1064	GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
1065	if (eph._sqrt_A < 1000.0) {
1066	#ifdef BNC_DEBUG_BCEP
1067	emit(newMessage(QString("%1: Block %2 (%3) SQRT_A %4 m!")
1068	.arg(_staID).arg(1019,4).arg(eph._prn.toString().c_str())
1069	.arg(eph._sqrt_A,10,'F',3).toLatin1(), true));
1070	#endif
1071	return false;
1072	}
1073	GETBITS(i, 16)
1074	i <<= 4;
1075	eph._TOEsec = i;
1076	bncTime t;
1077	t.set(i * 1000);
1078	eph._TOEweek = t.gpsw();
1079	int numOfRollOvers = int(floor(t.gpsw()/1024.0));
1080	week += (numOfRollOvers * 1024);
1081	/* week from HOW, differs from TOC, TOE week, we use adapted value instead */
1082	if (eph._TOEweek > week + 1 \|\| eph._TOEweek < week - 1) /* invalid week */
1083	return false;
1084	GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))
1085	GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1086	GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))
1087	GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1088	GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))
1089	GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1090	GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1091	GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1092	GETBITS(eph._health, 6)
1093	GETBITS(eph._L2PFlag, 1)
1094	GETBITS(fitIntervalFalg, 1)
1095	eph._fitInterval = fitIntervalFromFlag(fitIntervalFalg, eph._IODC, eph.type());
1096	eph._TOT = 0.9999e9;
1097	eph._navType = t_eph::LNAV;
1098
1099	emit newGPSEph(eph);
1100	decoded = true;
1101	}
1102	return decoded;
1103	}
1104
1105	//
1106	////////////////////////////////////////////////////////////////////////////
1107	bool RTCM3Decoder::DecodeGLONASSEphemeris(unsigned char* data, int size) {
1108	bool decoded = false;
1109
1110	if (size == 51) {
1111	t_ephGlo eph;
1112	int sv, i, tk;
1113	uint64_t numbits = 0, bitfield = 0;
1114
1115	data += 3; /* header */
1116	size -= 6; /* header + crc */
1117	SKIPBITS(12)
1118
1119	eph._receptDateTime = currentDateAndTimeGPS();
1120	eph._receptStaID = _staID;
1121
1122	eph._flags_unknown = true;
1123
1124	GETBITS(sv, 6)
1125	eph._prn.set('R', sv);
1126
1127	GETBITS(i, 5)
1128	eph._frequency_number = i - 7;
1129	GETBITS(eph._almanac_health, 1) /* almanac healthy */
1130	GETBITS(eph._almanac_health_availablility_indicator, 1) /* almanac health ok */
1131	/*
1132	if (eph._almanac_health_availablility_indicator == 0.0) {
1133	#ifdef BNC_DEBUG_BCEP
1134	emit(newMessage(QString("%1: Block %2 (%3): ALM = %4: missing data!")
1135	.arg(_staID).arg(1020,4).arg(eph._prn.toString().c_str())
1136	.arg(eph._almanac_health_availablility_indicator).toLatin1(), true));
1137	#endif
1138	return false;
1139	}
1140	*/
1141	GETBITS(eph._P1, 2) /* P1 */
1142	GETBITS(i, 5)
1143	tk = i * 60 * 60;
1144	GETBITS(i, 6)
1145	tk += i * 60;
1146	GETBITS(i, 1)
1147	tk += i * 30;
1148	eph._tki = tk - 36060;
1149	if(eph._tki < 0.0) {
1150	eph._tki += 86400.0;
1151	}
1152	GETBITS(eph._health, 1) /* MSB of Bn*/
1153	GETBITS(eph._P2, 1) /* P2 */
1154	GETBITS(i, 7)
1155	eph._TOC.setTk(i * 15 * 60 * 1000); /* tb */
1156
1157	GETFLOATSIGNM(eph._x_velocity, 24, 1.0 / (double )(1 << 20))
1158	GETFLOATSIGNM(eph._x_pos, 27, 1.0 / (double )(1 << 11))
1159	GETFLOATSIGNM(eph._x_acceleration, 5, 1.0 / (double )(1 << 30))
1160	GETFLOATSIGNM(eph._y_velocity, 24, 1.0 / (double )(1 << 20))
1161	GETFLOATSIGNM(eph._y_pos, 27, 1.0 / (double )(1 << 11))
1162	GETFLOATSIGNM(eph._y_acceleration, 5, 1.0 / (double )(1 << 30))
1163	GETFLOATSIGNM(eph._z_velocity, 24, 1.0 / (double )(1 << 20))
1164	GETFLOATSIGNM(eph._z_pos, 27, 1.0 / (double )(1 << 11))
1165	GETFLOATSIGNM(eph._z_acceleration, 5, 1.0 / (double )(1 << 30))
1166	GETBITS(eph._P3, 1) /* P3 */
1167	GETFLOATSIGNM(eph._gamma, 11, 1.0 / (double )(1 << 30) / (double )(1 << 10))
1168	GETBITS(eph._M_P, 2) /* GLONASS-M P, */
1169	GETBITS(eph._M_l3, 1) /* GLONASS-M ln (third string) */
1170	GETFLOATSIGNM(eph._tau, 22, 1.0 / (double )(1 << 30)) /* GLONASS tau n(tb) */
1171	GETFLOATSIGNM(eph._M_delta_tau, 5, 1.0 / (double )(1 << 30)) /* GLONASS-M delta tau n(tb) */
1172	GETBITS(eph._E, 5)
1173	GETBITS(eph._M_P4, 1) /* GLONASS-M P4 */
1174	GETBITS(eph._M_FT, 4) /* GLONASS-M Ft */
1175	GETBITS(eph._M_NT, 11) /* GLONASS-M Nt */
1176	if (eph._M_NT == 0.0) {
1177	#ifdef BNC_DEBUG_BCEP
1178	emit(newMessage(QString("%1: Block %2 (%3): NT = %4: missing data!")
1179	.arg(_staID).arg(1020,4).arg(eph._prn.toString().c_str()).arg(eph._M_NT,4).toLatin1(), true));
1180	#endif
1181	return false;
1182	}
1183	GETBITS(eph._M_M, 2) /* GLONASS-M M */
1184	GETBITS(eph._additional_data_availability, 1) /* GLONASS-M The Availability of Additional Data */
1185	if (eph._additional_data_availability == 0.0) {
1186	#ifdef BNC_DEBUG_BCEP
1187	emit(newMessage(QString("%1: Block %2 (%3): ADD = %4: missing data!")
1188	.arg(_staID).arg(1020,4).arg(eph._prn.toString().c_str())
1189	.arg(eph._additional_data_availability).toLatin1(), true));
1190	#endif
1191	return false;
1192	}
1193	GETBITS(eph._NA, 11) /* GLONASS-M Na */
1194	GETFLOATSIGNM(eph._tauC, 32, 1.0/(double)(1<<30)/(double)(1<<1)) /* GLONASS tau c */
1195	GETBITS(eph._M_N4, 5) /* GLONASS-M N4 */
1196	GETFLOATSIGNM(eph._M_tau_GPS, 22, 1.0/(double)(1<<30)) /* GLONASS-M tau GPS */
1197	GETBITS(eph._M_l5, 1) /* GLONASS-M ln (fifth string) */
1198
1199	unsigned year, month, day;
1200	eph._TOC.civil_date(year, month, day);
1201	eph._gps_utc = gnumleap(year, month, day);
1202	eph._tt = eph._TOC;
1203
1204	eph._xv(1) = eph._x_pos * 1.e3;
1205	eph._xv(2) = eph._y_pos * 1.e3;
1206	eph._xv(3) = eph._z_pos * 1.e3;
1207	if (eph._xv.Rows(1,3).NormFrobenius() < 1.0) {
1208	#ifdef BNC_DEBUG_BCEP
1209	emit(newMessage(QString("%1: Block %2 (%3): zero position!")
1210	.arg(_staID).arg(1020,4).arg(eph._prn.toString().c_str()).toLatin1(), true));
1211	#endif
1212	return false;
1213	}
1214	eph._xv(4) = eph._x_velocity * 1.e3;
1215	eph._xv(5) = eph._y_velocity * 1.e3;
1216	eph._xv(6) = eph._z_velocity * 1.e3;
1217	if (eph._xv.Rows(4,6).NormFrobenius() < 1.0) {
1218	#ifdef BNC_DEBUG_BCEP
1219	emit(newMessage(QString("%1: Block %2 (%3): zero velocity!")
1220	.arg(_staID).arg(1020,4).arg(eph._prn.toString().c_str()).toLatin1(), true));
1221	#endif
1222	return false;
1223	}
1224	GLOFreq[sv - 1] = 100 + eph._frequency_number ; /* store frequency for other users (MSM) */
1225	_gloFrq = QString("%1 %2").arg(eph._prn.toString().c_str()).arg(eph._frequency_number, 2, 'f', 0);
1226
1227	eph._navType = t_eph::FDMA;
1228
1229	emit newGlonassEph(eph);
1230	decoded = true;
1231	}
1232	return decoded;
1233	}
1234
1235	//
1236	////////////////////////////////////////////////////////////////////////////
1237	bool RTCM3Decoder::DecodeQZSSEphemeris(unsigned char* data, int size) {
1238	bool decoded = false;
1239
1240	if (size == 67) {
1241	t_ephGPS eph;
1242	int i, week;
1243	uint64_t numbits = 0, bitfield = 0;
1244	int fitIntervalFalg = 0;
1245
1246	data += 3; /* header */
1247	size -= 6; /* header + crc */
1248	SKIPBITS(12)
1249
1250	eph._receptDateTime = currentDateAndTimeGPS();
1251	eph._receptStaID = _staID;
1252
1253	GETBITS(i, 4)
1254	eph._prn.set('J', i);
1255
1256	GETBITS(i, 16)
1257	i <<= 4;
1258	eph._TOC.set(i * 1000);
1259
1260	GETFLOATSIGN(eph._clock_driftrate, 8, 1.0 / (double )(1 << 30) / (double )(1 << 25))
1261	GETFLOATSIGN(eph._clock_drift, 16, 1.0 / (double )(1 << 30) / (double )(1 << 13))
1262	GETFLOATSIGN(eph._clock_bias, 22, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1263	GETBITS(eph._IODE, 8)
1264	GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
1265	GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1266	GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1267	GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))
1268	GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
1269	GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))
1270	GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
1271	if (eph._sqrt_A < 1000.0) {
1272	#ifdef BNC_DEBUG_BCEP
1273	emit(newMessage(QString("%1: Block %2 (%3) SQRT_A %4 m!")
1274	.arg(_staID).arg(1044,4).arg(eph._prn.toString().c_str())
1275	.arg(eph._sqrt_A,10,'F',3).toLatin1(), true));
1276	#endif
1277	return false;
1278	}
1279	GETBITS(i, 16)
1280	i <<= 4;
1281	eph._TOEsec = i;
1282	bncTime t;
1283	t.set(i*1000);
1284	eph._TOEweek = t.gpsw();
1285	GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))
1286	GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1287	GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))
1288	GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1289	GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))
1290	GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1291	GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1292	GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1293	GETBITS(eph._L2Codes, 2)
1294	GETBITS(week, 10)
1295	int numOfRollOvers = int(floor(t.gpsw()/1024.0));
1296	week += (numOfRollOvers * 1024);
1297	/* week from HOW, differs from TOC, TOE week, we use adapted value instead */
1298	if (eph._TOEweek > week + 1 \|\| eph._TOEweek < week - 1) /* invalid week */
1299	return false;
1300
1301	GETBITS(i, 4)
1302	if (i <= 6)
1303	eph._ura = ceil(10.0 * pow(2.0, 1.0 + i / 2.0)) / 10.0;
1304	else
1305	eph._ura = ceil(10.0 * pow(2.0, i / 2.0)) / 10.0;
1306	GETBITS(eph._health, 6)
1307	GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1308	GETBITS(eph._IODC, 10)
1309	GETBITS(fitIntervalFalg, 1)
1310	eph._fitInterval = fitIntervalFromFlag(fitIntervalFalg, eph._IODC, eph.type());
1311	eph._TOT = 0.9999e9;
1312	eph._navType = t_eph::LNAV;
1313
1314	emit newGPSEph(eph);
1315	decoded = true;
1316	}
1317	return decoded;
1318	}
1319
1320	//
1321	////////////////////////////////////////////////////////////////////////////
1322	bool RTCM3Decoder::DecodeIRNSSEphemeris(unsigned char* data, int size) {
1323	bool decoded = false;
1324
1325	if (size == 67) {
1326	t_ephGPS eph;
1327	int i, week, L5Flag, SFlag;
1328	uint64_t numbits = 0, bitfield = 0;
1329
1330	data += 3; /* header */
1331	size -= 6; /* header + crc */
1332	SKIPBITS(12)
1333
1334	eph._receptDateTime = currentDateAndTimeGPS();
1335	eph._receptStaID = _staID;
1336
1337	GETBITS(i, 6)
1338	eph._prn.set('I', i);
1339	GETBITS(week, 10)
1340	GETFLOATSIGN(eph._clock_bias, 22, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1341	GETFLOATSIGN(eph._clock_drift, 16, 1.0 / (double )(1 << 30) / (double )(1 << 13))
1342	GETFLOATSIGN(eph._clock_driftrate, 8, 1.0 / (double )(1 << 30) / (double )(1 << 25))
1343	GETBITS(i, 4)
1344	eph._ura = accuracyFromIndex(i, eph.type());
1345	GETBITS(i, 16)
1346	i <<= 4;
1347	eph._TOC.set(i * 1000);
1348	GETFLOATSIGN(eph._TGD, 8, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1349	GETFLOATSIGN(eph._Delta_n, 22, R2R_PI/(double)(1<<30)/(double)(1 << 11))
1350	// IODCE
1351	GETBITS(eph._IODE, 8)
1352	eph._IODC = eph._IODE;
1353	SKIPBITS(10)
1354	GETBITS(L5Flag, 1)
1355	GETBITS(SFlag, 1)
1356	if (L5Flag == 0 && SFlag == 0) {
1357	eph._health = 0.0;
1358	}
1359	else if (L5Flag == 0 && SFlag == 1) {
1360	eph._health = 1.0;
1361	}
1362	else if (L5Flag == 1 && SFlag == 0) {
1363	eph._health = 2.0;
1364	}
1365	else if (L5Flag == 1 && SFlag == 1) {
1366	eph._health = 3.0;
1367	}
1368	GETFLOATSIGN(eph._Cuc, 15, 1.0 / (double )(1 << 28))
1369	GETFLOATSIGN(eph._Cus, 15, 1.0 / (double )(1 << 28))
1370	GETFLOATSIGN(eph._Cic, 15, 1.0 / (double )(1 << 28))
1371	GETFLOATSIGN(eph._Cis, 15, 1.0 / (double )(1 << 28))
1372	GETFLOATSIGN(eph._Crc, 15, 1.0 / (double )(1 << 4))
1373	GETFLOATSIGN(eph._Crs, 15, 1.0 / (double )(1 << 4))
1374	GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1375	SKIPBITS(2)
1376	GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<< 1))
1377	GETBITS(i, 16)
1378	i <<= 4;
1379	eph._TOEsec = i;
1380	bncTime t;
1381	t.set(i * 1000);
1382	eph._TOEweek = t.gpsw();
1383	int numOfRollOvers = int(floor(t.gpsw()/1024.0));
1384	week += (numOfRollOvers * 1024);
1385	/* week from HOW, differs from TOC, TOE week, we use adapted value instead */
1386	if (eph._TOEweek > week + 1 \|\| eph._TOEweek < week - 1) /* invalid week */
1387	return false;
1388	GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
1389	GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
1390	if (eph._sqrt_A < 1000.0) {
1391	#ifdef BNC_DEBUG_BCEP
1392	emit(newMessage(QString("%1: Block %2 (%3) SQRT_A %4 m!")
1393	.arg(_staID).arg(1041,4).arg(eph._prn.toString().c_str())
1394	.arg(eph._sqrt_A,10,'F',3).toLatin1(), true));
1395	#endif
1396	return false;
1397	}
1398	GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<< 1))
1399	GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<< 1))
1400	GETFLOATSIGN(eph._OMEGADOT, 22, R2R_PI/(double)(1<<30)/(double)(1<<11))
1401	GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<< 1))
1402	SKIPBITS(2)
1403	eph._TOT = 0.9999e9;
1404	eph._navType = t_eph::LNAV;
1405
1406	emit newGPSEph(eph);
1407	decoded = true;
1408	}
1409	return decoded;
1410	}
1411
1412	//
1413	////////////////////////////////////////////////////////////////////////////
1414	bool RTCM3Decoder::DecodeSBASEphemeris(unsigned char* data, int size) {
1415	bool decoded = false;
1416
1417	if (size == 35) {
1418	t_ephSBAS eph;
1419	int i;
1420	uint64_t numbits = 0, bitfield = 0;
1421
1422	data += 3; /* header */
1423	size -= 6; /* header + crc */
1424	SKIPBITS(12)
1425
1426	eph._receptDateTime = currentDateAndTimeGPS();
1427	eph._receptStaID = _staID;
1428
1429	GETBITS(i, 6)
1430	eph._prn.set('S', 20 + i);
1431	GETBITS(eph._IODN, 8)
1432	GETBITS(i, 13)
1433	i <<= 4;
1434	eph._TOC.setTOD(i * 1000);
1435	GETBITS(i, 4)
1436	eph._ura = accuracyFromIndex(i, eph.type());
1437	GETFLOATSIGN(eph._x_pos, 30, 0.08)
1438	GETFLOATSIGN(eph._y_pos, 30, 0.08)
1439	GETFLOATSIGN(eph._z_pos, 25, 0.4)
1440	ColumnVector pos(3);
1441	pos(1) = eph._x_pos; pos(2) = eph._y_pos; pos(3) = eph._z_pos;
1442	if (pos.NormFrobenius() < 1.0) {
1443	#ifdef BNC_DEBUG_BCEP
1444	emit(newMessage(QString("%1: Block %2 (%3): zero position!")
1445	.arg(_staID).arg(1043,4).arg(eph._prn.toString().c_str()).toLatin1(), true));
1446	#endif
1447	return false;
1448	}
1449	GETFLOATSIGN(eph._x_velocity, 17, 0.000625)
1450	GETFLOATSIGN(eph._y_velocity, 17, 0.000625)
1451	GETFLOATSIGN(eph._z_velocity, 18, 0.004)
1452	GETFLOATSIGN(eph._x_acceleration, 10, 0.0000125)
1453	GETFLOATSIGN(eph._y_acceleration, 10, 0.0000125)
1454	GETFLOATSIGN(eph._z_acceleration, 10, 0.0000625)
1455	GETFLOATSIGN(eph._agf0, 12, 1.0 / (1 << 30) / (1 << 1))
1456	GETFLOATSIGN(eph._agf1, 8, 1.0 / (1 << 30) / (1 << 10))
1457
1458	eph._TOT = 0.9999E9;
1459	eph._health = 0;
1460	eph._navType = t_eph::SBASL1;
1461
1462	emit newSBASEph(eph);
1463	decoded = true;
1464	}
1465	return decoded;
1466	}
1467
1468	//
1469	////////////////////////////////////////////////////////////////////////////
1470	bool RTCM3Decoder::DecodeGalileoEphemeris(unsigned char* data, int size) {
1471	bool decoded = false;
1472	uint64_t numbits = 0, bitfield = 0;
1473	int i;
1474
1475	data += 3; /* header */
1476	size -= 6; /* header + crc */
1477	GETBITS(i, 12)
1478
1479	if ((i == 1046 && size == 61) \|\| (i == 1045 && size == 60)) {
1480	t_ephGal eph;
1481
1482	eph._receptDateTime = currentDateAndTimeGPS();
1483	eph._receptStaID = _staID;
1484
1485	eph._inav = (i == 1046);
1486	eph._fnav = (i == 1045);
1487	GETBITS(i, 6)
1488	eph._prn.set('E', i, eph._inav ? 1 : 0);
1489
1490	GETBITS(eph._TOEweek, 12) //FIXME: roll-over after week 4095!!
1491	GETBITS(eph._IODnav, 10)
1492	GETBITS(i, 8)
1493	eph._SISA = accuracyFromIndex(i, eph.type());
1494	GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1495	GETBITSFACTOR(i, 14, 60)
1496	eph._TOC.set(1024 + eph._TOEweek, i);
1497	GETFLOATSIGN(eph._clock_driftrate, 6, 1.0 / (double )(1 << 30) / (double )(1 << 29))
1498	GETFLOATSIGN(eph._clock_drift, 21, 1.0 / (double )(1 << 30) / (double )(1 << 16))
1499	GETFLOATSIGN(eph._clock_bias, 31, 1.0 / (double )(1 << 30) / (double )(1 << 4))
1500	GETFLOATSIGN(eph._Crs, 16, 1.0 / (double )(1 << 5))
1501	GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1502	GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1503	GETFLOATSIGN(eph._Cuc, 16, 1.0 / (double )(1 << 29))
1504	GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
1505	GETFLOATSIGN(eph._Cus, 16, 1.0 / (double )(1 << 29))
1506	GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
1507	GETBITSFACTOR(eph._TOEsec, 14, 60)
1508	/* FIXME: overwrite value, copied from old code */
1509	eph._TOEsec = eph._TOC.gpssec();
1510	GETFLOATSIGN(eph._Cic, 16, 1.0 / (double )(1 << 29))
1511	GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1512	GETFLOATSIGN(eph._Cis, 16, 1.0 / (double )(1 << 29))
1513	GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1514	GETFLOATSIGN(eph._Crc, 16, 1.0 / (double )(1 << 5))
1515	GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1516	GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1517	GETFLOATSIGN(eph._BGD_1_5A, 10, 1.0 / (double )(1 << 30) / (double )(1 << 2))
1518	if (eph._inav) {
1519	/* set unused F/NAV values */
1520	eph._E5aHS = 0.0;
1521	eph._e5aDataInValid = false;
1522
1523	GETFLOATSIGN(eph._BGD_1_5B, 10, 1.0 / (double )(1 << 30) / (double )(1 << 2))
1524	GETBITS(eph._E5bHS, 2)
1525	GETBITS(eph._e5bDataInValid, 1)
1526	GETBITS(eph._E1_bHS, 2)
1527	GETBITS(eph._e1DataInValid, 1)
1528	if (eph._E5bHS != eph._E1_bHS) {
1529	#ifdef BNC_DEBUG_BCEP
1530	emit(newMessage(QString("%1: Block %2 (%3) SHS E5b %4 E1B %5: inconsistent health!")
1531	.arg(_staID).arg(1046,4).arg(eph._prn.toString().c_str())
1532	.arg(eph._E5bHS).arg(eph._E1_bHS).toLatin1(), true));
1533	#endif
1534	return false;
1535	}
1536	if ((eph._BGD_1_5A == 0.0 && fabs(eph._BGD_1_5B) > 1e-9) \|\|
1537	(eph._BGD_1_5B == 0.0 && fabs(eph._BGD_1_5A) > 1e-9)) {
1538	#ifdef BNC_DEBUG_BCEP
1539	emit(newMessage(QString("%1: Block %2 (%3) BGD_15a = %4 BGD_15b = %5: inconsistent BGD!")
1540	.arg(_staID).arg(1046,4).arg(eph._prn.toString().c_str())
1541	.arg(eph._BGD_1_5A,10,'E',3).arg(eph._BGD_1_5B,10,'E',3).toLatin1(), true));
1542	#endif
1543	return false;
1544	}
1545	eph._navType = t_eph::INAF;
1546	}
1547	else {
1548	/* set unused I/NAV values */
1549	eph._BGD_1_5B = 0.0;
1550	eph._E5bHS = 0.0;
1551	eph._E1_bHS = 0.0;
1552	eph._e1DataInValid = false;
1553	eph._e5bDataInValid = false;
1554
1555	GETBITS(eph._E5aHS, 2)
1556	GETBITS(eph._e5aDataInValid, 1)
1557	eph._navType = t_eph::FNAV;
1558	}
1559	eph._TOT = 0.9999e9;
1560
1561	if (eph._sqrt_A < 1000.0) {
1562	#ifdef BNC_DEBUG_BCEP
1563	emit(newMessage(QString("%1: Block %2 (%3) SQRT_A %4 m!")
1564	.arg(_staID).arg(eph._inav? 1046 : 1045,4).arg(eph._prn.toString().c_str())
1565	.arg(eph._sqrt_A,10,'F',3).toLatin1(), true));
1566	#endif
1567	return false;
1568	}
1569
1570	emit newGalileoEph(eph);
1571	decoded = true;
1572	}
1573	return decoded;
1574	}
1575
1576	//
1577	////////////////////////////////////////////////////////////////////////////
1578	bool RTCM3Decoder::DecodeBDSEphemeris(unsigned char* data, int size) {
1579	bool decoded = false;
1580	const double iMaxGEO = 10.0 / 180.0 * M_PI;
1581
1582	if (size == 70) {
1583	t_ephBDS eph;
1584	int i;
1585	uint64_t numbits = 0, bitfield = 0;
1586
1587	data += 3; /* header */
1588	size -= 6; /* header + crc */
1589	SKIPBITS(12)
1590
1591	eph._receptDateTime = currentDateAndTimeGPS();
1592	eph._receptStaID = _staID;
1593
1594	GETBITS(i, 6)
1595	eph._prn.set('C', i);
1596
1597	GETBITS(eph._BDTweek, 13)
1598	GETBITS(i, 4)
1599	eph._URA = accuracyFromIndex(i, eph.type());
1600	GETFLOATSIGN(eph._IDOT, 14, R2R_PI/(double)(1<<30)/(double)(1<<13))
1601	GETBITS(eph._AODE, 5)
1602	GETBITS(i, 17)
1603	i <<= 3;
1604	eph._TOC.setBDS(eph._BDTweek, i);
1605	GETFLOATSIGN(eph._clock_driftrate, 11, 1.0 / (double )(1 << 30) / (double )(1 << 30) / (double )(1 << 6))
1606	GETFLOATSIGN(eph._clock_drift, 22, 1.0 / (double )(1 << 30) / (double )(1 << 20))
1607	GETFLOATSIGN(eph._clock_bias, 24, 1.0 / (double )(1 << 30) / (double )(1 << 3))
1608	GETBITS(eph._AODC, 5)
1609	GETFLOATSIGN(eph._Crs, 18, 1.0 / (double )(1 << 6))
1610	GETFLOATSIGN(eph._Delta_n, 16, R2R_PI/(double)(1<<30)/(double)(1<<13))
1611	GETFLOATSIGN(eph._M0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1612	GETFLOATSIGN(eph._Cuc, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1613	GETFLOAT(eph._e, 32, 1.0 / (double )(1 << 30) / (double )(1 << 3))
1614	GETFLOATSIGN(eph._Cus, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1615	GETFLOAT(eph._sqrt_A, 32, 1.0 / (double )(1 << 19))
1616	if (eph._sqrt_A < 1000.0) {
1617	#ifdef BNC_DEBUG_BCEP
1618	emit(newMessage(QString("%1: Block %2 (%3) SQRT_A %4 m!")
1619	.arg(_staID).arg(1042,4).arg(eph._prn.toString().c_str())
1620	.arg(eph._sqrt_A,10,'F',3).toLatin1(), true));
1621	#endif
1622	return false;
1623	}
1624	GETBITS(i, 17)
1625	i <<= 3;
1626	eph._TOEsec = i;
1627	eph._TOE.setBDS(eph._BDTweek, i);
1628	GETFLOATSIGN(eph._Cic, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1629	GETFLOATSIGN(eph._OMEGA0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1630	GETFLOATSIGN(eph._Cis, 18, 1.0 / (double )(1 << 30) / (double )(1 << 1))
1631	GETFLOATSIGN(eph._i0, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1632	GETFLOATSIGN(eph._Crc, 18, 1.0 / (double )(1 << 6))
1633	GETFLOATSIGN(eph._omega, 32, R2R_PI/(double)(1<<30)/(double)(1<<1))
1634	GETFLOATSIGN(eph._OMEGADOT, 24, R2R_PI/(double)(1<<30)/(double)(1<<13))
1635	GETFLOATSIGN(eph._TGD1, 10, 0.0000000001)
1636	GETFLOATSIGN(eph._TGD2, 10, 0.0000000001)
1637	GETBITS(eph._SatH1, 1)
1638
1639	eph._TOT = 0.9999E9;
1640	if (eph._i0 > iMaxGEO) {
1641	eph._navType = t_eph::D1;
1642	}
1643	else {
1644	eph._navType = t_eph::D2;
1645	}
1646
1647	emit newBDSEph(eph);
1648	decoded = true;
1649	}
1650	return decoded;
1651	}
1652
1653	//
1654	////////////////////////////////////////////////////////////////////////////
1655	bool RTCM3Decoder::DecodeAntennaReceiver(unsigned char* data, int size) {
1656	char *antenna;
1657	char *antserialnum;
1658	char *receiver;
1659	char *recfirmware;
1660	char *recserialnum;
1661	int type;
1662	int antsernum = -1;
1663	int antnum = -1;
1664	int recnum = -1;
1665	int recsernum = -1;
1666	int recfirnum = -1;
1667	uint64_t numbits = 0, bitfield = 0;
1668
1669	data += 3; /* header*/
1670	size -= 6; /* header + crc */
1671
1672	GETBITS(type, 12)
1673	SKIPBITS(12) /* reference station ID */
1674	GETSTRING(antnum, antenna)
1675	if ((antnum > -1 && antnum < 265) &&
1676	(_antType.empty() \|\| strncmp(_antType.back().descriptor, antenna, recnum) != 0)) {
1677	_antType.push_back(t_antInfo());
1678	memcpy(_antType.back().descriptor, antenna, antnum);
1679	_antType.back().descriptor[antnum] = 0;
1680	}
1681	SKIPBITS(8) /* antenna setup ID */
1682	if (type == 1008 \|\| type == 1033 ) {
1683	GETSTRING(antsernum, antserialnum)
1684	if ((antsernum > -1 && antsernum < 265)) {
1685	memcpy(_antType.back().serialnumber, antserialnum, antsernum);
1686	_antType.back().serialnumber[antsernum] = 0;
1687	}
1688	}
1689
1690	if (type == 1033) {
1691	GETSTRING(recnum, receiver)
1692	GETSTRING(recfirnum, recfirmware)
1693	GETSTRING(recsernum, recserialnum)
1694	if ((recnum > -1 && recnum < 265) &&
1695	(_recType.empty() \|\| strncmp(_recType.back().descriptor, receiver, recnum) != 0)) {
1696	_recType.push_back(t_recInfo());
1697	memcpy(_recType.back().descriptor, receiver, recnum);
1698	_recType.back().descriptor[recnum] = 0;
1699	if (recfirnum > -1 && recfirnum < 265) {
1700	memcpy(_recType.back().firmware, recfirmware, recfirnum);
1701	_recType.back().firmware[recfirnum] = 0;
1702	}
1703	if (recsernum > -1 && recsernum < 265) {
1704	memcpy(_recType.back().serialnumber, recserialnum, recsernum);
1705	_recType.back().serialnumber[recsernum] = 0;
1706	}
1707	}
1708	}
1709	return true;
1710	}
1711
1712	//
1713	////////////////////////////////////////////////////////////////////////////
1714	bool RTCM3Decoder::DecodeAntennaPosition(unsigned char* data, int size) {
1715	int type;
1716	uint64_t numbits = 0, bitfield = 0;
1717	double x, y, z;
1718
1719	data += 3; /* header */
1720	size -= 6; /* header + crc */
1721
1722	GETBITS(type, 12)
1723	_antList.push_back(t_antRefPoint());
1724	_antList.back().type = t_antRefPoint::ARP;
1725	SKIPBITS(22)
1726	GETBITSSIGN(x, 38)
1727	_antList.back().xx = x * 1e-4;
1728	SKIPBITS(2)
1729	GETBITSSIGN(y, 38)
1730	_antList.back().yy = y * 1e-4;
1731	SKIPBITS(2)
1732	GETBITSSIGN(z, 38)
1733	_antList.back().zz = z * 1e-4;
1734	if (type == 1006) {
1735	double h;
1736	GETBITS(h, 16)
1737	_antList.back().height = h * 1e-4;
1738	_antList.back().height_f = true;
1739	}
1740	_antList.back().message = type;
1741
1742	return true;
1743	}
1744
1745	//
1746	////////////////////////////////////////////////////////////////////////////
1747	bool RTCM3Decoder::DecodeServiceCRS(unsigned char* data, int size) {
1748	t_serviceCrs serviceCrs;
1749	int servicecrsnum = -1;
1750
1751	uint64_t numbits = 0, bitfield = 0;
1752
1753	data += 3; // header
1754	size -= 6; // header + crc
1755
1756	SKIPBITS(12) // Message Number
1757
1758	GETBITS(servicecrsnum, 5)
1759	if (servicecrsnum > -1 && servicecrsnum <= 31) {
1760	for(int i = 0; i < servicecrsnum; i++) {
1761	GETBITS(serviceCrs._name[i], 8);
1762	}
1763	serviceCrs._name[servicecrsnum] = 0;
1764	}
1765	if (_serviceCrs.empty() \|\|
1766	(strncmp(_serviceCrs.back()._name, serviceCrs._name, servicecrsnum) != 0)) {
1767	_serviceCrs.push_back(serviceCrs);
1768	GETFLOAT(_serviceCrs.back()._CE, 16, 1/100.0)
1769	_serviceCrs.back().setCoordinateEpochFromCE();
1770	_serviceCrs.back().print();
1771	}
1772	return true;
1773
1774	}
1775
1776	//
1777	////////////////////////////////////////////////////////////////////////////
1778	bool RTCM3Decoder::DecodeRTCMCRS(unsigned char* data, int size) {
1779
1780	t_rtcmCrs rtcmCrs;
1781	int rtcmcrsnum = -1;
1782
1783	uint64_t numbits = 0, bitfield = 0;
1784
1785	data += 3; // header
1786	size -= 6; // header + crc
1787
1788	SKIPBITS(12) // Message Number
1789	GETBITS(rtcmcrsnum, 5)
1790	if (rtcmcrsnum > -1 && rtcmcrsnum <= 31) {
1791	for(int i = 0; i < rtcmcrsnum; i++) {
1792	GETBITS(rtcmCrs._name[i], 8);
1793	}
1794	rtcmCrs._name[rtcmcrsnum] = 0;
1795	}
1796	if (_rtcmCrs.empty() \|\|
1797	(strncmp(_rtcmCrs.back()._name, rtcmCrs._name, rtcmcrsnum) != 0)) {
1798	_rtcmCrs.push_back(rtcmCrs);
1799
1800	GETBITS(_rtcmCrs.back()._anchor, 1)
1801	GETBITS(_rtcmCrs.back()._plateNumber, 5)
1802
1803	int dblinksnum = 0;
1804	GETBITS(dblinksnum, 3)
1805	for (int i = 0; i < dblinksnum; i++) {
1806	int dblinknum = -1;
1807	char dblinkname[31];
1808	GETBITS(dblinknum, 5)
1809	if (dblinknum > -1 && dblinknum <= 31) {
1810	for(int i = 0; i < dblinknum; i++) {
1811	GETBITS(dblinkname[i], 8);
1812	}
1813	dblinkname[dblinknum] = 0;
1814	_rtcmCrs.back()._databaseLinks.append(QString("%1").arg(dblinkname));
1815	}
1816	}
1817	_rtcmCrs.back().print();
1818	}
1819
1820	return true;
1821	}
1822
1823
1824	////////////////////////////////////////////////////////////////////////////
1825	bool RTCM3Decoder::DecodeHelmertTrafoParameters(unsigned char* data, int size) {
1826
1827	t_helmertPar helmertPar;
1828	int sourcenum = -1;
1829	int targetnum = -1;
1830
1831	uint64_t numbits = 0, bitfield = 0;
1832	data += 3; // header
1833	size -= 6; // header + crc
1834
1835	SKIPBITS(12) // Message Number
1836	GETBITS(sourcenum, 5)
1837	if (sourcenum > -1 && sourcenum <= 31) {
1838	for(int i = 0; i < sourcenum; i++) {
1839	GETBITS(helmertPar._sourceName[i], 8);
1840	}
1841	helmertPar._sourceName[sourcenum] = 0;
1842	}
1843	GETBITS(targetnum, 5)
1844	if (targetnum > -1 && targetnum <= 31) {
1845	for(int i = 0; i < targetnum; i++) {
1846	GETBITS(helmertPar._targetName[i], 8);
1847	}
1848	helmertPar._targetName[targetnum] = 0;
1849	}
1850	GETBITS(helmertPar._sysIdentNum, 8)
1851	GETBITS(helmertPar._utilTrafoMessageIndicator, 10)
1852	int mjd;
1853	GETBITS(mjd, 16)
1854	helmertPar._t0 = mjd + 44244;
1855
1856	// delete old parameter entries if available
1857	if (!_helmertPar.empty()) {
1858	QList<t_helmertPar>::iterator it = _helmertPar.begin();
1859	while (it != _helmertPar.end()) {
1860	(helmertPar == *it) ? it = _helmertPar.erase(it) : ++it;
1861	}
1862	}
1863	_helmertPar.push_back(helmertPar);
1864
1865	GETFLOATSIGN(_helmertPar.back()._dx, 23, 0.001)
1866	GETFLOATSIGN(_helmertPar.back()._dy, 23, 0.001)
1867	GETFLOATSIGN(_helmertPar.back()._dz, 23, 0.001)
1868	GETFLOATSIGN(_helmertPar.back()._ox, 32, 0.00002)
1869	GETFLOATSIGN(_helmertPar.back()._oy, 32, 0.00002)
1870	GETFLOATSIGN(_helmertPar.back()._oz, 32, 0.00002)
1871	GETFLOATSIGN(_helmertPar.back()._sc, 25, 0.00001)
1872	GETFLOATSIGN(_helmertPar.back()._dxr, 17, 0.00002)
1873	GETFLOATSIGN(_helmertPar.back()._dyr, 17, 0.00002)
1874	GETFLOATSIGN(_helmertPar.back()._dzr, 17, 0.00002)
1875	GETFLOATSIGN(_helmertPar.back()._oxr, 17, 0.0000004)
1876	GETFLOATSIGN(_helmertPar.back()._oyr, 17, 0.0000004)
1877	GETFLOATSIGN(_helmertPar.back()._ozr, 17, 0.0000004)
1878	GETFLOATSIGN(_helmertPar.back()._scr, 14, 0.0000002)
1879
1880	_helmertPar.back().print();
1881
1882	return true;
1883	}
1884
1885
1886	//
1887	////////////////////////////////////////////////////////////////////////////
1888	t_irc RTCM3Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
1889	bool decoded = false;
1890
1891	errmsg.clear();
1892
1893	while (bufLen && _MessageSize < sizeof(_Message)) {
1894	int l = sizeof(_Message) - _MessageSize;
1895	if (l > bufLen)
1896	l = bufLen;
1897	memcpy(_Message + _MessageSize, buffer, l);
1898	_MessageSize += l;
1899	bufLen -= l;
1900	buffer += l;
1901	int id;
1902	while ((id = GetMessage())) {
1903	/* reset station ID for file loading as it can change */
1904	if (_rawFile)
1905	_staID = _rawFile->staID();
1906	/* store the id into the list of loaded blocks */
1907	_typeList.push_back(id);
1908
1909	/* SSR I+II data handled in another function, already pass the
1910	* extracted data block. That does no harm, as it anyway skip everything
1911	* else. */
1912	if ((id >= 1057 && id <= 1068) \|\|
1913	(id >= 1240 && id <= 1270) \|\|
1914	(id == 4076)) {
1915	if (!_coDecoders.contains(_staID.toLatin1())) {
1916	_coDecoders[_staID.toLatin1()] = new RTCM3coDecoder(_staID);
1917	if (id == 4076) {
1918	_coDecoders[_staID.toLatin1()]->initSsrFormatType(RTCM3coDecoder::IGSssr);
1919	}
1920	else {
1921	_coDecoders[_staID.toLatin1()]->initSsrFormatType(RTCM3coDecoder::RTCMssr);
1922	}
1923	}
1924	RTCM3coDecoder* coDecoder = _coDecoders[_staID.toLatin1()];
1925	if (coDecoder->Decode(reinterpret_cast<char *>(_Message), _BlockSize, errmsg) == success) {
1926	decoded = true;
1927	}
1928	}
1929	else if (id >= 1070 && id <= 1237) { /* MSM */
1930	if (DecodeRTCM3MSM(_Message, _BlockSize))
1931	decoded = true;
1932	}
1933	else {
1934	switch (id) {
1935	case 1001:
1936	case 1003:
1937	#ifdef BNC_DEBUG_OBS
1938	emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")
1939	.arg(_staID).arg(id).toLatin1(), true));
1940	#endif
1941	break; /* no use decoding partial data ATM, remove break when data can be used */
1942	case 1002:
1943	case 1004:
1944	if (DecodeRTCM3GPS(_Message, _BlockSize))
1945	decoded = true;
1946	break;
1947	case 1009:
1948	case 1011:
1949	#ifdef BNC_DEBUG_OBS
1950	emit(newMessage(QString("%1: Block %2 contain partial data! Ignored!")
1951	.arg(_staID).arg(id).toLatin1(), true));
1952	#endif
1953	break; /* no use decoding partial data ATM, remove break when data can be used */
1954	case 1010:
1955	case 1012:
1956	if (DecodeRTCM3GLONASS(_Message, _BlockSize))
1957	decoded = true;
1958	break;
1959	case 1019:
1960	if (DecodeGPSEphemeris(_Message, _BlockSize))
1961	decoded = true;
1962	break;
1963	case 1020:
1964	if (DecodeGLONASSEphemeris(_Message, _BlockSize))
1965	decoded = true;
1966	break;
1967	case 1043:
1968	if (DecodeSBASEphemeris(_Message, _BlockSize))
1969	decoded = true;
1970	break;
1971	case 1044:
1972	if (DecodeQZSSEphemeris(_Message, _BlockSize))
1973	decoded = true;
1974	break;
1975	case 1041:
1976	if (DecodeIRNSSEphemeris(_Message, _BlockSize))
1977	decoded = true;
1978	break;
1979	case 1045:
1980	case 1046:
1981	if (DecodeGalileoEphemeris(_Message, _BlockSize))
1982	decoded = true;
1983	break;
1984	case 1042:
1985	if (DecodeBDSEphemeris(_Message, _BlockSize))
1986	decoded = true;
1987	break;
1988	case 1007:
1989	case 1008:
1990	case 1033:
1991	DecodeAntennaReceiver(_Message, _BlockSize);
1992	break;
1993	case 1005:
1994	case 1006:
1995	DecodeAntennaPosition(_Message, _BlockSize);
1996	break;
1997	case 1300:
1998	DecodeServiceCRS(_Message, _BlockSize);
1999	break;
2000	case 1301:
2001	DecodeHelmertTrafoParameters(_Message, _BlockSize);
2002	break;
2003	case 1302:
2004	case 35:
2005	DecodeRTCMCRS(_Message, _BlockSize);
2006	break;
2007	}
2008	}
2009	}
2010	}
2011	/*
2012	for (int ii = 0; ii < _helmertParList.size(); ii++) {
2013	_helmertParList[ii].print();
2014	}*/
2015	return decoded ? success : failure;
2016	}
2017
2018	//
2019	////////////////////////////////////////////////////////////////////////////
2020	uint32_t RTCM3Decoder::CRC24(long size, const unsigned char *buf) {
2021	uint32_t crc = 0;
2022	int ii;
2023	while (size--) {
2024	crc ^= (*buf++) << (16);
2025	for (ii = 0; ii < 8; ii++) {
2026	crc <<= 1;
2027	if (crc & 0x1000000)
2028	crc ^= 0x01864cfb;
2029	}
2030	}
2031	return crc;
2032	}
2033
2034	//
2035	////////////////////////////////////////////////////////////////////////////
2036	int RTCM3Decoder::GetMessage(void) {
2037	unsigned char m, e;
2038	int i;
2039
2040	m = _Message + _SkipBytes;
2041	e = _Message + _MessageSize;
2042	_NeedBytes = _SkipBytes = 0;
2043	while (e - m >= 3) {
2044	if (m[0] == 0xD3) {
2045	_BlockSize = ((m[1] & 3) << 8) \| m[2];
2046	if (e - m >= static_cast<int>(_BlockSize + 6)) {
2047	if (static_cast<uint32_t>((m[3 + _BlockSize] << 16)
2048	\| (m[3 + _BlockSize + 1] << 8)
2049	\| (m[3 + _BlockSize + 2])) == CRC24(_BlockSize + 3, m)) {
2050	_BlockSize += 6;
2051	_SkipBytes = _BlockSize;
2052	break;
2053	}
2054	else
2055	++m;
2056	}
2057	else {
2058	_NeedBytes = _BlockSize;
2059	break;
2060	}
2061	}
2062	else
2063	++m;
2064	}
2065	if (e - m < 3)
2066	_NeedBytes = 3;
2067
2068	/* copy buffer to front */
2069	i = m - _Message;
2070	if (i && m < e)
2071	memmove(_Message, m, static_cast<size_t>(_MessageSize - i));
2072	_MessageSize -= i;
2073
2074	return !_NeedBytes ? ((_Message[3] << 4) \| (_Message[4] >> 4)) : 0;
2075	}
2076
2077	// Time of Corrections
2078	//////////////////////////////////////////////////////////////////////////////
2079	int RTCM3Decoder::corrGPSEpochTime() const {
2080	return
2081	_coDecoders.size() > 0 ?
2082	_coDecoders.begin().value()->corrGPSEpochTime() : -1;
2083	}

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

Download in other formats: