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

Last change on this file since 3965 was 3765, checked in by mervart, 13 years ago
File size: 30.7 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
977 _gps_utc = gnumleap(year, month, day);
978 hlpTime = hlpTime + _gps_utc;
979
980 _GPSweek = hlpTime.gpsw();
981 _GPSweeks = hlpTime.gpssec();
982
983 _tki = 0.0; // TODO ?
984
985 double second_tot;
986 if ( readDbl(line, pos[1], fieldLen, _tau ) ||
987 readDbl(line, pos[2], fieldLen, _gamma ) ||
988 readDbl(line, pos[3], fieldLen, second_tot) ) {
989 return;
990 }
991
992 _tau = -_tau;
993 }
994
995 else if ( iLine == 1 ) {
996 if ( readDbl(line, pos[0], fieldLen, _x_pos ) ||
997 readDbl(line, pos[1], fieldLen, _x_velocity ) ||
998 readDbl(line, pos[2], fieldLen, _x_acceleration) ||
999 readDbl(line, pos[3], fieldLen, _health ) ) {
1000 return;
1001 }
1002 }
1003
1004 else if ( iLine == 2 ) {
1005 if ( readDbl(line, pos[0], fieldLen, _y_pos ) ||
1006 readDbl(line, pos[1], fieldLen, _y_velocity ) ||
1007 readDbl(line, pos[2], fieldLen, _y_acceleration ) ||
1008 readDbl(line, pos[3], fieldLen, _frequency_number) ) {
1009 return;
1010 }
1011 }
1012
1013 else if ( iLine == 3 ) {
1014 if ( readDbl(line, pos[0], fieldLen, _z_pos ) ||
1015 readDbl(line, pos[1], fieldLen, _z_velocity ) ||
1016 readDbl(line, pos[2], fieldLen, _z_acceleration) ||
1017 readDbl(line, pos[3], fieldLen, _E ) ) {
1018 return;
1019 }
1020 }
1021 }
1022
1023 // Initialize status vector
1024 // ------------------------
1025 _tt = _GPSweeks;
1026
1027 _xv.ReSize(6);
1028
1029 _xv(1) = _x_pos * 1.e3;
1030 _xv(2) = _y_pos * 1.e3;
1031 _xv(3) = _z_pos * 1.e3;
1032 _xv(4) = _x_velocity * 1.e3;
1033 _xv(5) = _y_velocity * 1.e3;
1034 _xv(6) = _z_velocity * 1.e3;
1035
1036 _ok = true;
1037}
1038
1039// Constructor
1040//////////////////////////////////////////////////////////////////////////////
1041t_ephGal::t_ephGal(float /* rnxVersion */, const QStringList& /* lines */) {
1042
1043 _ok = false;
1044}
Note: See TracBrowser for help on using the repository browser.