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