source: ntrip/trunk/BNC/src/bncutils.cpp@ 9088

Last change on this file since 9088 was 9088, checked in by stuerze, 22 months ago

adjusted allocation of slip and LTI according to the respective RTCM version

File size: 30.9 KB
Line 
1// Part of BNC, a utility for retrieving decoding and
2// converting GNSS data streams from NTRIP broadcasters.
3//
4// Copyright (C) 2007
5// German Federal Agency for Cartography and Geodesy (BKG)
6// http://www.bkg.bund.de
7// Czech Technical University Prague, Department of Geodesy
8// http://www.fsv.cvut.cz
9//
10// Email: euref-ip@bkg.bund.de
11//
12// This program is free software; you can redistribute it and/or
13// modify it under the terms of the GNU General Public License
14// as published by the Free Software Foundation, version 2.
15//
16// This program is distributed in the hope that it will be useful,
17// but WITHOUT ANY WARRANTY; without even the implied warranty of
18// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19// GNU General Public License for more details.
20//
21// You should have received a copy of the GNU General Public License
22// along with this program; if not, write to the Free Software
23// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
24
25/* -------------------------------------------------------------------------
26 * BKG NTRIP Client
27 * -------------------------------------------------------------------------
28 *
29 * Class: bncutils
30 *
31 * Purpose: Auxiliary Functions
32 *
33 * Author: L. Mervart
34 *
35 * Created: 30-Aug-2006
36 *
37 * Changes:
38 *
39 * -----------------------------------------------------------------------*/
40
41#include <iostream>
42#include <ctime>
43#include <math.h>
44
45#include <QRegExp>
46#include <QStringList>
47#include <QDateTime>
48
49#include <newmatap.h>
50
51#include "bncutils.h"
52#include "bnccore.h"
53
54using namespace std;
55
56struct leapseconds { /* specify the day of leap second */
57 int day; /* this is the day, where 23:59:59 exists 2 times */
58 int month; /* not the next day! */
59 int year;
60 int taicount;
61};
62static const int months[13] = {0,31,28,31,30,31,30,31,31,30,31,30,31};
63static const struct leapseconds leap[] = {
64/*{31, 12, 1971, 10},*/
65/*{30, 06, 1972, 11},*/
66/*{31, 12, 1972, 12},*/
67/*{31, 12, 1973, 13},*/
68/*{31, 12, 1974, 14},*/
69/*{31, 12, 1975, 15},*/
70/*{31, 12, 1976, 16},*/
71/*{31, 12, 1977, 17},*/
72/*{31, 12, 1978, 18},*/
73/*{31, 12, 1979, 19},*/
74{30, 06, 1981,20},
75{30, 06, 1982,21},
76{30, 06, 1983,22},
77{30, 06, 1985,23},
78{31, 12, 1987,24},
79{31, 12, 1989,25},
80{31, 12, 1990,26},
81{30, 06, 1992,27},
82{30, 06, 1993,28},
83{30, 06, 1994,29},
84{31, 12, 1995,30},
85{30, 06, 1997,31},
86{31, 12, 1998,32},
87{31, 12, 2005,33},
88{31, 12, 2008,34},
89{30, 06, 2012,35},
90{30, 06, 2015,36},
91{01, 01, 2017,37},
92{0,0,0,0} /* end marker */
93};
94
95#define GPSLEAPSTART 19 /* 19 leap seconds existed at 6.1.1980 */
96
97static int longyear(int year, int month)
98{
99 if(!(year % 4) && (!(year % 400) || (year % 100)))
100 {
101 if(!month || month == 2)
102 return 1;
103 }
104 return 0;
105}
106
107int gnumleap(int year, int month, int day)
108{
109 int ls = 0;
110 const struct leapseconds *l;
111
112 for(l = leap; l->taicount && year >= l->year; ++l)
113 {
114 if(year > l->year || month > l->month || (month == l->month && day > l->day))
115 ls = l->taicount - GPSLEAPSTART;
116 }
117 return ls;
118}
119
120/* Convert Moscow time into UTC (fixnumleap == 1) or GPS (fixnumleap == 0) */
121void updatetime(int *week, int *secOfWeek, int mSecOfWeek, bool fixnumleap)
122{
123 int y,m,d,k,l, nul;
124 unsigned int j = *week*(7*24*60*60) + *secOfWeek + 5*24*60*60+3*60*60;
125 int glo_daynumber = 0, glo_timeofday;
126 for(y = 1980; j >= (unsigned int)(k = (l = (365+longyear(y,0)))*24*60*60)
127 + gnumleap(y+1,1,1); ++y)
128 {
129 j -= k; glo_daynumber += l;
130 }
131 for(m = 1; j >= (unsigned int)(k = (l = months[m]+longyear(y, m))*24*60*60)
132 + gnumleap(y, m+1, 1); ++m)
133 {
134 j -= k; glo_daynumber += l;
135 }
136 for(d = 1; j >= 24UL*60UL*60UL + gnumleap(y, m, d+1); ++d)
137 j -= 24*60*60;
138 glo_daynumber -= 16*365+4-d;
139 nul = gnumleap(y, m, d);
140 glo_timeofday = j-nul;
141
142 // original version
143 // if(mSecOfWeek < 5*60*1000 && glo_timeofday > 23*60*60)
144 // *secOfWeek += 24*60*60;
145 // else if(glo_timeofday < 5*60 && mSecOfWeek > 23*60*60*1000)
146 // *secOfWeek -= 24*60*60;
147
148 // new version
149 if(mSecOfWeek < 4*60*60*1000 && glo_timeofday > 20*60*60)
150 *secOfWeek += 24*60*60;
151 else if(glo_timeofday < 4*60*60 && mSecOfWeek > 20*60*60*1000)
152 *secOfWeek -= 24*60*60;
153
154 *secOfWeek += mSecOfWeek/1000-glo_timeofday;
155 if(fixnumleap)
156 *secOfWeek -= nul;
157 if(*secOfWeek < 0) {*secOfWeek += 24*60*60*7; --*week; }
158 if(*secOfWeek >= 24*60*60*7) {*secOfWeek -= 24*60*60*7; ++*week; }
159}
160
161//
162////////////////////////////////////////////////////////////////////////////
163void expandEnvVar(QString& str) {
164
165 QRegExp rx("(\\$\\{.+\\})");
166
167 if (rx.indexIn(str) != -1) {
168 QStringListIterator it(rx.capturedTexts());
169 if (it.hasNext()) {
170 QString rxStr = it.next();
171 QString envVar = rxStr.mid(2,rxStr.length()-3);
172 str.replace(rxStr, qgetenv(envVar.toLatin1()));
173 }
174 }
175}
176
177// Strip White Space
178////////////////////////////////////////////////////////////////////////////
179void stripWhiteSpace(string& str) {
180 if (!str.empty()) {
181 string::size_type beg = str.find_first_not_of(" \t\f\n\r\v");
182 string::size_type end = str.find_last_not_of(" \t\f\n\r\v");
183 if (beg > str.max_size())
184 str.erase();
185 else
186 str = str.substr(beg, end-beg+1);
187 }
188}
189
190//
191////////////////////////////////////////////////////////////////////////////
192QDateTime dateAndTimeFromGPSweek(int GPSWeek, double GPSWeeks) {
193
194 static const QDate zeroEpoch(1980, 1, 6);
195
196 QDate date(zeroEpoch);
197 QTime time(0,0,0,0);
198
199 int weekDays = int(GPSWeeks) / 86400;
200 date = date.addDays( GPSWeek * 7 + weekDays );
201 time = time.addMSecs( int( (GPSWeeks - 86400 * weekDays) * 1e3 ) );
202
203 return QDateTime(date,time);
204}
205
206//
207////////////////////////////////////////////////////////////////////////////
208void currentGPSWeeks(int& week, double& sec) {
209
210 QDateTime currDateTimeGPS;
211
212 if ( BNC_CORE->dateAndTimeGPSSet() ) {
213 currDateTimeGPS = BNC_CORE->dateAndTimeGPS();
214 }
215 else {
216 currDateTimeGPS = QDateTime::currentDateTime().toUTC();
217 QDate hlp = currDateTimeGPS.date();
218 currDateTimeGPS = currDateTimeGPS.addSecs(gnumleap(hlp.year(),
219 hlp.month(), hlp.day()));
220 }
221
222 QDate currDateGPS = currDateTimeGPS.date();
223 QTime currTimeGPS = currDateTimeGPS.time();
224
225 week = int( (double(currDateGPS.toJulianDay()) - 2444244.5) / 7 );
226
227 sec = (currDateGPS.dayOfWeek() % 7) * 24.0 * 3600.0 +
228 currTimeGPS.hour() * 3600.0 +
229 currTimeGPS.minute() * 60.0 +
230 currTimeGPS.second() +
231 currTimeGPS.msec() / 1000.0;
232}
233
234//
235////////////////////////////////////////////////////////////////////////////
236QDateTime currentDateAndTimeGPS() {
237 if ( BNC_CORE->dateAndTimeGPSSet() ) {
238 return BNC_CORE->dateAndTimeGPS();
239 }
240 else {
241 int GPSWeek;
242 double GPSWeeks;
243 currentGPSWeeks(GPSWeek, GPSWeeks);
244 return dateAndTimeFromGPSweek(GPSWeek, GPSWeeks);
245 }
246}
247
248//
249////////////////////////////////////////////////////////////////////////////
250bool checkForWrongObsEpoch(bncTime obsEpoch) {
251 const double maxDt = 600.0;
252 bncTime obsTime = obsEpoch;
253 int week;
254 double sec;
255 currentGPSWeeks(week, sec);
256 bncTime currTime(week, sec);
257
258 if (((currTime - obsTime) < 0.0) ||
259 (fabs(currTime - obsTime) > maxDt)) {
260 return true;
261 }
262 return false;
263}
264//
265////////////////////////////////////////////////////////////////////////////
266QByteArray ggaString(const QByteArray& latitude,
267 const QByteArray& longitude,
268 const QByteArray& height,
269 const QString& ggaType) {
270
271 double lat = strtod(latitude,NULL);
272 double lon = strtod(longitude,NULL);
273 double hei = strtod(height,NULL);
274 QString sentences = "GPGGA,";
275 if (ggaType.contains("GNGGA")) {
276 sentences = "GNGGA,";
277 }
278
279 const char* flagN="N";
280 const char* flagE="E";
281 if (lon >180.) {lon=(lon-360.)*(-1.); flagE="W";}
282 if ((lon < 0.) && (lon >= -180.)) {lon=lon*(-1.); flagE="W";}
283 if (lon < -180.) {lon=(lon+360.); flagE="E";}
284 if (lat < 0.) {lat=lat*(-1.); flagN="S";}
285 QTime ttime(QDateTime::currentDateTime().toUTC().time());
286 int lat_deg = (int)lat;
287 double lat_min=(lat-lat_deg)*60.;
288 int lon_deg = (int)lon;
289 double lon_min=(lon-lon_deg)*60.;
290 int hh = 0 , mm = 0;
291 double ss = 0.0;
292 hh=ttime.hour();
293 mm=ttime.minute();
294 ss=(double)ttime.second()+0.001*ttime.msec();
295 QString gga;
296 gga += sentences;
297 gga += QString("%1%2%3,").arg((int)hh, 2, 10, QLatin1Char('0')).arg((int)mm, 2, 10, QLatin1Char('0')).arg((int)ss, 2, 10, QLatin1Char('0'));
298 gga += QString("%1%2,").arg((int)lat_deg,2, 10, QLatin1Char('0')).arg(lat_min, 7, 'f', 4, QLatin1Char('0'));
299 gga += flagN;
300 gga += QString(",%1%2,").arg((int)lon_deg,3, 10, QLatin1Char('0')).arg(lon_min, 7, 'f', 4, QLatin1Char('0'));
301 gga += flagE + QString(",1,05,1.00");
302 gga += QString(",%1,").arg(hei, 2, 'f', 1);
303 gga += QString("M,10.000,M,,");
304
305 unsigned char XOR = 0;
306 for (int ii = 0; ii < gga.length(); ii++) {
307 XOR ^= (unsigned char) gga[ii].toLatin1();
308 }
309 gga = "$" + gga + QString("*%1").arg(XOR, 2, 16, QLatin1Char('0')) + "\n";
310
311 return gga.toLatin1();
312}
313
314//
315////////////////////////////////////////////////////////////////////////////
316void RSW_to_XYZ(const ColumnVector& rr, const ColumnVector& vv,
317 const ColumnVector& rsw, ColumnVector& xyz) {
318
319 ColumnVector along = vv / vv.NormFrobenius();
320 ColumnVector cross = crossproduct(rr, vv); cross /= cross.NormFrobenius();
321 ColumnVector radial = crossproduct(along, cross);
322
323 Matrix RR(3,3);
324 RR.Column(1) = radial;
325 RR.Column(2) = along;
326 RR.Column(3) = cross;
327
328 xyz = RR * rsw;
329}
330
331// Transformation xyz --> radial, along track, out-of-plane
332////////////////////////////////////////////////////////////////////////////
333void XYZ_to_RSW(const ColumnVector& rr, const ColumnVector& vv,
334 const ColumnVector& xyz, ColumnVector& rsw) {
335
336 ColumnVector along = vv / vv.NormFrobenius();
337 ColumnVector cross = crossproduct(rr, vv); cross /= cross.NormFrobenius();
338 ColumnVector radial = crossproduct(along, cross);
339
340 rsw.ReSize(3);
341 rsw(1) = DotProduct(xyz, radial);
342 rsw(2) = DotProduct(xyz, along);
343 rsw(3) = DotProduct(xyz, cross);
344}
345
346// Rectangular Coordinates -> Ellipsoidal Coordinates
347////////////////////////////////////////////////////////////////////////////
348t_irc xyz2ell(const double* XYZ, double* Ell) {
349
350 const double bell = t_CST::aell*(1.0-1.0/t_CST::fInv) ;
351 const double e2 = (t_CST::aell*t_CST::aell-bell*bell)/(t_CST::aell*t_CST::aell) ;
352 const double e2c = (t_CST::aell*t_CST::aell-bell*bell)/(bell*bell) ;
353
354 double nn, ss, zps, hOld, phiOld, theta, sin3, cos3;
355
356 ss = sqrt(XYZ[0]*XYZ[0]+XYZ[1]*XYZ[1]) ;
357 zps = XYZ[2]/ss ;
358 theta = atan( (XYZ[2]*t_CST::aell) / (ss*bell) );
359 sin3 = sin(theta) * sin(theta) * sin(theta);
360 cos3 = cos(theta) * cos(theta) * cos(theta);
361
362 // Closed formula
363 Ell[0] = atan( (XYZ[2] + e2c * bell * sin3) / (ss - e2 * t_CST::aell * cos3) );
364 Ell[1] = atan2(XYZ[1],XYZ[0]) ;
365 nn = t_CST::aell/sqrt(1.0-e2*sin(Ell[0])*sin(Ell[0])) ;
366 Ell[2] = ss / cos(Ell[0]) - nn;
367
368 const int MAXITER = 100;
369 for (int ii = 1; ii <= MAXITER; ii++) {
370 nn = t_CST::aell/sqrt(1.0-e2*sin(Ell[0])*sin(Ell[0])) ;
371 hOld = Ell[2] ;
372 phiOld = Ell[0] ;
373 Ell[2] = ss/cos(Ell[0])-nn ;
374 Ell[0] = atan(zps/(1.0-e2*nn/(nn+Ell[2]))) ;
375 if ( fabs(phiOld-Ell[0]) <= 1.0e-11 && fabs(hOld-Ell[2]) <= 1.0e-5 ) {
376 return success;
377 }
378 }
379
380 return failure;
381}
382
383// Rectangular Coordinates -> North, East, Up Components
384////////////////////////////////////////////////////////////////////////////
385void xyz2neu(const double* Ell, const double* xyz, double* neu) {
386
387 double sinPhi = sin(Ell[0]);
388 double cosPhi = cos(Ell[0]);
389 double sinLam = sin(Ell[1]);
390 double cosLam = cos(Ell[1]);
391
392 neu[0] = - sinPhi*cosLam * xyz[0]
393 - sinPhi*sinLam * xyz[1]
394 + cosPhi * xyz[2];
395
396 neu[1] = - sinLam * xyz[0]
397 + cosLam * xyz[1];
398
399 neu[2] = + cosPhi*cosLam * xyz[0]
400 + cosPhi*sinLam * xyz[1]
401 + sinPhi * xyz[2];
402}
403
404// North, East, Up Components -> Rectangular Coordinates
405////////////////////////////////////////////////////////////////////////////
406void neu2xyz(const double* Ell, const double* neu, double* xyz) {
407
408 double sinPhi = sin(Ell[0]);
409 double cosPhi = cos(Ell[0]);
410 double sinLam = sin(Ell[1]);
411 double cosLam = cos(Ell[1]);
412
413 xyz[0] = - sinPhi*cosLam * neu[0]
414 - sinLam * neu[1]
415 + cosPhi*cosLam * neu[2];
416
417 xyz[1] = - sinPhi*sinLam * neu[0]
418 + cosLam * neu[1]
419 + cosPhi*sinLam * neu[2];
420
421 xyz[2] = + cosPhi * neu[0]
422 + sinPhi * neu[2];
423}
424
425// Rectangular Coordinates -> Geocentric Coordinates
426////////////////////////////////////////////////////////////////////////////
427t_irc xyz2geoc(const double* XYZ, double* Geoc) {
428
429 const double bell = t_CST::aell*(1.0-1.0/t_CST::fInv) ;
430 const double e2 = (t_CST::aell*t_CST::aell-bell*bell)/(t_CST::aell*t_CST::aell) ;
431 double Ell[3];
432 if (xyz2ell(XYZ, Ell) != success) {
433 return failure;
434 }
435 double rho = sqrt(XYZ[0]*XYZ[0]+XYZ[1]*XYZ[1]+XYZ[2]*XYZ[2]);
436 double Rn = t_CST::aell/sqrt(1-e2*pow(sin(Ell[0]),2));
437
438 Geoc[0] = atan((1-e2 * Rn/(Rn + Ell[2])) * tan(Ell[0]));
439 Geoc[1] = Ell[1];
440 Geoc[2] = rho-t_CST::rgeoc;
441
442 return success;
443}
444
445//
446////////////////////////////////////////////////////////////////////////////
447double Frac (double x) {
448 return x-floor(x);
449}
450
451//
452////////////////////////////////////////////////////////////////////////////
453double Modulo (double x, double y) {
454 return y*Frac(x/y);
455}
456
457// Round to nearest integer
458////////////////////////////////////////////////////////////////////////////
459double nint(double val) {
460 return ((val < 0.0) ? -floor(fabs(val)+0.5) : floor(val+0.5));
461}
462
463//
464////////////////////////////////////////////////////////////////////////////
465double factorial(int n) {
466 if (n == 0) {
467 return 1;
468 }
469 else {
470 return (n * factorial(n - 1));
471 }
472}
473
474//
475////////////////////////////////////////////////////////////////////////////
476double associatedLegendreFunction(int n, int m, double t) {
477 double sum = 0.0;
478 int r = (int) floor((n - m) / 2);
479 for (int k = 0; k <= r; k++) {
480 sum += (pow(-1.0, (double)k) * factorial(2*n - 2*k)
481 / (factorial(k) * factorial(n-k) * factorial(n-m-2*k))
482 * pow(t, (double)n-m-2*k));
483 }
484 double fac = pow(2.0,(double) -n) * pow((1 - t*t), (double)m/2);
485 return sum *= fac;
486}
487
488
489// Jacobian XYZ --> NEU
490////////////////////////////////////////////////////////////////////////////
491void jacobiXYZ_NEU(const double* Ell, Matrix& jacobi) {
492
493 Tracer tracer("jacobiXYZ_NEU");
494
495 double sinPhi = sin(Ell[0]);
496 double cosPhi = cos(Ell[0]);
497 double sinLam = sin(Ell[1]);
498 double cosLam = cos(Ell[1]);
499
500 jacobi(1,1) = - sinPhi * cosLam;
501 jacobi(1,2) = - sinPhi * sinLam;
502 jacobi(1,3) = cosPhi;
503
504 jacobi(2,1) = - sinLam;
505 jacobi(2,2) = cosLam;
506 jacobi(2,3) = 0.0;
507
508 jacobi(3,1) = cosPhi * cosLam;
509 jacobi(3,2) = cosPhi * sinLam;
510 jacobi(3,3) = sinPhi;
511}
512
513// Jacobian Ell --> XYZ
514////////////////////////////////////////////////////////////////////////////
515void jacobiEll_XYZ(const double* Ell, Matrix& jacobi) {
516
517 Tracer tracer("jacobiEll_XYZ");
518
519 double sinPhi = sin(Ell[0]);
520 double cosPhi = cos(Ell[0]);
521 double sinLam = sin(Ell[1]);
522 double cosLam = cos(Ell[1]);
523 double hh = Ell[2];
524
525 double bell = t_CST::aell*(1.0-1.0/t_CST::fInv);
526 double e2 = (t_CST::aell*t_CST::aell-bell*bell)/(t_CST::aell*t_CST::aell) ;
527 double nn = t_CST::aell/sqrt(1.0-e2*sinPhi*sinPhi) ;
528
529 jacobi(1,1) = -(nn+hh) * sinPhi * cosLam;
530 jacobi(1,2) = -(nn+hh) * cosPhi * sinLam;
531 jacobi(1,3) = cosPhi * cosLam;
532
533 jacobi(2,1) = -(nn+hh) * sinPhi * sinLam;
534 jacobi(2,2) = (nn+hh) * cosPhi * cosLam;
535 jacobi(2,3) = cosPhi * sinLam;
536
537 jacobi(3,1) = (nn*(1.0-e2)+hh) * cosPhi;
538 jacobi(3,2) = 0.0;
539 jacobi(3,3) = sinPhi;
540}
541
542// Covariance Matrix in NEU
543////////////////////////////////////////////////////////////////////////////
544void covariXYZ_NEU(const SymmetricMatrix& QQxyz, const double* Ell,
545 SymmetricMatrix& Qneu) {
546
547 Tracer tracer("covariXYZ_NEU");
548
549 Matrix CC(3,3);
550 jacobiXYZ_NEU(Ell, CC);
551 Qneu << CC * QQxyz * CC.t();
552}
553
554// Covariance Matrix in XYZ
555////////////////////////////////////////////////////////////////////////////
556void covariNEU_XYZ(const SymmetricMatrix& QQneu, const double* Ell,
557 SymmetricMatrix& Qxyz) {
558
559 Tracer tracer("covariNEU_XYZ");
560
561 Matrix CC(3,3);
562 jacobiXYZ_NEU(Ell, CC);
563 Qxyz << CC.t() * QQneu * CC;
564}
565
566// Fourth order Runge-Kutta numerical integrator for ODEs
567////////////////////////////////////////////////////////////////////////////
568ColumnVector rungeKutta4(
569 double xi, // the initial x-value
570 const ColumnVector& yi, // vector of the initial y-values
571 double dx, // the step size for the integration
572 double* acc, // additional acceleration
573 ColumnVector (*der)(double x, const ColumnVector& y, double* acc)
574 // A pointer to a function that computes the
575 // derivative of a function at a point (x,y)
576 ) {
577
578 ColumnVector k1 = der(xi , yi , acc) * dx;
579 ColumnVector k2 = der(xi+dx/2.0, yi+k1/2.0, acc) * dx;
580 ColumnVector k3 = der(xi+dx/2.0, yi+k2/2.0, acc) * dx;
581 ColumnVector k4 = der(xi+dx , yi+k3 , acc) * dx;
582
583 ColumnVector yf = yi + k1/6.0 + k2/3.0 + k3/3.0 + k4/6.0;
584
585 return yf;
586}
587//
588////////////////////////////////////////////////////////////////////////////
589double djul(long jj, long mm, double tt) {
590 long ii, kk;
591 double djul ;
592 if( mm <= 2 ) {
593 jj = jj - 1;
594 mm = mm + 12;
595 }
596 ii = jj/100;
597 kk = 2 - ii + ii/4;
598 djul = (365.25*jj - fmod( 365.25*jj, 1.0 )) - 679006.0;
599 djul = djul + floor( 30.6001*(mm + 1) ) + tt + kk;
600 return djul;
601}
602
603//
604////////////////////////////////////////////////////////////////////////////
605double gpjd(double second, int nweek) {
606 double deltat;
607 deltat = nweek*7.0 + second/86400.0 ;
608 return( 44244.0 + deltat) ;
609}
610
611//
612////////////////////////////////////////////////////////////////////////////
613void jdgp(double tjul, double & second, long & nweek) {
614 double deltat;
615 deltat = tjul - 44244.0 ;
616 nweek = (long) floor(deltat/7.0);
617 second = (deltat - (nweek)*7.0)*86400.0;
618}
619
620//
621////////////////////////////////////////////////////////////////////////////
622void jmt(double djul, long& jj, long& mm, double& dd) {
623 long ih, ih1, ih2 ;
624 double t1, t2, t3, t4;
625 t1 = 1.0 + djul - fmod( djul, 1.0 ) + 2400000.0;
626 t4 = fmod( djul, 1.0 );
627 ih = long( (t1 - 1867216.25)/36524.25 );
628 t2 = t1 + 1 + ih - ih/4;
629 t3 = t2 - 1720995.0;
630 ih1 = long( (t3 - 122.1)/365.25 );
631 t1 = 365.25*ih1 - fmod( 365.25*ih1, 1.0 );
632 ih2 = long( (t3 - t1)/30.6001 );
633 dd = t3 - t1 - (int)( 30.6001*ih2 ) + t4;
634 mm = ih2 - 1;
635 if ( ih2 > 13 ) mm = ih2 - 13;
636 jj = ih1;
637 if ( mm <= 2 ) jj = jj + 1;
638}
639
640//
641////////////////////////////////////////////////////////////////////////////
642void GPSweekFromDateAndTime(const QDateTime& dateTime,
643 int& GPSWeek, double& GPSWeeks) {
644
645 static const QDateTime zeroEpoch(QDate(1980, 1, 6),QTime(),Qt::UTC);
646
647 GPSWeek = zeroEpoch.daysTo(dateTime) / 7;
648
649 int weekDay = dateTime.date().dayOfWeek() + 1; // Qt: Monday = 1
650 if (weekDay > 7) weekDay = 1;
651
652 GPSWeeks = (weekDay - 1) * 86400.0
653 - dateTime.time().msecsTo(QTime()) / 1e3;
654}
655
656//
657////////////////////////////////////////////////////////////////////////////
658void GPSweekFromYMDhms(int year, int month, int day, int hour, int min,
659 double sec, int& GPSWeek, double& GPSWeeks) {
660
661 double mjd = djul(year, month, day);
662
663 long GPSWeek_long;
664 jdgp(mjd, GPSWeeks, GPSWeek_long);
665 GPSWeek = GPSWeek_long;
666 GPSWeeks += hour * 3600.0 + min * 60.0 + sec;
667}
668
669//
670////////////////////////////////////////////////////////////////////////////
671void mjdFromDateAndTime(const QDateTime& dateTime, int& mjd, double& dayfrac) {
672
673 static const QDate zeroDate(1858, 11, 17);
674
675 mjd = zeroDate.daysTo(dateTime.date());
676
677 dayfrac = (dateTime.time().hour() +
678 (dateTime.time().minute() +
679 (dateTime.time().second() +
680 dateTime.time().msec() / 1000.0) / 60.0) / 60.0) / 24.0;
681}
682
683//
684////////////////////////////////////////////////////////////////////////////
685bool findInVector(const vector<QString>& vv, const QString& str) {
686 std::vector<QString>::const_iterator it;
687 for (it = vv.begin(); it != vv.end(); ++it) {
688 if ( (*it) == str) {
689 return true;
690 }
691 }
692 return false;
693}
694
695//
696////////////////////////////////////////////////////////////////////////////
697int readInt(const QString& str, int pos, int len, int& value) {
698 bool ok;
699 value = str.mid(pos, len).toInt(&ok);
700 return ok ? 0 : 1;
701}
702
703//
704////////////////////////////////////////////////////////////////////////////
705int readDbl(const QString& str, int pos, int len, double& value) {
706 QString hlp = str.mid(pos, len);
707 for (int ii = 0; ii < hlp.length(); ii++) {
708 if (hlp[ii]=='D' || hlp[ii]=='d' || hlp[ii] == 'E') {
709 hlp[ii]='e';
710 }
711 }
712 bool ok;
713 value = hlp.toDouble(&ok);
714 return ok ? 0 : 1;
715}
716
717// Topocentrical Distance and Elevation
718////////////////////////////////////////////////////////////////////////////
719void topos(double xRec, double yRec, double zRec,
720 double xSat, double ySat, double zSat,
721 double& rho, double& eleSat, double& azSat) {
722
723 double dx[3];
724 dx[0] = xSat-xRec;
725 dx[1] = ySat-yRec;
726 dx[2] = zSat-zRec;
727
728 rho = sqrt( dx[0]*dx[0] + dx[1]*dx[1] + dx[2]*dx[2] );
729
730 double xyzRec[3];
731 xyzRec[0] = xRec;
732 xyzRec[1] = yRec;
733 xyzRec[2] = zRec;
734
735 double Ell[3];
736 double neu[3];
737 xyz2ell(xyzRec, Ell);
738 xyz2neu(Ell, dx, neu);
739
740 eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
741 if (neu[2] < 0) {
742 eleSat *= -1.0;
743 }
744
745 azSat = atan2(neu[1], neu[0]);
746}
747
748// Degrees -> degrees, minutes, seconds
749////////////////////////////////////////////////////////////////////////////
750void deg2DMS(double decDeg, int& deg, int& min, double& sec) {
751 int sgn = (decDeg < 0.0 ? -1 : 1);
752 deg = static_cast<int>(decDeg);
753 min = sgn * static_cast<int>((decDeg - deg)*60);
754 sec = (sgn* (decDeg - deg) - min/60.0) * 3600.0;
755}
756
757//
758////////////////////////////////////////////////////////////////////////////
759QString fortranFormat(double value, int width, int prec) {
760 int expo = value == 0.0 ? 0 : int(log10(fabs(value)));
761 double mant = value == 0.0 ? 0 : value / pow(10.0, double(expo));
762 if (fabs(mant) >= 1.0) {
763 mant /= 10.0;
764 expo += 1;
765 }
766 if (expo >= 0) {
767 return QString("%1e+%2").arg(mant, width-4, 'f', prec).arg(expo, 2, 10, QChar('0'));
768 }
769 else {
770 return QString("%1e-%2").arg(mant, width-4, 'f', prec).arg(-expo, 2, 10, QChar('0'));
771 }
772}
773
774//
775//////////////////////////////////////////////////////////////////////////////
776void kalman(const Matrix& AA, const ColumnVector& ll, const DiagonalMatrix& PP,
777 SymmetricMatrix& QQ, ColumnVector& xx) {
778
779 Tracer tracer("kalman");
780
781 int nPar = AA.Ncols();
782 int nObs = AA.Nrows();
783 UpperTriangularMatrix SS = Cholesky(QQ).t();
784
785 Matrix SA = SS*AA.t();
786 Matrix SRF(nObs+nPar, nObs+nPar); SRF = 0;
787 for (int ii = 1; ii <= nObs; ++ii) {
788 SRF(ii,ii) = 1.0 / sqrt(PP(ii,ii));
789 }
790
791 SRF.SubMatrix (nObs+1, nObs+nPar, 1, nObs) = SA;
792 SRF.SymSubMatrix(nObs+1, nObs+nPar) = SS;
793
794 UpperTriangularMatrix UU;
795 QRZ(SRF, UU);
796
797 SS = UU.SymSubMatrix(nObs+1, nObs+nPar);
798 UpperTriangularMatrix SH_rt = UU.SymSubMatrix(1, nObs);
799 Matrix YY = UU.SubMatrix(1, nObs, nObs+1, nObs+nPar);
800
801 UpperTriangularMatrix SHi = SH_rt.i();
802
803 Matrix KT = SHi * YY;
804 SymmetricMatrix Hi; Hi << SHi * SHi.t();
805
806 xx += KT.t() * (ll - AA * xx);
807 QQ << (SS.t() * SS);
808}
809
810//
811////////////////////////////////////////////////////////////////////////////
812double accuracyFromIndex(int index, t_eph::e_type type) {
813double accuracy = -1.0;
814
815 if (type == t_eph::GPS ||
816 type == t_eph::BDS ||
817 type == t_eph::SBAS||
818 type == t_eph::QZSS) {
819 if ((index >= 0) && (index <= 6)) {
820 if (index == 3) {
821 accuracy = ceil(10.0 * pow(2.0, (double(index) / 2.0) + 1.0)) / 10.0;
822 }
823 else {
824 accuracy = floor(10.0 * pow(2.0, (double(index) / 2.0) + 1.0)) / 10.0;
825 }
826 }
827 else if ((index > 6) && (index <= 15)) {
828 accuracy = (10.0 * pow(2.0, (double(index) - 2.0))) / 10.0;
829 }
830 else {
831 accuracy = 8192.0;
832 }
833 }
834 else if (type == t_eph::Galileo) {
835 if ((index >= 0) && (index <= 49)) {
836 accuracy = (double(index) / 100.0);
837 }
838 else if ((index > 49) && (index <= 74)) {
839 accuracy = (50.0 + (double(index) - 50.0) * 2.0) / 100.0;
840 }
841 else if ((index > 74) && (index <= 99)) {
842 accuracy = 1.0 + (double(index) - 75.0) * 0.04;
843 }
844 else if ((index > 99) && (index <= 125)) {
845 accuracy = 2.0 + (double(index) - 100.0) * 0.16;
846 }
847 else {
848 accuracy = -1.0;
849 }
850 }
851 else if (type == t_eph::IRNSS) {
852 if ((index >= 0) && (index <= 6)) {
853 if (index == 1) {
854 accuracy = 2.8;
855 }
856 else if (index == 3) {
857 accuracy = 5.7;
858 }
859 else if (index == 5) {
860 accuracy = 11.3;
861 }
862 else {
863 accuracy = pow(2, 1 + index / 2);
864 }
865 }
866 else if ((index > 6) && (index <= 15)) {
867 accuracy = pow(2, index - 2);
868 }
869 }
870 return accuracy;
871}
872
873//
874////////////////////////////////////////////////////////////////////////////
875int indexFromAccuracy(double accuracy, t_eph::e_type type) {
876
877 if (type == t_eph::GPS || type == t_eph::BDS || type == t_eph::SBAS
878 || type == t_eph::QZSS) {
879
880 if (accuracy <= 2.40) {
881 return 0;
882 }
883 else if (accuracy <= 3.40) {
884 return 1;
885 }
886 else if (accuracy <= 4.85) {
887 return 2;
888 }
889 else if (accuracy <= 6.85) {
890 return 3;
891 }
892 else if (accuracy <= 9.65) {
893 return 4;
894 }
895 else if (accuracy <= 13.65) {
896 return 5;
897 }
898 else if (accuracy <= 24.00) {
899 return 6;
900 }
901 else if (accuracy <= 48.00) {
902 return 7;
903 }
904 else if (accuracy <= 96.00) {
905 return 8;
906 }
907 else if (accuracy <= 192.00) {
908 return 9;
909 }
910 else if (accuracy <= 384.00) {
911 return 10;
912 }
913 else if (accuracy <= 768.00) {
914 return 11;
915 }
916 else if (accuracy <= 1536.00) {
917 return 12;
918 }
919 else if (accuracy <= 3072.00) {
920 return 13;
921 }
922 else if (accuracy <= 6144.00) {
923 return 14;
924 }
925 else {
926 return 15;
927 }
928 }
929
930 if (type == t_eph::Galileo) {
931
932 if (accuracy <= 0.49) {
933 return int(ceil(accuracy * 100.0));
934 }
935 else if (accuracy <= 0.98) {
936 return int(50.0 + (((accuracy * 100.0) - 50) / 2.0));
937 }
938 else if (accuracy <= 2.0) {
939 return int(75.0 + ((accuracy - 1.0) / 0.04));
940 }
941 else if (accuracy <= 6.0) {
942 return int(100.0 + ((accuracy - 2.0) / 0.16));
943 }
944 else {
945 return 255;
946 }
947 }
948
949 return (type == t_eph::Galileo) ? 255 : 15;
950}
951
952// Returns fit interval in hours from flag
953////////////////////////////////////////////////////////////////////////////
954double fitIntervalFromFlag(int flag, double iodc, t_eph::e_type type) {
955 double fitInterval = 0.0;
956
957 switch (flag) {
958 case 0:
959 if (type == t_eph::GPS) {
960 fitInterval = 4.0;
961 }
962 else if (type == t_eph::QZSS) {
963 fitInterval = 2.0;
964 }
965 break;
966 case 1:
967 if (type == t_eph::GPS) {
968 if (iodc >= 240 && iodc <= 247) {
969 fitInterval = 8.0;
970 }
971 else if ((iodc >= 248 && iodc <= 255) ||
972 (iodc == 496) ) {
973 fitInterval = 14.0;
974 }
975 else if ((iodc >= 497 && iodc <= 503) ||
976 (iodc >= 2021 && iodc <= 1023) ) {
977 fitInterval = 26.0;
978 }
979 else {
980 fitInterval = 6.0;
981 }
982 }
983 break;
984 }
985 return fitInterval;
986}
987
988// Returns CRC24
989////////////////////////////////////////////////////////////////////////////
990unsigned long CRC24(long size, const unsigned char *buf) {
991 unsigned long crc = 0;
992 int ii;
993 while (size--) {
994 crc ^= (*buf++) << (16);
995 for(ii = 0; ii < 8; ii++) {
996 crc <<= 1;
997 if (crc & 0x1000000) {
998 crc ^= 0x01864cfb;
999 }
1000 }
1001 }
1002 return crc;
1003}
1004
1005// Convert RTCM3 lock-time indicator to minimum lock time in seconds
1006////////////////////////////////////////////////////////////////////////////
1007double lti2sec(int type, int lti) {
1008
1009 if ( (type>=1001 && type<=1004) ||
1010 (type>=1009 && type<=1012) ) { // RTCM3 msg 100[1...4] and 10[09...12]
1011 if (lti< 0) return -1;
1012 else if (lti< 24) return 1*lti; // [ 0 1 23]
1013 else if (lti< 48) return 2*lti-24; // [ 24 2 70]
1014 else if (lti< 72) return 4*lti-120; // [ 72 4 164]
1015 else if (lti< 96) return 8*lti-408; // [168 8 352]
1016 else if (lti< 120) return 16*lti-1176; // [360 16 728]
1017 else if (lti< 127) return 32*lti-3096; // [744 32 905]
1018 else if (lti==127) return 937;
1019 else return -1.0;
1020 }
1021 else if (type%10==2 || type%10==3 ||
1022 type%10==4 || type%10==5) { // RTCM3 MSM-2/-3/-4/-5
1023 switch(lti) {
1024 case( 0) : return 0;
1025 case( 1) : return 32e-3;
1026 case( 2) : return 64e-3;
1027 case( 3) : return 128e-3;
1028 case( 4) : return 256e-3;
1029 case( 5) : return 512e-3;
1030 case( 6) : return 1024e-3;
1031 case( 7) : return 2048e-3;
1032 case( 8) : return 4096e-3;
1033 case( 9) : return 8192e-3;
1034 case(10) : return 16384e-3;
1035 case(11) : return 32768e-3;
1036 case(12) : return 65536e-3;
1037 case(13) : return 131072e-3;
1038 case(14) : return 262144e-3;
1039 case(15) : return 524288e-3;
1040 default : return -1.0;
1041 };
1042 }
1043 else if (type%10==6 || type%10==7) { // RTCM3 MSM-6 and MSM-7
1044 if (lti< 0) return ( -1 );
1045 else if (lti< 64) return ( 1*lti )*1e-3;
1046 else if (lti< 96) return ( 2*lti-64 )*1e-3;
1047 else if (lti< 128) return ( 4*lti-256 )*1e-3;
1048 else if (lti< 160) return ( 8*lti-768 )*1e-3;
1049 else if (lti< 192) return ( 16*lti-2048 )*1e-3;
1050 else if (lti< 224) return ( 32*lti-5120 )*1e-3;
1051 else if (lti< 256) return ( 64*lti-12288 )*1e-3;
1052 else if (lti< 288) return ( 128*lti-28672 )*1e-3;
1053 else if (lti< 320) return ( 256*lti-65536 )*1e-3;
1054 else if (lti< 352) return ( 512*lti-147456 )*1e-3;
1055 else if (lti< 384) return ( 1024*lti-327680 )*1e-3;
1056 else if (lti< 416) return ( 2048*lti-720896 )*1e-3;
1057 else if (lti< 448) return ( 4096*lti-1572864 )*1e-3;
1058 else if (lti< 480) return ( 8192*lti-3407872 )*1e-3;
1059 else if (lti< 512) return ( 16384*lti-7340032 )*1e-3;
1060 else if (lti< 544) return ( 32768*lti-15728640 )*1e-3;
1061 else if (lti< 576) return ( 65536*lti-33554432 )*1e-3;
1062 else if (lti< 608) return ( 131072*lti-71303168 )*1e-3;
1063 else if (lti< 640) return ( 262144*lti-150994944 )*1e-3;
1064 else if (lti< 672) return ( 524288*lti-318767104 )*1e-3;
1065 else if (lti< 704) return (1048576*lti-671088640 )*1e-3;
1066 else if (lti==704) return (2097152*lti-1409286144)*1e-3;
1067 else return ( -1.0 );
1068 }
1069 else {
1070 return -1.0;
1071 };
1072};
Note: See TracBrowser for help on using the repository browser.