Context Navigation

ephemeris.cpp@ 10587

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

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

Context Navigation

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

Download in other formats: