Context Navigation

ephemeris.cpp@ 10605

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

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

Context Navigation

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

Download in other formats: