source: ntrip/trunk/BNC/RTCM3/ephemeris.cpp@ 3734

Last change on this file since 3734 was 3734, checked in by stoecker, 12 years ago

fix crash with Galileo ephemeris available, some other valgrind fixes as well
File size: 30.6 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 "bncapp.h"
14
15using namespace std;
16
17#define PI 3.1415926535898
18// Returns nearest integer value
19////////////////////////////////////////////////////////////////////////////
20static double NearestInt(double fl, double * remain)
21{
22 bool isneg = fl < 0.0;
23 double intval;
24 if(isneg) fl *= -1.0;
25 intval = (double)((unsigned long)(fl+0.5));
26 if(isneg) {fl *= -1.0; intval *= -1.0;}
27 if(remain)
28 *remain = fl-intval;
29 return intval;
30} /* NearestInt() */
31
32// Returns CRC24
33////////////////////////////////////////////////////////////////////////////
34static unsigned long CRC24(long size, const unsigned char *buf)
35{
36 unsigned long crc = 0;
37 int i;
38
39 while(size--)
40 {
41 crc ^= (*buf++) << (16);
42 for(i = 0; i < 8; i++)
43 {
44 crc <<= 1;
45 if(crc & 0x1000000)
46 crc ^= 0x01864cfb;
47 }
48 }
49 return crc;
50} /* CRC24 */
51
52//
53////////////////////////////////////////////////////////////////////////////
54bool t_eph::isNewerThan(const t_eph* eph) const {
55 if (_GPSweek > eph->_GPSweek ||
56 (_GPSweek == eph->_GPSweek && _GPSweeks > eph->_GPSweeks)) {
57 return true;
58 }
59 else {
60 return false;
61 }
62}
63
64// Set GPS Satellite Position
65////////////////////////////////////////////////////////////////////////////
66void t_ephGPS::set(const gpsephemeris* ee) {
67
68 _prn = QString("G%1").arg(ee->satellite, 2, 10, QChar('0'));
69
70 // TODO: check if following two lines are correct
71 _GPSweek = ee->GPSweek;
72 _GPSweeks = ee->TOE;
73
74 _TOW = ee->TOW;
75 _TOC = ee->TOC;
76 _TOE = ee->TOE;
77 _IODE = ee->IODE;
78 _IODC = ee->IODC;
79
80 _clock_bias = ee->clock_bias;
81 _clock_drift = ee->clock_drift;
82 _clock_driftrate = ee->clock_driftrate;
83
84 _Crs = ee->Crs;
85 _Delta_n = ee->Delta_n;
86 _M0 = ee->M0;
87 _Cuc = ee->Cuc;
88 _e = ee->e;
89 _Cus = ee->Cus;
90 _sqrt_A = ee->sqrt_A;
91 _Cic = ee->Cic;
92 _OMEGA0 = ee->OMEGA0;
93 _Cis = ee->Cis;
94 _i0 = ee->i0;
95 _Crc = ee->Crc;
96 _omega = ee->omega;
97 _OMEGADOT = ee->OMEGADOT;
98 _IDOT = ee->IDOT;
99
100 _TGD = ee->TGD;
101
102 _ok = true;
103
104 /* FIXME: convert URAindex and flags! */
105 _ura = 0;
106 _L2Codes = 0;
107 _L2PFlag = 0;
108 _health = ee->SVhealth;
109}
110
111// Compute GPS Satellite Position (virtual)
112////////////////////////////////////////////////////////////////////////////
113void t_ephGPS::position(int GPSweek, double GPSweeks,
114 double* xc,
115 double* vv) const {
116
117 static const double secPerWeek = 7 * 86400.0;
118 static const double omegaEarth = 7292115.1467e-11;
119 static const double gmWGS = 398.6005e12;
120
121 memset(xc, 0, 4*sizeof(double));
122 memset(vv, 0, 3*sizeof(double));
123
124 double a0 = _sqrt_A * _sqrt_A;
125 if (a0 == 0) {
126 return;
127 }
128
129 double n0 = sqrt(gmWGS/(a0*a0*a0));
130 double tk = GPSweeks - _TOE;
131 if (GPSweek != _GPSweek) {
132 tk += (GPSweek - _GPSweek) * secPerWeek;
133 }
134 double n = n0 + _Delta_n;
135 double M = _M0 + n*tk;
136 double E = M;
137 double E_last;
138 do {
139 E_last = E;
140 E = M + _e*sin(E);
141 } while ( fabs(E-E_last)*a0 > 0.001 );
142 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
143 double u0 = v + _omega;
144 double sin2u0 = sin(2*u0);
145 double cos2u0 = cos(2*u0);
146 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
147 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
148 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
149 double xp = r*cos(u);
150 double yp = r*sin(u);
151 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
152 omegaEarth*_TOE;
153
154 double sinom = sin(OM);
155 double cosom = cos(OM);
156 double sini = sin(i);
157 double cosi = cos(i);
158 xc[0] = xp*cosom - yp*cosi*sinom;
159 xc[1] = xp*sinom + yp*cosi*cosom;
160 xc[2] = yp*sini;
161
162 double tc = GPSweeks - _TOC;
163 if (GPSweek != _GPSweek) {
164 tc += (GPSweek - _GPSweek) * secPerWeek;
165 }
166 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
167
168 // Velocity
169 // --------
170 double tanv2 = tan(v/2);
171 double dEdM = 1 / (1 - _e*cos(E));
172 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
173 * dEdM * n;
174 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
175 double dotom = _OMEGADOT - omegaEarth;
176 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
177 double dotr = a0 * _e*sin(E) * dEdM * n
178 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
179 double dotx = dotr*cos(u) - r*sin(u)*dotu;
180 double doty = dotr*sin(u) + r*cos(u)*dotu;
181
182 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
183 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
184 + yp*sini*sinom*doti; // dX / di
185
186 vv[1] = sinom *dotx + cosi*cosom *doty
187 + xp*cosom*dotom - yp*cosi*sinom*dotom
188 - yp*sini*cosom*doti;
189
190 vv[2] = sini *doty + yp*cosi *doti;
191
192 // Relativistic Correction
193 // -----------------------
194 // xc(4) -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
195 xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
196}
197
198// build up RTCM3 for GPS
199////////////////////////////////////////////////////////////////////////////
200#define GPSTOINT(type, value) static_cast<type>(NearestInt(value,0))
201
202#define GPSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
203 |(GPSTOINT(long long,b)&((1ULL<<a)-1)); \
204 numbits += (a); \
205 while(numbits >= 8) { \
206 buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
207#define GPSADDBITSFLOAT(a,b,c) {long long i = GPSTOINT(long long,(b)/(c)); \
208 GPSADDBITS(a,i)};
209
210int t_ephGPS::RTCM3(unsigned char *buffer)
211{
212
213 unsigned char *startbuffer = buffer;
214 buffer= buffer+3;
215 int size = 0;
216 int numbits = 0;
217 unsigned long long bitbuffer = 0;
218 if (_ura <= 2.40){
219 _ura = 0;
220 }
221 else if (_ura <= 3.40){
222 _ura = 1;
223 }
224 else if (_ura <= 6.85){
225 _ura = 2;
226 }
227 else if (_ura <= 9.65){
228 _ura = 3;
229 }
230 else if (_ura <= 13.65){
231 _ura = 4;
232 }
233 else if (_ura <= 24.00){
234 _ura = 5;
235 }
236 else if (_ura <= 48.00){
237 _ura = 6;
238 }
239 else if (_ura <= 96.00){
240 _ura = 7;
241 }
242 else if (_ura <= 192.00){
243 _ura = 8;
244 }
245 else if (_ura <= 384.00){
246 _ura = 9;
247 }
248 else if (_ura <= 768.00){
249 _ura = 10;
250 }
251 else if (_ura <= 1536.00){
252 _ura = 11;
253 }
254 else if (_ura <= 1536.00){
255 _ura = 12;
256 }
257 else if (_ura <= 2072.00){
258 _ura = 13;
259 }
260 else if (_ura <= 6144.00){
261 _ura = 14;
262 }
263 else{
264 _ura = 15;
265 }
266
267 GPSADDBITS(12, 1019)
268 GPSADDBITS(6,_prn.right((_prn.length()-1)).toInt())
269 GPSADDBITS(10, _GPSweek)
270 GPSADDBITS(4, _ura)
271 GPSADDBITS(2,_L2Codes)
272 GPSADDBITSFLOAT(14, _IDOT, PI/static_cast<double>(1<<30)
273 /static_cast<double>(1<<13))
274 GPSADDBITS(8, _IODE)
275 GPSADDBITS(16, static_cast<int>(_TOC)>>4)
276 GPSADDBITSFLOAT(8, _clock_driftrate, 1.0/static_cast<double>(1<<30)
277 /static_cast<double>(1<<25))
278 GPSADDBITSFLOAT(16, _clock_drift, 1.0/static_cast<double>(1<<30)
279 /static_cast<double>(1<<13))
280 GPSADDBITSFLOAT(22, _clock_bias, 1.0/static_cast<double>(1<<30)
281 /static_cast<double>(1<<1))
282 GPSADDBITS(10, _IODC)
283 GPSADDBITSFLOAT(16, _Crs, 1.0/static_cast<double>(1<<5))
284 GPSADDBITSFLOAT(16, _Delta_n, PI/static_cast<double>(1<<30)
285 /static_cast<double>(1<<13))
286 GPSADDBITSFLOAT(32, _M0, PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
287 GPSADDBITSFLOAT(16, _Cuc, 1.0/static_cast<double>(1<<29))
288 GPSADDBITSFLOAT(32, _e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
289 GPSADDBITSFLOAT(16, _Cus, 1.0/static_cast<double>(1<<29))
290 GPSADDBITSFLOAT(32, _sqrt_A, 1.0/static_cast<double>(1<<19))
291 GPSADDBITS(16, static_cast<int>(_TOE)>>4)
292 GPSADDBITSFLOAT(16, _Cic, 1.0/static_cast<double>(1<<29))
293 GPSADDBITSFLOAT(32, _OMEGA0, PI/static_cast<double>(1<<30)
294 /static_cast<double>(1<<1))
295 GPSADDBITSFLOAT(16, _Cis, 1.0/static_cast<double>(1<<29))
296 GPSADDBITSFLOAT(32, _i0, PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
297 GPSADDBITSFLOAT(16, _Crc, 1.0/static_cast<double>(1<<5))
298 GPSADDBITSFLOAT(32, _omega, PI/static_cast<double>(1<<30)
299 /static_cast<double>(1<<1))
300 GPSADDBITSFLOAT(24, _OMEGADOT, PI/static_cast<double>(1<<30)
301 /static_cast<double>(1<<13))
302 GPSADDBITSFLOAT(8, _TGD, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
303 GPSADDBITS(6, _health)
304 GPSADDBITS(1, _L2PFlag)
305 GPSADDBITS(1, 0) /* GPS fit interval */
306
307 startbuffer[0]=0xD3;
308 startbuffer[1]=(size >> 8);
309 startbuffer[2]=size;
310 unsigned long i = CRC24(size+3, startbuffer);
311 buffer[size++] = i >> 16;
312 buffer[size++] = i >> 8;
313 buffer[size++] = i;
314 size += 3;
315 return size;
316}
317
318// Derivative of the state vector using a simple force model (static)
319////////////////////////////////////////////////////////////////////////////
320ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv,
321 double* acc) {
322
323 // State vector components
324 // -----------------------
325 ColumnVector rr = xv.rows(1,3);
326 ColumnVector vv = xv.rows(4,6);
327
328 // Acceleration
329 // ------------
330 static const double GM = 398.60044e12;
331 static const double AE = 6378136.0;
332 static const double OMEGA = 7292115.e-11;
333 static const double C20 = -1082.6257e-6;
334
335 double rho = rr.norm_Frobenius();
336 double t1 = -GM/(rho*rho*rho);
337 double t2 = 3.0/2.0 * C20 * (GM*AE*AE) / (rho*rho*rho*rho*rho);
338 double t3 = OMEGA * OMEGA;
339 double t4 = 2.0 * OMEGA;
340 double z2 = rr(3) * rr(3);
341
342 // Vector of derivatives
343 // ---------------------
344 ColumnVector va(6);
345 va(1) = vv(1);
346 va(2) = vv(2);
347 va(3) = vv(3);
348 va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0];
349 va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1) + acc[1];
350 va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3) + acc[2];
351
352 return va;
353}
354
355// Compute Glonass Satellite Position (virtual)
356////////////////////////////////////////////////////////////////////////////
357void t_ephGlo::position(int GPSweek, double GPSweeks,
358 double* xc, double* vv) const {
359
360 static const double secPerWeek = 7 * 86400.0;
361 static const double nominalStep = 10.0;
362
363 memset(xc, 0, 4*sizeof(double));
364 memset(vv, 0, 3*sizeof(double));
365
366 double dtPos = GPSweeks - _tt;
367 if (GPSweek != _GPSweek) {
368 dtPos += (GPSweek - _GPSweek) * secPerWeek;
369 }
370
371 int nSteps = int(fabs(dtPos) / nominalStep) + 1;
372 double step = dtPos / nSteps;
373
374 double acc[3];
375 acc[0] = _x_acceleration * 1.e3;
376 acc[1] = _y_acceleration * 1.e3;
377 acc[2] = _z_acceleration * 1.e3;
378 for (int ii = 1; ii <= nSteps; ii++) {
379 _xv = rungeKutta4(_tt, _xv, step, acc, glo_deriv);
380 _tt += step;
381 }
382
383 // Position and Velocity
384 // ---------------------
385 xc[0] = _xv(1);
386 xc[1] = _xv(2);
387 xc[2] = _xv(3);
388
389 vv[0] = _xv(4);
390 vv[1] = _xv(5);
391 vv[2] = _xv(6);
392
393 // Clock Correction
394 // ----------------
395 double dtClk = GPSweeks - _GPSweeks;
396 if (GPSweek != _GPSweek) {
397 dtClk += (GPSweek - _GPSweek) * secPerWeek;
398 }
399 xc[3] = -_tau + _gamma * dtClk;
400}
401
402// IOD of Glonass Ephemeris (virtual)
403////////////////////////////////////////////////////////////////////////////
404int t_ephGlo::IOD() const {
405 bncTime tGPS(_GPSweek, _GPSweeks);
406 bncTime tMoscow = tGPS - _gps_utc + 3 * 3600.0;
407 return int(tMoscow.daysec() / 900);
408}
409
410// Set Glonass Ephemeris
411////////////////////////////////////////////////////////////////////////////
412void t_ephGlo::set(const glonassephemeris* ee) {
413
414 _prn = QString("R%1").arg(ee->almanac_number, 2, 10, QChar('0'));
415
416 int ww = ee->GPSWeek;
417 int tow = ee->GPSTOW;
418 updatetime(&ww, &tow, ee->tb*1000, 0); // Moscow -> GPS
419
420 // Check the day once more
421 // -----------------------
422 {
423 const double secPerDay = 24 * 3600.0;
424 const double secPerWeek = 7 * secPerDay;
425 int ww_old = ww;
426 int tow_old = tow;
427 int currentWeek;
428 double currentSec;
429 currentGPSWeeks(currentWeek, currentSec);
430 bncTime currentTime(currentWeek, currentSec);
431 bncTime hTime(ww, (double) tow);
432
433 bool changed = false;
434 if (hTime - currentTime > secPerDay/2.0) {
435 changed = true;
436 tow -= secPerDay;
437 if (tow < 0) {
438 tow += secPerWeek;
439 ww -= 1;
440 }
441 }
442 else if (hTime - currentTime < -secPerDay/2.0) {
443 changed = true;
444 tow += secPerDay;
445 if (tow > secPerWeek) {
446 tow -= secPerWeek;
447 ww += 1;
448 }
449 }
450
451 if (changed && ((bncApp*) qApp)->mode() == bncApp::batchPostProcessing) {
452 bncTime newHTime(ww, (double) tow);
453 cout << "GLONASS " << ee->almanac_number << " Time Changed at "
454 << currentTime.datestr() << " " << currentTime.timestr()
455 << endl
456 << "old: " << hTime.datestr() << " " << hTime.timestr()
457 << endl
458 << "new: " << newHTime.datestr() << " " << newHTime.timestr()
459 << endl
460 << "eph: " << ee->GPSWeek << " " << ee->GPSTOW << " " << ee->tb
461 << endl
462 << "ww, tow (old): " << ww_old << " " << tow_old
463 << endl
464 << "ww, tow (new): " << ww << " " << tow
465 << endl << endl;
466 }
467 }
468
469 bncTime hlpTime(ww, (double) tow);
470 unsigned year, month, day;
471 hlpTime.civil_date(year, month, day);
472 _gps_utc = gnumleap(year, month, day);
473
474 _GPSweek = ww;
475 _GPSweeks = tow;
476 _E = ee->E;
477 _tau = ee->tau;
478 _gamma = ee->gamma;
479 _x_pos = ee->x_pos;
480 _x_velocity = ee->x_velocity;
481 _x_acceleration = ee->x_acceleration;
482 _y_pos = ee->y_pos;
483 _y_velocity = ee->y_velocity;
484 _y_acceleration = ee->y_acceleration;
485 _z_pos = ee->z_pos;
486 _z_velocity = ee->z_velocity;
487 _z_acceleration = ee->z_acceleration;
488 _health = 0;
489 _frequency_number = ee->frequency_number;
490 _tki = ee->tk-3*60*60; if (_tki < 0) _tki += 86400;
491
492 // Initialize status vector
493 // ------------------------
494 _tt = _GPSweeks;
495
496 _xv(1) = _x_pos * 1.e3;
497 _xv(2) = _y_pos * 1.e3;
498 _xv(3) = _z_pos * 1.e3;
499 _xv(4) = _x_velocity * 1.e3;
500 _xv(5) = _y_velocity * 1.e3;
501 _xv(6) = _z_velocity * 1.e3;
502
503 _ok = true;
504}
505
506// build up RTCM3 for GLONASS
507////////////////////////////////////////////////////////////////////////////
508#define GLONASSTOINT(type, value) static_cast<type>(NearestInt(value,0))
509
510#define GLONASSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
511 |(GLONASSTOINT(long long,b)&((1ULL<<(a))-1)); \
512 numbits += (a); \
513 while(numbits >= 8) { \
514 buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
515#define GLONASSADDBITSFLOATM(a,b,c) {int s; long long i; \
516 if(b < 0.0) \
517 { \
518 s = 1; \
519 i = GLONASSTOINT(long long,(-b)/(c)); \
520 if(!i) s = 0; \
521 } \
522 else \
523 { \
524 s = 0; \
525 i = GLONASSTOINT(long long,(b)/(c)); \
526 } \
527 GLONASSADDBITS(1,s) \
528 GLONASSADDBITS(a-1,i)}
529
530int t_ephGlo::RTCM3(unsigned char *buffer)
531{
532
533 int size = 0;
534 int numbits = 0;
535 long long bitbuffer = 0;
536 unsigned char *startbuffer = buffer;
537 buffer= buffer+3;
538
539 GLONASSADDBITS(12, 1020)
540 GLONASSADDBITS(6, _prn.right((_prn.length()-1)).toInt())
541 GLONASSADDBITS(5, 7+_frequency_number)
542 GLONASSADDBITS(1, 0)
543 GLONASSADDBITS(1, 0)
544 GLONASSADDBITS(2, 0)
545 _tki=_tki+3*60*60;
546 GLONASSADDBITS(5, static_cast<int>(_tki)/(60*60))
547 GLONASSADDBITS(6, (static_cast<int>(_tki)/60)%60)
548 GLONASSADDBITS(1, (static_cast<int>(_tki)/30)%30)
549 GLONASSADDBITS(1, _health)
550 GLONASSADDBITS(1, 0)
551 unsigned long long timeofday = (static_cast<int>(_tt+3*60*60-_gps_utc)%86400);
552 GLONASSADDBITS(7, timeofday/60/15)
553 GLONASSADDBITSFLOATM(24, _x_velocity*1000, 1000.0/static_cast<double>(1<<20))
554 GLONASSADDBITSFLOATM(27, _x_pos*1000, 1000.0/static_cast<double>(1<<11))
555 GLONASSADDBITSFLOATM(5, _x_acceleration*1000, 1000.0/static_cast<double>(1<<30))
556 GLONASSADDBITSFLOATM(24, _y_velocity*1000, 1000.0/static_cast<double>(1<<20))
557 GLONASSADDBITSFLOATM(27, _y_pos*1000, 1000.0/static_cast<double>(1<<11))
558 GLONASSADDBITSFLOATM(5, _y_acceleration*1000, 1000.0/static_cast<double>(1<<30))
559 GLONASSADDBITSFLOATM(24, _z_velocity*1000, 1000.0/static_cast<double>(1<<20))
560 GLONASSADDBITSFLOATM(27,_z_pos*1000, 1000.0/static_cast<double>(1<<11))
561 GLONASSADDBITSFLOATM(5, _z_acceleration*1000, 1000.0/static_cast<double>(1<<30))
562 GLONASSADDBITS(1, 0)
563 GLONASSADDBITSFLOATM(11, _gamma, 1.0/static_cast<double>(1<<30)
564 /static_cast<double>(1<<10))
565 GLONASSADDBITS(2, 0) /* GLONASS-M P */
566 GLONASSADDBITS(1, 0) /* GLONASS-M ln(3) */
567 GLONASSADDBITSFLOATM(22, _tau, 1.0/static_cast<double>(1<<30))
568 GLONASSADDBITS(5, 0) /* GLONASS-M delta tau */
569 GLONASSADDBITS(5, _E)
570 GLONASSADDBITS(1, 0) /* GLONASS-M P4 */
571 GLONASSADDBITS(4, 0) /* GLONASS-M FT */
572 GLONASSADDBITS(11, 0) /* GLONASS-M NT */
573 GLONASSADDBITS(2, 0) /* GLONASS-M active? */
574 GLONASSADDBITS(1, 0) /* GLONASS additional data */
575 GLONASSADDBITS(11, 0) /* GLONASS NA */
576 GLONASSADDBITS(32, 0) /* GLONASS tau C */
577 GLONASSADDBITS(5, 0) /* GLONASS-M N4 */
578 GLONASSADDBITS(22, 0) /* GLONASS-M tau GPS */
579 GLONASSADDBITS(1, 0) /* GLONASS-M ln(5) */
580 GLONASSADDBITS(7, 0) /* Reserved */
581
582 startbuffer[0]=0xD3;
583 startbuffer[1]=(size >> 8);
584 startbuffer[2]=size;
585 unsigned long i = CRC24(size+3, startbuffer);
586 buffer[size++] = i >> 16;
587 buffer[size++] = i >> 8;
588 buffer[size++] = i;
589 size += 3;
590 return size;
591}
592
593// Set Galileo Satellite Position
594////////////////////////////////////////////////////////////////////////////
595void t_ephGal::set(const galileoephemeris* ee) {
596
597 _prn = QString("E%1").arg(ee->satellite, 2, 10, QChar('0'));
598
599 _GPSweek = ee->Week;
600 _GPSweeks = ee->TOE;
601
602 _TOC = ee->TOC;
603 _TOE = ee->TOE;
604 _IODnav = ee->IODnav;
605
606 _clock_bias = ee->clock_bias;
607 _clock_drift = ee->clock_drift;
608 _clock_driftrate = ee->clock_driftrate;
609
610 _Crs = ee->Crs;
611 _Delta_n = ee->Delta_n;
612 _M0 = ee->M0;
613 _Cuc = ee->Cuc;
614 _e = ee->e;
615 _Cus = ee->Cus;
616 _sqrt_A = ee->sqrt_A;
617 _Cic = ee->Cic;
618 _OMEGA0 = ee->OMEGA0;
619 _Cis = ee->Cis;
620 _i0 = ee->i0;
621 _Crc = ee->Crc;
622 _omega = ee->omega;
623 _OMEGADOT = ee->OMEGADOT;
624 _IDOT = ee->IDOT;
625 _SISA = ee->SISA;
626 _BGD_1_5A = ee->BGD_1_5A;
627 _BGD_1_5B = ee->BGD_1_5B;
628 _E5aHS = ee->E5aHS;
629
630 _ok = true;
631}
632
633// Compute Galileo Satellite Position (virtual)
634////////////////////////////////////////////////////////////////////////////
635void t_ephGal::position(int GPSweek, double GPSweeks,
636 double* xc,
637 double* vv) const {
638
639 static const double secPerWeek = 7 * 86400.0;
640 static const double omegaEarth = 7292115.1467e-11;
641 static const double gmWGS = 398.6005e12;
642
643 memset(xc, 0, 4*sizeof(double));
644 memset(vv, 0, 3*sizeof(double));
645
646 double a0 = _sqrt_A * _sqrt_A;
647 if (a0 == 0) {
648 return;
649 }
650
651 double n0 = sqrt(gmWGS/(a0*a0*a0));
652 double tk = GPSweeks - _TOE;
653 if (GPSweek != _GPSweek) {
654 tk += (GPSweek - _GPSweek) * secPerWeek;
655 }
656 double n = n0 + _Delta_n;
657 double M = _M0 + n*tk;
658 double E = M;
659 double E_last;
660 do {
661 E_last = E;
662 E = M + _e*sin(E);
663 } while ( fabs(E-E_last)*a0 > 0.001 );
664 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
665 double u0 = v + _omega;
666 double sin2u0 = sin(2*u0);
667 double cos2u0 = cos(2*u0);
668 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
669 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
670 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
671 double xp = r*cos(u);
672 double yp = r*sin(u);
673 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
674 omegaEarth*_TOE;
675
676 double sinom = sin(OM);
677 double cosom = cos(OM);
678 double sini = sin(i);
679 double cosi = cos(i);
680 xc[0] = xp*cosom - yp*cosi*sinom;
681 xc[1] = xp*sinom + yp*cosi*cosom;
682 xc[2] = yp*sini;
683
684 double tc = GPSweeks - _TOC;
685 if (GPSweek != _GPSweek) {
686 tc += (GPSweek - _GPSweek) * secPerWeek;
687 }
688 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc;
689
690 // Velocity
691 // --------
692 double tanv2 = tan(v/2);
693 double dEdM = 1 / (1 - _e*cos(E));
694 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
695 * dEdM * n;
696 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
697 double dotom = _OMEGADOT - omegaEarth;
698 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
699 double dotr = a0 * _e*sin(E) * dEdM * n
700 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
701 double dotx = dotr*cos(u) - r*sin(u)*dotu;
702 double doty = dotr*sin(u) + r*cos(u)*dotu;
703
704 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
705 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
706 + yp*sini*sinom*doti; // dX / di
707
708 vv[1] = sinom *dotx + cosi*cosom *doty
709 + xp*cosom*dotom - yp*cosi*sinom*dotom
710 - yp*sini*cosom*doti;
711
712 vv[2] = sini *doty + yp*cosi *doti;
713
714 // Relativistic Correction
715 // -----------------------
716 // xc(4) -= 4.442807633e-10 * _e * sqrt(a0) *sin(E);
717 xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c;
718}
719
720// build up RTCM3 for Galileo
721////////////////////////////////////////////////////////////////////////////
722#define GALILEOTOINT(type, value) static_cast<type>(NearestInt(value, 0))
723
724#define GALILEOADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
725 |(GALILEOTOINT(long long,b)&((1LL<<a)-1)); \
726 numbits += (a); \
727 while(numbits >= 8) { \
728 buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
729#define GALILEOADDBITSFLOAT(a,b,c) {long long i = GALILEOTOINT(long long,(b)/(c)); \
730 GALILEOADDBITS(a,i)};
731
732int t_ephGal::RTCM3(unsigned char *buffer) {
733 int size = 0;
734 int numbits = 0;
735 long long bitbuffer = 0;
736 unsigned char *startbuffer = buffer;
737 buffer= buffer+3;
738
739 GALILEOADDBITS(12, /*inav ? 1046 :*/ 1045)
740 GALILEOADDBITS(6, _prn.right((_prn.length()-1)).toInt())
741 GALILEOADDBITS(12, _GPSweek)
742 GALILEOADDBITS(10, _IODnav)
743 GALILEOADDBITS(8, _SISA)
744 GALILEOADDBITSFLOAT(14, _IDOT, PI/static_cast<double>(1<<30)
745 /static_cast<double>(1<<13))
746 GALILEOADDBITS(14, _TOC/60)
747 GALILEOADDBITSFLOAT(6, _clock_driftrate, 1.0/static_cast<double>(1<<30)
748 /static_cast<double>(1<<29))
749 GALILEOADDBITSFLOAT(21, _clock_drift, 1.0/static_cast<double>(1<<30)
750 /static_cast<double>(1<<16))
751 GALILEOADDBITSFLOAT(31, _clock_bias, 1.0/static_cast<double>(1<<30)
752 /static_cast<double>(1<<4))
753 GALILEOADDBITSFLOAT(16, _Crs, 1.0/static_cast<double>(1<<5))
754 GALILEOADDBITSFLOAT(16, _Delta_n, PI/static_cast<double>(1<<30)
755 /static_cast<double>(1<<13))
756 GALILEOADDBITSFLOAT(32, _M0, PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
757 GALILEOADDBITSFLOAT(16, _Cuc, 1.0/static_cast<double>(1<<29))
758 GALILEOADDBITSFLOAT(32, _e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
759 GALILEOADDBITSFLOAT(16, _Cus, 1.0/static_cast<double>(1<<29))
760 GALILEOADDBITSFLOAT(32, _sqrt_A, 1.0/static_cast<double>(1<<19))
761 GALILEOADDBITS(14, _TOE/60)
762 GALILEOADDBITSFLOAT(16, _Cic, 1.0/static_cast<double>(1<<29))
763 GALILEOADDBITSFLOAT(32, _OMEGA0, PI/static_cast<double>(1<<30)
764 /static_cast<double>(1<<1))
765 GALILEOADDBITSFLOAT(16, _Cis, 1.0/static_cast<double>(1<<29))
766 GALILEOADDBITSFLOAT(32, _i0, PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
767 GALILEOADDBITSFLOAT(16, _Crc, 1.0/static_cast<double>(1<<5))
768 GALILEOADDBITSFLOAT(32, _omega, PI/static_cast<double>(1<<30)
769 /static_cast<double>(1<<1))
770 GALILEOADDBITSFLOAT(24, _OMEGADOT, PI/static_cast<double>(1<<30)
771 /static_cast<double>(1<<13))
772 GALILEOADDBITSFLOAT(10, _BGD_1_5A, 1.0/static_cast<double>(1<<30)
773 /static_cast<double>(1<<2))
774 /*if(inav)
775 {
776 GALILEOADDBITSFLOAT(10, _BGD_1_5B, 1.0/static_cast<double>(1<<30)
777 /static_cast<double>(1<<2))
778 GALILEOADDBITS(2, _E5bHS)
779 GALILEOADDBITS(1, flags & MNFGALEPHF_E5BDINVALID)
780 }
781 else*/
782 {
783 GALILEOADDBITS(2, _E5aHS)
784 GALILEOADDBITS(1, /*flags & MNFGALEPHF_E5ADINVALID*/0)
785 }
786 _TOE = 0.9999E9;
787 GALILEOADDBITS(20, _TOE)
788
789 GALILEOADDBITS(/*inav ? 1 :*/ 3, 0) /* fill up */
790
791 startbuffer[0]=0xD3;
792 startbuffer[1]=(size >> 8);
793 startbuffer[2]=size;
794 unsigned long i = CRC24(size+3, startbuffer);
795 buffer[size++] = i >> 16;
796 buffer[size++] = i >> 8;
797 buffer[size++] = i;
798 size += 3;
799 return size;
800}
801
802// Constructor
803//////////////////////////////////////////////////////////////////////////////
804t_ephGPS::t_ephGPS(float rnxVersion, const QStringList& lines) {
805
806 const int nLines = 8;
807
808 _ok = false;
809
810 if (lines.size() != nLines) {
811 return;
812 }
813
814 // RINEX Format
815 // ------------
816 int fieldLen = 19;
817
818 int pos[4];
819 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
820 pos[1] = pos[0] + fieldLen;
821 pos[2] = pos[1] + fieldLen;
822 pos[3] = pos[2] + fieldLen;
823
824 // Read eight lines
825 // ----------------
826 for (int iLine = 0; iLine < nLines; iLine++) {
827 QString line = lines[iLine];
828
829 if ( iLine == 0 ) {
830 QTextStream in(line.left(pos[1]).toAscii());
831
832 int year, month, day, hour, min;
833 double sec;
834
835 in >> _prn >> year >> month >> day >> hour >> min >> sec;
836
837 if (_prn.at(0) != 'G') {
838 _prn = QString("G%1").arg(_prn.toInt(), 2, 10, QLatin1Char('0'));
839 }
840
841 if (year < 80) {
842 year += 2000;
843 }
844 else if (year < 100) {
845 year += 1900;
846 }
847
848 bncTime hlpTime;
849 hlpTime.set(year, month, day, hour, min, sec);
850 _GPSweek = hlpTime.gpsw();
851 _GPSweeks = hlpTime.gpssec();
852 _TOC = _GPSweeks;
853
854 if ( readDbl(line, pos[1], fieldLen, _clock_bias ) ||
855 readDbl(line, pos[2], fieldLen, _clock_drift ) ||
856 readDbl(line, pos[3], fieldLen, _clock_driftrate) ) {
857 return;
858 }
859 }
860
861 else if ( iLine == 1 ) {
862 if ( readDbl(line, pos[0], fieldLen, _IODE ) ||
863 readDbl(line, pos[1], fieldLen, _Crs ) ||
864 readDbl(line, pos[2], fieldLen, _Delta_n) ||
865 readDbl(line, pos[3], fieldLen, _M0 ) ) {
866 return;
867 }
868 }
869
870 else if ( iLine == 2 ) {
871 if ( readDbl(line, pos[0], fieldLen, _Cuc ) ||
872 readDbl(line, pos[1], fieldLen, _e ) ||
873 readDbl(line, pos[2], fieldLen, _Cus ) ||
874 readDbl(line, pos[3], fieldLen, _sqrt_A) ) {
875 return;
876 }
877 }
878
879 else if ( iLine == 3 ) {
880 if ( readDbl(line, pos[0], fieldLen, _TOE ) ||
881 readDbl(line, pos[1], fieldLen, _Cic ) ||
882 readDbl(line, pos[2], fieldLen, _OMEGA0) ||
883 readDbl(line, pos[3], fieldLen, _Cis ) ) {
884 return;
885 }
886 }
887
888 else if ( iLine == 4 ) {
889 if ( readDbl(line, pos[0], fieldLen, _i0 ) ||
890 readDbl(line, pos[1], fieldLen, _Crc ) ||
891 readDbl(line, pos[2], fieldLen, _omega ) ||
892 readDbl(line, pos[3], fieldLen, _OMEGADOT) ) {
893 return;
894 }
895 }
896
897 else if ( iLine == 5 ) {
898 double dummy, TOEw;
899 if ( readDbl(line, pos[0], fieldLen, _IDOT) ||
900 readDbl(line, pos[1], fieldLen, dummy) ||
901 readDbl(line, pos[2], fieldLen, TOEw ) ||
902 readDbl(line, pos[3], fieldLen, dummy) ) {
903 return;
904 }
905 }
906
907 else if ( iLine == 6 ) {
908 double dummy;
909 if ( readDbl(line, pos[0], fieldLen, dummy ) ||
910 readDbl(line, pos[1], fieldLen, _health) ||
911 readDbl(line, pos[2], fieldLen, _TGD ) ||
912 readDbl(line, pos[3], fieldLen, _IODC ) ) {
913 return;
914 }
915 }
916
917 else if ( iLine == 7 ) {
918 double TOT;
919 if ( readDbl(line, pos[0], fieldLen, TOT) ) {
920 return;
921 }
922 }
923 }
924
925 _ok = true;
926}
927
928// Constructor
929//////////////////////////////////////////////////////////////////////////////
930t_ephGlo::t_ephGlo(float rnxVersion, const QStringList& lines) {
931
932 const int nLines = 4;
933
934 _ok = false;
935
936 if (lines.size() != nLines) {
937 return;
938 }
939
940 // RINEX Format
941 // ------------
942 int fieldLen = 19;
943
944 int pos[4];
945 pos[0] = (rnxVersion <= 2.12) ? 3 : 4;
946 pos[1] = pos[0] + fieldLen;
947 pos[2] = pos[1] + fieldLen;
948 pos[3] = pos[2] + fieldLen;
949
950 // Read four lines
951 // ---------------
952 for (int iLine = 0; iLine < nLines; iLine++) {
953 QString line = lines[iLine];
954
955 if ( iLine == 0 ) {
956 QTextStream in(line.left(pos[1]).toAscii());
957
958 int year, month, day, hour, min;
959 double sec;
960
961 in >> _prn >> year >> month >> day >> hour >> min >> sec;
962
963 if (_prn.at(0) != 'R') {
964 _prn = QString("R%1").arg(_prn.toInt(), 2, 10, QLatin1Char('0'));
965 }
966
967 if (year < 80) {
968 year += 2000;
969 }
970 else if (year < 100) {
971 year += 1900;
972 }
973
974 bncTime hlpTime;
975 hlpTime.set(year, month, day, hour, min, sec);
976 _GPSweek = hlpTime.gpsw();
977 _GPSweeks = hlpTime.gpssec();
978
979 _gps_utc = gnumleap(year, month, day);
980 _tki = 0.0; // TODO ?
981
982 double second_tot;
983 if ( readDbl(line, pos[1], fieldLen, _tau ) ||
984 readDbl(line, pos[2], fieldLen, _gamma ) ||
985 readDbl(line, pos[3], fieldLen, second_tot) ) {
986 return;
987 }
988 }
989
990 else if ( iLine == 1 ) {
991 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
992 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
993 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
994 readDbl(line, pos[3], fieldLen, _health ) ) {
995 return;
996 }
997 }
998
999 else if ( iLine == 2 ) {
1000 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
1001 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
1002 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
1003 readDbl(line, pos[3], fieldLen, _frequency_number) ) {
1004 return;
1005 }
1006 }
1007
1008 else if ( iLine == 3 ) {
1009 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
1010 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
1011 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
1012 readDbl(line, pos[3], fieldLen, _E ) ) {
1013 return;
1014 }
1015 }
1016 }
1017
1018 // Initialize status vector
1019 // ------------------------
1020 _tt = _GPSweeks;
1021
1022 _xv.ReSize(6);
1023
1024 _xv(1) = _x_pos * 1.e3;
1025 _xv(2) = _y_pos * 1.e3;
1026 _xv(3) = _z_pos * 1.e3;
1027 _xv(4) = _x_velocity * 1.e3;
1028 _xv(5) = _y_velocity * 1.e3;
1029 _xv(6) = _z_velocity * 1.e3;
1030
1031 _ok = true;
1032}
1033
1034// Constructor
1035//////////////////////////////////////////////////////////////////////////////
1036t_ephGal::t_ephGal(float /* rnxVersion */, const QStringList& /* lines */) {
1037
1038 _ok = false;
1039}
Note: See TracBrowser for help on using the repository browser.