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

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

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