Context Navigation

ephemeris.cpp@ 10628

Last change on this file since 10628 was 10628, checked in by stuerze, 3 days ago

File size: 87.8 KB

Line
1	#include <sstream>
2	#include <iostream>
3	#include <iomanip>
4	#include <cstring>
5
6	#include <newmatio.h>
7
8	#include "ephemeris.h"
9	#include "bncutils.h"
10	#include "bnctime.h"
11	#include "bnccore.h"
12	#include "bncutils.h"
13	#include "satObs.h"
14	#include "pppInclude.h"
15	#include "pppModel.h"
16	#include "RTCM3/bits.h"
17
18	using namespace std;
19
20	// Constructor
21	////////////////////////////////////////////////////////////////////////////
22	t_eph::t_eph() {
23	_checkState = unchecked;
24	_type = undefined;
25	_orbCorr = 0;
26	_clkCorr = 0;
27	}
28	// Destructor
29	////////////////////////////////////////////////////////////////////////////
30	t_eph::~t_eph() {
31	if (_orbCorr)
32	delete _orbCorr;
33	if (_clkCorr)
34	delete _clkCorr;
35	}
36
37	//
38	////////////////////////////////////////////////////////////////////////////
39	void t_eph::setOrbCorr(const t_orbCorr *orbCorr) {
40	if (_orbCorr) {
41	delete _orbCorr;
42	_orbCorr = 0;
43	}
44	_orbCorr = new t_orbCorr(*orbCorr);
45	}
46
47	//
48	////////////////////////////////////////////////////////////////////////////
49	void t_eph::setClkCorr(const t_clkCorr *clkCorr) {
50	if (_clkCorr) {
51	delete _clkCorr;
52	_clkCorr = 0;
53	}
54	_clkCorr = new t_clkCorr(*clkCorr);
55	}
56
57	//
58	////////////////////////////////////////////////////////////////////////////
59	t_irc t_eph::getCrd(const bncTime &tt, ColumnVector &xc, ColumnVector &vv,
60	bool useCorr) const {
61
62	if (_checkState == bad \|\|
63	_checkState == unhealthy \|\|
64	_checkState == outdated) {
65	return failure;
66	}
67
68	xc.ReSize(6);
69	vv.ReSize(3);
70	if (position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data()) != success) {
71	return failure;
72	}
73	if (useCorr) {
74	if (_orbCorr && _clkCorr) {
75	double dtO = tt - _orbCorr->_time;
76	if (_orbCorr->_updateInt) {
77	dtO -= (0.5 * ssrUpdateInt[_orbCorr->_updateInt]);
78	}
79	ColumnVector dx(3);
80	dx[0] = _orbCorr->_xr[0] + _orbCorr->_dotXr[0] * dtO;
81	dx[1] = _orbCorr->_xr[1] + _orbCorr->_dotXr[1] * dtO;
82	dx[2] = _orbCorr->_xr[2] + _orbCorr->_dotXr[2] * dtO;
83
84	RSW_to_XYZ(xc.Rows(1, 3), vv.Rows(1, 3), dx, dx);
85
86	xc[0] -= dx[0];
87	xc[1] -= dx[1];
88	xc[2] -= dx[2];
89
90	ColumnVector dv(3);
91	RSW_to_XYZ(xc.Rows(1, 3), vv.Rows(1, 3), _orbCorr->_dotXr, dv);
92
93	vv[0] -= dv[0];
94	vv[1] -= dv[1];
95	vv[2] -= dv[2];
96
97	double dtC = tt - _clkCorr->_time;
98	if (_clkCorr->_updateInt) {
99	dtC -= (0.5 * ssrUpdateInt[_clkCorr->_updateInt]);
100	}
101	xc[3] += _clkCorr->_dClk + _clkCorr->_dotDClk * dtC
102	+ _clkCorr->_dotDotDClk * dtC * dtC;
103	} else {
104	return failure;
105	}
106	}
107	return success;
108	}
109
110	//
111	//////////////////////////////////////////////////////////////////////////////
112	void t_eph::setType(QString typeStr) {
113
114	if (typeStr == "LNAV") {
115	_type = t_eph::LNAV;
116	} else if (typeStr == "FDMA") {
117	_type = t_eph::FDMA;
118	} else if (typeStr == "FNAV") {
119	_type = t_eph::FNAV;
120	} else if (typeStr == "INAV") {
121	_type = t_eph::INAV;
122	} else if (typeStr == "D1") {
123	_type = t_eph::D1;
124	} else if (typeStr == "D2") {
125	_type = t_eph::D2;
126	} else if (typeStr == "SBAS") {
127	_type = t_eph::SBASL1;
128	} else if (typeStr == "CNAV") {
129	_type = t_eph::CNAV;
130	} else if (typeStr == "CNV1") {
131	_type = t_eph::CNV1;
132	} else if (typeStr == "CNV2") {
133	_type = t_eph::CNV2;
134	} else if (typeStr == "CNV3") {
135	_type = t_eph::CNV3;
136	} else if (typeStr == "L1NV") {
137	_type = t_eph::L1NV;
138	} else if (typeStr == "L1OC") {
139	_type = t_eph::L1OC;
140	} else if (typeStr == "L3OC") {
141	_type = t_eph::L3OC;
142	} else {
143	_type = t_eph::undefined;
144	}
145
146	}
147
148
149	//
150	//////////////////////////////////////////////////////////////////////////////
151	QString t_eph::typeStr(e_type type, const t_prn &prn, double version) {
152	QString typeStr = "";
153	QString epochStart;
154	QString eolStr;
155
156	if (version < 4.0) {
157	return typeStr;
158	}
159
160	if (version == 99.0) { // log output for OUTDATED, WRONG or UNHEALTHY satellites
161	epochStart = "";
162	eolStr = "";
163	} else {
164	epochStart = "> ";
165	eolStr = "\n";
166	}
167
168	QString ephStr = QString("EPH %1 ").arg(prn.toString().c_str());
169	switch (type) {
170	case undefined:
171	typeStr = epochStart + ephStr + "unknown" + eolStr;
172	break;
173	case LNAV:
174	typeStr = epochStart + ephStr + "LNAV" + eolStr;
175	break;
176	case FDMA:
177	case FDMA_M:
178	typeStr = epochStart + ephStr + "FDMA" + eolStr;
179	break;
180	case FNAV:
181	typeStr = epochStart + ephStr + "FNAV" + eolStr;
182	break;
183	case INAV:
184	typeStr = epochStart + ephStr + "INAV" + eolStr;
185	break;
186	case D1:
187	typeStr = epochStart + ephStr + "D1 " + eolStr;
188	break;
189	case D2:
190	typeStr = epochStart + ephStr + "D2 " + eolStr;
191	break;
192	case SBASL1:
193	typeStr = epochStart + ephStr + "SBAS" + eolStr;
194	break;
195	case CNAV:
196	typeStr = epochStart + ephStr + "CNAV" + eolStr;
197	break;
198	case CNV1:
199	typeStr = epochStart + ephStr + "CNV1" + eolStr;
200	break;
201	case CNV2:
202	typeStr = epochStart + ephStr + "CNV2" + eolStr;
203	break;
204	case CNV3:
205	typeStr = epochStart + ephStr + "CNV3" + eolStr;
206	break;
207	case L1NV:
208	typeStr = epochStart + ephStr + "L1NV" + eolStr;
209	break;
210	case L1OC:
211	typeStr = epochStart + ephStr + "L1OC" + eolStr;
212	break;
213	case L3OC:
214	typeStr = epochStart + ephStr + "L3OC" + eolStr;
215	break;
216	}
217	return typeStr;
218	}
219
220	//
221	//////////////////////////////////////////////////////////////////////////////
222	QString t_eph::rinexDateStr(const bncTime &tt, const t_prn &prn,
223	double version) {
224	QString prnStr(prn.toString().c_str());
225	return rinexDateStr(tt, prnStr, version);
226	}
227
228	//
229	//////////////////////////////////////////////////////////////////////////////
230	QString t_eph::rinexDateStr(const bncTime &tt, const QString &prnStr,
231	double version) {
232
233	QString datStr;
234
235	unsigned year, month, day, hour, min;
236	double sec;
237	tt.civil_date(year, month, day);
238	tt.civil_time(hour, min, sec);
239
240	QTextStream out(&datStr);
241
242	if (version < 3.0) {
243	QString prnHlp = prnStr.mid(1, 2);
244	if (prnHlp[0] == '0')
245	prnHlp[0] = ' ';
246	out << prnHlp
247	<< QString(" %1 %2 %3 %4 %5%6").arg(year % 100, 2, 10, QChar('0')).arg(
248	month, 2).arg(day, 2).arg(hour, 2).arg(min, 2).arg(sec, 5, 'f', 1);
249	}
250	else if (version == 99) {
251	out
252	<< QString(" %1 %2 %3 %4 %5 %6").arg(year, 4).arg(month, 2, 10,
253	QChar('0')).arg(day, 2, 10, QChar('0')).arg(hour, 2, 10, QChar('0')).arg(
254	min, 2, 10, QChar('0')).arg(int(sec), 2, 10, QChar('0'));
255	}
256	else {
257	out << prnStr
258	<< QString(" %1 %2 %3 %4 %5 %6").arg(year, 4).arg(month, 2, 10,
259	QChar('0')).arg(day, 2, 10, QChar('0')).arg(hour, 2, 10, QChar('0')).arg(
260	min, 2, 10, QChar('0')).arg(int(sec), 2, 10, QChar('0'));
261	}
262
263	return datStr;
264	}
265
266	// Constructor
267	//////////////////////////////////////////////////////////////////////////////
268	t_ephGPS::t_ephGPS(double rnxVersion, const QStringList &lines, QString typeStr) {
269
270	setType(typeStr);
271
272	int nLines = 8; // LNAV
273	// Source RINEX version < 4
274	if (type() == t_eph::undefined) {
275	_type = t_eph::LNAV;
276	}
277
278	if (type() == t_eph::CNAV \|\|
279	type() == t_eph::L1NV) {
280	nLines += 1;
281	}
282	if (type() == t_eph::CNV2) {
283	nLines += 2;
284	}
285
286	if (lines.size() != nLines) {
287	_checkState = bad;
288	return;
289	}
290
291	// RINEX Format
292	// ------------
293	int fieldLen = 19;
294	double statusflags = 0.0;
295
296	int pos[4];
297	pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
298	pos[1] = pos[0] + fieldLen;
299	pos[2] = pos[1] + fieldLen;
300	pos[3] = pos[2] + fieldLen;
301
302	// Read nLines lines
303	// ------------------
304	for (int iLine = 0; iLine < nLines; iLine++) {
305	QString line = lines[iLine];
306
307	if (iLine == 0) {
308	QTextStream in(line.left(pos[1]).toLatin1());
309	int year, month, day, hour, min;
310	double sec;
311
312	QString prnStr, n;
313	in >> prnStr;
314
315	if (prnStr.size() == 1 &&
316	(prnStr[0] == 'G' \|\|
317	prnStr[0] == 'J' \|\|
318	prnStr[0] == 'I')) {
319	in >> n;
320	prnStr.append(n);
321	}
322
323	if ( prnStr.at(0) == 'G') {
324	_prn.set('G', prnStr.mid(1).toInt());
325	} else if (prnStr.at(0) == 'J') {
326	_prn.set('J', prnStr.mid(1).toInt());
327	} else if (prnStr.at(0) == 'I') {
328	_prn.set('I', prnStr.mid(1).toInt());
329	} else {
330	_prn.set('G', prnStr.toInt());
331	}
332	_prn.setFlag(type());
333
334	in >> year >> month >> day >> hour >> min >> sec;
335
336	if (year < 80) {
337	year += 2000;
338	} else if (year < 100) {
339	year += 1900;
340	}
341
342	_TOC.set(year, month, day, hour, min, sec);
343
344	if ( readDbl(line, pos[1], fieldLen, _clock_bias)
345	\|\| readDbl(line, pos[2], fieldLen, _clock_drift)
346	\|\| readDbl(line, pos[3], fieldLen, _clock_driftrate)) {
347	_checkState = bad;
348	return;
349	}
350	}
351	// =====================
352	// BROADCAST ORBIT - 1
353	// =====================
354	else if (iLine == 1) {
355	if (type() == t_eph::CNAV \|\|
356	type() == t_eph::CNV2 \|\|
357	type() == t_eph::L1NV) {
358	if ( readDbl(line, pos[0], fieldLen, _ADOT)
359	\|\| readDbl(line, pos[1], fieldLen, _Crs)
360	\|\| readDbl(line, pos[2], fieldLen, _Delta_n)
361	\|\| readDbl(line, pos[3], fieldLen, _M0)) {
362	_checkState = bad;
363	return;
364	}
365	} else { // LNAV
366	if ( readDbl(line, pos[0], fieldLen, _IODE)
367	\|\| readDbl(line, pos[1], fieldLen, _Crs)
368	\|\| readDbl(line, pos[2], fieldLen, _Delta_n)
369	\|\| readDbl(line, pos[3], fieldLen, _M0)) {
370	_checkState = bad;
371	return;
372	}
373	}
374	}
375	// =====================
376	// BROADCAST ORBIT - 2
377	// =====================
378	else if (iLine == 2) {
379	if ( readDbl(line, pos[0], fieldLen, _Cuc)
380	\|\| readDbl(line, pos[1], fieldLen, _e)
381	\|\| readDbl(line, pos[2], fieldLen, _Cus)
382	\|\| readDbl(line, pos[3], fieldLen, _sqrt_A)) {
383	_checkState = bad;
384	return;
385	}
386	}
387	// =====================
388	// BROADCAST ORBIT - 3
389	// =====================
390	else if (iLine == 3) {
391	if (type() == t_eph::CNAV \|\|
392	type() == t_eph::CNV2) {
393	if ( readDbl(line, pos[0], fieldLen, _top)
394	\|\| readDbl(line, pos[1], fieldLen, _Cic)
395	\|\| readDbl(line, pos[2], fieldLen, _OMEGA0)
396	\|\| readDbl(line, pos[3], fieldLen, _Cis)) {
397	_checkState = bad;
398	return;
399	}
400	} else if (type() == t_eph::L1NV) {
401	if ( readDbl(line, pos[0], fieldLen, _IODE)
402	\|\| readDbl(line, pos[1], fieldLen, _Cic)
403	\|\| readDbl(line, pos[2], fieldLen, _OMEGA0)
404	\|\| readDbl(line, pos[3], fieldLen, _Cis)) {
405	_checkState = bad;
406	return;
407	}
408	} else { // LNAV
409	if ( readDbl(line, pos[0], fieldLen, _TOEsec)
410	\|\| readDbl(line, pos[1], fieldLen, _Cic)
411	\|\| readDbl(line, pos[2], fieldLen, _OMEGA0)
412	\|\| readDbl(line, pos[3], fieldLen, _Cis)) {
413	_checkState = bad;
414	return;
415	}
416	}
417	}
418	// =====================
419	// BROADCAST ORBIT - 4
420	// =====================
421	else if (iLine == 4) {
422	if ( readDbl(line, pos[0], fieldLen, _i0)
423	\|\| readDbl(line, pos[1], fieldLen, _Crc)
424	\|\| readDbl(line, pos[2], fieldLen, _omega)
425	\|\| readDbl(line, pos[3], fieldLen, _OMEGADOT)) {
426	_checkState = bad;
427	return;
428	}
429	}
430	// =====================
431	// BROADCAST ORBIT - 5
432	// =====================
433	else if (iLine == 5 && system() != t_eph::NavIC) {
434	if (type() == t_eph::CNAV \|\|
435	type() == t_eph::CNV2) {
436	if ( readDbl(line, pos[0], fieldLen, _IDOT)
437	\|\| readDbl(line, pos[1], fieldLen, _Delta_n_dot)
438	\|\| readDbl(line, pos[2], fieldLen, _URAI_NED0)
439	\|\| readDbl(line, pos[3], fieldLen, _URAI_NED1)) {
440	_checkState = bad;
441	return;
442	}
443	}
444	else { // LNAV
445	if ( readDbl(line, pos[0], fieldLen, _IDOT)
446	\|\| readDbl(line, pos[1], fieldLen, _L2Codes)
447	\|\| readDbl(line, pos[2], fieldLen, _TOEweek)
448	\|\| readDbl(line, pos[3], fieldLen, _L2PFlag)) {
449	_checkState = bad;
450	return;
451	}
452	}
453	} else if (iLine == 5 && system() == t_eph::NavIC) {
454	if (type() == t_eph::LNAV) {
455	if ( readDbl(line, pos[0], fieldLen, _IDOT)
456	\|\| readDbl(line, pos[2], fieldLen, _TOEweek)) {
457	_checkState = bad;
458	return;
459	}
460	}
461	else if (type() == t_eph::L1NV) {
462	if ( readDbl(line, pos[0], fieldLen, _IDOT)
463	\|\| readDbl(line, pos[1], fieldLen, _Delta_n_dot)
464	\|\| readDbl(line, pos[3], fieldLen, _RSF)) {
465	_checkState = bad;
466	return;
467	}
468	}
469	}
470	// =====================
471	// BROADCAST ORBIT - 6
472	// =====================
473	else if (iLine == 6 && system() != t_eph::NavIC) {
474	if (type() == t_eph::CNAV \|\|
475	type() == t_eph::CNV2) {
476	if ( readDbl(line, pos[0], fieldLen, _URAI_ED)
477	\|\| readDbl(line, pos[1], fieldLen, _health)
478	\|\| readDbl(line, pos[2], fieldLen, _TGD)
479	\|\| readDbl(line, pos[3], fieldLen, _URAI_NED2)) {
480	_checkState = bad;
481	return;
482	}
483	} else { // LNAV
484	if ( readDbl(line, pos[0], fieldLen, _ura)
485	\|\| readDbl(line, pos[1], fieldLen, _health)
486	\|\| readDbl(line, pos[2], fieldLen, _TGD)
487	\|\| readDbl(line, pos[3], fieldLen, _IODC)) {
488	_checkState = bad;
489	return;
490	}
491	}
492	}
493	else if (iLine == 6 && system() == t_eph::NavIC) {
494	if (type() == t_eph::LNAV) {
495	if ( readDbl(line, pos[0], fieldLen, _ura)
496	\|\| readDbl(line, pos[1], fieldLen, _health)
497	\|\| readDbl(line, pos[2], fieldLen, _TGD)) {
498	_checkState = bad;
499	return;
500	}
501	}
502	else if (type() == t_eph::L1NV) {
503	int i = 0;
504	(!_RSF) ? i = 2 : i = 3;
505	if ( readDbl(line, pos[0], fieldLen, _URAI)
506	\|\| readDbl(line, pos[1], fieldLen, _health)
507	\|\| readDbl(line, pos[i], fieldLen, _TGD)) {
508	_checkState = bad;
509	return;
510	}
511	_ura = accuracyFromIndex(int(_URAI), system());
512	}
513	}
514	// =====================
515	// BROADCAST ORBIT - 7
516	// =====================
517	else if (iLine == 7) {
518	if (type() == t_eph::LNAV) {
519	if (readDbl(line, pos[0], fieldLen, _TOT)) {
520	_checkState = bad;
521	return;
522	}
523	if (system() != t_eph::NavIC) {
524	double fitIntervalRnx;
525	if (readDbl(line, pos[1], fieldLen, fitIntervalRnx)) {
526	//fit interval BLK, do nothing
527	_flags_unknown = true;
528	} else {
529	_flags_unknown = false;
530	if (system() == t_eph::GPS) { // in RINEX specified always as time period for GPS
531	_fitInterval = fitIntervalRnx;
532	}
533	else if (system() == t_eph::QZSS) { // specified as flag for QZSS
534	if (rnxVersion == 3.02) {
535	_fitInterval = fitIntervalRnx; // specified as time period
536	} else {
537	_fitInterval = fitIntervalFromFlag(fitIntervalRnx, _IODC, t_eph::QZSS);
538	}
539	}
540	}
541	}
542	}
543	else if (type() == t_eph::CNAV \|\|
544	type() == t_eph::CNV2) {
545	if ( readDbl(line, pos[0], fieldLen, _ISC_L1CA)
546	\|\| readDbl(line, pos[1], fieldLen, _ISC_L2C)
547	\|\| readDbl(line, pos[2], fieldLen, _ISC_L5I5)
548	\|\| readDbl(line, pos[3], fieldLen, _ISC_L5Q5)) {
549	_checkState = bad;
550	return;
551	}
552	}
553	else if (type() == t_eph::L1NV) {
554	if (!_RSF) {
555	if ( readDbl(line, pos[0], fieldLen, _ISC_S)
556	\|\| readDbl(line, pos[1], fieldLen, _ISC_L1D)) {
557	_checkState = bad;
558	return;
559	}
560	} else {
561	if ( readDbl(line, pos[2], fieldLen, _ISC_L1P)
562	\|\| readDbl(line, pos[3], fieldLen, _ISC_L1D)) {
563	_checkState = bad;
564	return;
565	}
566	}
567	}
568	}
569	// =====================
570	// BROADCAST ORBIT - 8
571	// =====================
572	else if (iLine == 8) {
573	if (type() == t_eph::CNAV) {
574	if ( readDbl(line, pos[0], fieldLen, _TOT)
575	\|\| readDbl(line, pos[1], fieldLen, _wnop)) {
576	_checkState = bad;
577	return;
578	}
579	if (readDbl(line, pos[2], fieldLen, statusflags)) {
580	_flags_unknown = true;
581	}
582	else {
583	_flags_unknown = false;
584	// Bit 0:
585	_intSF = double(bitExtracted(unsigned(statusflags), 1, 0));
586	// Bit 1:
587	_L2Cphasing = double(bitExtracted(unsigned(statusflags), 1, 1));
588	// Bit 2:
589	_alert = double(bitExtracted(unsigned(statusflags), 1, 2));
590	}
591	}
592	else if (type() == t_eph::CNV2) {
593	if ( readDbl(line, pos[0], fieldLen, _ISC_L1Cd)
594	\|\| readDbl(line, pos[1], fieldLen, _ISC_L1Cp)) {
595	_checkState = bad;
596	return;
597	}
598	}
599	else if (type() == t_eph::L1NV) {
600	if ( readDbl(line, pos[0], fieldLen, _TOT)) {
601	_checkState = bad;
602	return;
603	}
604	}
605	}
606	// =====================
607	// BROADCAST ORBIT - 9
608	// =====================
609	else if (iLine == 9) {
610	if (type() == t_eph::CNV2) {
611	if ( readDbl(line, pos[0], fieldLen, _TOT)
612	\|\| readDbl(line, pos[1], fieldLen, _wnop)) {
613	_checkState = bad;
614	return;
615	}
616	if (readDbl(line, pos[2], fieldLen, statusflags)) {
617	_flags_unknown = true;
618	}
619	else {
620	_flags_unknown = false;
621	// Bit 0:
622	_intSF = double(bitExtracted(int(statusflags), 1, 0));
623	if (system() == t_eph::QZSS) {
624	// Bit 1:
625	_ephSF = double(bitExtracted(int(statusflags), 1, 1));
626	}
627	}
628	}
629	}
630	}
631	}
632
633	// Compute GPS Satellite Position (virtual)
634	////////////////////////////////////////////////////////////////////////////
635	t_irc t_ephGPS::position(int GPSweek, double GPSweeks, double *xc,
636	double *vv) const {
637
638	static const double omegaEarth = 7292115.1467e-11;
639	static const double gmGRS = 398.6005e12;
640
641	memset(xc, 0, 6 * sizeof(double));
642	memset(vv, 0, 3 * sizeof(double));
643
644	double a0 = _sqrt_A * _sqrt_A;
645	if (a0 == 0) {
646	return failure;
647	}
648
649	double n0 = sqrt(gmGRS / (a0 * a0 * a0));
650
651	bncTime tt(GPSweek, GPSweeks);
652	double tk = tt - bncTime(int(_TOEweek), _TOEsec);
653
654	double n = n0 + _Delta_n;
655	double M = _M0 + n * tk;
656	double E = M;
657	double E_last;
658	int nLoop = 0;
659	do {
660	E_last = E;
661	E = M + _e * sin(E);
662
663	if (++nLoop == 100) {
664	return failure;
665	}
666	} while (fabs(E - E_last) * a0 > 0.001);
667	double v = 2.0 * atan(sqrt((1.0 + _e) / (1.0 - _e)) * tan(E / 2));
668	double u0 = v + _omega;
669	double sin2u0 = sin(2 * u0);
670	double cos2u0 = cos(2 * u0);
671	double r = a0 * (1 - _e * cos(E)) + _Crc * cos2u0 + _Crs * sin2u0;
672	double i = _i0 + _IDOT * tk + _Cic * cos2u0 + _Cis * sin2u0;
673	double u = u0 + _Cuc * cos2u0 + _Cus * sin2u0;
674	double xp = r * cos(u);
675	double yp = r * sin(u);
676	double OM = _OMEGA0 + (_OMEGADOT - omegaEarth) * tk - omegaEarth * _TOEsec;
677
678	double sinom = sin(OM);
679	double cosom = cos(OM);
680	double sini = sin(i);
681	double cosi = cos(i);
682	xc[0] = xp * cosom - yp * cosi * sinom;
683	xc[1] = xp * sinom + yp * cosi * cosom;
684	xc[2] = yp * sini;
685
686	double tc = tt - _TOC;
687	xc[3] = _clock_bias + _clock_drift * tc + _clock_driftrate * tc * tc;
688
689	// Velocity
690	// --------
691	double tanv2 = tan(v / 2);
692	double dEdM = 1 / (1 - _e * cos(E));
693	double dotv = sqrt((1.0 + _e) / (1.0 - _e)) / cos(E / 2) / cos(E / 2)
694	/ (1 + tanv2 * tanv2) * dEdM * n;
695	double dotu = dotv + (-_Cuc * sin2u0 + _Cus * cos2u0) * 2 * dotv;
696	double dotom = _OMEGADOT - omegaEarth;
697	double doti = _IDOT + (-_Cic * sin2u0 + _Cis * cos2u0) * 2 * dotv;
698	double dotr = a0 * _e * sin(E) * dEdM * n
699	+ (-_Crc * sin2u0 + _Crs * cos2u0) * 2 * dotv;
700	double dotx = dotr * cos(u) - r * sin(u) * dotu;
701	double doty = dotr * sin(u) + r * cos(u) * dotu;
702
703	vv[0] = cosom * dotx - cosi * sinom * doty // dX / dr
704	- xp * sinom * dotom - yp * cosi * cosom * dotom // dX / dOMEGA
705	+ yp * sini * sinom * doti; // dX / di
706
707	vv[1] = sinom * dotx + cosi * cosom * doty + xp * cosom * dotom
708	- yp * cosi * sinom * dotom - yp * sini * cosom * doti;
709
710	vv[2] = sini * doty + yp * cosi * doti;
711
712	// Relativistic Correction
713	// -----------------------
714	xc[3] -= 4.442807633e-10 * _e * sqrt(a0) * sin(E);
715
716	xc[4] = _clock_drift + _clock_driftrate * tc;
717	xc[5] = _clock_driftrate;
718
719	return success;
720	}
721
722	// RINEX Format String
723	//////////////////////////////////////////////////////////////////////////////
724	QString t_ephGPS::toString(double version) const {
725
726	if (version < 4.0 &&
727	(type() == t_eph::CNAV \|\|
728	type() == t_eph::CNV2 \|\|
729	type() == t_eph::L1NV )) {
730	return "";
731	}
732
733	QString ephStr = typeStr(_type, _prn, version);
734	QString rnxStr = ephStr + rinexDateStr(_TOC, _prn, version);
735
736	QTextStream out(&rnxStr);
737
738	out
739	<< QString("%1%2%3\n")
740	.arg(_clock_bias, 19, 'e', 12)
741	.arg(_clock_drift, 19, 'e', 12)
742	.arg(_clock_driftrate, 19, 'e', 12);
743
744	QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
745
746	// =====================
747	// BROADCAST ORBIT - 1
748	// =====================
749	if (type() == t_eph::CNAV \|\|
750	type() == t_eph::CNV2 \|\|
751	type() == t_eph::L1NV) {
752	out
753	<< QString(fmt)
754	.arg(_ADOT, 19, 'e', 12)
755	.arg(_Crs, 19, 'e', 12)
756	.arg(_Delta_n, 19, 'e', 12)
757	.arg(_M0, 19, 'e', 12);
758	} else { // LNAV, undefined
759	out
760	<< QString(fmt)
761	.arg(_IODE, 19, 'e', 12)
762	.arg(_Crs, 19, 'e', 12)
763	.arg(_Delta_n, 19, 'e', 12)
764	.arg(_M0, 19, 'e', 12);
765	}
766	// =====================
767	// BROADCAST ORBIT - 2
768	// =====================
769	out
770	<< QString(fmt)
771	.arg(_Cuc, 19, 'e', 12)
772	.arg(_e, 19, 'e', 12)
773	.arg(_Cus, 19, 'e', 12)
774	.arg(_sqrt_A, 19, 'e', 12);
775	// =====================
776	// BROADCAST ORBIT - 3
777	// =====================
778	if (type() == t_eph::CNAV \|\|
779	type() == t_eph::CNV2) {
780	out
781	<< QString(fmt)
782	.arg(_top, 19, 'e', 12)
783	.arg(_Cic, 19, 'e', 12)
784	.arg(_OMEGA0, 19, 'e', 12)
785	.arg(_Cis, 19, 'e', 12);
786	}
787	else if (type() == t_eph::L1NV) {
788	out
789	<< QString(fmt)
790	.arg(_IODE, 19, 'e', 12)
791	.arg(_Cic, 19, 'e', 12)
792	.arg(_OMEGA0, 19, 'e', 12)
793	.arg(_Cis, 19, 'e', 12);
794	}
795	else { // LNAV, undefined
796	out
797	<< QString(fmt)
798	.arg(_TOEsec, 19, 'e', 12)
799	.arg(_Cic, 19, 'e', 12)
800	.arg(_OMEGA0, 19, 'e', 12)
801	.arg(_Cis, 19, 'e', 12);
802	}
803	// =====================
804	// BROADCAST ORBIT - 4
805	// =====================
806	out
807	<< QString(fmt)
808	.arg(_i0, 19, 'e', 12)
809	.arg(_Crc, 19, 'e', 12)
810	.arg(_omega, 19, 'e', 12)
811	.arg(_OMEGADOT, 19, 'e', 12);
812	// =====================
813	// BROADCAST ORBIT - 5
814	// =====================
815	if (system() != t_eph::NavIC) {
816	if (type() == t_eph::CNAV \|\|
817	type() == t_eph::CNV2) {
818	out
819	<< QString(fmt)
820	.arg(_IDOT, 19, 'e', 12)
821	.arg(_Delta_n_dot, 19, 'e', 12)
822	.arg(_URAI_NED0, 19, 'e', 12)
823	.arg(_URAI_NED1, 19, 'e', 12);
824	}
825	else { // LNAV, undefined
826	out
827	<< QString(fmt)
828	.arg(_IDOT, 19, 'e', 12)
829	.arg(_L2Codes, 19, 'e', 12)
830	.arg(_TOEweek, 19, 'e', 12)
831	.arg(_L2PFlag, 19, 'e', 12);
832	}
833	}
834	else {
835	if (type() == t_eph::LNAV \|\|
836	type() == t_eph::undefined) {
837	out
838	<< QString(fmt)
839	.arg(_IDOT, 19, 'e', 12)
840	.arg("", 19, QChar(' '))
841	.arg(_TOEweek, 19, 'e', 12)
842	.arg("", 19, QChar(' '));
843	}
844	else if (type() == t_eph::L1NV) {
845	out
846	<< QString(fmt)
847	.arg(_IDOT, 19, 'e', 12)
848	.arg(_Delta_n_dot, 19, 'e', 12)
849	.arg("", 19, QChar(' '))
850	.arg(_RSF, 19, 'e', 12);
851	}
852	}
853	// =====================
854	// BROADCAST ORBIT - 6
855	// =====================
856	if (system() != t_eph::NavIC) {
857	if (type() == t_eph::CNAV \|\|
858	type() == t_eph::CNV2) {
859	out
860	<< QString(fmt)
861	.arg(_URAI_ED, 19, 'e', 12)
862	.arg(_health, 19, 'e', 12)
863	.arg(_TGD, 19, 'e', 12)
864	.arg(_URAI_NED2, 19, 'e', 12);
865	}
866	else { // LNAV, undefined
867	out
868	<< QString(fmt)
869	.arg(_ura, 19, 'e', 12)
870	.arg(_health, 19, 'e', 12)
871	.arg(_TGD, 19, 'e', 12)
872	.arg(_IODC, 19, 'e', 12);
873	}
874	}
875	else {
876	if (type() == t_eph::LNAV \|\|
877	type() == t_eph::undefined) {
878	out
879	<< QString(fmt)
880	.arg(_ura, 19, 'e', 12)
881	.arg(_health, 19, 'e', 12)
882	.arg(_TGD, 19, 'e', 12);
883	}
884	else if (type() == t_eph::L1NV) {
885	int i = 0; (!_RSF) ? i = 2 : i = 3;
886	if (i == 2) {
887	out
888	<< QString(fmt)
889	.arg(_URAI, 19, 'e', 12)
890	.arg(_health, 19, 'e', 12)
891	.arg(_TGD, 19, 'e', 12)
892	.arg("", 19, QChar(' '));
893	}
894	else {
895	out
896	<< QString(fmt)
897	.arg(_URAI, 19, 'e', 12)
898	.arg(_health, 19, 'e', 12)
899	.arg("", 19, QChar(' '))
900	.arg(_TGD, 19, 'e', 12);
901	}
902	}
903	}
904	// =====================
905	// BROADCAST ORBIT - 7
906	// =====================
907	if (type() == t_eph::LNAV \|\|
908	type() == t_eph::undefined) {
909
910	double tot = _TOT;
911	if (tot == 0.9999e9 && version < 3.0) {
912	tot = 0.0;
913	}
914	// fitInterval
915	if (system() == t_eph::NavIC) {
916	out
917	<< QString(fmt)
918	.arg(tot, 19, 'e', 12)
919	.arg("", 19, QChar(' '))
920	.arg("", 19, QChar(' '))
921	.arg("", 19, QChar(' '));
922	}
923	else {
924	if (_flags_unknown) {
925	out
926	<< QString(fmt)
927	.arg(tot, 19, 'e', 12)
928	.arg("", 19, QChar(' '))
929	.arg("", 19, QChar(' '))
930	.arg("", 19, QChar(' '));
931	}
932	else {
933	// for GPS and QZSS in version 3.02 specified in hours
934	double fitIntervalRnx = _fitInterval;
935	// otherwise specified as flag
936	if (system() == t_eph::QZSS && version != 3.02) {
937	(_fitInterval == 2.0) ? fitIntervalRnx = 0.0 : fitIntervalRnx = 1.0;
938	}
939	out
940	<< QString(fmt)
941	.arg(tot, 19, 'e', 12)
942	.arg(fitIntervalRnx, 19, 'e', 12)
943	.arg("", 19, QChar(' '))
944	.arg("", 19, QChar(' '));
945	}
946	}
947	}
948	else if (type() == t_eph::CNAV \|\|
949	type() == t_eph::CNV2) {
950	out
951	<< QString(fmt)
952	.arg(_ISC_L1CA, 19, 'e', 12)
953	.arg(_ISC_L2C, 19, 'e', 12)
954	.arg(_ISC_L5I5, 19, 'e', 12)
955	.arg(_ISC_L5Q5, 19, 'e', 12);
956	}
957	else if (type() == t_eph::L1NV) {
958	if (_RSF) {
959	out
960	<< QString(fmt)
961	.arg(_ISC_S, 19, 'e', 12)
962	.arg(_ISC_L1D, 19, 'e', 12)
963	.arg("", 19, QChar(' '))
964	.arg("", 19, QChar(' '));
965	}
966	else {
967	out
968	<< QString(fmt)
969	.arg("", 19, QChar(' '))
970	.arg("", 19, QChar(' '))
971	.arg(_ISC_L1P, 19, 'e', 12)
972	.arg(_ISC_L1D, 19, 'e', 12);
973	}
974	}
975	// =====================
976	// BROADCAST ORBIT - 8
977	// =====================
978	if (type() == t_eph::CNAV) {
979	int intFlags = 0;
980	if (!_flags_unknown) {
981	// Bit 0:
982	if (_intSF) {intFlags \|= (1 << 0);}
983	// Bit 1:
984	if (_L2Cphasing) {intFlags \|= (1 << 1);}
985	// Bit 2:
986	if (_alert) {intFlags \|= (1 << 2);}
987	out
988	<< QString(fmt)
989	.arg(_TOT, 19, 'e', 12)
990	.arg(_wnop, 19, 'e', 12)
991	.arg(double(intFlags), 19, 'e', 12)
992	.arg("", 19, QChar(' '));
993	}
994	else {
995	out
996	<< QString(fmt)
997	.arg(_TOT, 19, 'e', 12)
998	.arg(_wnop, 19, 'e', 12)
999	.arg("", 19, QChar(' '))
1000	.arg("", 19, QChar(' '));
1001	}
1002	}
1003	else if (type() == t_eph::CNV2) {
1004	out
1005	<< QString(fmt)
1006	.arg(_ISC_L1Cd, 19, 'e', 12)
1007	.arg(_ISC_L1Cp, 19, 'e', 12)
1008	.arg("", 19, QChar(' '))
1009	.arg("", 19, QChar(' '));
1010	}
1011	else if (type() == t_eph::L1NV) {
1012	out
1013	<< QString(fmt)
1014	.arg(_TOT, 19, 'e', 12)
1015	.arg("", 19, QChar(' '))
1016	.arg("", 19, QChar(' '))
1017	.arg("", 19, QChar(' '));
1018	}
1019	// =====================
1020	// BROADCAST ORBIT - 9
1021	// =====================
1022	if (type() == t_eph::CNV2) {
1023	int intFlags = 0;
1024	if (!_flags_unknown) {
1025	// Bit 0:
1026	if (_intSF) {intFlags \|= (1 << 0);}
1027	if (system() == t_eph::QZSS) {
1028	// Bit 1:
1029	if (_ephSF) {intFlags \|= (1 << 1);}
1030	}
1031	out
1032	<< QString(fmt)
1033	.arg(_TOT, 19, 'e', 12)
1034	.arg(_wnop, 19, 'e', 12)
1035	.arg(double(intFlags), 19, 'e', 12)
1036	.arg("", 19, QChar(' '));
1037	}
1038	else {
1039	out
1040	<< QString(fmt)
1041	.arg(_TOT, 19, 'e', 12)
1042	.arg(_wnop, 19, 'e', 12)
1043	.arg("", 19, QChar(' '))
1044	.arg("", 19, QChar(' '));
1045	}
1046	}
1047	return rnxStr;
1048	}
1049
1050	// Health status of GPS Ephemeris (virtual)
1051	////////////////////////////////////////////////////////////////////////////
1052	unsigned int t_ephGPS::isUnhealthy() const {
1053
1054	switch (system()) {
1055	case t_eph::GPS:
1056	case t_eph::QZSS:
1057	switch (type()) {
1058	case t_eph::LNAV:
1059	case t_eph::CNAV:
1060	case t_eph::CNV2:
1061	if (_health == 0.0) {
1062	return 0;
1063	}
1064	else {
1065	return 1;
1066	}
1067	break;
1068	}
1069	break;
1070	case t_eph::NavIC:
1071	switch (type()) {
1072	case t_eph::LNAV:
1073	if (_health == 0.0) { // L1 & S healthy
1074	return 0;
1075	}
1076	if (_health == 1.0 \|\| // L5 healthy and S unhealthy,
1077	_health == 2.0 \|\| // L5 unhealthy and S healthy
1078	_health == 3.0) { // both L5 and S unhealthy
1079	return 1;
1080	}
1081	break;
1082	case t_eph::L1NV:
1083	if (_health == 0.0) { // All navigation data on L1-SPS signal OK
1084	return 0;
1085	}
1086	if (_health == 1.0) { // Some or all navigation data on L1-SPS signal bad
1087	return 1;
1088	}
1089	break;
1090	}
1091	break;
1092	}
1093	return 0;
1094	}
1095
1096	// Constructor
1097	//////////////////////////////////////////////////////////////////////////////
1098	t_ephGlo::t_ephGlo(double rnxVersion, const QStringList &lines, const QString typeStr) {
1099
1100	setType(typeStr);
1101
1102	int nLines = 4;
1103
1104	// Source RINEX version < 4
1105	if (type() == t_eph::undefined) {
1106	// cannot be determined from input data
1107	// but is set to be able to work with old RINEX files in BNC applications
1108	_type = t_eph::FDMA_M;
1109	}
1110
1111	if (rnxVersion >= 3.05) {
1112	nLines += 1;
1113	} else {
1114	_M_delta_tau = 0.9999e9; // unknown
1115	_M_FT = 1.5e1; // unknown
1116	_statusflags_unknown = true;
1117	_healthflags_unknown = true;
1118	}
1119
1120	if (lines.size() != nLines) {
1121	_checkState = bad;
1122	return;
1123	}
1124
1125	// RINEX Format
1126	// ------------
1127	int fieldLen = 19;
1128	double statusflags = 0.0;
1129	double healthflags = 0.0;
1130	double sourceflags = 0.0;
1131	_tauC = 0.0;
1132	_tau1 = 0.0;
1133	_tau2 = 0.0;
1134	_additional_data_availability = 0.0;
1135
1136	int pos[4];
1137	pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1138	pos[1] = pos[0] + fieldLen;
1139	pos[2] = pos[1] + fieldLen;
1140	pos[3] = pos[2] + fieldLen;
1141
1142	// Read four lines
1143	// ---------------
1144	for (int iLine = 0; iLine < nLines; iLine++) {
1145	QString line = lines[iLine];
1146
1147	if (iLine == 0) {
1148	QTextStream in(line.left(pos[1]).toLatin1());
1149
1150	int year, month, day, hour, min;
1151	double sec;
1152
1153	QString prnStr, n;
1154	in >> prnStr;
1155	if (prnStr.size() == 1 && prnStr[0] == 'R') {
1156	in >> n;
1157	prnStr.append(n);
1158	}
1159
1160	if (prnStr.at(0) == 'R') {
1161	_prn.set('R', prnStr.mid(1).toInt());
1162	} else {
1163	_prn.set('R', prnStr.toInt());
1164	}
1165
1166	in >> year >> month >> day >> hour >> min >> sec;
1167	if (year < 80) {
1168	year += 2000;
1169	} else if (year < 100) {
1170	year += 1900;
1171	}
1172
1173	_gps_utc = gnumleap(year, month, day);
1174
1175	_TOC.set(year, month, day, hour, min, sec);
1176	_TOC = _TOC + _gps_utc;
1177	int nd = int((_TOC.gpssec())) / (24.0 * 60.0 * 60.0);
1178	if ( readDbl(line, pos[1], fieldLen, _tau)
1179	\|\| readDbl(line, pos[2], fieldLen, _gamma)
1180	\|\| readDbl(line, pos[3], fieldLen, _tki)) {
1181	_checkState = bad;
1182	return;
1183	}
1184	_tki -= nd * 86400.0;
1185	_tau = -_tau;
1186	}
1187	// =====================
1188	// BROADCAST ORBIT - 1
1189	// =====================
1190	else if (iLine == 1) {
1191	if ( readDbl(line, pos[0], fieldLen, _x_pos)
1192	\|\| readDbl(line, pos[1], fieldLen, _x_vel)
1193	\|\| readDbl(line, pos[2], fieldLen, _x_acc)
1194	\|\| readDbl(line, pos[3], fieldLen, _health)) {
1195	_checkState = bad;
1196	return;
1197	}
1198	}
1199	// =====================
1200	// BROADCAST ORBIT - 2
1201	// =====================
1202	else if (iLine == 2) {
1203	if (type() == t_eph::FDMA \|\|
1204	type() == t_eph::FDMA_M) {
1205	if ( readDbl(line, pos[0], fieldLen, _y_pos)
1206	\|\| readDbl(line, pos[1], fieldLen, _y_vel)
1207	\|\| readDbl(line, pos[2], fieldLen, _y_acc)
1208	\|\| readDbl(line, pos[3], fieldLen, _frq_num)) {
1209	_checkState = bad;
1210	return;
1211	}
1212	}
1213	else { //L1OC, L3OC
1214	if ( readDbl(line, pos[0], fieldLen, _y_pos)
1215	\|\| readDbl(line, pos[1], fieldLen, _y_vel)
1216	\|\| readDbl(line, pos[2], fieldLen, _y_acc)
1217	\|\| readDbl(line, pos[3], fieldLen, statusflags)) {
1218	_checkState = bad;
1219	return;
1220	}
1221	_data_validity = int(statusflags);
1222	}
1223	}
1224	// =====================
1225	// BROADCAST ORBIT - 3
1226	// =====================
1227	else if (iLine == 3) {
1228	if (type() == t_eph::FDMA \|\|
1229	type() == t_eph::FDMA_M) {
1230	if ( readDbl(line, pos[0], fieldLen, _z_pos)
1231	\|\| readDbl(line, pos[1], fieldLen, _z_vel)
1232	\|\| readDbl(line, pos[2], fieldLen, _z_acc)
1233	\|\| readDbl(line, pos[3], fieldLen, _E)) {
1234	_checkState = bad;
1235	return;
1236	}
1237	}
1238	else if (type() == t_eph::L1OC) {
1239	if ( readDbl(line, pos[0], fieldLen, _z_pos)
1240	\|\| readDbl(line, pos[1], fieldLen, _z_vel)
1241	\|\| readDbl(line, pos[2], fieldLen, _z_acc)
1242	\|\| readDbl(line, pos[3], fieldLen, _TGD_L2OCp)) {
1243	_checkState = bad;
1244	return;
1245	}
1246	}
1247	else if (type() == t_eph::L3OC) {
1248	if ( readDbl(line, pos[0], fieldLen, _z_pos)
1249	\|\| readDbl(line, pos[1], fieldLen, _z_vel)
1250	\|\| readDbl(line, pos[2], fieldLen, _z_acc)
1251	\|\| readDbl(line, pos[3], fieldLen, _TGD_L3OCp)) {
1252	_checkState = bad;
1253	return;
1254	}
1255	}
1256	}
1257	// =====================
1258	// BROADCAST ORBIT - 4
1259	// =====================
1260	else if (iLine == 4) {
1261	if (type() == t_eph::FDMA \|\|
1262	type() == t_eph::FDMA_M) {
1263	if (readDbl(line, pos[0], fieldLen, statusflags)) {
1264	//status flags BLK, do nothing
1265	_statusflags_unknown = true;
1266	} else {
1267	_statusflags_unknown = false;
1268	// status flags
1269	// ============
1270	// bit 0-1
1271	_M_P = double(bitExtracted(int(statusflags), 2, 0));
1272	// bit 2-3
1273	_P1 = double(bitExtracted(int(statusflags), 2, 2));
1274	// bit 4
1275	_P2 = double(bitExtracted(int(statusflags), 1, 4));
1276	// bit 5
1277	_P3 = double(bitExtracted(int(statusflags), 1, 5));
1278	// bit 6
1279	_M_P4 = double(bitExtracted(int(statusflags), 1, 6));
1280	// bit 7-8
1281	_M_M = double(bitExtracted(int(statusflags), 2, 7));
1282	/// GLO M/K exclusive flags/values only valid if flag M is set to '01'
1283	if (!_M_M) {
1284	_M_P = 0.0;
1285	_M_l3 = 0.0;
1286	_M_P4 = 0.0;
1287	_M_FE = 0.0;
1288	_M_FT = 0.0;
1289	_M_NA = 0.0;
1290	_M_NT = 0.0;
1291	_M_N4 = 0.0;
1292	_M_l5 = 0.0;
1293	_M_tau_GPS = 0.0;
1294	_M_delta_tau = 0.0;
1295	_type = t_eph::FDMA;
1296	}
1297	else {
1298	_type = t_eph::FDMA_M;
1299	}
1300	}
1301	if ( readDbl(line, pos[1], fieldLen, _M_delta_tau)
1302	\|\| readDbl(line, pos[2], fieldLen, _M_FT)) {
1303	_checkState = bad;
1304	return;
1305	}
1306	if (readDbl(line, pos[3], fieldLen, healthflags)) {
1307	// health flags BLK
1308	_healthflags_unknown = true;
1309	} else {
1310	_healthflags_unknown = false;
1311	// health flags
1312	// ============
1313	// bit 0 (is to be ignored, if bit 1 is zero)
1314	_almanac_health = double(bitExtracted(int(healthflags), 1, 0));
1315	// bit 1
1316	_almanac_health_availablility_indicator =
1317	double(bitExtracted(int(healthflags), 1, 1));
1318	// bit 2; GLO-M/K only, health bit of string 3
1319	_M_l3 = double(bitExtracted(int(healthflags), 1, 2));
1320	}
1321	}
1322	else if (type() == t_eph::L1OC \|\|
1323	type() == t_eph::L3OC) {
1324	if ( readDbl(line, pos[0], fieldLen, _sat_type)
1325	\|\| readDbl(line, pos[1], fieldLen, sourceflags)
1326	\|\| readDbl(line, pos[2], fieldLen, _EE)
1327	\|\| readDbl(line, pos[3], fieldLen, _ET)) {
1328	_checkState = bad;
1329	return;
1330	}
1331	// sourceflags:
1332	// ============
1333	// bit 0-1
1334	_RT = double(bitExtracted(int(sourceflags), 2, 0));
1335	// bit 2-3:
1336	_RE = double(bitExtracted(int(sourceflags), 2, 2));
1337	}
1338	}
1339	// =====================
1340	// BROADCAST ORBIT - 5
1341	// =====================
1342	else if (iLine == 5) {
1343	if (type() == t_eph::L1OC \|\|
1344	type() == t_eph::L3OC) {
1345	if ( readDbl(line, pos[0], fieldLen, _attitude_P2)
1346	\|\| readDbl(line, pos[1], fieldLen, _Tin)
1347	\|\| readDbl(line, pos[2], fieldLen, _tau1)
1348	\|\| readDbl(line, pos[3], fieldLen, _tau2)) {
1349	_checkState = bad;
1350	return;
1351	}
1352	}
1353	}
1354	// =====================
1355	// BROADCAST ORBIT - 6
1356	// =====================
1357	else if (iLine == 6) {
1358	if (type() == t_eph::L1OC \|\|
1359	type() == t_eph::L3OC) {
1360	if ( readDbl(line, pos[0], fieldLen, _yaw)
1361	\|\| readDbl(line, pos[1], fieldLen, _sn)
1362	\|\| readDbl(line, pos[2], fieldLen, _angular_rate)
1363	\|\| readDbl(line, pos[3], fieldLen, _angular_acc)) {
1364	_checkState = bad;
1365	return;
1366	}
1367	}
1368	}
1369	// =====================
1370	// BROADCAST ORBIT - 7
1371	// =====================
1372	else if (iLine == 7) {
1373	if (type() == t_eph::L1OC \|\|
1374	type() == t_eph::L3OC) {
1375	if ( readDbl(line, pos[0], fieldLen, _angular_rate_max)
1376	\|\| readDbl(line, pos[1], fieldLen, _X_PC)
1377	\|\| readDbl(line, pos[2], fieldLen, _Y_PC)
1378	\|\| readDbl(line, pos[3], fieldLen, _Z_PC)) {
1379	_checkState = bad;
1380	return;
1381	}
1382	}
1383	}
1384	// =====================
1385	// BROADCAST ORBIT - 8
1386	// =====================
1387	else if (iLine == 8) {
1388	if (type() == t_eph::L1OC \|\|
1389	type() == t_eph::L3OC) {
1390	if ( readDbl(line, pos[0], fieldLen, _M_FE)
1391	\|\| readDbl(line, pos[1], fieldLen, _M_FT)
1392	\|\| readDbl(line, pos[3], fieldLen, _TOT)) {
1393	_checkState = bad;
1394	return;
1395	}
1396	}
1397	}
1398	}
1399
1400	_prn.setFlag(type());
1401
1402	// Initialize status vector
1403	// ------------------------
1404	_tt = _TOC;
1405	_xv.ReSize(6);
1406	_xv = 0.0;
1407	_xv(1) = _x_pos * 1.e3;
1408	_xv(2) = _y_pos * 1.e3;
1409	_xv(3) = _z_pos * 1.e3;
1410	_xv(4) = _x_vel * 1.e3;
1411	_xv(5) = _y_vel * 1.e3;
1412	_xv(6) = _z_vel * 1.e3;
1413	}
1414
1415	// Compute Glonass Satellite Position (virtual)
1416	////////////////////////////////////////////////////////////////////////////
1417	t_irc t_ephGlo::position(int GPSweek, double GPSweeks, double *xc,
1418	double *vv) const {
1419
1420	static const double nominalStep = 10.0;
1421
1422	memset(xc, 0, 6 * sizeof(double));
1423	memset(vv, 0, 3 * sizeof(double));
1424
1425	double dtPos = bncTime(GPSweek, GPSweeks) - _tt;
1426
1427	if (fabs(dtPos) > 24 * 3600.0) {
1428	return failure;
1429	}
1430
1431	int nSteps = int(fabs(dtPos) / nominalStep) + 1;
1432	double step = dtPos / nSteps;
1433
1434	double acc[3];
1435	acc[0] = _x_acc * 1.e3;
1436	acc[1] = _y_acc * 1.e3;
1437	acc[2] = _z_acc * 1.e3;
1438
1439	for (int ii = 1; ii <= nSteps; ii++) {
1440	_xv = rungeKutta4(_tt.gpssec(), _xv, step, acc, glo_deriv);
1441	_tt = _tt + step;
1442	}
1443
1444	// Position and Velocity
1445	// ---------------------
1446	xc[0] = _xv(1);
1447	xc[1] = _xv(2);
1448	xc[2] = _xv(3);
1449
1450	vv[0] = _xv(4);
1451	vv[1] = _xv(5);
1452	vv[2] = _xv(6);
1453
1454	// Clock Correction
1455	// ----------------
1456	double dtClk = bncTime(GPSweek, GPSweeks) - _TOC;
1457	xc[3] = -_tau + _gamma * dtClk;
1458
1459	xc[4] = _gamma;
1460	xc[5] = 0.0;
1461
1462	return success;
1463	}
1464
1465	// RINEX Format String
1466	//////////////////////////////////////////////////////////////////////////////
1467	QString t_ephGlo::toString(double version) const {
1468
1469	if (version < 4.0 &&
1470	(type() == t_eph::L1OC \|\|
1471	type() == t_eph::L3OC )) {
1472	return "";
1473	}
1474
1475	QString ephStr = typeStr(_type, _prn, version);
1476	QString rnxStr = ephStr + rinexDateStr(_TOC - _gps_utc, _prn, version);
1477	int nd = int((_TOC - _gps_utc).gpssec()) / (24.0 * 60.0 * 60.0);
1478	QTextStream out(&rnxStr);
1479
1480	out
1481	<< QString("%1%2%3\n")
1482	.arg(-_tau, 19, 'e', 12)
1483	.arg(_gamma, 19, 'e', 12)
1484	.arg(_tki + nd * 86400.0, 19, 'e', 12);
1485
1486	QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1487	// =====================
1488	// BROADCAST ORBIT - 1
1489	// =====================
1490	out
1491	<< QString(fmt)
1492	.arg(_x_pos, 19, 'e', 12)
1493	.arg(_x_vel, 19, 'e', 12)
1494	.arg(_x_acc, 19, 'e', 12)
1495	.arg(_health, 19, 'e', 12);
1496
1497	// =====================
1498	// BROADCAST ORBIT - 2
1499	// =====================
1500	if (type() == t_eph::FDMA \|\|
1501	type() == t_eph::FDMA_M) {
1502	out
1503	<< QString(fmt)
1504	.arg(_y_pos, 19, 'e', 12)
1505	.arg(_y_vel, 19, 'e', 12)
1506	.arg(_y_acc, 19, 'e', 12)
1507	.arg(_frq_num, 19, 'e', 12);
1508	}
1509	else { //L1OC, L3OC
1510	out
1511	<< QString(fmt)
1512	.arg(_y_pos, 19, 'e', 12)
1513	.arg(_y_vel, 19, 'e', 12)
1514	.arg(_y_acc, 19, 'e', 12)
1515	.arg(double(_data_validity), 19, 'e', 12);
1516	}
1517	// =====================
1518	// BROADCAST ORBIT - 3
1519	// =====================
1520	if (type() == t_eph::FDMA \|\|
1521	type() == t_eph::FDMA_M) {
1522	out
1523	<< QString(fmt)
1524	.arg(_z_pos, 19, 'e', 12)
1525	.arg(_z_vel, 19, 'e', 12)
1526	.arg(_z_acc, 19, 'e', 12)
1527	.arg(_E, 19, 'e', 12);
1528	}
1529	else if (type() == t_eph::L1OC) {
1530	out
1531	<< QString(fmt)
1532	.arg(_z_pos, 19, 'e', 12)
1533	.arg(_z_vel, 19, 'e', 12)
1534	.arg(_z_acc, 19, 'e', 12)
1535	.arg(_TGD_L2OCp, 19, 'e', 12);
1536	}
1537	else if (type() == t_eph::L3OC) {
1538	out
1539	<< QString(fmt)
1540	.arg(_z_pos, 19, 'e', 12)
1541	.arg(_z_vel, 19, 'e', 12)
1542	.arg(_z_acc, 19, 'e', 12)
1543	.arg(_TGD_L3OCp, 19, 'e', 12);
1544	}
1545	if (version >= 3.05) {
1546	// =====================
1547	// BROADCAST ORBIT - 4
1548	// =====================
1549	if (type() == t_eph::FDMA \|\|
1550	type() == t_eph::FDMA_M){
1551	int statusflags = 0;
1552	int healthflags = 0;
1553	if (!_statusflags_unknown ) {
1554	// bit 0-1
1555	if (_M_P == 1.0) {statusflags \|= (1 << 0);}
1556	else if (_M_P == 2.0) {statusflags \|= (1 << 1);}
1557	else if (_M_P == 3.0) {statusflags \|= (1 << 0); statusflags \|= (1 << 1);}
1558	// bit 2-3
1559	if (_P1 == 1.0) {statusflags \|= (1 << 2);}
1560	else if (_P1 == 2.0) {statusflags \|= (1 << 3);}
1561	else if (_P1 == 3.0) {statusflags \|= (1 << 2); statusflags \|= (1 << 3);}
1562	// bit 4
1563	if (_P2) {statusflags \|= (1 << 4);}
1564	// bit 5
1565	if (_P3) {statusflags \|= (1 << 5);}
1566	// bit 6
1567	if (_M_P4) {statusflags \|= (1 << 6);}
1568	// bit 7-8
1569	if (_M_M == 1.0) {statusflags \|= (1 << 7);}
1570	}
1571	if (!_healthflags_unknown) {
1572	// bit 0 (is to be ignored, if bit 1 is zero)
1573	if (_almanac_health) {healthflags \|= (1 << 0);}
1574	// bit 1
1575	if (_almanac_health_availablility_indicator) {healthflags \|= (1 << 1);}
1576	// bit 2
1577	if (_M_l3) {healthflags \|= (1 << 2);}
1578	}
1579
1580	if (_statusflags_unknown && _healthflags_unknown) {
1581	out
1582	<< QString(fmt)
1583	.arg("", 19, QChar(' ')) // status-flags BNK (unknown)
1584	.arg(_M_delta_tau, 19, 'e', 12)
1585	.arg(_M_FT, 19, 'e', 12)
1586	.arg("", 19, QChar(' '));// health-flags BNK (unknown)
1587	}
1588	else if (!_statusflags_unknown && _healthflags_unknown) {
1589	out
1590	<< QString(fmt)
1591	.arg(double(statusflags), 19, 'e', 12)
1592	.arg(_M_delta_tau, 19, 'e', 12)
1593	.arg(_M_FT, 19, 'e', 12)
1594	.arg("", 19, QChar(' '));// health-flags BNK (unknown)
1595	}
1596	else if (_statusflags_unknown && !_healthflags_unknown) {
1597	out
1598	<< QString(fmt)
1599	.arg("", 19, QChar(' ')) // status-flags BNK (unknown)
1600	.arg(_M_delta_tau, 19, 'e', 12)
1601	.arg(_M_FT, 19, 'e', 12)
1602	.arg(double(healthflags), 19, 'e', 12);
1603	}
1604	else if (!_statusflags_unknown && !_healthflags_unknown) {
1605	out
1606	<< QString(fmt)
1607	.arg(double(statusflags), 19, 'e', 12)
1608	.arg(_M_delta_tau, 19, 'e', 12)
1609	.arg(_M_FT, 19, 'e', 12)
1610	.arg(double(healthflags), 19, 'e', 12);
1611	}
1612	}
1613	else if (type() == t_eph::L1OC \|\|
1614	type() == t_eph::L3OC) {
1615	int sourceflags = 0;
1616	// bit 0-1
1617	if (_RT == 1.0) {sourceflags \|= (1 << 0);}
1618	else if (_RT == 2.0) {sourceflags \|= (1 << 1);}
1619	else if (_RT == 3.0) {sourceflags \|= (1 << 0); sourceflags \|= (1 << 1);}
1620	// bit 2-3
1621	if (_RE == 1.0) {sourceflags \|= (1 << 2);}
1622	else if (_RE == 2.0) {sourceflags \|= (1 << 3);}
1623	else if (_RE == 3.0) {sourceflags \|= (1 << 2); sourceflags \|= (1 << 3);}
1624	out
1625	<< QString(fmt)
1626	.arg(_sat_type , 19, 'e', 12)
1627	.arg(double(sourceflags), 19, 'e', 12)
1628	.arg(_ET, 19, 'e', 12)
1629	.arg(_EE, 19, 'e', 12);
1630	}
1631	// =====================
1632	// BROADCAST ORBIT - 5
1633	// =====================
1634	if (type() == t_eph::L1OC \|\|
1635	type() == t_eph::L3OC) {
1636	out
1637	<< QString(fmt)
1638	.arg(_attitude_P2, 19, 'e', 12)
1639	.arg(_Tin, 19, 'e', 12)
1640	.arg(_tau1, 19, 'e', 12)
1641	.arg(_tau2, 19, 'e', 12);
1642	}
1643	// =====================
1644	// BROADCAST ORBIT - 6
1645	// =====================
1646	if (type() == t_eph::L1OC \|\|
1647	type() == t_eph::L3OC) {
1648	out
1649	<< QString(fmt)
1650	.arg(_yaw, 19, 'e', 12)
1651	.arg(_sn, 19, 'e', 12)
1652	.arg(_angular_rate, 19, 'e', 12)
1653	.arg(_angular_acc, 19, 'e', 12);
1654	}
1655	// =====================
1656	// BROADCAST ORBIT - 7
1657	// =====================
1658	if (type() == t_eph::L1OC \|\|
1659	type() == t_eph::L3OC) {
1660	out
1661	<< QString(fmt)
1662	.arg(_angular_rate_max, 19, 'e', 12)
1663	.arg(_X_PC, 19, 'e', 12)
1664	.arg(_Y_PC, 19, 'e', 12)
1665	.arg(_Z_PC, 19, 'e', 12);
1666	}
1667	// =====================
1668	// BROADCAST ORBIT - 8
1669	// =====================
1670	if (type() == t_eph::L1OC \|\|
1671	type() == t_eph::L3OC) {
1672	out
1673	<< QString(fmt)
1674	.arg(_M_FE, 19, 'e', 12)
1675	.arg(_M_FT, 19, 'e', 12)
1676	.arg("", 19, QChar(' '))
1677	.arg(_TOT, 19, 'e', 12);
1678	}
1679	}
1680	return rnxStr;
1681	}
1682
1683	// Derivative of the state vector using a simple force model (static)
1684	////////////////////////////////////////////////////////////////////////////
1685	ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector &xv,
1686	double *acc) {
1687
1688	// State vector components
1689	// -----------------------
1690	ColumnVector rr = xv.rows(1, 3);
1691	ColumnVector vv = xv.rows(4, 6);
1692
1693	// Acceleration
1694	// ------------
1695	static const double gmWGS = 398.60044e12;
1696	static const double AE = 6378136.0;
1697	static const double OMEGA = 7292115.e-11;
1698	static const double C20 = -1082.6257e-6;
1699
1700	double rho = rr.NormFrobenius();
1701	double t1 = -gmWGS / (rho * rho * rho);
1702	double t2 = 3.0 / 2.0 * C20 * (gmWGS * AE * AE)
1703	/ (rho * rho * rho * rho * rho);
1704	double t3 = OMEGA * OMEGA;
1705	double t4 = 2.0 * OMEGA;
1706	double z2 = rr(3) * rr(3);
1707
1708	// Vector of derivatives
1709	// ---------------------
1710	ColumnVector va(6);
1711	va(1) = vv(1);
1712	va(2) = vv(2);
1713	va(3) = vv(3);
1714	va(4) = (t1 + t2 * (1.0 - 5.0 * z2 / (rho * rho)) + t3) * rr(1) + t4 * vv(2)
1715	+ acc[0];
1716	va(5) = (t1 + t2 * (1.0 - 5.0 * z2 / (rho * rho)) + t3) * rr(2) - t4 * vv(1)
1717	+ acc[1];
1718	va(6) = (t1 + t2 * (3.0 - 5.0 * z2 / (rho * rho))) * rr(3) + acc[2];
1719
1720	return va;
1721	}
1722
1723	// IOD of Glonass Ephemeris (virtual)
1724	////////////////////////////////////////////////////////////////////////////
1725	unsigned int t_ephGlo::IOD() const {
1726	bncTime tMoscow = _TOC - _gps_utc + 3 * 3600.0;
1727	return (unsigned long) tMoscow.daysec() / 900;
1728	}
1729
1730	// Health status of Glonass Ephemeris (virtual)
1731	////////////////////////////////////////////////////////////////////////////
1732	unsigned int t_ephGlo::isUnhealthy() const {
1733
1734	if (_almanac_health_availablility_indicator) {
1735	if ((_health == 0 && _almanac_health == 0)
1736	\|\| (_health == 1 && _almanac_health == 0)
1737	\|\| (_health == 1 && _almanac_health == 1)) {
1738	return 1;
1739	}
1740	} else if (!_almanac_health_availablility_indicator) {
1741	if (_health) {
1742	return 1;
1743	}
1744	}
1745	return 0; /* (_health == 0 && _almanac_health == 1) or (_health == 0) */
1746	}
1747
1748	// Constructor
1749	//////////////////////////////////////////////////////////////////////////////
1750	t_ephGal::t_ephGal(double rnxVersion, const QStringList &lines, const QString typeStr) {
1751
1752	setType(typeStr);
1753
1754	const int nLines = 8;
1755
1756	if (lines.size() != nLines) {
1757	_checkState = bad;
1758	return;
1759	}
1760
1761	// RINEX Format
1762	// ------------
1763	int fieldLen = 19;
1764	double SVhealth = 0.0;
1765	double datasource = 0.0;
1766
1767	int pos[4];
1768	pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1769	pos[1] = pos[0] + fieldLen;
1770	pos[2] = pos[1] + fieldLen;
1771	pos[3] = pos[2] + fieldLen;
1772
1773	// Read eight lines
1774	// ----------------
1775	for (int iLine = 0; iLine < nLines; iLine++) {
1776	QString line = lines[iLine];
1777
1778	if (iLine == 0) {
1779	QTextStream in(line.left(pos[1]).toLatin1());
1780
1781	int year, month, day, hour, min;
1782	double sec;
1783
1784	QString prnStr, n;
1785	in >> prnStr;
1786	if (prnStr.size() == 1 && prnStr[0] == 'E') {
1787	in >> n;
1788	prnStr.append(n);
1789	}
1790	if (prnStr.at(0) == 'E') {
1791	_prn.set('E', prnStr.mid(1).toInt());
1792	} else {
1793	_prn.set('E', prnStr.toInt());
1794	}
1795
1796	in >> year >> month >> day >> hour >> min >> sec;
1797	if (year < 80) {
1798	year += 2000;
1799	} else if (year < 100) {
1800	year += 1900;
1801	}
1802
1803	_TOC.set(year, month, day, hour, min, sec);
1804
1805	if ( readDbl(line, pos[1], fieldLen, _clock_bias)
1806	\|\| readDbl(line, pos[2], fieldLen, _clock_drift)
1807	\|\| readDbl(line, pos[3], fieldLen, _clock_driftrate)) {
1808	_checkState = bad;
1809	return;
1810	}
1811	}
1812	// =====================
1813	// BROADCAST ORBIT - 1
1814	// =====================
1815	else if (iLine == 1) {
1816	if ( readDbl(line, pos[0], fieldLen, _IODnav)
1817	\|\| readDbl(line, pos[1], fieldLen, _Crs)
1818	\|\| readDbl(line, pos[2], fieldLen, _Delta_n)
1819	\|\| readDbl(line, pos[3], fieldLen, _M0)) {
1820	_checkState = bad;
1821	return;
1822	}
1823	}
1824	// =====================
1825	// BROADCAST ORBIT - 2
1826	// =====================
1827	else if (iLine == 2) {
1828	if ( readDbl(line, pos[0], fieldLen, _Cuc)
1829	\|\| readDbl(line, pos[1], fieldLen, _e)
1830	\|\| readDbl(line, pos[2], fieldLen, _Cus)
1831	\|\| readDbl(line, pos[3], fieldLen, _sqrt_A)) {
1832	_checkState = bad;
1833	return;
1834	}
1835	}
1836	// =====================
1837	// BROADCAST ORBIT - 3
1838	// =====================
1839	else if (iLine == 3) {
1840	if ( readDbl(line, pos[0], fieldLen, _TOEsec)
1841	\|\| readDbl(line, pos[1], fieldLen, _Cic)
1842	\|\| readDbl(line, pos[2], fieldLen, _OMEGA0)
1843	\|\| readDbl(line, pos[3], fieldLen, _Cis)) {
1844	_checkState = bad;
1845	return;
1846	}
1847	}
1848	// =====================
1849	// BROADCAST ORBIT - 4
1850	// =====================
1851	else if (iLine == 4) {
1852	if ( readDbl(line, pos[0], fieldLen, _i0)
1853	\|\| readDbl(line, pos[1], fieldLen, _Crc)
1854	\|\| readDbl(line, pos[2], fieldLen, _omega)
1855	\|\| readDbl(line, pos[3], fieldLen, _OMEGADOT)) {
1856	_checkState = bad;
1857	return;
1858	}
1859	}
1860	// =====================
1861	// BROADCAST ORBIT - 5
1862	// =====================
1863	else if (iLine == 5) {
1864	if ( readDbl(line, pos[0], fieldLen, _IDOT)
1865	\|\| readDbl(line, pos[1], fieldLen, datasource)
1866	\|\| readDbl(line, pos[2], fieldLen, _TOEweek)) {
1867	_checkState = bad;
1868	return;
1869	}
1870	else {
1871	if (bitExtracted(unsigned(datasource), 1, 8)) {
1872	_fnav = true;
1873	_type = t_eph::FNAV;
1874	_inav = false;
1875	/* set unused I/NAV values */
1876	_E5b_HS = 0.0;
1877	_E1B_HS = 0.0;
1878	_E1B_DataInvalid = false;
1879	_E5b_DataInvalid = false;
1880	}
1881	if (bitExtracted(unsigned(datasource), 1, 9)) {
1882	_fnav = false;
1883	_inav = true;
1884	_type = t_eph::INAV;
1885	/* set unused F/NAV values */
1886	_E5a_HS = 0.0;
1887	_E5a_DataInvalid = false;
1888	}
1889	// GAL week # in RINEX is aligned/identical to continuous GPS week # used in RINEX
1890	// but GST week # started at the first GPS roll-over (continuous GPS week 1024)
1891	_TOEweek -= 1024.0;
1892	}
1893	}
1894	// =====================
1895	// BROADCAST ORBIT - 6
1896	// =====================
1897	else if (iLine == 6) {
1898	if ( readDbl(line, pos[0], fieldLen, _SISA)
1899	\|\| readDbl(line, pos[1], fieldLen, SVhealth)
1900	\|\| readDbl(line, pos[2], fieldLen, _BGD_1_5A)
1901	\|\| readDbl(line, pos[3], fieldLen, _BGD_1_5B)) {
1902	_checkState = bad;
1903	return;
1904	} else {
1905	// Bit 0
1906	_E1B_DataInvalid = bitExtracted(int(SVhealth), 1, 0);
1907	// Bit 1-2
1908	_E1B_HS = double(bitExtracted(int(SVhealth), 2, 1));
1909	// Bit 3
1910	_E5a_DataInvalid = bitExtracted(int(SVhealth), 1, 3);
1911	// Bit 4-5
1912	_E5a_HS = double(bitExtracted(int(SVhealth), 2, 4));
1913	// Bit 6
1914	_E5b_DataInvalid = bitExtracted(int(SVhealth), 1, 6);
1915	// Bit 7-8
1916	_E5b_HS = double(bitExtracted(int(SVhealth), 2, 7));
1917	if (_fnav) {
1918	_BGD_1_5B = 0.0;
1919	}
1920	}
1921	}
1922	// =====================
1923	// BROADCAST ORBIT - 7
1924	// =====================
1925	else if (iLine == 7) {
1926	if (readDbl(line, pos[0], fieldLen, _TOT)) {
1927	_checkState = bad;
1928	return;
1929	}
1930	}
1931	}
1932	_prn.setFlag(type());
1933	}
1934
1935	// Compute Galileo Satellite Position (virtual)
1936	////////////////////////////////////////////////////////////////////////////
1937	t_irc t_ephGal::position(int GPSweek, double GPSweeks, double *xc,
1938	double *vv) const {
1939
1940	static const double omegaEarth = 7292115.1467e-11;
1941	static const double gmWGS = 398.6004418e12;
1942
1943	memset(xc, 0, 6 * sizeof(double));
1944	memset(vv, 0, 3 * sizeof(double));
1945
1946	double a0 = _sqrt_A * _sqrt_A;
1947	if (a0 == 0) {
1948	return failure;
1949	}
1950
1951	double n0 = sqrt(gmWGS / (a0 * a0 * a0));
1952
1953	bncTime tt(GPSweek, GPSweeks);
1954	double tk = tt - bncTime(_TOC.gpsw(), _TOEsec);
1955
1956	double n = n0 + _Delta_n;
1957	double M = _M0 + n * tk;
1958	double E = M;
1959	double E_last;
1960	int nLoop = 0;
1961	do {
1962	E_last = E;
1963	E = M + _e * sin(E);
1964
1965	if (++nLoop == 100) {
1966	return failure;
1967	}
1968	} while (fabs(E - E_last) * a0 > 0.001);
1969	double v = 2.0 * atan(sqrt((1.0 + _e) / (1.0 - _e)) * tan(E / 2));
1970	double u0 = v + _omega;
1971	double sin2u0 = sin(2 * u0);
1972	double cos2u0 = cos(2 * u0);
1973	double r = a0 * (1 - _e * cos(E)) + _Crc * cos2u0 + _Crs * sin2u0;
1974	double i = _i0 + _IDOT * tk + _Cic * cos2u0 + _Cis * sin2u0;
1975	double u = u0 + _Cuc * cos2u0 + _Cus * sin2u0;
1976	double xp = r * cos(u);
1977	double yp = r * sin(u);
1978	double OM = _OMEGA0 + (_OMEGADOT - omegaEarth) * tk - omegaEarth * _TOEsec;
1979
1980	double sinom = sin(OM);
1981	double cosom = cos(OM);
1982	double sini = sin(i);
1983	double cosi = cos(i);
1984	xc[0] = xp * cosom - yp * cosi * sinom;
1985	xc[1] = xp * sinom + yp * cosi * cosom;
1986	xc[2] = yp * sini;
1987
1988	double tc = tt - _TOC;
1989	xc[3] = _clock_bias + _clock_drift * tc + _clock_driftrate * tc * tc;
1990
1991	// Velocity
1992	// --------
1993	double tanv2 = tan(v / 2);
1994	double dEdM = 1 / (1 - _e * cos(E));
1995	double dotv = sqrt((1.0 + _e) / (1.0 - _e)) / cos(E / 2) / cos(E / 2)
1996	/ (1 + tanv2 * tanv2) * dEdM * n;
1997	double dotu = dotv + (-_Cuc * sin2u0 + _Cus * cos2u0) * 2 * dotv;
1998	double dotom = _OMEGADOT - omegaEarth;
1999	double doti = _IDOT + (-_Cic * sin2u0 + _Cis * cos2u0) * 2 * dotv;
2000	double dotr = a0 * _e * sin(E) * dEdM * n
2001	+ (-_Crc * sin2u0 + _Crs * cos2u0) * 2 * dotv;
2002	double dotx = dotr * cos(u) - r * sin(u) * dotu;
2003	double doty = dotr * sin(u) + r * cos(u) * dotu;
2004
2005	vv[0] = cosom * dotx - cosi * sinom * doty // dX / dr
2006	- xp * sinom * dotom - yp * cosi * cosom * dotom // dX / dOMEGA
2007	+ yp * sini * sinom * doti; // dX / di
2008
2009	vv[1] = sinom * dotx + cosi * cosom * doty + xp * cosom * dotom
2010	- yp * cosi * sinom * dotom - yp * sini * cosom * doti;
2011
2012	vv[2] = sini * doty + yp * cosi * doti;
2013
2014	// Relativistic Correction
2015	// -----------------------
2016	xc[3] -= 4.442807309e-10 * _e * sqrt(a0) * sin(E);
2017
2018	xc[4] = _clock_drift + _clock_driftrate * tc;
2019	xc[5] = _clock_driftrate;
2020
2021	return success;
2022	}
2023
2024	// Health status of Galileo Ephemeris (virtual)
2025	////////////////////////////////////////////////////////////////////////////
2026	unsigned int t_ephGal::isUnhealthy() const {
2027	// SHS; 1 = Out of Service, 3 = In Test, 0 = Signal Ok, 2 = Extended Operations Mode
2028	if (_E5a_HS == 1 \|\| _E5a_HS == 3 \|\|
2029	_E5b_HS == 1 \|\| _E5b_HS == 3 \|\|
2030	_E1B_HS == 1 \|\| _E1B_HS == 3) {
2031	return 1;
2032	}
2033	if (_E5a_DataInvalid \|\|
2034	_E5b_DataInvalid \|\|
2035	_E1B_DataInvalid) {
2036	return 1;
2037	}
2038	if (_SISA == 255.0) { // NAPA: No Accuracy Prediction Available
2039	return 1;
2040	}
2041	/*
2042	* SDD v1.3: SHS=2 leads to a newly-defined "EOM" status.
2043	* It also means that the satellite signal may be used for PNT.
2044	if (_E5aHS == 2 \|\|
2045	_E5bHS == 2 \|\|
2046	_E1_bHS == 2 ) {
2047	return 1;
2048	}
2049	*/
2050	return 0;
2051
2052	}
2053
2054	// RINEX Format String
2055	//////////////////////////////////////////////////////////////////////////////
2056	QString t_ephGal::toString(double version) const {
2057
2058	QString ephStr = typeStr(_type, _prn, version);
2059	QString rnxStr = ephStr + rinexDateStr(_TOC, _prn, version);
2060
2061	QTextStream out(&rnxStr);
2062
2063	out
2064	<< QString("%1%2%3\n").arg(_clock_bias, 19, 'e', 12).arg(_clock_drift, 19,
2065	'e', 12).arg(_clock_driftrate, 19, 'e', 12);
2066
2067	QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
2068	// =====================
2069	// BROADCAST ORBIT - 1
2070	// =====================
2071	out
2072	<< QString(fmt)
2073	.arg(_IODnav, 19, 'e', 12)
2074	.arg(_Crs, 19, 'e', 12)
2075	.arg(_Delta_n, 19, 'e', 12)
2076	.arg(_M0, 19, 'e', 12);
2077	// =====================
2078	// BROADCAST ORBIT - 2
2079	// =====================
2080	out
2081	<< QString(fmt)
2082	.arg(_Cuc, 19, 'e', 12).
2083	arg(_e, 19, 'e', 12)
2084	.arg(_Cus, 19, 'e', 12)
2085	.arg(_sqrt_A, 19, 'e', 12);
2086	// =====================
2087	// BROADCAST ORBIT - 3
2088	// =====================
2089	out
2090	<< QString(fmt).
2091	arg(_TOEsec, 19, 'e', 12)
2092	.arg(_Cic, 19, 'e', 12)
2093	.arg(_OMEGA0, 19, 'e', 12)
2094	.arg(_Cis, 19, 'e', 12);
2095	// =====================
2096	// BROADCAST ORBIT - 4
2097	// =====================
2098	out
2099	<< QString(fmt)
2100	.arg(_i0, 19, 'e', 12)
2101	.arg(_Crc, 19, 'e', 12)
2102	.arg(_omega, 19, 'e', 12)
2103	.arg(_OMEGADOT, 19, 'e', 12);
2104	// =====================
2105	// BROADCAST ORBIT - 5/6
2106	// =====================
2107	int dataSource = 0;
2108	int SVhealth = 0;
2109	double BGD_1_5A = _BGD_1_5A;
2110	double BGD_1_5B = _BGD_1_5B;
2111	if (_fnav) {
2112	dataSource \|= (1 << 1);
2113	dataSource \|= (1 << 8);
2114	BGD_1_5B = 0.0;
2115	// SVhealth
2116	// Bit 3 : E5a DVS
2117	if (_E5a_DataInvalid) {
2118	SVhealth \|= (1 << 3);
2119	}
2120	// Bit 4-5: E5a HS
2121	if (_E5a_HS == 1.0) {
2122	SVhealth \|= (1 << 4);
2123	}
2124	else if (_E5a_HS == 2.0) {
2125	SVhealth \|= (1 << 5);
2126	}
2127	else if (_E5a_HS == 3.0) {
2128	SVhealth \|= (1 << 4);
2129	SVhealth \|= (1 << 5);
2130	}
2131	} else if (_inav) {
2132	// Bit 2 and 0 are set because from MT1046 the data source cannot be determined
2133	// and RNXv3.03 says both can be set if the navigation messages were merged
2134	dataSource \|= (1 << 0);
2135	dataSource \|= (1 << 2);
2136	dataSource \|= (1 << 9);
2137	// SVhealth
2138	// Bit 0 : E1-B DVS
2139	if (_E1B_DataInvalid) {
2140	SVhealth \|= (1 << 0);
2141	}
2142	// Bit 1-2: E1-B HS
2143	if (_E1B_HS == 1.0) {
2144	SVhealth \|= (1 << 1);
2145	}
2146	else if (_E1B_HS == 2.0) {
2147	SVhealth \|= (1 << 2);
2148	}
2149	else if (_E1B_HS == 3.0) {
2150	SVhealth \|= (1 << 1);
2151	SVhealth \|= (1 << 2);
2152	}
2153	// Bit 6 : E5b DVS
2154	if (_E5b_DataInvalid) {
2155	SVhealth \|= (1 << 6);
2156	}
2157	// Bit 7-8: E5b HS
2158	if (_E5b_HS == 1.0) {
2159	SVhealth \|= (1 << 7);
2160	}
2161	else if (_E5b_HS == 2.0) {
2162	SVhealth \|= (1 << 8);
2163	}
2164	else if (_E5b_HS == 3.0) {
2165	SVhealth \|= (1 << 7);
2166	SVhealth \|= (1 << 8);
2167	}
2168	}
2169	// =====================
2170	// BROADCAST ORBIT - 5
2171	// =====================
2172	out
2173	<< QString(fmt)
2174	.arg(_IDOT, 19, 'e', 12)
2175	.arg(double(dataSource), 19, 'e', 12)
2176	.arg(_TOEweek + 1024.0, 19, 'e', 12)
2177	.arg(0.0, 19, 'e', 12);
2178	// =====================
2179	// BROADCAST ORBIT - 6
2180	// =====================
2181	out
2182	<< QString(fmt)
2183	.arg(_SISA, 19, 'e', 12)
2184	.arg(double(SVhealth), 19, 'e', 12)
2185	.arg(BGD_1_5A, 19, 'e', 12)
2186	.arg(BGD_1_5B, 19, 'e', 12);
2187	// =====================
2188	// BROADCAST ORBIT - 7
2189	// =====================
2190	double tot = _TOT;
2191	if (tot == 0.9999e9 && version < 3.0) {
2192	tot = 0.0;
2193	}
2194	out
2195	<< QString(fmt)
2196	.arg(tot, 19, 'e', 12)
2197	.arg("", 19, QChar(' '))
2198	.arg("", 19, QChar(' '))
2199	.arg("", 19, QChar(' '));
2200
2201	return rnxStr;
2202	}
2203
2204	// Constructor
2205	//////////////////////////////////////////////////////////////////////////////
2206	t_ephSBAS::t_ephSBAS(double rnxVersion, const QStringList &lines, const QString typeStr) {
2207
2208	setType(typeStr);
2209
2210	const int nLines = 4;
2211
2212	// Source RINEX version < 4
2213	if (type() == t_eph::undefined) {
2214	_type = t_eph::SBASL1;
2215	}
2216
2217	if (lines.size() != nLines) {
2218	_checkState = bad;
2219	return;
2220	}
2221
2222	// RINEX Format
2223	// ------------
2224	int fieldLen = 19;
2225
2226	int pos[4];
2227	pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
2228	pos[1] = pos[0] + fieldLen;
2229	pos[2] = pos[1] + fieldLen;
2230	pos[3] = pos[2] + fieldLen;
2231
2232	// Read four lines
2233	// ---------------
2234	for (int iLine = 0; iLine < nLines; iLine++) {
2235	QString line = lines[iLine];
2236
2237	if (iLine == 0) {
2238	QTextStream in(line.left(pos[1]).toLatin1());
2239
2240	int year, month, day, hour, min;
2241	double sec;
2242
2243	QString prnStr, n;
2244	in >> prnStr;
2245	if (prnStr.size() == 1 && prnStr[0] == 'S') {
2246	in >> n;
2247	prnStr.append(n);
2248	}
2249	if (prnStr.at(0) == 'S') {
2250	_prn.set('S', prnStr.mid(1).toInt());
2251	} else {
2252	_prn.set('S', prnStr.toInt());
2253	}
2254	_prn.setFlag(type());
2255
2256	in >> year >> month >> day >> hour >> min >> sec;
2257
2258	if (year < 80) {
2259	year += 2000;
2260	} else if (year < 100) {
2261	year += 1900;
2262	}
2263
2264	_TOC.set(year, month, day, hour, min, sec);
2265
2266	if ( readDbl(line, pos[1], fieldLen, _agf0)
2267	\|\| readDbl(line, pos[2], fieldLen, _agf1)
2268	\|\| readDbl(line, pos[3], fieldLen, _TOT)) {
2269	_checkState = bad;
2270	return;
2271	}
2272	}
2273	// =====================
2274	// BROADCAST ORBIT - 1
2275	// =====================
2276	else if (iLine == 1) {
2277	if ( readDbl(line, pos[0], fieldLen, _x_pos)
2278	\|\| readDbl(line, pos[1], fieldLen, _x_vel)
2279	\|\| readDbl(line, pos[2], fieldLen, _x_acc)
2280	\|\| readDbl(line, pos[3], fieldLen, _health)) {
2281	_checkState = bad;
2282	return;
2283	}
2284	}
2285	// =====================
2286	// BROADCAST ORBIT - 2
2287	// =====================
2288	else if (iLine == 2) {
2289	if ( readDbl(line, pos[0], fieldLen, _y_pos)
2290	\|\| readDbl(line, pos[1], fieldLen, _y_vel)
2291	\|\| readDbl(line, pos[2], fieldLen, _y_acc)
2292	\|\| readDbl(line, pos[3], fieldLen, _ura)) {
2293	_checkState = bad;
2294	return;
2295	}
2296	}
2297	// =====================
2298	// BROADCAST ORBIT - 3
2299	// =====================
2300	else if (iLine == 3) {
2301	double iodn;
2302	if ( readDbl(line, pos[0], fieldLen, _z_pos)
2303	\|\| readDbl(line, pos[1], fieldLen, _z_vel)
2304	\|\| readDbl(line, pos[2], fieldLen, _z_acc)
2305	\|\| readDbl(line, pos[3], fieldLen, iodn)) {
2306	_checkState = bad;
2307	return;
2308	} else {
2309	_IODN = int(iodn);
2310	}
2311	}
2312	}
2313	_x_pos *= 1.e3;
2314	_y_pos *= 1.e3;
2315	_z_pos *= 1.e3;
2316	_x_vel *= 1.e3;
2317	_y_vel *= 1.e3;
2318	_z_vel *= 1.e3;
2319	_x_acc *= 1.e3;
2320	_y_acc *= 1.e3;
2321	_z_acc *= 1.e3;
2322	}
2323
2324	// IOD of SBAS Ephemeris (virtual)
2325	////////////////////////////////////////////////////////////////////////////
2326
2327	unsigned int t_ephSBAS::IOD() const {
2328	unsigned char buffer[80];
2329	int size = 0;
2330	int numbits = 0;
2331	long long bitbuffer = 0;
2332	unsigned char *startbuffer = buffer;
2333
2334	SBASADDBITSFLOAT(30, this->_x_pos, 0.08)
2335	SBASADDBITSFLOAT(30, this->_y_pos, 0.08)
2336	SBASADDBITSFLOAT(25, this->_z_pos, 0.4)
2337	SBASADDBITSFLOAT(17, this->_x_vel, 0.000625)
2338	SBASADDBITSFLOAT(17, this->_y_vel, 0.000625)
2339	SBASADDBITSFLOAT(18, this->_z_vel, 0.004)
2340	SBASADDBITSFLOAT(10, this->_x_acc, 0.0000125)
2341	SBASADDBITSFLOAT(10, this->_y_acc, 0.0000125)
2342	SBASADDBITSFLOAT(10, this->_z_acc, 0.0000625)
2343	SBASADDBITSFLOAT(12, this->_agf0,
2344	1.0 / static_cast<double>(1 << 30) / static_cast<double>(1 << 1))
2345	SBASADDBITSFLOAT(8, this->_agf1,
2346	1.0 / static_cast<double>(1 << 30) / static_cast<double>(1 << 10))
2347	SBASADDBITS(5, 0); // the last byte is filled by 0-bits to obtain a length of an integer multiple of 8
2348
2349	return CRC24(size, startbuffer);
2350	}
2351
2352	// Compute SBAS Satellite Position (virtual)
2353	////////////////////////////////////////////////////////////////////////////
2354	t_irc t_ephSBAS::position(int GPSweek, double GPSweeks, double *xc,
2355	double *vv) const {
2356
2357	bncTime tt(GPSweek, GPSweeks);
2358	double dt = tt - _TOC;
2359
2360	xc[0] = _x_pos + _x_vel * dt + _x_acc * dt * dt / 2.0;
2361	xc[1] = _y_pos + _y_vel * dt + _y_acc * dt * dt / 2.0;
2362	xc[2] = _z_pos + _z_vel * dt + _z_acc * dt * dt / 2.0;
2363
2364	vv[0] = _x_vel + _x_acc * dt;
2365	vv[1] = _y_vel + _y_acc * dt;
2366	vv[2] = _z_vel + _z_acc * dt;
2367
2368	xc[3] = _agf0 + _agf1 * dt;
2369
2370	xc[4] = _agf1;
2371	xc[5] = 0.0;
2372
2373	return success;
2374	}
2375
2376	// Health status of SBAS Ephemeris (virtual)
2377	////////////////////////////////////////////////////////////////////////////
2378	unsigned int t_ephSBAS::isUnhealthy() const {
2379
2380	// Bit 5
2381	bool URAindexIs15 = bitExtracted(int(_health), 1, 5);
2382	if (URAindexIs15) {
2383	// in this case it is recommended
2384	// to set the bits 0,1,2,3 to 1 (MT17health = 15)
2385	return 1;
2386	}
2387
2388	// Bit 4
2389	bool MT17HealthIsUnavailable = bitExtracted(int(_health), 1, 4);
2390	if (MT17HealthIsUnavailable) {
2391	return 0;
2392	}
2393
2394	// Bit 0-3
2395	int MT17health = bitExtracted(int(_health), 4, 0);
2396	if (MT17health) {
2397	return 1;
2398	}
2399
2400	return 0;
2401	}
2402
2403	// RINEX Format String
2404	//////////////////////////////////////////////////////////////////////////////
2405	QString t_ephSBAS::toString(double version) const {
2406
2407	QString ephStr = typeStr(_type, _prn, version);
2408	QString rnxStr = ephStr + rinexDateStr(_TOC, _prn, version);
2409
2410	QTextStream out(&rnxStr);
2411
2412	out
2413	<< QString("%1%2%3\n")
2414	.arg(_agf0, 19, 'e', 12)
2415	.arg(_agf1, 19, 'e', 12)
2416	.arg(_TOT, 19, 'e', 12);
2417
2418	QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
2419	// =====================
2420	// BROADCAST ORBIT - 1
2421	// =====================
2422	out
2423	<< QString(fmt)
2424	.arg(1.e-3 * _x_pos, 19, 'e', 12)
2425	.arg(1.e-3 * _x_vel, 19, 'e', 12)
2426	.arg(1.e-3 * _x_acc, 19, 'e', 12)
2427	.arg(_health, 19, 'e', 12);
2428	// =====================
2429	// BROADCAST ORBIT - 2
2430	// =====================
2431	out
2432	<< QString(fmt)
2433	.arg(1.e-3 * _y_pos, 19, 'e', 12)
2434	.arg(1.e-3 * _y_vel, 19, 'e', 12)
2435	.arg(1.e-3 * _y_acc, 19, 'e', 12)
2436	.arg(_ura, 19, 'e', 12);
2437	// =====================
2438	// BROADCAST ORBIT - 3
2439	// =====================
2440	out
2441	<< QString(fmt)
2442	.arg(1.e-3 * _z_pos, 19, 'e', 12)
2443	.arg(1.e-3 * _z_vel, 19, 'e', 12)
2444	.arg(1.e-3 * _z_acc, 19, 'e', 12)
2445	.arg(double(_IODN), 19, 'e', 12);
2446
2447	return rnxStr;
2448	}
2449
2450	// Constructor
2451	//////////////////////////////////////////////////////////////////////////////
2452	t_ephBDS::t_ephBDS(double rnxVersion, const QStringList &lines, const QString typeStr) {
2453
2454	setType(typeStr);
2455
2456	int nLines = 8;
2457
2458	if (type() == t_eph::CNV1 \|\|
2459	type() == t_eph::CNV2) {
2460	nLines += 2;
2461	}
2462	if (type() == t_eph::CNV3) {
2463	nLines += 1;
2464	}
2465
2466	if (lines.size() != nLines) {
2467	_checkState = bad;
2468	return;
2469	}
2470
2471	// RINEX Format
2472	// ------------
2473	int fieldLen = 19;
2474
2475	int pos[4];
2476	pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
2477	pos[1] = pos[0] + fieldLen;
2478	pos[2] = pos[1] + fieldLen;
2479	pos[3] = pos[2] + fieldLen;
2480
2481	// Read eight lines
2482	// ----------------
2483	for (int iLine = 0; iLine < nLines; iLine++) {
2484	QString line = lines[iLine];
2485
2486	if (iLine == 0) {
2487	QTextStream in(line.left(pos[1]).toLatin1());
2488
2489	int year, month, day, hour, min;
2490	double sec;
2491
2492	QString prnStr, n;
2493	in >> prnStr;
2494	if (prnStr.size() == 1 && prnStr[0] == 'C') {
2495	in >> n;
2496	prnStr.append(n);
2497	}
2498	if (prnStr.at(0) == 'C') {
2499	_prn.set('C', prnStr.mid(1).toInt());
2500	} else {
2501	_prn.set('C', prnStr.toInt());
2502	}
2503
2504	in >> year >> month >> day >> hour >> min >> sec;
2505	if (year < 80) {
2506	year += 2000;
2507	} else if (year < 100) {
2508	year += 1900;
2509	}
2510
2511	_TOC.setBDS(year, month, day, hour, min, sec);
2512
2513	if ( readDbl(line, pos[1], fieldLen, _clock_bias)
2514	\|\| readDbl(line, pos[2], fieldLen, _clock_drift)
2515	\|\| readDbl(line, pos[3], fieldLen, _clock_driftrate)) {
2516	_checkState = bad;
2517	return;
2518	}
2519	}
2520	// =====================
2521	// BROADCAST ORBIT - 1
2522	// =====================
2523	else if (iLine == 1) {
2524	if (type() == t_eph::D1 \|\|
2525	type() == t_eph::D2 \|\|
2526	type() == t_eph::undefined) {
2527	double aode;
2528	if ( readDbl(line, pos[0], fieldLen, aode)
2529	\|\| readDbl(line, pos[1], fieldLen, _Crs)
2530	\|\| readDbl(line, pos[2], fieldLen, _Delta_n)
2531	\|\| readDbl(line, pos[3], fieldLen, _M0)) {
2532	_checkState = bad;
2533	return;
2534	}
2535	_AODE = int(aode);
2536	}
2537	else { //CNV1, CNV2, CNV3
2538	if ( readDbl(line, pos[0], fieldLen, _ADOT)
2539	\|\| readDbl(line, pos[1], fieldLen, _Crs)
2540	\|\| readDbl(line, pos[2], fieldLen, _Delta_n)
2541	\|\| readDbl(line, pos[3], fieldLen, _M0)) {
2542	_checkState = bad;
2543	return;
2544	}
2545	}
2546	}
2547	// =====================
2548	// BROADCAST ORBIT - 2
2549	// =====================
2550	else if (iLine == 2) {
2551	if ( readDbl(line, pos[0], fieldLen, _Cuc)
2552	\|\| readDbl(line, pos[1], fieldLen, _e)
2553	\|\| readDbl(line, pos[2], fieldLen, _Cus)
2554	\|\| readDbl(line, pos[3], fieldLen, _sqrt_A)) {
2555	_checkState = bad;
2556	return;
2557	}
2558	}
2559	// =====================
2560	// BROADCAST ORBIT - 3
2561	// =====================
2562	else if (iLine == 3) {
2563	if ( readDbl(line, pos[0], fieldLen, _TOEsec)
2564	\|\| readDbl(line, pos[1], fieldLen, _Cic)
2565	\|\| readDbl(line, pos[2], fieldLen, _OMEGA0)
2566	\|\| readDbl(line, pos[3], fieldLen, _Cis)) {
2567	_checkState = bad;
2568	return;
2569	}
2570	}
2571	// =====================
2572	// BROADCAST ORBIT - 4
2573	// =====================
2574	else if (iLine == 4) {
2575	if ( readDbl(line, pos[0], fieldLen, _i0)
2576	\|\| readDbl(line, pos[1], fieldLen, _Crc)
2577	\|\| readDbl(line, pos[2], fieldLen, _omega)
2578	\|\| readDbl(line, pos[3], fieldLen, _OMEGADOT)) {
2579	_checkState = bad;
2580	return;
2581	}
2582	else {
2583	// Source RINEX version < 4
2584	if (type() == t_eph::undefined) {
2585	const double iMaxGEO = 10.0 / 180.0 * M_PI;
2586	if (_i0 > iMaxGEO) {
2587	_type = t_eph::D1;
2588	}
2589	else {
2590	_type = t_eph::D2;
2591	}
2592	}
2593	}
2594	}
2595	// =====================
2596	// BROADCAST ORBIT - 5
2597	// =====================
2598	else if (iLine == 5) {
2599	if (type() == t_eph::CNV1 \|\|
2600	type() == t_eph::CNV2 \|\|
2601	type() == t_eph::CNV3) {
2602	if ( readDbl(line, pos[0], fieldLen, _IDOT)
2603	\|\| readDbl(line, pos[1], fieldLen, _Delta_n_dot)
2604	\|\| readDbl(line, pos[2], fieldLen, _satType)
2605	\|\| readDbl(line, pos[3], fieldLen, _top)) {
2606	_checkState = bad;
2607	return;
2608	}
2609	}
2610	else { // D1, D2
2611	if ( readDbl(line, pos[0], fieldLen, _IDOT)
2612	\|\| readDbl(line, pos[2], fieldLen, _BDTweek)) {
2613	_checkState = bad;
2614	return;
2615	}
2616	}
2617	}
2618	// =====================
2619	// BROADCAST ORBIT - 6
2620	// =====================
2621	else if (iLine == 6) {
2622	if (type() == t_eph::CNV1 \|\|
2623	type() == t_eph::CNV2 \|\|
2624	type() == t_eph::CNV3) {
2625	if ( readDbl(line, pos[0], fieldLen, _SISAI_oe)
2626	\|\| readDbl(line, pos[1], fieldLen, _SISAI_ocb)
2627	\|\| readDbl(line, pos[2], fieldLen, _SISAI_oc1)
2628	\|\| readDbl(line, pos[3], fieldLen, _SISAI_oc2)) {
2629	_checkState = bad;
2630	return;
2631	}
2632	}
2633	else { // D1, D2
2634	double SatH1;
2635	if ( readDbl(line, pos[0], fieldLen, _ura)
2636	\|\| readDbl(line, pos[1], fieldLen, SatH1)
2637	\|\| readDbl(line, pos[2], fieldLen, _TGD1)
2638	\|\| readDbl(line, pos[3], fieldLen, _TGD2)) {
2639	_checkState = bad;
2640	return;
2641	}
2642	_SatH1 = int(SatH1);
2643	}
2644	}
2645	// =====================
2646	// BROADCAST ORBIT - 7
2647	// =====================
2648	else if (iLine == 7) {
2649	if (type() == t_eph::CNV1) {
2650	if ( readDbl(line, pos[0], fieldLen, _ISC_B1Cd)
2651	\|\| readDbl(line, pos[2], fieldLen, _TGD_B1Cp)
2652	\|\| readDbl(line, pos[3], fieldLen, _TGD_B2ap)) {
2653	_checkState = bad;
2654	return;
2655	}
2656	}
2657	else if (type() == t_eph::CNV2) {
2658	if ( readDbl(line, pos[1], fieldLen, _ISC_B2ad)
2659	\|\| readDbl(line, pos[2], fieldLen, _TGD_B1Cp)
2660	\|\| readDbl(line, pos[3], fieldLen, _TGD_B2ap)) {
2661	_checkState = bad;
2662	return;
2663	}
2664	}
2665	else if (type() == t_eph::CNV3) {
2666	double health;
2667	if ( readDbl(line, pos[0], fieldLen, _SISMAI)
2668	\|\| readDbl(line, pos[1], fieldLen, health)
2669	\|\| readDbl(line, pos[2], fieldLen, _INTEGRITYF_B2b)
2670	\|\| readDbl(line, pos[3], fieldLen, _TGD_B2bI)) {
2671	_checkState = bad;
2672	return;
2673	}
2674	_health = int(health);
2675	}
2676	else { // D1, D2
2677	double aodc;
2678	if ( readDbl(line, pos[0], fieldLen, _TOT)
2679	\|\| readDbl(line, pos[1], fieldLen, aodc)) {
2680	_checkState = bad;
2681	return;
2682	}
2683	if (_TOT == 0.9999e9) { // 0.9999e9 means not known (RINEX standard)
2684	_TOT = _TOEsec;
2685	}
2686	_AODC = int(aodc);
2687	}
2688	}
2689	// =====================
2690	// BROADCAST ORBIT - 8
2691	// =====================
2692	else if (iLine == 8) {
2693	double health;
2694	if (type() == t_eph::CNV1) {
2695	if ( readDbl(line, pos[0], fieldLen, _SISMAI)
2696	\|\| readDbl(line, pos[1], fieldLen, health)
2697	\|\| readDbl(line, pos[2], fieldLen, _INTEGRITYF_B1C)
2698	\|\| readDbl(line, pos[3], fieldLen, _IODC)) {
2699	_checkState = bad;
2700	return;
2701	}
2702	_health = int(health);
2703	}
2704	else if (type() == t_eph::CNV2) {
2705	if ( readDbl(line, pos[0], fieldLen, _SISMAI)
2706	\|\| readDbl(line, pos[1], fieldLen, health)
2707	\|\| readDbl(line, pos[2], fieldLen, _INTEGRITYF_B2aB1C)
2708	\|\| readDbl(line, pos[3], fieldLen, _IODC)) {
2709	_checkState = bad;
2710	return;
2711	}
2712	_health = int(health);
2713	}
2714	else if (type() == t_eph::CNV3) {
2715	if (readDbl(line, pos[0], fieldLen, _TOT)) {
2716	_checkState = bad;
2717	return;
2718	}
2719	}
2720	}
2721	// =====================
2722	// BROADCAST ORBIT - 9
2723	// =====================
2724	else if (iLine == 9) {
2725	if (type() == t_eph::CNV1 \|\|
2726	type() == t_eph::CNV2) {
2727	if ( readDbl(line, pos[0], fieldLen, _TOT)
2728	\|\| readDbl(line, pos[3], fieldLen, _IODE)) {
2729	_checkState = bad;
2730	return;
2731	}
2732	}
2733	}
2734	}
2735	_prn.setFlag(type());
2736
2737	_TOE.setBDS(int(_BDTweek), _TOEsec);
2738	// remark: actually should be computed from second_tot
2739	// but it seems to be unreliable in RINEX files
2740	//_TOT = _TOC.bdssec();
2741	}
2742
2743	// IOD of BDS Ephemeris (virtual)
2744	////////////////////////////////////////////////////////////////////////////
2745	unsigned int t_ephBDS::IOD() const {
2746	return (int(_TOC.gpssec()) / 720) % 240; //return (int(_TOEsec)/720) % 240;
2747	}
2748
2749	// Compute BDS Satellite Position (virtual)
2750	//////////////////////////////////////////////////////////////////////////////
2751	t_irc t_ephBDS::position(int GPSweek, double GPSweeks, double *xc,
2752	double *vv) const {
2753
2754	static const double gmBDS = 398.6004418e12;
2755	static const double omegaBDS = 7292115.0000e-11;
2756
2757	xc[0] = xc[1] = xc[2] = xc[3] = 0.0;
2758	vv[0] = vv[1] = vv[2] = 0.0;
2759
2760	bncTime tt(GPSweek, GPSweeks);
2761
2762	if (_sqrt_A == 0) {
2763	return failure;
2764	}
2765	double a0 = _sqrt_A * _sqrt_A;
2766
2767	double n0 = sqrt(gmBDS / (a0 * a0 * a0));
2768	double tk = tt - _TOE;
2769	double n = n0 + _Delta_n;
2770	double M = _M0 + n * tk;
2771	double E = M;
2772	double E_last;
2773	int nLoop = 0;
2774	do {
2775	E_last = E;
2776	E = M + _e * sin(E);
2777
2778	if (++nLoop == 100) {
2779	return failure;
2780	}
2781	} while (fabs(E - E_last) * a0 > 0.001);
2782
2783	double v = atan2(sqrt(1 - _e * _e) * sin(E), cos(E) - _e);
2784	double u0 = v + _omega;
2785	double sin2u0 = sin(2 * u0);
2786	double cos2u0 = cos(2 * u0);
2787	double r = a0 * (1 - _e * cos(E)) + _Crc * cos2u0 + _Crs * sin2u0;
2788	double i = _i0 + _IDOT * tk + _Cic * cos2u0 + _Cis * sin2u0;
2789	double u = u0 + _Cuc * cos2u0 + _Cus * sin2u0;
2790	double xp = r * cos(u);
2791	double yp = r * sin(u);
2792	double toesec = (_TOE.gpssec() - 14.0);
2793	double sinom = 0;
2794	double cosom = 0;
2795	double sini = 0;
2796	double cosi = 0;
2797
2798	// Velocity
2799	// --------
2800	double tanv2 = tan(v / 2);
2801	double dEdM = 1 / (1 - _e * cos(E));
2802	double dotv = sqrt((1.0 + _e) / (1.0 - _e)) / cos(E / 2) / cos(E / 2)
2803	/ (1 + tanv2 * tanv2) * dEdM * n;
2804	double dotu = dotv + (-_Cuc * sin2u0 + _Cus * cos2u0) * 2 * dotv;
2805	double doti = _IDOT + (-_Cic * sin2u0 + _Cis * cos2u0) * 2 * dotv;
2806	double dotr = a0 * _e * sin(E) * dEdM * n
2807	+ (-_Crc * sin2u0 + _Crs * cos2u0) * 2 * dotv;
2808
2809	double dotx = dotr * cos(u) - r * sin(u) * dotu;
2810	double doty = dotr * sin(u) + r * cos(u) * dotu;
2811
2812	const double iMaxGEO = 10.0 / 180.0 * M_PI;
2813
2814	// MEO/IGSO satellite
2815	// ------------------
2816	if (_i0 > iMaxGEO) {
2817	double OM = _OMEGA0 + (_OMEGADOT - omegaBDS) * tk - omegaBDS * toesec;
2818
2819	sinom = sin(OM);
2820	cosom = cos(OM);
2821	sini = sin(i);
2822	cosi = cos(i);
2823
2824	xc[0] = xp * cosom - yp * cosi * sinom;
2825	xc[1] = xp * sinom + yp * cosi * cosom;
2826	xc[2] = yp * sini;
2827
2828	// Velocity
2829	// --------
2830
2831	double dotom = _OMEGADOT - t_CST::omega;
2832
2833	vv[0] = cosom * dotx - cosi * sinom * doty // dX / dr
2834	- xp * sinom * dotom - yp * cosi * cosom * dotom // dX / dOMEGA
2835	+ yp * sini * sinom * doti; // dX / di
2836
2837	vv[1] = sinom * dotx + cosi * cosom * doty + xp * cosom * dotom
2838	- yp * cosi * sinom * dotom - yp * sini * cosom * doti;
2839
2840	vv[2] = sini * doty + yp * cosi * doti;
2841
2842	}
2843
2844	// GEO satellite
2845	// -------------
2846	else {
2847	double OM = _OMEGA0 + _OMEGADOT * tk - omegaBDS * toesec;
2848	double ll = omegaBDS * tk;
2849
2850	sinom = sin(OM);
2851	cosom = cos(OM);
2852	sini = sin(i);
2853	cosi = cos(i);
2854
2855	double xx = xp * cosom - yp * cosi * sinom;
2856	double yy = xp * sinom + yp * cosi * cosom;
2857	double zz = yp * sini;
2858
2859	Matrix RX = BNC_PPP::t_astro::rotX(-5.0 / 180.0 * M_PI);
2860	Matrix RZ = BNC_PPP::t_astro::rotZ(ll);
2861
2862	ColumnVector X1(3);
2863	X1 << xx << yy << zz;
2864	ColumnVector X2 = RZ * RX * X1;
2865
2866	xc[0] = X2(1);
2867	xc[1] = X2(2);
2868	xc[2] = X2(3);
2869
2870	double dotom = _OMEGADOT;
2871
2872	double vx = cosom * dotx - cosi * sinom * doty - xp * sinom * dotom
2873	- yp * cosi * cosom * dotom + yp * sini * sinom * doti;
2874
2875	double vy = sinom * dotx + cosi * cosom * doty + xp * cosom * dotom
2876	- yp * cosi * sinom * dotom - yp * sini * cosom * doti;
2877
2878	double vz = sini * doty + yp * cosi * doti;
2879
2880	ColumnVector V(3);
2881	V << vx << vy << vz;
2882
2883	Matrix RdotZ(3, 3);
2884	double C = cos(ll);
2885	double S = sin(ll);
2886	Matrix UU(3, 3);
2887	UU[0][0] = -S;
2888	UU[0][1] = +C;
2889	UU[0][2] = 0.0;
2890	UU[1][0] = -C;
2891	UU[1][1] = -S;
2892	UU[1][2] = 0.0;
2893	UU[2][0] = 0.0;
2894	UU[2][1] = 0.0;
2895	UU[2][2] = 0.0;
2896	RdotZ = omegaBDS * UU;
2897
2898	ColumnVector VV(3);
2899	VV = RZ * RX * V + RdotZ * RX * X1;
2900
2901	vv[0] = VV(1);
2902	vv[1] = VV(2);
2903	vv[2] = VV(3);
2904	}
2905
2906	double tc = tt - _TOC;
2907	xc[3] = _clock_bias + _clock_drift * tc + _clock_driftrate * tc * tc;
2908
2909	// dotC = _clock_drift + _clock_driftrate*tc
2910	// - 4.442807309e-10_e sqrt(a0) * cos(E) * dEdM * n;
2911
2912	// Relativistic Correction
2913	// -----------------------
2914	xc[3] -= 4.442807309e-10 * _e * sqrt(a0) * sin(E);
2915
2916	xc[4] = _clock_drift + _clock_driftrate * tc;
2917	xc[5] = _clock_driftrate;
2918
2919	return success;
2920	}
2921
2922	// Health status of SBAS Ephemeris (virtual)
2923	////////////////////////////////////////////////////////////////////////////
2924	unsigned int t_ephBDS::isUnhealthy() const {
2925
2926	if (type() == t_eph::CNV1 \|\|
2927	type() == t_eph::CNV2 \|\|
2928	type() == t_eph::CNV3) {
2929	return static_cast<unsigned int>(_health);
2930	}
2931
2932	return static_cast<unsigned int>(_SatH1);
2933
2934	}
2935
2936	// RINEX Format String
2937	//////////////////////////////////////////////////////////////////////////////
2938	QString t_ephBDS::toString(double version) const {
2939
2940	if (version < 4.0 &&
2941	(type() == t_eph::CNV1 \|\|
2942	type() == t_eph::CNV2 \|\|
2943	type() == t_eph::CNV3 )) {
2944	return "";
2945	}
2946
2947	QString ephStr = typeStr(_type, _prn, version);
2948	QString rnxStr = ephStr + rinexDateStr(_TOC - 14.0, _prn, version);
2949
2950	QTextStream out(&rnxStr);
2951
2952	out
2953	<< QString("%1%2%3\n")
2954	.arg(_clock_bias, 19, 'e', 12)
2955	.arg(_clock_drift, 19, 'e', 12)
2956	.arg(_clock_driftrate, 19, 'e', 12);
2957
2958	QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
2959	// =====================
2960	// BROADCAST ORBIT - 1
2961	// =====================
2962	if (type() == t_eph::D1 \|\|
2963	type() == t_eph::D2 \|\|
2964	type() == t_eph::undefined) {
2965	out
2966	<< QString(fmt)
2967	.arg(double(_AODE), 19, 'e', 12)
2968	.arg(_Crs, 19, 'e', 12)
2969	.arg(_Delta_n, 19, 'e', 12)
2970	.arg(_M0, 19, 'e', 12);
2971	}
2972	else { //CNV1, CNV2, CNV3
2973	out
2974	<< QString(fmt)
2975	.arg(_ADOT, 19, 'e', 12)
2976	.arg(_Crs, 19, 'e', 12)
2977	.arg(_Delta_n, 19, 'e', 12)
2978	.arg(_M0, 19, 'e', 12);
2979	}
2980
2981	// =====================
2982	// BROADCAST ORBIT - 2
2983	// =====================
2984	out
2985	<< QString(fmt)
2986	.arg(_Cuc, 19, 'e', 12)
2987	.arg(_e, 19, 'e', 12)
2988	.arg(_Cus, 19, 'e', 12)
2989	.arg(_sqrt_A, 19, 'e', 12);
2990
2991	// =====================
2992	// BROADCAST ORBIT - 3
2993	// =====================
2994	out
2995	<< QString(fmt)
2996	.arg(_TOEsec, 19, 'e', 12)
2997	.arg(_Cic, 19, 'e', 12)
2998	.arg(_OMEGA0, 19, 'e', 12)
2999	.arg(_Cis, 19, 'e', 12);
3000	// =====================
3001	// BROADCAST ORBIT - 4
3002	// =====================
3003	out
3004	<< QString(fmt)
3005	.arg(_i0, 19, 'e', 12)
3006	.arg(_Crc, 19, 'e', 12)
3007	.arg(_omega, 19, 'e', 12)
3008	.arg(_OMEGADOT, 19, 'e', 12);
3009	// =====================
3010	// BROADCAST ORBIT - 5
3011	// =====================
3012	if (type() == t_eph::CNV1 \|\|
3013	type() == t_eph::CNV2 \|\|
3014	type() == t_eph::CNV3) {
3015	out
3016	<< QString(fmt)
3017	.arg(_IDOT, 19, 'e', 12)
3018	.arg(_Delta_n_dot, 19, 'e', 12)
3019	.arg(_satType, 19, 'e', 12)
3020	.arg(_top, 19, 'e', 12);
3021	}
3022	else { // D1, D2,
3023	out
3024	<< QString(fmt)
3025	.arg(_IDOT, 19, 'e', 12)
3026	.arg("", 19, QChar(' '))
3027	.arg(_BDTweek, 19, 'e', 12)
3028	.arg("", 19, QChar(' '));
3029	}
3030	// =====================
3031	// BROADCAST ORBIT - 6
3032	// =====================
3033	if (type() == t_eph::CNV1 \|\|
3034	type() == t_eph::CNV2 \|\|
3035	type() == t_eph::CNV3) {
3036	out
3037	<< QString(fmt)
3038	.arg(_SISAI_oe, 19, 'e', 12)
3039	.arg(_SISAI_ocb, 19, 'e', 12)
3040	.arg(_SISAI_oc1, 19, 'e', 12)
3041	.arg(_SISAI_oc2, 19, 'e', 12);
3042	}
3043	else { // D1, D2, undefined
3044	out
3045	<< QString(fmt)
3046	.arg(_ura, 19, 'e', 12)
3047	.arg(double(_SatH1), 19, 'e', 12)
3048	.arg(_TGD1, 19, 'e', 12)
3049	.arg(_TGD2, 19, 'e', 12);
3050	}
3051	// =====================
3052	// BROADCAST ORBIT - 7
3053	// =====================
3054	if (type() == t_eph::CNV1) {
3055	out
3056	<< QString(fmt)
3057	.arg(_ISC_B1Cd, 19, 'e', 12)
3058	.arg("", 19, QChar(' '))
3059	.arg(_TGD_B1Cp, 19, 'e', 12)
3060	.arg(_TGD_B2ap, 19, 'e', 12);
3061	}
3062	else if (type() == t_eph::CNV2) {
3063	out
3064	<< QString(fmt)
3065	.arg("", 19, QChar(' '))
3066	.arg(_ISC_B2ad, 19, 'e', 12)
3067	.arg(_TGD_B1Cp, 19, 'e', 12)
3068	.arg(_TGD_B2ap, 19, 'e', 12);
3069	}
3070	else if (type() == t_eph::CNV3) {
3071	out
3072	<< QString(fmt)
3073	.arg(_SISMAI, 19, 'e', 12)
3074	.arg(double(_health), 19, 'e', 12)
3075	.arg(_INTEGRITYF_B2b, 19, 'e', 12)
3076	.arg(_TGD_B2bI, 19, 'e', 12);
3077	}
3078	else { // D1, D2, undefined
3079	double tots = 0.0;
3080	if (_receptDateTime.isValid()) { // RTCM stream input
3081	tots = _TOE.bdssec();
3082	} else { // RINEX input
3083	tots = _TOT;
3084	}
3085	out
3086	<< QString(fmt)
3087	.arg(tots, 19, 'e', 12)
3088	.arg(double(_AODC), 19, 'e', 12)
3089	.arg("", 19, QChar(' '))
3090	.arg("", 19, QChar(' '));
3091	}
3092
3093	// =====================
3094	// BROADCAST ORBIT - 8
3095	// =====================
3096	if (type() == t_eph::CNV1) {
3097	out
3098	<< QString(fmt)
3099	.arg(_SISMAI, 19, 'e', 12)
3100	.arg(double(_health), 19, 'e', 12)
3101	.arg(_INTEGRITYF_B1C, 19, 'e', 12)
3102	.arg(_IODC, 19, 'e', 12);
3103	}
3104	else if (type() == t_eph::CNV2) {
3105	out
3106	<< QString(fmt)
3107	.arg(_SISMAI, 19, 'e', 12)
3108	.arg(double(_health), 19, 'e', 12)
3109	.arg(_INTEGRITYF_B2aB1C, 19, 'e', 12)
3110	.arg(_IODC, 19, 'e', 12);
3111	}
3112	else if (type() == t_eph::CNV3) {
3113	out
3114	<< QString(fmt)
3115	.arg(_TOT, 19, 'e', 12)
3116	.arg("", 19, QChar(' '))
3117	.arg("", 19, QChar(' '))
3118	.arg("", 19, QChar(' '));
3119	}
3120
3121	// =====================
3122	// BROADCAST ORBIT - 9
3123	// =====================
3124	if (type() == t_eph::CNV1 \|\|
3125	type() == t_eph::CNV2) {
3126	out
3127	<< QString(fmt)
3128	.arg(_TOT, 19, 'e', 12)
3129	.arg("", 19, QChar(' '))
3130	.arg("", 19, QChar(' '))
3131	.arg(_IODE, 19, 'e', 12);
3132	}
3133
3134	return rnxStr;
3135	}

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

Context Navigation

source: ntrip/trunk/BNC/src/ephemeris.cpp@ 10628

Download in other formats: