Context Navigation

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

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

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

Download in other formats: