Context Navigation

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

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

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

Download in other formats: