source: ntrip/trunk/BNS/bnseph.cpp@ 2258

Last change on this file since 2258 was 2017, checked in by mervart, 14 years ago

* empty log message *
File size: 19.6 KB
Line 
1/* -------------------------------------------------------------------------
2 * BKG NTRIP Server
3 * -------------------------------------------------------------------------
4 *
5 * Class: bnseph
6 *
7 * Purpose: Retrieve broadcast ephemeris from BNC
8 *
9 * Author: L. Mervart
10 *
11 * Created: 29-Mar-2008
12 *
13 * Changes:
14 *
15 * -----------------------------------------------------------------------*/
16
17#include <iostream>
18
19#include "bnseph.h"
20#include "bnsutils.h"
21#include "bnssettings.h"
22
23#define PI 3.1415926535898
24
25using namespace std;
26
27// Constructor
28////////////////////////////////////////////////////////////////////////////
29t_bnseph::t_bnseph(QObject* parent) : QThread(parent) {
30
31 this->setTerminationEnabled(true);
32
33 _socket = 0;
34
35 bnsSettings settings;
36
37 QIODevice::OpenMode oMode;
38 if (Qt::CheckState(settings.value("fileAppend").toInt()) == Qt::Checked) {
39 oMode = QIODevice::WriteOnly | QIODevice::Unbuffered | QIODevice::Append;
40 }
41 else {
42 oMode = QIODevice::WriteOnly | QIODevice::Unbuffered;
43 }
44
45 // Echo ephemeris into a file
46 // ---------------------------
47 QString echoFileName = settings.value("ephEcho").toString();
48 if (echoFileName.isEmpty()) {
49 _echoFile = 0;
50 _echoStream = 0;
51 }
52 else {
53 _echoFile = new QFile(echoFileName);
54 if (_echoFile->open(oMode)) {
55 _echoStream = new QTextStream(_echoFile);
56 }
57 else {
58 _echoStream = 0;
59 }
60 }
61}
62
63// Destructor
64////////////////////////////////////////////////////////////////////////////
65t_bnseph::~t_bnseph() {
66 delete _socket;
67 delete _echoStream;
68 delete _echoFile;
69}
70
71// Connect the Socket
72////////////////////////////////////////////////////////////////////////////
73void t_bnseph::reconnect() {
74
75 delete _socket;
76
77 bnsSettings settings;
78 QString host = settings.value("ephHost").toString();
79 if (host.isEmpty()) {
80 host = "localhost";
81 }
82 int port = settings.value("ephPort").toInt();
83
84 _socket = new QTcpSocket();
85 _socket->connectToHost(host, port);
86
87 const int timeOut = 30*1000; // 30 seconds
88 if (!_socket->waitForConnected(timeOut)) {
89// emit(newMessage("bnseph::run Connect Timeout"));
90 emit(newMessage("Ephemeris server: Connection timeout")); // weber
91 msleep(1000);
92 }
93}
94
95// Start
96////////////////////////////////////////////////////////////////////////////
97void t_bnseph::run() {
98
99//emit(newMessage("bnseph::run Start"));
100 emit(newMessage("Ephemeris server: Connection opened")); // weber
101
102 while (true) {
103 if ( _socket == 0 ||
104 (_socket->state() != QAbstractSocket::ConnectingState &&
105 _socket->state() != QAbstractSocket::ConnectedState) ) {
106 reconnect();
107 }
108 if (_socket && _socket->state() == QAbstractSocket::ConnectedState) {
109 readEph();
110 }
111 else {
112 msleep(10);
113 }
114 }
115}
116
117// Read One Ephemeris
118////////////////////////////////////////////////////////////////////////////
119void t_bnseph::readEph() {
120
121 int nBytes = 0;
122 t_eph* eph = 0;
123
124 QByteArray line = waitForLine(_socket);
125
126 if (line.isEmpty()) {
127 return;
128 }
129
130 if (_echoStream) {
131 *_echoStream << line;
132 _echoStream->flush();
133 }
134
135 nBytes += line.length();
136
137 QTextStream in(line);
138 QString prn;
139
140 in >> prn;
141
142 int numlines = 0;
143 if (prn.indexOf('R') != -1) {
144 eph = new t_ephGlo();
145 numlines = 4;
146 }
147 else {
148 eph = new t_ephGPS();
149 numlines = 8;
150 }
151
152 QStringList lines;
153 lines << line;
154
155 for (int ii = 2; ii <= numlines; ii++) {
156 QByteArray line = waitForLine(_socket);
157 if (line.isEmpty()) {
158 delete eph;
159 return;
160 }
161
162 if (_echoStream) {
163 *_echoStream << line;
164 _echoStream->flush();
165 }
166
167 nBytes += line.length();
168 lines << line;
169 }
170
171 eph->read(lines);
172
173 emit(newEph(eph, nBytes));
174}
175
176// Compare Time
177////////////////////////////////////////////////////////////////////////////
178bool t_eph::isNewerThan(const t_eph* eph) const {
179 if (_GPSweek > eph->_GPSweek ||
180 (_GPSweek == eph->_GPSweek && _GPSweeks > eph->_GPSweeks)) {
181 return true;
182 }
183 else {
184 return false;
185 }
186}
187
188// Read GPS Ephemeris
189////////////////////////////////////////////////////////////////////////////
190void t_ephGPS::read(const QStringList& lines) {
191
192 for (int ii = 1; ii <= lines.size(); ii++) {
193 QTextStream in(lines.at(ii-1).toAscii());
194
195 if (ii == 1) {
196 double year, month, day, hour, minute, second;
197 in >> _prn >> year >> month >> day >> hour >> minute >> second
198 >> _clock_bias >> _clock_drift >> _clock_driftrate;
199
200 if (year < 100) year += 2000;
201
202 QDateTime* dateTime = new QDateTime(QDate(int(year), int(month), int(day)),
203 QTime(int(hour), int(minute), int(second)), Qt::UTC);
204
205 GPSweekFromDateAndTime(*dateTime, _GPSweek, _GPSweeks);
206
207 delete dateTime;
208
209 _TOC = _GPSweeks;
210 }
211 else if (ii == 2) {
212 in >> _IODE >> _Crs >> _Delta_n >> _M0;
213 }
214 else if (ii == 3) {
215 in >> _Cuc >> _e >> _Cus >> _sqrt_A;
216 }
217 else if (ii == 4) {
218 in >> _TOE >> _Cic >> _OMEGA0 >> _Cis;
219 }
220 else if (ii == 5) {
221 in >> _i0 >> _Crc >> _omega >> _OMEGADOT;
222 }
223 else if (ii == 6) {
224 double GPSweek;
225 in >> _IDOT >> _L2Codes >> GPSweek >> _L2PFlag;
226 }
227 else if (ii == 7) {
228 in >> _ura >> _health >> _TGD >> _IODC;
229 }
230 else if (ii == 8) {
231 in >> _TOW;
232 }
233 }
234}
235
236// Returns nearest integer value
237////////////////////////////////////////////////////////////////////////////
238static double NearestInt(double fl, double * remain)
239{
240 bool isneg = fl < 0.0;
241 double intval;
242 if(isneg) fl *= -1.0;
243 intval = (double)((unsigned long)(fl+0.5));
244 if(isneg) {fl *= -1.0; intval *= -1.0;}
245 if(remain)
246 *remain = fl-intval;
247 return intval;
248} /* NearestInt() */
249
250// Returns CRC24
251////////////////////////////////////////////////////////////////////////////
252static unsigned long CRC24(long size, const unsigned char *buf)
253{
254 unsigned long crc = 0;
255 int i;
256
257 while(size--)
258 {
259 crc ^= (*buf++) << (16);
260 for(i = 0; i < 8; i++)
261 {
262 crc <<= 1;
263 if(crc & 0x1000000)
264 crc ^= 0x01864cfb;
265 }
266 }
267 return crc;
268} /* CRC24 */
269
270// build up RTCM3 for GPS
271////////////////////////////////////////////////////////////////////////////
272
273#define GPSTOINT(type, value) static_cast<type>(NearestInt(value,0))
274
275#define GPSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
276 |(GPSTOINT(long long,b)&((1ULL<<a)-1)); \
277 numbits += (a); \
278 while(numbits >= 8) { \
279 buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
280#define GPSADDBITSFLOAT(a,b,c) {long long i = GPSTOINT(long long,(b)/(c)); \
281 GPSADDBITS(a,i)};
282
283int t_ephGPS::RTCM3(unsigned char *buffer)
284{
285
286 unsigned char *startbuffer = buffer;
287 buffer= buffer+3;
288 int size = 0;
289 int numbits = 0;
290 unsigned long long bitbuffer = 0;
291 if (_ura <= 2.40){
292 _ura = 0;
293 }
294 else if (_ura <= 3.40){
295 _ura = 1;
296 }
297 else if (_ura <= 6.85){
298 _ura = 2;
299 }
300 else if (_ura <= 9.65){
301 _ura = 3;
302 }
303 else if (_ura <= 13.65){
304 _ura = 4;
305 }
306 else if (_ura <= 24.00){
307 _ura = 5;
308 }
309 else if (_ura <= 48.00){
310 _ura = 6;
311 }
312 else if (_ura <= 96.00){
313 _ura = 7;
314 }
315 else if (_ura <= 192.00){
316 _ura = 8;
317 }
318 else if (_ura <= 384.00){
319 _ura = 9;
320 }
321 else if (_ura <= 768.00){
322 _ura = 10;
323 }
324 else if (_ura <= 1536.00){
325 _ura = 11;
326 }
327 else if (_ura <= 1536.00){
328 _ura = 12;
329 }
330 else if (_ura <= 2072.00){
331 _ura = 13;
332 }
333 else if (_ura <= 6144.00){
334 _ura = 14;
335 }
336 else{
337 _ura = 15;
338 }
339
340 GPSADDBITS(12, 1019)
341 GPSADDBITS(6,_prn.right((_prn.length()-1)).toInt())
342 GPSADDBITS(10, _GPSweek)
343 GPSADDBITS(4, _ura)
344 GPSADDBITS(2,_L2Codes)
345 GPSADDBITSFLOAT(14, _IDOT, PI/static_cast<double>(1<<30)
346 /static_cast<double>(1<<13))
347 GPSADDBITS(8, _IODE)
348 GPSADDBITS(16, static_cast<int>(_TOC)>>4)
349 GPSADDBITSFLOAT(8, _clock_driftrate, 1.0/static_cast<double>(1<<30)
350 /static_cast<double>(1<<25))
351 GPSADDBITSFLOAT(16, _clock_drift, 1.0/static_cast<double>(1<<30)
352 /static_cast<double>(1<<13))
353 GPSADDBITSFLOAT(22, _clock_bias, 1.0/static_cast<double>(1<<30)
354 /static_cast<double>(1<<1))
355 GPSADDBITS(10, _IODC)
356 GPSADDBITSFLOAT(16, _Crs, 1.0/static_cast<double>(1<<5))
357 GPSADDBITSFLOAT(16, _Delta_n, PI/static_cast<double>(1<<30)
358 /static_cast<double>(1<<13))
359 GPSADDBITSFLOAT(32, _M0, PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
360 GPSADDBITSFLOAT(16, _Cuc, 1.0/static_cast<double>(1<<29))
361 GPSADDBITSFLOAT(32, _e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
362 GPSADDBITSFLOAT(16, _Cus, 1.0/static_cast<double>(1<<29))
363 GPSADDBITSFLOAT(32, _sqrt_A, 1.0/static_cast<double>(1<<19))
364 GPSADDBITS(16, static_cast<int>(_TOE)>>4)
365 GPSADDBITSFLOAT(16, _Cic, 1.0/static_cast<double>(1<<29))
366 GPSADDBITSFLOAT(32, _OMEGA0, PI/static_cast<double>(1<<30)
367 /static_cast<double>(1<<1))
368 GPSADDBITSFLOAT(16, _Cis, 1.0/static_cast<double>(1<<29))
369 GPSADDBITSFLOAT(32, _i0, PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
370 GPSADDBITSFLOAT(16, _Crc, 1.0/static_cast<double>(1<<5))
371 GPSADDBITSFLOAT(32, _omega, PI/static_cast<double>(1<<30)
372 /static_cast<double>(1<<1))
373 GPSADDBITSFLOAT(24, _OMEGADOT, PI/static_cast<double>(1<<30)
374 /static_cast<double>(1<<13))
375 GPSADDBITSFLOAT(8, _TGD, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
376 GPSADDBITS(6, _health)
377 GPSADDBITS(1, _L2PFlag)
378 GPSADDBITS(1, 0) /* GPS fit interval */
379
380 startbuffer[0]=0xD3;
381 startbuffer[1]=(size >> 8);
382 startbuffer[2]=size;
383 unsigned long i = CRC24(size+3, startbuffer);
384 buffer[size++] = i >> 16;
385 buffer[size++] = i >> 8;
386 buffer[size++] = i;
387 size += 3;
388 return size;
389}
390
391// Compute GPS Satellite Position
392////////////////////////////////////////////////////////////////////////////
393void t_ephGPS::position(int GPSweek, double GPSweeks, ColumnVector& xc,
394 ColumnVector& vv) const {
395
396 static const double secPerWeek = 7 * 86400.0;
397 static const double omegaEarth = 7292115.1467e-11;
398 static const double gmWGS = 398.6005e12;
399
400 if (xc.Nrows() < 4) {
401 xc.ReSize(4);
402 }
403 xc = 0.0;
404
405 if (vv.Nrows() < 3) {
406 vv.ReSize(3);
407 }
408 vv = 0.0;
409
410 double a0 = _sqrt_A * _sqrt_A;
411 if (a0 == 0) {
412 return;
413 }
414
415 double n0 = sqrt(gmWGS/(a0*a0*a0));
416 double tk = GPSweeks - _TOE;
417 if (GPSweek != _GPSweek) {
418 tk += (GPSweek - _GPSweek) * secPerWeek;
419 }
420 double n = n0 + _Delta_n;
421 double M = _M0 + n*tk;
422 double E = M;
423 double E_last;
424 do {
425 E_last = E;
426 E = M + _e*sin(E);
427 } while ( fabs(E-E_last)*a0 > 0.001 );
428 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
429 double u0 = v + _omega;
430 double sin2u0 = sin(2*u0);
431 double cos2u0 = cos(2*u0);
432 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
433 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
434 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
435 double xp = r*cos(u);
436 double yp = r*sin(u);
437 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
438 omegaEarth*_TOE;
439
440 double sinom = sin(OM);
441 double cosom = cos(OM);
442 double sini = sin(i);
443 double cosi = cos(i);
444 xc(1) = xp*cosom - yp*cosi*sinom;
445 xc(2) = xp*sinom + yp*cosi*cosom;
446 xc(3) = yp*sini;
447
448 double tc = GPSweeks - _TOC;
449 if (GPSweek != _GPSweek) {
450 tc += (GPSweek - _GPSweek) * secPerWeek;
451 }
452 xc(4) = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc
453 - 4.442807633e-10 * _e * sqrt(a0) *sin(E);
454
455 // Velocity
456 // --------
457 double tanv2 = tan(v/2);
458 double dEdM = 1 / (1 - _e*cos(E));
459 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
460 * dEdM * n;
461 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
462 double dotom = _OMEGADOT - omegaEarth;
463 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
464 double dotr = a0 * _e*sin(E) * dEdM * n
465 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
466 double dotx = dotr*cos(u) - r*sin(u)*dotu;
467 double doty = dotr*sin(u) + r*cos(u)*dotu;
468
469 vv(1) = cosom *dotx - cosi*sinom *doty // dX / dr
470 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
471 + yp*sini*sinom*doti; // dX / di
472
473 vv(2) = sinom *dotx + cosi*cosom *doty
474 + xp*cosom*dotom - yp*cosi*sinom*dotom
475 - yp*sini*cosom*doti;
476
477 vv(3) = sini *doty + yp*cosi *doti;
478}
479
480// Read Glonass Ephemeris
481////////////////////////////////////////////////////////////////////////////
482void t_ephGlo::read(const QStringList& lines) {
483
484 for (int ii = 1; ii <= lines.size(); ii++) {
485 QTextStream in(lines.at(ii-1).toAscii());
486
487 if (ii == 1) {
488 double year, month, day, hour, minute, second;
489 in >> _prn >> year >> month >> day >> hour >> minute >> second
490 >> _tau >> _gamma >> _tki;
491
492 _tau = -_tau;
493
494 if (year < 100) year += 2000;
495
496 QDateTime* dateTime = new QDateTime(QDate(int(year), int(month), int(day)),
497 QTime(int(hour), int(minute), int(second)), Qt::UTC);
498
499 GPSweekFromDateAndTime(*dateTime, _GPSweek, _GPSweeks);
500
501 delete dateTime;
502
503 //// beg test
504 //// _gps_utc = 14.0;
505 //// end test
506
507 _GPSweeks += _gps_utc;
508 }
509 else if (ii == 2) {
510 in >>_x_pos >> _x_velocity >> _x_acceleration >> _health;
511 }
512 else if (ii == 3) {
513 in >>_y_pos >> _y_velocity >> _y_acceleration >> _frequency_number;
514 }
515 else if (ii == 4) {
516 in >>_z_pos >> _z_velocity >> _z_acceleration >> _E;
517 }
518 }
519
520 // Initialize status vector
521 // ------------------------
522 _tt = _GPSweeks;
523
524 _xv(1) = _x_pos * 1.e3;
525 _xv(2) = _y_pos * 1.e3;
526 _xv(3) = _z_pos * 1.e3;
527 _xv(4) = _x_velocity * 1.e3;
528 _xv(5) = _y_velocity * 1.e3;
529 _xv(6) = _z_velocity * 1.e3;
530}
531
532
533// build up RTCM3 for GLONASS
534////////////////////////////////////////////////////////////////////////////
535#define GLONASSTOINT(type, value) static_cast<type>(NearestInt(value,0))
536
537#define GLONASSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
538 |(GLONASSTOINT(long long,b)&((1ULL<<(a))-1)); \
539 numbits += (a); \
540 while(numbits >= 8) { \
541 buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
542#define GLONASSADDBITSFLOATM(a,b,c) {int s; long long i; \
543 if(b < 0.0) \
544 { \
545 s = 1; \
546 i = GLONASSTOINT(long long,(-b)/(c)); \
547 if(!i) s = 0; \
548 } \
549 else \
550 { \
551 s = 0; \
552 i = GLONASSTOINT(long long,(b)/(c)); \
553 } \
554 GLONASSADDBITS(1,s) \
555 GLONASSADDBITS(a-1,i)}
556
557int t_ephGlo::RTCM3(unsigned char *buffer)
558{
559
560 int size = 0;
561 int numbits = 0;
562 long long bitbuffer = 0;
563 unsigned char *startbuffer = buffer;
564 buffer= buffer+3;
565
566 GLONASSADDBITS(12, 1020)
567 GLONASSADDBITS(6, _prn.right((_prn.length()-1)).toInt())
568 GLONASSADDBITS(5, 7+_frequency_number)
569 GLONASSADDBITS(1, 0)
570 GLONASSADDBITS(1, 0)
571 GLONASSADDBITS(2, 0)
572 _tki=_tki+3*60*60;
573 GLONASSADDBITS(5, static_cast<int>(_tki)/(60*60))
574 GLONASSADDBITS(6, (static_cast<int>(_tki)/60)%60)
575 GLONASSADDBITS(1, (static_cast<int>(_tki)/30)%30)
576 GLONASSADDBITS(1, _health)
577 GLONASSADDBITS(1, 0)
578 unsigned long long timeofday = (static_cast<int>(_tt+3*60*60-_gps_utc)%86400);
579 GLONASSADDBITS(7, timeofday/60/15)
580 GLONASSADDBITSFLOATM(24, _x_velocity*1000, 1000.0/static_cast<double>(1<<20))
581 GLONASSADDBITSFLOATM(27, _x_pos*1000, 1000.0/static_cast<double>(1<<11))
582 GLONASSADDBITSFLOATM(5, _x_acceleration*1000, 1000.0/static_cast<double>(1<<30))
583 GLONASSADDBITSFLOATM(24, _y_velocity*1000, 1000.0/static_cast<double>(1<<20))
584 GLONASSADDBITSFLOATM(27, _y_pos*1000, 1000.0/static_cast<double>(1<<11))
585 GLONASSADDBITSFLOATM(5, _y_acceleration*1000, 1000.0/static_cast<double>(1<<30))
586 GLONASSADDBITSFLOATM(24, _z_velocity*1000, 1000.0/static_cast<double>(1<<20))
587 GLONASSADDBITSFLOATM(27,_z_pos*1000, 1000.0/static_cast<double>(1<<11))
588 GLONASSADDBITSFLOATM(5, _z_acceleration*1000, 1000.0/static_cast<double>(1<<30))
589 GLONASSADDBITS(1, 0)
590 GLONASSADDBITSFLOATM(11, _gamma, 1.0/static_cast<double>(1<<30)
591 /static_cast<double>(1<<10))
592 GLONASSADDBITS(2, 0) /* GLONASS-M P */
593 GLONASSADDBITS(1, 0) /* GLONASS-M ln(3) */
594 GLONASSADDBITSFLOATM(22, _tau, 1.0/static_cast<double>(1<<30))
595 GLONASSADDBITS(5, 0) /* GLONASS-M delta tau */
596 GLONASSADDBITS(5, _E)
597 GLONASSADDBITS(1, 0) /* GLONASS-M P4 */
598 GLONASSADDBITS(4, 0) /* GLONASS-M FT */
599 GLONASSADDBITS(11, 0) /* GLONASS-M NT */
600 GLONASSADDBITS(2, 0) /* GLONASS-M active? */
601 GLONASSADDBITS(1, 0) /* GLONASS additional data */
602 GLONASSADDBITS(11, 0) /* GLONASS NA */
603 GLONASSADDBITS(32, 0) /* GLONASS tau C */
604 GLONASSADDBITS(5, 0) /* GLONASS-M N4 */
605 GLONASSADDBITS(22, 0) /* GLONASS-M tau GPS */
606 GLONASSADDBITS(1, 0) /* GLONASS-M ln(5) */
607 GLONASSADDBITS(7, 0) /* Reserved */
608
609 startbuffer[0]=0xD3;
610 startbuffer[1]=(size >> 8);
611 startbuffer[2]=size;
612 unsigned long i = CRC24(size+3, startbuffer);
613 buffer[size++] = i >> 16;
614 buffer[size++] = i >> 8;
615 buffer[size++] = i;
616 size += 3;
617 return size;
618}
619
620// Derivative of the state vector using a simple force model (static)
621////////////////////////////////////////////////////////////////////////////
622ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv) {
623
624 // State vector components
625 // -----------------------
626 ColumnVector rr = xv.rows(1,3);
627 ColumnVector vv = xv.rows(4,6);
628
629 // Acceleration
630 // ------------
631 static const double GM = 398.60044e12;
632 static const double AE = 6378136.0;
633 static const double OMEGA = 7292115.e-11;
634 static const double C20 = -1082.63e-6;
635
636 double rho = rr.norm_Frobenius();
637 double t1 = -GM/(rho*rho*rho);
638 double t2 = 3.0/2.0 * C20 * (GM*AE*AE) / (rho*rho*rho*rho*rho);
639 double t3 = OMEGA * OMEGA;
640 double t4 = 2.0 * OMEGA;
641 double z2 = rr(3) * rr(3);
642
643 // Vector of derivatives
644 // ---------------------
645 ColumnVector va(6);
646 va(1) = vv(1);
647 va(2) = vv(2);
648 va(3) = vv(3);
649 va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2);
650 va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1);
651 va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3);
652
653 return va;
654}
655
656// Compute Glonass Satellite Position
657////////////////////////////////////////////////////////////////////////////
658void t_ephGlo::position(int GPSweek, double GPSweeks, ColumnVector& xc,
659 ColumnVector& vv) const {
660
661 static const double secPerWeek = 7 * 86400.0;
662 static const double nominalStep = 10.0;
663
664 double dtPos = GPSweeks - _tt;
665 if (GPSweek != _GPSweek) {
666 dtPos += (GPSweek - _GPSweek) * secPerWeek;
667 }
668
669 int nSteps = int(fabs(dtPos) / nominalStep) + 1;
670 double step = dtPos / nSteps;
671
672 for (int ii = 1; ii <= nSteps; ii++) {
673 _xv = rungeKutta4(_tt, _xv, step, glo_deriv);
674 _tt += step;
675 }
676
677 // Position and Velocity
678 // ---------------------
679 xc(1) = _xv(1);
680 xc(2) = _xv(2);
681 xc(3) = _xv(3);
682
683 vv(1) = _xv(4);
684 vv(2) = _xv(5);
685 vv(3) = _xv(6);
686
687 // Clock Correction
688 // ----------------
689 double dtClk = GPSweeks - _GPSweeks;
690 if (GPSweek != _GPSweek) {
691 dtClk += (GPSweek - _GPSweek) * secPerWeek;
692 }
693 xc(4) = -_tau + _gamma * dtClk;
694}
695
696// Glonass IOD
697////////////////////////////////////////////////////////////////////////////
698int t_ephGlo::IOD() const {
699
700 bool old = false;
701
702 if (old) { // 5 LSBs of iod are equal to 5 LSBs of tb
703 unsigned int tb = int(fmod(_GPSweeks,86400.0)); //sec of day
704 const int shift = sizeof(tb) * 8 - 5;
705 unsigned int iod = tb << shift;
706 return (iod >> shift);
707 }
708 else {
709 return int(fmod(_tki, 3600)) / 30;
710 }
711}
Note: See TracBrowser for help on using the repository browser.