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

Last change on this file since 5807 was 5789, checked in by mervart, 10 years ago
File size: 39.0 KB
Line 
1#include <math.h>
2#include <sstream>
3#include <iostream>
4#include <iomanip>
5#include <cstring>
6
7#include <newmatio.h>
8
9#include "ephemeris.h"
10#include "bncutils.h"
11#include "timeutils.h"
12#include "bnctime.h"
13#include "bnccore.h"
14#include "PPP/ppp.h"
15
16using namespace BNC;
17using namespace std;
18
19// Returns CRC24
20////////////////////////////////////////////////////////////////////////////
21static unsigned long CRC24(long size, const unsigned char *buf) {
22 unsigned long crc = 0;
23 int ii;
24 while (size--) {
25 crc ^= (*buf++) << (16);
26 for(ii = 0; ii < 8; ii++) {
27 crc <<= 1;
28 if (crc & 0x1000000) {
29 crc ^= 0x01864cfb;
30 }
31 }
32 }
33 return crc;
34}
35
36// Constructor
37////////////////////////////////////////////////////////////////////////////
38t_eph::t_eph() {
39 _ok = false;
40 _orbCorr = 0;
41 _clkCorr = 0;
42}
43
44//
45////////////////////////////////////////////////////////////////////////////
46void t_eph::setOrbCorr(const BNC::t_orbCorr* orbCorr) {
47 delete _orbCorr;
48 _orbCorr = new t_orbCorr(*orbCorr);
49}
50
51//
52////////////////////////////////////////////////////////////////////////////
53void t_eph::setClkCorr(const BNC::t_clkCorr* clkCorr) {
54 delete _clkCorr;
55 _clkCorr = new t_clkCorr(*clkCorr);
56}
57
58//
59////////////////////////////////////////////////////////////////////////////
60t_irc t_eph::getCrd(const bncTime& tt, ColumnVector& xc,
61 ColumnVector& vv, bool useCorr) const {
62 xc.ReSize(4);
63 vv.ReSize(3);
64 position(tt.gpsw(), tt.gpssec(), xc.data(), vv.data());
65 if (useCorr) {
66 throw "t_eph::getCrd: not yet implemented";
67 }
68 return success;
69}
70
71// Set GPS Satellite Position
72////////////////////////////////////////////////////////////////////////////
73void t_ephGPS::set(const gpsephemeris* ee) {
74
75 _receptDateTime = currentDateAndTimeGPS();
76
77 _prn.set('G', ee->satellite);
78
79 _TOC.set(ee->GPSweek, ee->TOC);
80 _clock_bias = ee->clock_bias;
81 _clock_drift = ee->clock_drift;
82 _clock_driftrate = ee->clock_driftrate;
83
84 _IODE = ee->IODE;
85 _Crs = ee->Crs;
86 _Delta_n = ee->Delta_n;
87 _M0 = ee->M0;
88
89 _Cuc = ee->Cuc;
90 _e = ee->e;
91 _Cus = ee->Cus;
92 _sqrt_A = ee->sqrt_A;
93
94 _TOEsec = ee->TOE;
95 _Cic = ee->Cic;
96 _OMEGA0 = ee->OMEGA0;
97 _Cis = ee->Cis;
98
99 _i0 = ee->i0;
100 _Crc = ee->Crc;
101 _omega = ee->omega;
102 _OMEGADOT = ee->OMEGADOT;
103
104 _IDOT = ee->IDOT;
105 _L2Codes = 0.0;
106 _TOEweek = ee->GPSweek;
107 _L2PFlag = 0.0;
108
109 if (ee->URAindex <= 6) {
110 _ura = ceil(10.0*pow(2.0, 1.0+((double)ee->URAindex)/2.0))/10.0;
111 }
112 else {
113 _ura = ceil(10.0*pow(2.0, ((double)ee->URAindex)/2.0))/10.0;
114 }
115 _health = ee->SVhealth;
116 _TGD = ee->TGD;
117 _IODC = ee->IODC;
118
119 _TOT = 0.9999e9;
120 _fitInterval = 0.0;
121
122 _ok = true;
123}
124
125// Compute GPS Satellite Position (virtual)
126////////////////////////////////////////////////////////////////////////////
127void t_ephGPS::position(int GPSweek, double GPSweeks,
128 double* xc,
129 double* vv) const {
130
131
132 static const double omegaEarth = 7292115.1467e-11;
133 static const double gmGRS = 398.6005e12;
134
135 memset(xc, 0, 4*sizeof(double));
136 memset(vv, 0, 3*sizeof(double));
137
138 double a0 = _sqrt_A * _sqrt_A;
139 if (a0 == 0) {
140 return;
141 }
142
143 double n0 = sqrt(gmGRS/(a0*a0*a0));
144
145 bncTime tt(GPSweek, GPSweeks);
146 double tk = tt - bncTime(int(_TOEweek), _TOEsec);
147
148 double n = n0 + _Delta_n;
149 double M = _M0 + n*tk;
150 double E = M;
151 double E_last;
152 do {
153 E_last = E;
154 E = M + _e*sin(E);
155 } while ( fabs(E-E_last)*a0 > 0.001 );
156 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
157 double u0 = v + _omega;
158 double sin2u0 = sin(2*u0);
159 double cos2u0 = cos(2*u0);
160 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
161 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
162 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
163 double xp = r*cos(u);
164 double yp = r*sin(u);
165 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
166 omegaEarth*_TOEsec;
167
168 double sinom = sin(OM);
169 double cosom = cos(OM);
170 double sini = sin(i);
171 double cosi = cos(i);
172 xc[0] = xp*cosom - yp*cosi*sinom;
173 xc[1] = xp*sinom + yp*cosi*cosom;
174 xc[2] = yp*sini;
175
176 double tc = tt - _TOC;
177 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
178
179 // Velocity
180 // --------
181 double tanv2 = tan(v/2);
182 double dEdM = 1 / (1 - _e*cos(E));
183 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
184 * dEdM * n;
185 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
186 double dotom = _OMEGADOT - omegaEarth;
187 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
188 double dotr = a0 * _e*sin(E) * dEdM * n
189 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
190 double dotx = dotr*cos(u) - r*sin(u)*dotu;
191 double doty = dotr*sin(u) + r*cos(u)*dotu;
192
193 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
194 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
195 + yp*sini*sinom*doti; // dX / di
196
197 vv[1] = sinom *dotx + cosi*cosom *doty
198 + xp*cosom*dotom - yp*cosi*sinom*dotom
199 - yp*sini*cosom*doti;
200
201 vv[2] = sini *doty + yp*cosi *doti;
202
203 // Relativistic Correction
204 // -----------------------
205 xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
206}
207
208// build up RTCM3 for GPS
209////////////////////////////////////////////////////////////////////////////
210#define GPSTOINT(type, value) static_cast<type>(round(value))
211
212#define GPSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
213 |(GPSTOINT(long long,b)&((1ULL<<a)-1)); \
214 numbits += (a); \
215 while(numbits >= 8) { \
216 buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
217
218#define GPSADDBITSFLOAT(a,b,c) {long long i = GPSTOINT(long long,(b)/(c)); \
219 GPSADDBITS(a,i)};
220
221int t_ephGPS::RTCM3(unsigned char *buffer) {
222
223 unsigned char *startbuffer = buffer;
224 buffer= buffer+3;
225 int size = 0;
226 int numbits = 0;
227 unsigned long long bitbuffer = 0;
228 if (_ura <= 2.40){
229 _ura = 0;
230 }
231 else if (_ura <= 3.40){
232 _ura = 1;
233 }
234 else if (_ura <= 6.85){
235 _ura = 2;
236 }
237 else if (_ura <= 9.65){
238 _ura = 3;
239 }
240 else if (_ura <= 13.65){
241 _ura = 4;
242 }
243 else if (_ura <= 24.00){
244 _ura = 5;
245 }
246 else if (_ura <= 48.00){
247 _ura = 6;
248 }
249 else if (_ura <= 96.00){
250 _ura = 7;
251 }
252 else if (_ura <= 192.00){
253 _ura = 8;
254 }
255 else if (_ura <= 384.00){
256 _ura = 9;
257 }
258 else if (_ura <= 768.00){
259 _ura = 10;
260 }
261 else if (_ura <= 1536.00){
262 _ura = 11;
263 }
264 else if (_ura <= 1536.00){
265 _ura = 12;
266 }
267 else if (_ura <= 2072.00){
268 _ura = 13;
269 }
270 else if (_ura <= 6144.00){
271 _ura = 14;
272 }
273 else{
274 _ura = 15;
275 }
276
277 GPSADDBITS(12, 1019)
278 GPSADDBITS(6,_prn.number())
279 GPSADDBITS(10, _TOC.gpsw())
280 GPSADDBITS(4, _ura)
281 GPSADDBITS(2,_L2Codes)
282 GPSADDBITSFLOAT(14, _IDOT, M_PI/static_cast<double>(1<<30)
283 /static_cast<double>(1<<13))
284 GPSADDBITS(8, _IODE)
285 GPSADDBITS(16, static_cast<int>(_TOC.gpssec())>>4)
286 GPSADDBITSFLOAT(8, _clock_driftrate, 1.0/static_cast<double>(1<<30)
287 /static_cast<double>(1<<25))
288 GPSADDBITSFLOAT(16, _clock_drift, 1.0/static_cast<double>(1<<30)
289 /static_cast<double>(1<<13))
290 GPSADDBITSFLOAT(22, _clock_bias, 1.0/static_cast<double>(1<<30)
291 /static_cast<double>(1<<1))
292 GPSADDBITS(10, _IODC)
293 GPSADDBITSFLOAT(16, _Crs, 1.0/static_cast<double>(1<<5))
294 GPSADDBITSFLOAT(16, _Delta_n, M_PI/static_cast<double>(1<<30)
295 /static_cast<double>(1<<13))
296 GPSADDBITSFLOAT(32, _M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
297 GPSADDBITSFLOAT(16, _Cuc, 1.0/static_cast<double>(1<<29))
298 GPSADDBITSFLOAT(32, _e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
299 GPSADDBITSFLOAT(16, _Cus, 1.0/static_cast<double>(1<<29))
300 GPSADDBITSFLOAT(32, _sqrt_A, 1.0/static_cast<double>(1<<19))
301 GPSADDBITS(16, static_cast<int>(_TOEsec)>>4)
302 GPSADDBITSFLOAT(16, _Cic, 1.0/static_cast<double>(1<<29))
303 GPSADDBITSFLOAT(32, _OMEGA0, M_PI/static_cast<double>(1<<30)
304 /static_cast<double>(1<<1))
305 GPSADDBITSFLOAT(16, _Cis, 1.0/static_cast<double>(1<<29))
306 GPSADDBITSFLOAT(32, _i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
307 GPSADDBITSFLOAT(16, _Crc, 1.0/static_cast<double>(1<<5))
308 GPSADDBITSFLOAT(32, _omega, M_PI/static_cast<double>(1<<30)
309 /static_cast<double>(1<<1))
310 GPSADDBITSFLOAT(24, _OMEGADOT, M_PI/static_cast<double>(1<<30)
311 /static_cast<double>(1<<13))
312 GPSADDBITSFLOAT(8, _TGD, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
313 GPSADDBITS(6, _health)
314 GPSADDBITS(1, _L2PFlag)
315 GPSADDBITS(1, 0) /* GPS fit interval */
316
317 startbuffer[0]=0xD3;
318 startbuffer[1]=(size >> 8);
319 startbuffer[2]=size;
320 unsigned long i = CRC24(size+3, startbuffer);
321 buffer[size++] = i >> 16;
322 buffer[size++] = i >> 8;
323 buffer[size++] = i;
324 size += 3;
325 return size;
326}
327
328// Derivative of the state vector using a simple force model (static)
329////////////////////////////////////////////////////////////////////////////
330ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv,
331 double* acc) {
332
333 // State vector components
334 // -----------------------
335 ColumnVector rr = xv.rows(1,3);
336 ColumnVector vv = xv.rows(4,6);
337
338 // Acceleration
339 // ------------
340 static const double gmWGS = 398.60044e12;
341 static const double AE = 6378136.0;
342 static const double OMEGA = 7292115.e-11;
343 static const double C20 = -1082.6257e-6;
344
345 double rho = rr.norm_Frobenius();
346 double t1 = -gmWGS/(rho*rho*rho);
347 double t2 = 3.0/2.0 * C20 * (gmWGS*AE*AE) / (rho*rho*rho*rho*rho);
348 double t3 = OMEGA * OMEGA;
349 double t4 = 2.0 * OMEGA;
350 double z2 = rr(3) * rr(3);
351
352 // Vector of derivatives
353 // ---------------------
354 ColumnVector va(6);
355 va(1) = vv(1);
356 va(2) = vv(2);
357 va(3) = vv(3);
358 va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0];
359 va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1) + acc[1];
360 va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3) + acc[2];
361
362 return va;
363}
364
365// Compute Glonass Satellite Position (virtual)
366////////////////////////////////////////////////////////////////////////////
367void t_ephGlo::position(int GPSweek, double GPSweeks,
368 double* xc, double* vv) const {
369
370 static const double nominalStep = 10.0;
371
372 memset(xc, 0, 4*sizeof(double));
373 memset(vv, 0, 3*sizeof(double));
374
375 double dtPos = bncTime(GPSweek, GPSweeks) - _tt;
376
377 int nSteps = int(fabs(dtPos) / nominalStep) + 1;
378 double step = dtPos / nSteps;
379
380 double acc[3];
381 acc[0] = _x_acceleration * 1.e3;
382 acc[1] = _y_acceleration * 1.e3;
383 acc[2] = _z_acceleration * 1.e3;
384 for (int ii = 1; ii <= nSteps; ii++) {
385 _xv = rungeKutta4(_tt.gpssec(), _xv, step, acc, glo_deriv);
386 _tt = _tt + step;
387 }
388
389 // Position and Velocity
390 // ---------------------
391 xc[0] = _xv(1);
392 xc[1] = _xv(2);
393 xc[2] = _xv(3);
394
395 vv[0] = _xv(4);
396 vv[1] = _xv(5);
397 vv[2] = _xv(6);
398
399 // Clock Correction
400 // ----------------
401 double dtClk = bncTime(GPSweek, GPSweeks) - _TOC;
402 xc[3] = -_tau + _gamma * dtClk;
403}
404
405// IOD of Glonass Ephemeris (virtual)
406////////////////////////////////////////////////////////////////////////////
407int t_ephGlo::IOD() const {
408 bncTime tMoscow = _TOC - _gps_utc + 3 * 3600.0;
409 return int(tMoscow.daysec() / 900);
410}
411
412// Set Glonass Ephemeris
413////////////////////////////////////////////////////////////////////////////
414void t_ephGlo::set(const glonassephemeris* ee) {
415
416 _receptDateTime = currentDateAndTimeGPS();
417
418 _prn.set('R', ee->almanac_number);
419
420 int ww = ee->GPSWeek;
421 int tow = ee->GPSTOW;
422 updatetime(&ww, &tow, ee->tb*1000, 0); // Moscow -> GPS
423
424 // Check the day once more
425 // -----------------------
426 bool timeChanged = false;
427 {
428 const double secPerDay = 24 * 3600.0;
429 const double secPerWeek = 7 * secPerDay;
430 int ww_old = ww;
431 int tow_old = tow;
432 int currentWeek;
433 double currentSec;
434 currentGPSWeeks(currentWeek, currentSec);
435 bncTime currentTime(currentWeek, currentSec);
436 bncTime hTime(ww, (double) tow);
437
438 if (hTime - currentTime > secPerDay/2.0) {
439 timeChanged = true;
440 tow -= int(secPerDay);
441 if (tow < 0) {
442 tow += int(secPerWeek);
443 ww -= 1;
444 }
445 }
446 else if (hTime - currentTime < -secPerDay/2.0) {
447 timeChanged = true;
448 tow += int(secPerDay);
449 if (tow > secPerWeek) {
450 tow -= int(secPerWeek);
451 ww += 1;
452 }
453 }
454
455 if (false && timeChanged && BNC_CORE->mode() == t_bncCore::batchPostProcessing) {
456 bncTime newHTime(ww, (double) tow);
457 cout << "GLONASS " << ee->almanac_number << " Time Changed at "
458 << currentTime.datestr() << " " << currentTime.timestr()
459 << endl
460 << "old: " << hTime.datestr() << " " << hTime.timestr()
461 << endl
462 << "new: " << newHTime.datestr() << " " << newHTime.timestr()
463 << endl
464 << "eph: " << ee->GPSWeek << " " << ee->GPSTOW << " " << ee->tb
465 << endl
466 << "ww, tow (old): " << ww_old << " " << tow_old
467 << endl
468 << "ww, tow (new): " << ww << " " << tow
469 << endl << endl;
470 }
471 }
472
473 bncTime hlpTime(ww, (double) tow);
474 unsigned year, month, day;
475 hlpTime.civil_date(year, month, day);
476 _gps_utc = gnumleap(year, month, day);
477
478 _TOC.set(ww, tow);
479 _E = ee->E;
480 _tau = ee->tau;
481 _gamma = ee->gamma;
482 _x_pos = ee->x_pos;
483 _x_velocity = ee->x_velocity;
484 _x_acceleration = ee->x_acceleration;
485 _y_pos = ee->y_pos;
486 _y_velocity = ee->y_velocity;
487 _y_acceleration = ee->y_acceleration;
488 _z_pos = ee->z_pos;
489 _z_velocity = ee->z_velocity;
490 _z_acceleration = ee->z_acceleration;
491 _health = 0;
492 _frequency_number = ee->frequency_number;
493 _tki = ee->tk-3*60*60; if (_tki < 0) _tki += 86400;
494
495 // Initialize status vector
496 // ------------------------
497 _tt = _TOC;
498
499 _xv(1) = _x_pos * 1.e3;
500 _xv(2) = _y_pos * 1.e3;
501 _xv(3) = _z_pos * 1.e3;
502 _xv(4) = _x_velocity * 1.e3;
503 _xv(5) = _y_velocity * 1.e3;
504 _xv(6) = _z_velocity * 1.e3;
505
506 _ok = true;
507}
508
509// build up RTCM3 for GLONASS
510////////////////////////////////////////////////////////////////////////////
511#define GLONASSTOINT(type, value) static_cast<type>(round(value))
512
513#define GLONASSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
514 |(GLONASSTOINT(long long,b)&((1ULL<<(a))-1)); \
515 numbits += (a); \
516 while(numbits >= 8) { \
517 buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
518#define GLONASSADDBITSFLOATM(a,b,c) {int s; long long i; \
519 if(b < 0.0) \
520 { \
521 s = 1; \
522 i = GLONASSTOINT(long long,(-b)/(c)); \
523 if(!i) s = 0; \
524 } \
525 else \
526 { \
527 s = 0; \
528 i = GLONASSTOINT(long long,(b)/(c)); \
529 } \
530 GLONASSADDBITS(1,s) \
531 GLONASSADDBITS(a-1,i)}
532
533int t_ephGlo::RTCM3(unsigned char *buffer)
534{
535
536 int size = 0;
537 int numbits = 0;
538 long long bitbuffer = 0;
539 unsigned char *startbuffer = buffer;
540 buffer= buffer+3;
541
542 GLONASSADDBITS(12, 1020)
543 GLONASSADDBITS(6, _prn.number())
544 GLONASSADDBITS(5, 7+_frequency_number)
545 GLONASSADDBITS(1, 0)
546 GLONASSADDBITS(1, 0)
547 GLONASSADDBITS(2, 0)
548 _tki=_tki+3*60*60;
549 GLONASSADDBITS(5, static_cast<int>(_tki)/(60*60))
550 GLONASSADDBITS(6, (static_cast<int>(_tki)/60)%60)
551 GLONASSADDBITS(1, (static_cast<int>(_tki)/30)%30)
552 GLONASSADDBITS(1, _health)
553 GLONASSADDBITS(1, 0)
554 unsigned long long timeofday = (static_cast<int>(_tt.gpssec()+3*60*60-_gps_utc)%86400);
555 GLONASSADDBITS(7, timeofday/60/15)
556 GLONASSADDBITSFLOATM(24, _x_velocity*1000, 1000.0/static_cast<double>(1<<20))
557 GLONASSADDBITSFLOATM(27, _x_pos*1000, 1000.0/static_cast<double>(1<<11))
558 GLONASSADDBITSFLOATM(5, _x_acceleration*1000, 1000.0/static_cast<double>(1<<30))
559 GLONASSADDBITSFLOATM(24, _y_velocity*1000, 1000.0/static_cast<double>(1<<20))
560 GLONASSADDBITSFLOATM(27, _y_pos*1000, 1000.0/static_cast<double>(1<<11))
561 GLONASSADDBITSFLOATM(5, _y_acceleration*1000, 1000.0/static_cast<double>(1<<30))
562 GLONASSADDBITSFLOATM(24, _z_velocity*1000, 1000.0/static_cast<double>(1<<20))
563 GLONASSADDBITSFLOATM(27,_z_pos*1000, 1000.0/static_cast<double>(1<<11))
564 GLONASSADDBITSFLOATM(5, _z_acceleration*1000, 1000.0/static_cast<double>(1<<30))
565 GLONASSADDBITS(1, 0)
566 GLONASSADDBITSFLOATM(11, _gamma, 1.0/static_cast<double>(1<<30)
567 /static_cast<double>(1<<10))
568 GLONASSADDBITS(2, 0) /* GLONASS-M P */
569 GLONASSADDBITS(1, 0) /* GLONASS-M ln(3) */
570 GLONASSADDBITSFLOATM(22, _tau, 1.0/static_cast<double>(1<<30))
571 GLONASSADDBITS(5, 0) /* GLONASS-M delta tau */
572 GLONASSADDBITS(5, _E)
573 GLONASSADDBITS(1, 0) /* GLONASS-M P4 */
574 GLONASSADDBITS(4, 0) /* GLONASS-M FT */
575 GLONASSADDBITS(11, 0) /* GLONASS-M NT */
576 GLONASSADDBITS(2, 0) /* GLONASS-M active? */
577 GLONASSADDBITS(1, 0) /* GLONASS additional data */
578 GLONASSADDBITS(11, 0) /* GLONASS NA */
579 GLONASSADDBITS(32, 0) /* GLONASS tau C */
580 GLONASSADDBITS(5, 0) /* GLONASS-M N4 */
581 GLONASSADDBITS(22, 0) /* GLONASS-M tau GPS */
582 GLONASSADDBITS(1, 0) /* GLONASS-M ln(5) */
583 GLONASSADDBITS(7, 0) /* Reserved */
584
585 startbuffer[0]=0xD3;
586 startbuffer[1]=(size >> 8);
587 startbuffer[2]=size;
588 unsigned long i = CRC24(size+3, startbuffer);
589 buffer[size++] = i >> 16;
590 buffer[size++] = i >> 8;
591 buffer[size++] = i;
592 size += 3;
593 return size;
594}
595
596// Set Galileo Satellite Position
597////////////////////////////////////////////////////////////////////////////
598void t_ephGal::set(const galileoephemeris* ee) {
599
600 _receptDateTime = currentDateAndTimeGPS();
601
602 _prn.set('E', ee->satellite);
603
604 _TOC.set(ee->Week, ee->TOC);
605 _clock_bias = ee->clock_bias;
606 _clock_drift = ee->clock_drift;
607 _clock_driftrate = ee->clock_driftrate;
608
609 _IODnav = ee->IODnav;
610 _Crs = ee->Crs;
611 _Delta_n = ee->Delta_n;
612 _M0 = ee->M0;
613
614 _Cuc = ee->Cuc;
615 _e = ee->e;
616 _Cus = ee->Cus;
617 _sqrt_A = ee->sqrt_A;
618
619 _TOEsec = _TOC.gpssec();
620 //// _TOEsec = ee->TOE; //// TODO:
621 _Cic = ee->Cic;
622 _OMEGA0 = ee->OMEGA0;
623 _Cis = ee->Cis;
624
625 _i0 = ee->i0;
626 _Crc = ee->Crc;
627 _omega = ee->omega;
628 _OMEGADOT = ee->OMEGADOT;
629
630 _IDOT = ee->IDOT;
631 _TOEweek = ee->Week;
632
633 _SISA = ee->SISA;
634 _E5aHS = ee->E5aHS;
635 _E5bHS = ee->E5bHS;
636 _BGD_1_5A = ee->BGD_1_5A;
637 _BGD_1_5B = ee->BGD_1_5B;
638
639 _TOT = 0.9999e9;
640
641 _flags = ee->flags;
642
643 _ok = true;
644}
645
646// Compute Galileo Satellite Position (virtual)
647////////////////////////////////////////////////////////////////////////////
648void t_ephGal::position(int GPSweek, double GPSweeks,
649 double* xc,
650 double* vv) const {
651
652 static const double omegaEarth = 7292115.1467e-11;
653 static const double gmWGS = 398.60044e12;
654
655 memset(xc, 0, 4*sizeof(double));
656 memset(vv, 0, 3*sizeof(double));
657
658 double a0 = _sqrt_A * _sqrt_A;
659 if (a0 == 0) {
660 return;
661 }
662
663 double n0 = sqrt(gmWGS/(a0*a0*a0));
664
665 bncTime tt(GPSweek, GPSweeks);
666 double tk = tt - bncTime(_TOC.gpsw(), _TOEsec);
667
668 double n = n0 + _Delta_n;
669 double M = _M0 + n*tk;
670 double E = M;
671 double E_last;
672 do {
673 E_last = E;
674 E = M + _e*sin(E);
675 } while ( fabs(E-E_last)*a0 > 0.001 );
676 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
677 double u0 = v + _omega;
678 double sin2u0 = sin(2*u0);
679 double cos2u0 = cos(2*u0);
680 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
681 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
682 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
683 double xp = r*cos(u);
684 double yp = r*sin(u);
685 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
686 omegaEarth*_TOEsec;
687
688 double sinom = sin(OM);
689 double cosom = cos(OM);
690 double sini = sin(i);
691 double cosi = cos(i);
692 xc[0] = xp*cosom - yp*cosi*sinom;
693 xc[1] = xp*sinom + yp*cosi*cosom;
694 xc[2] = yp*sini;
695
696 double tc = tt - _TOC;
697 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
698
699 // Velocity
700 // --------
701 double tanv2 = tan(v/2);
702 double dEdM = 1 / (1 - _e*cos(E));
703 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
704 * dEdM * n;
705 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
706 double dotom = _OMEGADOT - omegaEarth;
707 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
708 double dotr = a0 * _e*sin(E) * dEdM * n
709 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
710 double dotx = dotr*cos(u) - r*sin(u)*dotu;
711 double doty = dotr*sin(u) + r*cos(u)*dotu;
712
713 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
714 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
715 + yp*sini*sinom*doti; // dX / di
716
717 vv[1] = sinom *dotx + cosi*cosom *doty
718 + xp*cosom*dotom - yp*cosi*sinom*dotom
719 - yp*sini*cosom*doti;
720
721 vv[2] = sini *doty + yp*cosi *doti;
722
723 // Relativistic Correction
724 // -----------------------
725 // xc(4) -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
726 xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
727}
728
729// build up RTCM3 for Galileo
730////////////////////////////////////////////////////////////////////////////
731#define GALILEOTOINT(type, value) static_cast<type>(round(value))
732
733#define GALILEOADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
734 |(GALILEOTOINT(long long,b)&((1LL<<a)-1)); \
735 numbits += (a); \
736 while(numbits >= 8) { \
737 buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
738#define GALILEOADDBITSFLOAT(a,b,c) {long long i = GALILEOTOINT(long long,(b)/(c)); \
739 GALILEOADDBITS(a,i)};
740
741int t_ephGal::RTCM3(unsigned char *buffer) {
742 int size = 0;
743 int numbits = 0;
744 long long bitbuffer = 0;
745 unsigned char *startbuffer = buffer;
746 buffer= buffer+3;
747
748 bool inav = ( (_flags & GALEPHF_INAV) == GALEPHF_INAV );
749
750 GALILEOADDBITS(12, inav ? 1046 : 1045)
751 GALILEOADDBITS(6, _prn.number())
752 GALILEOADDBITS(12, _TOC.gpsw())
753 GALILEOADDBITS(10, _IODnav)
754 GALILEOADDBITS(8, _SISA)
755 GALILEOADDBITSFLOAT(14, _IDOT, M_PI/static_cast<double>(1<<30)
756 /static_cast<double>(1<<13))
757 GALILEOADDBITS(14, _TOC.gpssec()/60)
758 GALILEOADDBITSFLOAT(6, _clock_driftrate, 1.0/static_cast<double>(1<<30)
759 /static_cast<double>(1<<29))
760 GALILEOADDBITSFLOAT(21, _clock_drift, 1.0/static_cast<double>(1<<30)
761 /static_cast<double>(1<<16))
762 GALILEOADDBITSFLOAT(31, _clock_bias, 1.0/static_cast<double>(1<<30)
763 /static_cast<double>(1<<4))
764 GALILEOADDBITSFLOAT(16, _Crs, 1.0/static_cast<double>(1<<5))
765 GALILEOADDBITSFLOAT(16, _Delta_n, M_PI/static_cast<double>(1<<30)
766 /static_cast<double>(1<<13))
767 GALILEOADDBITSFLOAT(32, _M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
768 GALILEOADDBITSFLOAT(16, _Cuc, 1.0/static_cast<double>(1<<29))
769 GALILEOADDBITSFLOAT(32, _e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
770 GALILEOADDBITSFLOAT(16, _Cus, 1.0/static_cast<double>(1<<29))
771 GALILEOADDBITSFLOAT(32, _sqrt_A, 1.0/static_cast<double>(1<<19))
772 GALILEOADDBITS(14, _TOEsec/60)
773 GALILEOADDBITSFLOAT(16, _Cic, 1.0/static_cast<double>(1<<29))
774 GALILEOADDBITSFLOAT(32, _OMEGA0, M_PI/static_cast<double>(1<<30)
775 /static_cast<double>(1<<1))
776 GALILEOADDBITSFLOAT(16, _Cis, 1.0/static_cast<double>(1<<29))
777 GALILEOADDBITSFLOAT(32, _i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
778 GALILEOADDBITSFLOAT(16, _Crc, 1.0/static_cast<double>(1<<5))
779 GALILEOADDBITSFLOAT(32, _omega, M_PI/static_cast<double>(1<<30)
780 /static_cast<double>(1<<1))
781 GALILEOADDBITSFLOAT(24, _OMEGADOT, M_PI/static_cast<double>(1<<30)
782 /static_cast<double>(1<<13))
783 GALILEOADDBITSFLOAT(10, _BGD_1_5A, 1.0/static_cast<double>(1<<30)
784 /static_cast<double>(1<<2))
785 if(inav)
786 {
787 GALILEOADDBITSFLOAT(10, _BGD_1_5B, 1.0/static_cast<double>(1<<30)
788 /static_cast<double>(1<<2))
789 GALILEOADDBITS(2, static_cast<int>(_E5bHS))
790 GALILEOADDBITS(1, _flags & GALEPHF_E5BDINVALID)
791 }
792 else
793 {
794 GALILEOADDBITS(2, static_cast<int>(_E5aHS))
795 GALILEOADDBITS(1, _flags & GALEPHF_E5ADINVALID)
796 }
797 _TOEsec = 0.9999E9;
798 GALILEOADDBITS(20, _TOEsec)
799
800 GALILEOADDBITS(inav ? 1 : 3, 0)
801
802 startbuffer[0]=0xD3;
803 startbuffer[1]=(size >> 8);
804 startbuffer[2]=size;
805 unsigned long i = CRC24(size+3, startbuffer);
806 buffer[size++] = i >> 16;
807 buffer[size++] = i >> 8;
808 buffer[size++] = i;
809 size += 3;
810 return size;
811}
812
813// Constructor
814//////////////////////////////////////////////////////////////////////////////
815t_ephGPS::t_ephGPS(float rnxVersion, const QStringList& lines) {
816
817 const int nLines = 8;
818
819 _ok = false;
820
821 if (lines.size() != nLines) {
822 return;
823 }
824
825 // RINEX Format
826 // ------------
827 int fieldLen = 19;
828
829 int pos[4];
830 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
831 pos[1] = pos[0] + fieldLen;
832 pos[2] = pos[1] + fieldLen;
833 pos[3] = pos[2] + fieldLen;
834
835 // Read eight lines
836 // ----------------
837 for (int iLine = 0; iLine < nLines; iLine++) {
838 QString line = lines[iLine];
839
840 if ( iLine == 0 ) {
841 QTextStream in(line.left(pos[1]).toAscii());
842
843 int year, month, day, hour, min;
844 double sec;
845
846 QString prnStr;
847 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
848 if (prnStr.at(0) == 'G') {
849 _prn.set('G', prnStr.mid(1).toInt());
850 }
851 else {
852 _prn.set('G', prnStr.toInt());
853 }
854
855 if (year < 80) {
856 year += 2000;
857 }
858 else if (year < 100) {
859 year += 1900;
860 }
861
862 _TOC.set(year, month, day, hour, min, sec);
863
864 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
865 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
866 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
867 return;
868 }
869 }
870
871 else if ( iLine == 1 ) {
872 if ( readDbl(line, pos[0], fieldLen, _IODE ) ||
873 readDbl(line, pos[1], fieldLen, _Crs ) ||
874 readDbl(line, pos[2], fieldLen, _Delta_n) ||
875 readDbl(line, pos[3], fieldLen, _M0 ) ) {
876 return;
877 }
878 }
879
880 else if ( iLine == 2 ) {
881 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
882 readDbl(line, pos[1], fieldLen, _e ) ||
883 readDbl(line, pos[2], fieldLen, _Cus ) ||
884 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
885 return;
886 }
887 }
888
889 else if ( iLine == 3 ) {
890 if ( readDbl(line, pos[0], fieldLen, _TOEsec) ||
891 readDbl(line, pos[1], fieldLen, _Cic ) ||
892 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
893 readDbl(line, pos[3], fieldLen, _Cis ) ) {
894 return;
895 }
896 }
897
898 else if ( iLine == 4 ) {
899 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
900 readDbl(line, pos[1], fieldLen, _Crc ) ||
901 readDbl(line, pos[2], fieldLen, _omega ) ||
902 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
903 return;
904 }
905 }
906
907 else if ( iLine == 5 ) {
908 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
909 readDbl(line, pos[1], fieldLen, _L2Codes) ||
910 readDbl(line, pos[2], fieldLen, _TOEweek ) ||
911 readDbl(line, pos[3], fieldLen, _L2PFlag) ) {
912 return;
913 }
914 }
915
916 else if ( iLine == 6 ) {
917 if ( readDbl(line, pos[0], fieldLen, _ura ) ||
918 readDbl(line, pos[1], fieldLen, _health) ||
919 readDbl(line, pos[2], fieldLen, _TGD ) ||
920 readDbl(line, pos[3], fieldLen, _IODC ) ) {
921 return;
922 }
923 }
924
925 else if ( iLine == 7 ) {
926 if ( readDbl(line, pos[0], fieldLen, _TOT) ) {
927 return;
928 }
929 readDbl(line, pos[1], fieldLen, _fitInterval); // _fitInterval optional
930 }
931 }
932
933 _ok = true;
934}
935
936// Constructor
937//////////////////////////////////////////////////////////////////////////////
938t_ephGlo::t_ephGlo(float rnxVersion, const QStringList& lines) {
939
940 const int nLines = 4;
941
942 _ok = false;
943
944 if (lines.size() != nLines) {
945 return;
946 }
947
948 // RINEX Format
949 // ------------
950 int fieldLen = 19;
951
952 int pos[4];
953 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
954 pos[1] = pos[0] + fieldLen;
955 pos[2] = pos[1] + fieldLen;
956 pos[3] = pos[2] + fieldLen;
957
958 // Read four lines
959 // ---------------
960 for (int iLine = 0; iLine < nLines; iLine++) {
961 QString line = lines[iLine];
962
963 if ( iLine == 0 ) {
964 QTextStream in(line.left(pos[1]).toAscii());
965
966 int year, month, day, hour, min;
967 double sec;
968
969 QString prnStr;
970 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
971 if (prnStr.at(0) == 'R') {
972 _prn.set('R', prnStr.mid(1).toInt());
973 }
974 else {
975 _prn.set('R', prnStr.toInt());
976 }
977
978 if (year < 80) {
979 year += 2000;
980 }
981 else if (year < 100) {
982 year += 1900;
983 }
984
985 _gps_utc = gnumleap(year, month, day);
986
987 _TOC.set(year, month, day, hour, min, sec);
988 _TOC = _TOC + _gps_utc;
989
990 if ( readDbl(line, pos[1], fieldLen, _tau ) ||
991 readDbl(line, pos[2], fieldLen, _gamma) ||
992 readDbl(line, pos[3], fieldLen, _tki ) ) {
993 return;
994 }
995
996 _tau = -_tau;
997 }
998
999 else if ( iLine == 1 ) {
1000 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
1001 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
1002 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
1003 readDbl(line, pos[3], fieldLen, _health ) ) {
1004 return;
1005 }
1006 }
1007
1008 else if ( iLine == 2 ) {
1009 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
1010 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
1011 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
1012 readDbl(line, pos[3], fieldLen, _frequency_number) ) {
1013 return;
1014 }
1015 }
1016
1017 else if ( iLine == 3 ) {
1018 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
1019 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
1020 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
1021 readDbl(line, pos[3], fieldLen, _E ) ) {
1022 return;
1023 }
1024 }
1025 }
1026
1027 // Initialize status vector
1028 // ------------------------
1029 _tt = _TOC;
1030 _xv.ReSize(6);
1031 _xv(1) = _x_pos * 1.e3;
1032 _xv(2) = _y_pos * 1.e3;
1033 _xv(3) = _z_pos * 1.e3;
1034 _xv(4) = _x_velocity * 1.e3;
1035 _xv(5) = _y_velocity * 1.e3;
1036 _xv(6) = _z_velocity * 1.e3;
1037
1038 _ok = true;
1039}
1040
1041// Constructor
1042//////////////////////////////////////////////////////////////////////////////
1043t_ephGal::t_ephGal(float rnxVersion, const QStringList& lines) {
1044
1045 const int nLines = 8;
1046
1047 _ok = false;
1048
1049 if (lines.size() != nLines) {
1050 return;
1051 }
1052
1053 // RINEX Format
1054 // ------------
1055 int fieldLen = 19;
1056
1057 int pos[4];
1058 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
1059 pos[1] = pos[0] + fieldLen;
1060 pos[2] = pos[1] + fieldLen;
1061 pos[3] = pos[2] + fieldLen;
1062
1063 // Read eight lines
1064 // ----------------
1065 for (int iLine = 0; iLine < nLines; iLine++) {
1066 QString line = lines[iLine];
1067
1068 if ( iLine == 0 ) {
1069 QTextStream in(line.left(pos[1]).toAscii());
1070
1071 int year, month, day, hour, min;
1072 double sec;
1073
1074 QString prnStr;
1075 in >> prnStr >> year >> month >> day >> hour >> min >> sec;
1076 if (prnStr.at(0) == 'E') {
1077 _prn.set('E', prnStr.mid(1).toInt());
1078 }
1079 else {
1080 _prn.set('E', prnStr.toInt());
1081 }
1082
1083 if (year < 80) {
1084 year += 2000;
1085 }
1086 else if (year < 100) {
1087 year += 1900;
1088 }
1089
1090 _TOC.set(year, month, day, hour, min, sec);
1091
1092 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
1093 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
1094 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
1095 return;
1096 }
1097 }
1098
1099 else if ( iLine == 1 ) {
1100 if ( readDbl(line, pos[0], fieldLen, _IODnav ) ||
1101 readDbl(line, pos[1], fieldLen, _Crs ) ||
1102 readDbl(line, pos[2], fieldLen, _Delta_n) ||
1103 readDbl(line, pos[3], fieldLen, _M0 ) ) {
1104 return;
1105 }
1106 }
1107
1108 else if ( iLine == 2 ) {
1109 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
1110 readDbl(line, pos[1], fieldLen, _e ) ||
1111 readDbl(line, pos[2], fieldLen, _Cus ) ||
1112 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
1113 return;
1114 }
1115 }
1116
1117 else if ( iLine == 3 ) {
1118 if ( readDbl(line, pos[0], fieldLen, _TOEsec) ||
1119 readDbl(line, pos[1], fieldLen, _Cic ) ||
1120 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
1121 readDbl(line, pos[3], fieldLen, _Cis ) ) {
1122 return;
1123 }
1124 }
1125
1126 else if ( iLine == 4 ) {
1127 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
1128 readDbl(line, pos[1], fieldLen, _Crc ) ||
1129 readDbl(line, pos[2], fieldLen, _omega ) ||
1130 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
1131 return;
1132 }
1133 }
1134
1135 else if ( iLine == 5 ) {
1136 if ( readDbl(line, pos[0], fieldLen, _IDOT ) ||
1137 readDbl(line, pos[2], fieldLen, _TOEweek) ) {
1138 return;
1139 }
1140 }
1141
1142 else if ( iLine == 6 ) {
1143 if ( readDbl(line, pos[0], fieldLen, _SISA ) ||
1144 readDbl(line, pos[1], fieldLen, _E5aHS ) ||
1145 readDbl(line, pos[2], fieldLen, _BGD_1_5A) ||
1146 readDbl(line, pos[3], fieldLen, _BGD_1_5B) ) {
1147 return;
1148 }
1149 }
1150
1151 else if ( iLine == 7 ) {
1152 if ( readDbl(line, pos[0], fieldLen, _TOT) ) {
1153 return;
1154 }
1155 }
1156 }
1157
1158 _ok = true;
1159}
1160
1161//
1162//////////////////////////////////////////////////////////////////////////////
1163QString t_eph::rinexDateStr(const bncTime& tt, const t_prn& prn, double version) {
1164 QString prnStr(prn.toString().c_str());
1165 return rinexDateStr(tt, prnStr, version);
1166}
1167
1168//
1169//////////////////////////////////////////////////////////////////////////////
1170QString t_eph::rinexDateStr(const bncTime& tt, const QString& prnStr, double version) {
1171
1172 QString datStr;
1173
1174 unsigned year, month, day, hour, min;
1175 double sec;
1176 tt.civil_date(year, month, day);
1177 tt.civil_time(hour, min, sec);
1178
1179 QTextStream out(&datStr);
1180
1181 if (version < 3.0) {
1182 QString prnHlp = prnStr.mid(1,2); if (prnHlp[0] == '0') prnHlp[0] = ' ';
1183 out << prnHlp << QString(" %1 %2 %3 %4 %5%6")
1184 .arg(year % 100, 2, 10, QChar('0'))
1185 .arg(month, 2)
1186 .arg(day, 2)
1187 .arg(hour, 2)
1188 .arg(min, 2)
1189 .arg(sec, 5, 'f',1);
1190 }
1191 else {
1192 out << prnStr << QString(" %1 %2 %3 %4 %5 %6")
1193 .arg(year, 4)
1194 .arg(month, 2, 10, QChar('0'))
1195 .arg(day, 2, 10, QChar('0'))
1196 .arg(hour, 2, 10, QChar('0'))
1197 .arg(min, 2, 10, QChar('0'))
1198 .arg(int(sec), 2, 10, QChar('0'));
1199 }
1200
1201 return datStr;
1202}
1203
1204// RINEX Format String
1205//////////////////////////////////////////////////////////////////////////////
1206QString t_ephGPS::toString(double version) const {
1207
1208 QString rnxStr = rinexDateStr(_TOC, _prn, version);
1209
1210 QTextStream out(&rnxStr);
1211
1212 out << QString("%1%2%3\n")
1213 .arg(_clock_bias, 19, 'e', 12)
1214 .arg(_clock_drift, 19, 'e', 12)
1215 .arg(_clock_driftrate, 19, 'e', 12);
1216
1217 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1218
1219 out << QString(fmt)
1220 .arg(_IODE, 19, 'e', 12)
1221 .arg(_Crs, 19, 'e', 12)
1222 .arg(_Delta_n, 19, 'e', 12)
1223 .arg(_M0, 19, 'e', 12);
1224
1225 out << QString(fmt)
1226 .arg(_Cuc, 19, 'e', 12)
1227 .arg(_e, 19, 'e', 12)
1228 .arg(_Cus, 19, 'e', 12)
1229 .arg(_sqrt_A, 19, 'e', 12);
1230
1231 out << QString(fmt)
1232 .arg(_TOEsec, 19, 'e', 12)
1233 .arg(_Cic, 19, 'e', 12)
1234 .arg(_OMEGA0, 19, 'e', 12)
1235 .arg(_Cis, 19, 'e', 12);
1236
1237 out << QString(fmt)
1238 .arg(_i0, 19, 'e', 12)
1239 .arg(_Crc, 19, 'e', 12)
1240 .arg(_omega, 19, 'e', 12)
1241 .arg(_OMEGADOT, 19, 'e', 12);
1242
1243 out << QString(fmt)
1244 .arg(_IDOT, 19, 'e', 12)
1245 .arg(_L2Codes, 19, 'e', 12)
1246 .arg(_TOEweek, 19, 'e', 12)
1247 .arg(_L2PFlag, 19, 'e', 12);
1248
1249 out << QString(fmt)
1250 .arg(_ura, 19, 'e', 12)
1251 .arg(_health, 19, 'e', 12)
1252 .arg(_TGD, 19, 'e', 12)
1253 .arg(_IODC, 19, 'e', 12);
1254
1255 out << QString(fmt)
1256 .arg(_TOT, 19, 'e', 12)
1257 .arg(_fitInterval, 19, 'e', 12)
1258 .arg("", 19, QChar(' '))
1259 .arg("", 19, QChar(' '));
1260
1261 return rnxStr;
1262}
1263
1264// RINEX Format String
1265//////////////////////////////////////////////////////////////////////////////
1266QString t_ephGlo::toString(double version) const {
1267
1268 QString rnxStr = rinexDateStr(_TOC-_gps_utc, _prn, version);
1269
1270 QTextStream out(&rnxStr);
1271
1272 out << QString("%1%2%3\n")
1273 .arg(-_tau, 19, 'e', 12)
1274 .arg(_gamma, 19, 'e', 12)
1275 .arg(_tki, 19, 'e', 12);
1276
1277 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1278
1279 out << QString(fmt)
1280 .arg(_x_pos, 19, 'e', 12)
1281 .arg(_x_velocity, 19, 'e', 12)
1282 .arg(_x_acceleration, 19, 'e', 12)
1283 .arg(_health, 19, 'e', 12);
1284
1285 out << QString(fmt)
1286 .arg(_y_pos, 19, 'e', 12)
1287 .arg(_y_velocity, 19, 'e', 12)
1288 .arg(_y_acceleration, 19, 'e', 12)
1289 .arg(_frequency_number, 19, 'e', 12);
1290
1291 out << QString(fmt)
1292 .arg(_z_pos, 19, 'e', 12)
1293 .arg(_z_velocity, 19, 'e', 12)
1294 .arg(_z_acceleration, 19, 'e', 12)
1295 .arg(_E, 19, 'e', 12);
1296
1297 return rnxStr;
1298}
1299
1300// RINEX Format String
1301//////////////////////////////////////////////////////////////////////////////
1302QString t_ephGal::toString(double version) const {
1303
1304 QString rnxStr = rinexDateStr(_TOC, _prn, version);
1305
1306 QTextStream out(&rnxStr);
1307
1308 out << QString("%1%2%3\n")
1309 .arg(_clock_bias, 19, 'e', 12)
1310 .arg(_clock_drift, 19, 'e', 12)
1311 .arg(_clock_driftrate, 19, 'e', 12);
1312
1313 QString fmt = version < 3.0 ? " %1%2%3%4\n" : " %1%2%3%4\n";
1314
1315 out << QString(fmt)
1316 .arg(_IODnav, 19, 'e', 12)
1317 .arg(_Crs, 19, 'e', 12)
1318 .arg(_Delta_n, 19, 'e', 12)
1319 .arg(_M0, 19, 'e', 12);
1320
1321 out << QString(fmt)
1322 .arg(_Cuc, 19, 'e', 12)
1323 .arg(_e, 19, 'e', 12)
1324 .arg(_Cus, 19, 'e', 12)
1325 .arg(_sqrt_A, 19, 'e', 12);
1326
1327 out << QString(fmt)
1328 .arg(_TOEsec, 19, 'e', 12)
1329 .arg(_Cic, 19, 'e', 12)
1330 .arg(_OMEGA0, 19, 'e', 12)
1331 .arg(_Cis, 19, 'e', 12);
1332
1333 out << QString(fmt)
1334 .arg(_i0, 19, 'e', 12)
1335 .arg(_Crc, 19, 'e', 12)
1336 .arg(_omega, 19, 'e', 12)
1337 .arg(_OMEGADOT, 19, 'e', 12);
1338
1339 int dataSource = 0;
1340 double HS = 0.0;
1341 if ( (_flags & GALEPHF_INAV) == GALEPHF_INAV ) {
1342 dataSource |= (1<<0);
1343 dataSource |= (1<<9);
1344 HS = _E5bHS;
1345 }
1346 else if ( (_flags & GALEPHF_FNAV) == GALEPHF_FNAV ) {
1347 dataSource |= (1<<1);
1348 dataSource |= (1<<8);
1349 HS = _E5aHS;
1350 }
1351 out << QString(fmt)
1352 .arg(_IDOT, 19, 'e', 12)
1353 .arg(double(dataSource), 19, 'e', 12)
1354 .arg(_TOEweek, 19, 'e', 12)
1355 .arg(0.0, 19, 'e', 12);
1356
1357 out << QString(fmt)
1358 .arg(_SISA, 19, 'e', 12)
1359 .arg(HS, 19, 'e', 12)
1360 .arg(_BGD_1_5A, 19, 'e', 12)
1361 .arg(_BGD_1_5B, 19, 'e', 12);
1362
1363 out << QString(fmt)
1364 .arg(_TOT, 19, 'e', 12)
1365 .arg("", 19, QChar(' '))
1366 .arg("", 19, QChar(' '))
1367 .arg("", 19, QChar(' '));
1368
1369 return rnxStr;
1370}
1371
Note: See TracBrowser for help on using the repository browser.