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

Last change on this file since 9245 was 9245, checked in by stuerze, 20 months ago

another check is added, to prevent the usage of not updated nav data sets during ssr upload

File size: 32.0 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//
266////////////////////////////////////////////////////////////////////////////
267bool outDatedBcep(const t_eph *eph) {
268 bncTime toc = eph->TOC();
269 QDateTime now = currentDateAndTimeGPS();
270 bncTime currentTime(now.toString(Qt::ISODate).toStdString());
271 double dt = currentTime - toc;
272
273 // update interval: 2h, data sets are valid for 4 hours
274 if (eph->type() == t_eph::GPS && dt > 4*3600.0) {
275 return true;
276 }
277 // update interval: 3h, data sets are valid for 4 hours
278 else if (eph->type() == t_eph::Galileo && dt > 4*3600.0) {
279 return true;
280 }
281 // updated every 30 minutes
282 else if (eph->type() == t_eph::GLONASS && dt > 1*3600.0) {
283 return true;
284 }
285 // orbit parameters are valid for 7200 seconds (minimum)
286 else if (eph->type() == t_eph::QZSS && dt > 2*3600.0) {
287 return true;
288 }
289 // maximum update interval: 300 sec
290 else if (eph->type() == t_eph::SBAS && dt > 600.0) {
291 return true;
292 }
293 // updates 1h (GEO) up to 6 hours non-GEO
294 else if (eph->type() == t_eph::BDS && dt > 6*3600.0) {
295 return true;
296 }
297 // update interval: up to 24 hours
298 else if (eph->type() == t_eph::IRNSS && dt > 24*3600.0) {
299 return true;
300 }
301 return false;
302}
303
304//
305////////////////////////////////////////////////////////////////////////////
306QByteArray ggaString(const QByteArray& latitude,
307 const QByteArray& longitude,
308 const QByteArray& height,
309 const QString& ggaType) {
310
311 double lat = strtod(latitude,NULL);
312 double lon = strtod(longitude,NULL);
313 double hei = strtod(height,NULL);
314 QString sentences = "GPGGA,";
315 if (ggaType.contains("GNGGA")) {
316 sentences = "GNGGA,";
317 }
318
319 const char* flagN="N";
320 const char* flagE="E";
321 if (lon >180.) {lon=(lon-360.)*(-1.); flagE="W";}
322 if ((lon < 0.) && (lon >= -180.)) {lon=lon*(-1.); flagE="W";}
323 if (lon < -180.) {lon=(lon+360.); flagE="E";}
324 if (lat < 0.) {lat=lat*(-1.); flagN="S";}
325 QTime ttime(QDateTime::currentDateTime().toUTC().time());
326 int lat_deg = (int)lat;
327 double lat_min=(lat-lat_deg)*60.;
328 int lon_deg = (int)lon;
329 double lon_min=(lon-lon_deg)*60.;
330 int hh = 0 , mm = 0;
331 double ss = 0.0;
332 hh=ttime.hour();
333 mm=ttime.minute();
334 ss=(double)ttime.second()+0.001*ttime.msec();
335 QString gga;
336 gga += sentences;
337 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'));
338 gga += QString("%1%2,").arg((int)lat_deg,2, 10, QLatin1Char('0')).arg(lat_min, 7, 'f', 4, QLatin1Char('0'));
339 gga += flagN;
340 gga += QString(",%1%2,").arg((int)lon_deg,3, 10, QLatin1Char('0')).arg(lon_min, 7, 'f', 4, QLatin1Char('0'));
341 gga += flagE + QString(",1,05,1.00");
342 gga += QString(",%1,").arg(hei, 2, 'f', 1);
343 gga += QString("M,10.000,M,,");
344
345 unsigned char XOR = 0;
346 for (int ii = 0; ii < gga.length(); ii++) {
347 XOR ^= (unsigned char) gga[ii].toLatin1();
348 }
349 gga = "$" + gga + QString("*%1").arg(XOR, 2, 16, QLatin1Char('0')) + "\n";
350
351 return gga.toLatin1();
352}
353
354//
355////////////////////////////////////////////////////////////////////////////
356void RSW_to_XYZ(const ColumnVector& rr, const ColumnVector& vv,
357 const ColumnVector& rsw, ColumnVector& xyz) {
358
359 ColumnVector along = vv / vv.NormFrobenius();
360 ColumnVector cross = crossproduct(rr, vv); cross /= cross.NormFrobenius();
361 ColumnVector radial = crossproduct(along, cross);
362
363 Matrix RR(3,3);
364 RR.Column(1) = radial;
365 RR.Column(2) = along;
366 RR.Column(3) = cross;
367
368 xyz = RR * rsw;
369}
370
371// Transformation xyz --> radial, along track, out-of-plane
372////////////////////////////////////////////////////////////////////////////
373void XYZ_to_RSW(const ColumnVector& rr, const ColumnVector& vv,
374 const ColumnVector& xyz, ColumnVector& rsw) {
375
376 ColumnVector along = vv / vv.NormFrobenius();
377 ColumnVector cross = crossproduct(rr, vv); cross /= cross.NormFrobenius();
378 ColumnVector radial = crossproduct(along, cross);
379
380 rsw.ReSize(3);
381 rsw(1) = DotProduct(xyz, radial);
382 rsw(2) = DotProduct(xyz, along);
383 rsw(3) = DotProduct(xyz, cross);
384}
385
386// Rectangular Coordinates -> Ellipsoidal Coordinates
387////////////////////////////////////////////////////////////////////////////
388t_irc xyz2ell(const double* XYZ, double* Ell) {
389
390 const double bell = t_CST::aell*(1.0-1.0/t_CST::fInv) ;
391 const double e2 = (t_CST::aell*t_CST::aell-bell*bell)/(t_CST::aell*t_CST::aell) ;
392 const double e2c = (t_CST::aell*t_CST::aell-bell*bell)/(bell*bell) ;
393
394 double nn, ss, zps, hOld, phiOld, theta, sin3, cos3;
395
396 ss = sqrt(XYZ[0]*XYZ[0]+XYZ[1]*XYZ[1]) ;
397 zps = XYZ[2]/ss ;
398 theta = atan( (XYZ[2]*t_CST::aell) / (ss*bell) );
399 sin3 = sin(theta) * sin(theta) * sin(theta);
400 cos3 = cos(theta) * cos(theta) * cos(theta);
401
402 // Closed formula
403 Ell[0] = atan( (XYZ[2] + e2c * bell * sin3) / (ss - e2 * t_CST::aell * cos3) );
404 Ell[1] = atan2(XYZ[1],XYZ[0]) ;
405 nn = t_CST::aell/sqrt(1.0-e2*sin(Ell[0])*sin(Ell[0])) ;
406 Ell[2] = ss / cos(Ell[0]) - nn;
407
408 const int MAXITER = 100;
409 for (int ii = 1; ii <= MAXITER; ii++) {
410 nn = t_CST::aell/sqrt(1.0-e2*sin(Ell[0])*sin(Ell[0])) ;
411 hOld = Ell[2] ;
412 phiOld = Ell[0] ;
413 Ell[2] = ss/cos(Ell[0])-nn ;
414 Ell[0] = atan(zps/(1.0-e2*nn/(nn+Ell[2]))) ;
415 if ( fabs(phiOld-Ell[0]) <= 1.0e-11 && fabs(hOld-Ell[2]) <= 1.0e-5 ) {
416 return success;
417 }
418 }
419
420 return failure;
421}
422
423// Rectangular Coordinates -> North, East, Up Components
424////////////////////////////////////////////////////////////////////////////
425void xyz2neu(const double* Ell, const double* xyz, double* neu) {
426
427 double sinPhi = sin(Ell[0]);
428 double cosPhi = cos(Ell[0]);
429 double sinLam = sin(Ell[1]);
430 double cosLam = cos(Ell[1]);
431
432 neu[0] = - sinPhi*cosLam * xyz[0]
433 - sinPhi*sinLam * xyz[1]
434 + cosPhi * xyz[2];
435
436 neu[1] = - sinLam * xyz[0]
437 + cosLam * xyz[1];
438
439 neu[2] = + cosPhi*cosLam * xyz[0]
440 + cosPhi*sinLam * xyz[1]
441 + sinPhi * xyz[2];
442}
443
444// North, East, Up Components -> Rectangular Coordinates
445////////////////////////////////////////////////////////////////////////////
446void neu2xyz(const double* Ell, const double* neu, double* xyz) {
447
448 double sinPhi = sin(Ell[0]);
449 double cosPhi = cos(Ell[0]);
450 double sinLam = sin(Ell[1]);
451 double cosLam = cos(Ell[1]);
452
453 xyz[0] = - sinPhi*cosLam * neu[0]
454 - sinLam * neu[1]
455 + cosPhi*cosLam * neu[2];
456
457 xyz[1] = - sinPhi*sinLam * neu[0]
458 + cosLam * neu[1]
459 + cosPhi*sinLam * neu[2];
460
461 xyz[2] = + cosPhi * neu[0]
462 + sinPhi * neu[2];
463}
464
465// Rectangular Coordinates -> Geocentric Coordinates
466////////////////////////////////////////////////////////////////////////////
467t_irc xyz2geoc(const double* XYZ, double* Geoc) {
468
469 const double bell = t_CST::aell*(1.0-1.0/t_CST::fInv) ;
470 const double e2 = (t_CST::aell*t_CST::aell-bell*bell)/(t_CST::aell*t_CST::aell) ;
471 double Ell[3];
472 if (xyz2ell(XYZ, Ell) != success) {
473 return failure;
474 }
475 double rho = sqrt(XYZ[0]*XYZ[0]+XYZ[1]*XYZ[1]+XYZ[2]*XYZ[2]);
476 double Rn = t_CST::aell/sqrt(1-e2*pow(sin(Ell[0]),2));
477
478 Geoc[0] = atan((1-e2 * Rn/(Rn + Ell[2])) * tan(Ell[0]));
479 Geoc[1] = Ell[1];
480 Geoc[2] = rho-t_CST::rgeoc;
481
482 return success;
483}
484
485//
486////////////////////////////////////////////////////////////////////////////
487double Frac (double x) {
488 return x-floor(x);
489}
490
491//
492////////////////////////////////////////////////////////////////////////////
493double Modulo (double x, double y) {
494 return y*Frac(x/y);
495}
496
497// Round to nearest integer
498////////////////////////////////////////////////////////////////////////////
499double nint(double val) {
500 return ((val < 0.0) ? -floor(fabs(val)+0.5) : floor(val+0.5));
501}
502
503//
504////////////////////////////////////////////////////////////////////////////
505double factorial(int n) {
506 if (n == 0) {
507 return 1;
508 }
509 else {
510 return (n * factorial(n - 1));
511 }
512}
513
514//
515////////////////////////////////////////////////////////////////////////////
516double associatedLegendreFunction(int n, int m, double t) {
517 double sum = 0.0;
518 int r = (int) floor((n - m) / 2);
519 for (int k = 0; k <= r; k++) {
520 sum += (pow(-1.0, (double)k) * factorial(2*n - 2*k)
521 / (factorial(k) * factorial(n-k) * factorial(n-m-2*k))
522 * pow(t, (double)n-m-2*k));
523 }
524 double fac = pow(2.0,(double) -n) * pow((1 - t*t), (double)m/2);
525 return sum *= fac;
526}
527
528
529// Jacobian XYZ --> NEU
530////////////////////////////////////////////////////////////////////////////
531void jacobiXYZ_NEU(const double* Ell, Matrix& jacobi) {
532
533 Tracer tracer("jacobiXYZ_NEU");
534
535 double sinPhi = sin(Ell[0]);
536 double cosPhi = cos(Ell[0]);
537 double sinLam = sin(Ell[1]);
538 double cosLam = cos(Ell[1]);
539
540 jacobi(1,1) = - sinPhi * cosLam;
541 jacobi(1,2) = - sinPhi * sinLam;
542 jacobi(1,3) = cosPhi;
543
544 jacobi(2,1) = - sinLam;
545 jacobi(2,2) = cosLam;
546 jacobi(2,3) = 0.0;
547
548 jacobi(3,1) = cosPhi * cosLam;
549 jacobi(3,2) = cosPhi * sinLam;
550 jacobi(3,3) = sinPhi;
551}
552
553// Jacobian Ell --> XYZ
554////////////////////////////////////////////////////////////////////////////
555void jacobiEll_XYZ(const double* Ell, Matrix& jacobi) {
556
557 Tracer tracer("jacobiEll_XYZ");
558
559 double sinPhi = sin(Ell[0]);
560 double cosPhi = cos(Ell[0]);
561 double sinLam = sin(Ell[1]);
562 double cosLam = cos(Ell[1]);
563 double hh = Ell[2];
564
565 double bell = t_CST::aell*(1.0-1.0/t_CST::fInv);
566 double e2 = (t_CST::aell*t_CST::aell-bell*bell)/(t_CST::aell*t_CST::aell) ;
567 double nn = t_CST::aell/sqrt(1.0-e2*sinPhi*sinPhi) ;
568
569 jacobi(1,1) = -(nn+hh) * sinPhi * cosLam;
570 jacobi(1,2) = -(nn+hh) * cosPhi * sinLam;
571 jacobi(1,3) = cosPhi * cosLam;
572
573 jacobi(2,1) = -(nn+hh) * sinPhi * sinLam;
574 jacobi(2,2) = (nn+hh) * cosPhi * cosLam;
575 jacobi(2,3) = cosPhi * sinLam;
576
577 jacobi(3,1) = (nn*(1.0-e2)+hh) * cosPhi;
578 jacobi(3,2) = 0.0;
579 jacobi(3,3) = sinPhi;
580}
581
582// Covariance Matrix in NEU
583////////////////////////////////////////////////////////////////////////////
584void covariXYZ_NEU(const SymmetricMatrix& QQxyz, const double* Ell,
585 SymmetricMatrix& Qneu) {
586
587 Tracer tracer("covariXYZ_NEU");
588
589 Matrix CC(3,3);
590 jacobiXYZ_NEU(Ell, CC);
591 Qneu << CC * QQxyz * CC.t();
592}
593
594// Covariance Matrix in XYZ
595////////////////////////////////////////////////////////////////////////////
596void covariNEU_XYZ(const SymmetricMatrix& QQneu, const double* Ell,
597 SymmetricMatrix& Qxyz) {
598
599 Tracer tracer("covariNEU_XYZ");
600
601 Matrix CC(3,3);
602 jacobiXYZ_NEU(Ell, CC);
603 Qxyz << CC.t() * QQneu * CC;
604}
605
606// Fourth order Runge-Kutta numerical integrator for ODEs
607////////////////////////////////////////////////////////////////////////////
608ColumnVector rungeKutta4(
609 double xi, // the initial x-value
610 const ColumnVector& yi, // vector of the initial y-values
611 double dx, // the step size for the integration
612 double* acc, // additional acceleration
613 ColumnVector (*der)(double x, const ColumnVector& y, double* acc)
614 // A pointer to a function that computes the
615 // derivative of a function at a point (x,y)
616 ) {
617
618 ColumnVector k1 = der(xi , yi , acc) * dx;
619 ColumnVector k2 = der(xi+dx/2.0, yi+k1/2.0, acc) * dx;
620 ColumnVector k3 = der(xi+dx/2.0, yi+k2/2.0, acc) * dx;
621 ColumnVector k4 = der(xi+dx , yi+k3 , acc) * dx;
622
623 ColumnVector yf = yi + k1/6.0 + k2/3.0 + k3/3.0 + k4/6.0;
624
625 return yf;
626}
627//
628////////////////////////////////////////////////////////////////////////////
629double djul(long jj, long mm, double tt) {
630 long ii, kk;
631 double djul ;
632 if( mm <= 2 ) {
633 jj = jj - 1;
634 mm = mm + 12;
635 }
636 ii = jj/100;
637 kk = 2 - ii + ii/4;
638 djul = (365.25*jj - fmod( 365.25*jj, 1.0 )) - 679006.0;
639 djul = djul + floor( 30.6001*(mm + 1) ) + tt + kk;
640 return djul;
641}
642
643//
644////////////////////////////////////////////////////////////////////////////
645double gpjd(double second, int nweek) {
646 double deltat;
647 deltat = nweek*7.0 + second/86400.0 ;
648 return( 44244.0 + deltat) ;
649}
650
651//
652////////////////////////////////////////////////////////////////////////////
653void jdgp(double tjul, double & second, long & nweek) {
654 double deltat;
655 deltat = tjul - 44244.0 ;
656 nweek = (long) floor(deltat/7.0);
657 second = (deltat - (nweek)*7.0)*86400.0;
658}
659
660//
661////////////////////////////////////////////////////////////////////////////
662void jmt(double djul, long& jj, long& mm, double& dd) {
663 long ih, ih1, ih2 ;
664 double t1, t2, t3, t4;
665 t1 = 1.0 + djul - fmod( djul, 1.0 ) + 2400000.0;
666 t4 = fmod( djul, 1.0 );
667 ih = long( (t1 - 1867216.25)/36524.25 );
668 t2 = t1 + 1 + ih - ih/4;
669 t3 = t2 - 1720995.0;
670 ih1 = long( (t3 - 122.1)/365.25 );
671 t1 = 365.25*ih1 - fmod( 365.25*ih1, 1.0 );
672 ih2 = long( (t3 - t1)/30.6001 );
673 dd = t3 - t1 - (int)( 30.6001*ih2 ) + t4;
674 mm = ih2 - 1;
675 if ( ih2 > 13 ) mm = ih2 - 13;
676 jj = ih1;
677 if ( mm <= 2 ) jj = jj + 1;
678}
679
680//
681////////////////////////////////////////////////////////////////////////////
682void GPSweekFromDateAndTime(const QDateTime& dateTime,
683 int& GPSWeek, double& GPSWeeks) {
684
685 static const QDateTime zeroEpoch(QDate(1980, 1, 6),QTime(),Qt::UTC);
686
687 GPSWeek = zeroEpoch.daysTo(dateTime) / 7;
688
689 int weekDay = dateTime.date().dayOfWeek() + 1; // Qt: Monday = 1
690 if (weekDay > 7) weekDay = 1;
691
692 GPSWeeks = (weekDay - 1) * 86400.0
693 - dateTime.time().msecsTo(QTime()) / 1e3;
694}
695
696//
697////////////////////////////////////////////////////////////////////////////
698void GPSweekFromYMDhms(int year, int month, int day, int hour, int min,
699 double sec, int& GPSWeek, double& GPSWeeks) {
700
701 double mjd = djul(year, month, day);
702
703 long GPSWeek_long;
704 jdgp(mjd, GPSWeeks, GPSWeek_long);
705 GPSWeek = GPSWeek_long;
706 GPSWeeks += hour * 3600.0 + min * 60.0 + sec;
707}
708
709//
710////////////////////////////////////////////////////////////////////////////
711void mjdFromDateAndTime(const QDateTime& dateTime, int& mjd, double& dayfrac) {
712
713 static const QDate zeroDate(1858, 11, 17);
714
715 mjd = zeroDate.daysTo(dateTime.date());
716
717 dayfrac = (dateTime.time().hour() +
718 (dateTime.time().minute() +
719 (dateTime.time().second() +
720 dateTime.time().msec() / 1000.0) / 60.0) / 60.0) / 24.0;
721}
722
723//
724////////////////////////////////////////////////////////////////////////////
725bool findInVector(const vector<QString>& vv, const QString& str) {
726 std::vector<QString>::const_iterator it;
727 for (it = vv.begin(); it != vv.end(); ++it) {
728 if ( (*it) == str) {
729 return true;
730 }
731 }
732 return false;
733}
734
735//
736////////////////////////////////////////////////////////////////////////////
737int readInt(const QString& str, int pos, int len, int& value) {
738 bool ok;
739 value = str.mid(pos, len).toInt(&ok);
740 return ok ? 0 : 1;
741}
742
743//
744////////////////////////////////////////////////////////////////////////////
745int readDbl(const QString& str, int pos, int len, double& value) {
746 QString hlp = str.mid(pos, len);
747 for (int ii = 0; ii < hlp.length(); ii++) {
748 if (hlp[ii]=='D' || hlp[ii]=='d' || hlp[ii] == 'E') {
749 hlp[ii]='e';
750 }
751 }
752 bool ok;
753 value = hlp.toDouble(&ok);
754 return ok ? 0 : 1;
755}
756
757// Topocentrical Distance and Elevation
758////////////////////////////////////////////////////////////////////////////
759void topos(double xRec, double yRec, double zRec,
760 double xSat, double ySat, double zSat,
761 double& rho, double& eleSat, double& azSat) {
762
763 double dx[3];
764 dx[0] = xSat-xRec;
765 dx[1] = ySat-yRec;
766 dx[2] = zSat-zRec;
767
768 rho = sqrt( dx[0]*dx[0] + dx[1]*dx[1] + dx[2]*dx[2] );
769
770 double xyzRec[3];
771 xyzRec[0] = xRec;
772 xyzRec[1] = yRec;
773 xyzRec[2] = zRec;
774
775 double Ell[3];
776 double neu[3];
777 xyz2ell(xyzRec, Ell);
778 xyz2neu(Ell, dx, neu);
779
780 eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
781 if (neu[2] < 0) {
782 eleSat *= -1.0;
783 }
784
785 azSat = atan2(neu[1], neu[0]);
786}
787
788// Degrees -> degrees, minutes, seconds
789////////////////////////////////////////////////////////////////////////////
790void deg2DMS(double decDeg, int& deg, int& min, double& sec) {
791 int sgn = (decDeg < 0.0 ? -1 : 1);
792 deg = static_cast<int>(decDeg);
793 min = sgn * static_cast<int>((decDeg - deg)*60);
794 sec = (sgn* (decDeg - deg) - min/60.0) * 3600.0;
795}
796
797//
798////////////////////////////////////////////////////////////////////////////
799QString fortranFormat(double value, int width, int prec) {
800 int expo = value == 0.0 ? 0 : int(log10(fabs(value)));
801 double mant = value == 0.0 ? 0 : value / pow(10.0, double(expo));
802 if (fabs(mant) >= 1.0) {
803 mant /= 10.0;
804 expo += 1;
805 }
806 if (expo >= 0) {
807 return QString("%1e+%2").arg(mant, width-4, 'f', prec).arg(expo, 2, 10, QChar('0'));
808 }
809 else {
810 return QString("%1e-%2").arg(mant, width-4, 'f', prec).arg(-expo, 2, 10, QChar('0'));
811 }
812}
813
814//
815//////////////////////////////////////////////////////////////////////////////
816void kalman(const Matrix& AA, const ColumnVector& ll, const DiagonalMatrix& PP,
817 SymmetricMatrix& QQ, ColumnVector& xx) {
818
819 Tracer tracer("kalman");
820
821 int nPar = AA.Ncols();
822 int nObs = AA.Nrows();
823 UpperTriangularMatrix SS = Cholesky(QQ).t();
824
825 Matrix SA = SS*AA.t();
826 Matrix SRF(nObs+nPar, nObs+nPar); SRF = 0;
827 for (int ii = 1; ii <= nObs; ++ii) {
828 SRF(ii,ii) = 1.0 / sqrt(PP(ii,ii));
829 }
830
831 SRF.SubMatrix (nObs+1, nObs+nPar, 1, nObs) = SA;
832 SRF.SymSubMatrix(nObs+1, nObs+nPar) = SS;
833
834 UpperTriangularMatrix UU;
835 QRZ(SRF, UU);
836
837 SS = UU.SymSubMatrix(nObs+1, nObs+nPar);
838 UpperTriangularMatrix SH_rt = UU.SymSubMatrix(1, nObs);
839 Matrix YY = UU.SubMatrix(1, nObs, nObs+1, nObs+nPar);
840
841 UpperTriangularMatrix SHi = SH_rt.i();
842
843 Matrix KT = SHi * YY;
844 SymmetricMatrix Hi; Hi << SHi * SHi.t();
845
846 xx += KT.t() * (ll - AA * xx);
847 QQ << (SS.t() * SS);
848}
849
850//
851////////////////////////////////////////////////////////////////////////////
852double accuracyFromIndex(int index, t_eph::e_type type) {
853double accuracy = -1.0;
854
855 if (type == t_eph::GPS ||
856 type == t_eph::BDS ||
857 type == t_eph::SBAS||
858 type == t_eph::QZSS) {
859 if ((index >= 0) && (index <= 6)) {
860 if (index == 3) {
861 accuracy = ceil(10.0 * pow(2.0, (double(index) / 2.0) + 1.0)) / 10.0;
862 }
863 else {
864 accuracy = floor(10.0 * pow(2.0, (double(index) / 2.0) + 1.0)) / 10.0;
865 }
866 }
867 else if ((index > 6) && (index <= 15)) {
868 accuracy = (10.0 * pow(2.0, (double(index) - 2.0))) / 10.0;
869 }
870 else {
871 accuracy = 8192.0;
872 }
873 }
874 else if (type == t_eph::Galileo) {
875 if ((index >= 0) && (index <= 49)) {
876 accuracy = (double(index) / 100.0);
877 }
878 else if ((index > 49) && (index <= 74)) {
879 accuracy = (50.0 + (double(index) - 50.0) * 2.0) / 100.0;
880 }
881 else if ((index > 74) && (index <= 99)) {
882 accuracy = 1.0 + (double(index) - 75.0) * 0.04;
883 }
884 else if ((index > 99) && (index <= 125)) {
885 accuracy = 2.0 + (double(index) - 100.0) * 0.16;
886 }
887 else {
888 accuracy = -1.0;
889 }
890 }
891 else if (type == t_eph::IRNSS) {
892 if ((index >= 0) && (index <= 6)) {
893 if (index == 1) {
894 accuracy = 2.8;
895 }
896 else if (index == 3) {
897 accuracy = 5.7;
898 }
899 else if (index == 5) {
900 accuracy = 11.3;
901 }
902 else {
903 accuracy = pow(2, 1 + index / 2);
904 }
905 }
906 else if ((index > 6) && (index <= 15)) {
907 accuracy = pow(2, index - 2);
908 }
909 }
910 return accuracy;
911}
912
913//
914////////////////////////////////////////////////////////////////////////////
915int indexFromAccuracy(double accuracy, t_eph::e_type type) {
916
917 if (type == t_eph::GPS || type == t_eph::BDS || type == t_eph::SBAS
918 || type == t_eph::QZSS) {
919
920 if (accuracy <= 2.40) {
921 return 0;
922 }
923 else if (accuracy <= 3.40) {
924 return 1;
925 }
926 else if (accuracy <= 4.85) {
927 return 2;
928 }
929 else if (accuracy <= 6.85) {
930 return 3;
931 }
932 else if (accuracy <= 9.65) {
933 return 4;
934 }
935 else if (accuracy <= 13.65) {
936 return 5;
937 }
938 else if (accuracy <= 24.00) {
939 return 6;
940 }
941 else if (accuracy <= 48.00) {
942 return 7;
943 }
944 else if (accuracy <= 96.00) {
945 return 8;
946 }
947 else if (accuracy <= 192.00) {
948 return 9;
949 }
950 else if (accuracy <= 384.00) {
951 return 10;
952 }
953 else if (accuracy <= 768.00) {
954 return 11;
955 }
956 else if (accuracy <= 1536.00) {
957 return 12;
958 }
959 else if (accuracy <= 3072.00) {
960 return 13;
961 }
962 else if (accuracy <= 6144.00) {
963 return 14;
964 }
965 else {
966 return 15;
967 }
968 }
969
970 if (type == t_eph::Galileo) {
971
972 if (accuracy <= 0.49) {
973 return int(ceil(accuracy * 100.0));
974 }
975 else if (accuracy <= 0.98) {
976 return int(50.0 + (((accuracy * 100.0) - 50) / 2.0));
977 }
978 else if (accuracy <= 2.0) {
979 return int(75.0 + ((accuracy - 1.0) / 0.04));
980 }
981 else if (accuracy <= 6.0) {
982 return int(100.0 + ((accuracy - 2.0) / 0.16));
983 }
984 else {
985 return 255;
986 }
987 }
988
989 return (type == t_eph::Galileo) ? 255 : 15;
990}
991
992// Returns fit interval in hours from flag
993////////////////////////////////////////////////////////////////////////////
994double fitIntervalFromFlag(int flag, double iodc, t_eph::e_type type) {
995 double fitInterval = 0.0;
996
997 switch (flag) {
998 case 0:
999 if (type == t_eph::GPS) {
1000 fitInterval = 4.0;
1001 }
1002 else if (type == t_eph::QZSS) {
1003 fitInterval = 2.0;
1004 }
1005 break;
1006 case 1:
1007 if (type == t_eph::GPS) {
1008 if (iodc >= 240 && iodc <= 247) {
1009 fitInterval = 8.0;
1010 }
1011 else if ((iodc >= 248 && iodc <= 255) ||
1012 (iodc == 496) ) {
1013 fitInterval = 14.0;
1014 }
1015 else if ((iodc >= 497 && iodc <= 503) ||
1016 (iodc >= 2021 && iodc <= 1023) ) {
1017 fitInterval = 26.0;
1018 }
1019 else {
1020 fitInterval = 6.0;
1021 }
1022 }
1023 break;
1024 }
1025 return fitInterval;
1026}
1027
1028// Returns CRC24
1029////////////////////////////////////////////////////////////////////////////
1030unsigned long CRC24(long size, const unsigned char *buf) {
1031 unsigned long crc = 0;
1032 int ii;
1033 while (size--) {
1034 crc ^= (*buf++) << (16);
1035 for(ii = 0; ii < 8; ii++) {
1036 crc <<= 1;
1037 if (crc & 0x1000000) {
1038 crc ^= 0x01864cfb;
1039 }
1040 }
1041 }
1042 return crc;
1043}
1044
1045// Convert RTCM3 lock-time indicator to minimum lock time in seconds
1046////////////////////////////////////////////////////////////////////////////
1047double lti2sec(int type, int lti) {
1048
1049 if ( (type>=1001 && type<=1004) ||
1050 (type>=1009 && type<=1012) ) { // RTCM3 msg 100[1...4] and 10[09...12]
1051 if (lti< 0) return -1;
1052 else if (lti< 24) return 1*lti; // [ 0 1 23]
1053 else if (lti< 48) return 2*lti-24; // [ 24 2 70]
1054 else if (lti< 72) return 4*lti-120; // [ 72 4 164]
1055 else if (lti< 96) return 8*lti-408; // [168 8 352]
1056 else if (lti< 120) return 16*lti-1176; // [360 16 728]
1057 else if (lti< 127) return 32*lti-3096; // [744 32 905]
1058 else if (lti==127) return 937;
1059 else return -1.0;
1060 }
1061 else if (type%10==2 || type%10==3 ||
1062 type%10==4 || type%10==5) { // RTCM3 MSM-2/-3/-4/-5
1063 switch(lti) {
1064 case( 0) : return 0;
1065 case( 1) : return 32e-3;
1066 case( 2) : return 64e-3;
1067 case( 3) : return 128e-3;
1068 case( 4) : return 256e-3;
1069 case( 5) : return 512e-3;
1070 case( 6) : return 1024e-3;
1071 case( 7) : return 2048e-3;
1072 case( 8) : return 4096e-3;
1073 case( 9) : return 8192e-3;
1074 case(10) : return 16384e-3;
1075 case(11) : return 32768e-3;
1076 case(12) : return 65536e-3;
1077 case(13) : return 131072e-3;
1078 case(14) : return 262144e-3;
1079 case(15) : return 524288e-3;
1080 default : return -1.0;
1081 };
1082 }
1083 else if (type%10==6 || type%10==7) { // RTCM3 MSM-6 and MSM-7
1084 if (lti< 0) return ( -1 );
1085 else if (lti< 64) return ( 1*lti )*1e-3;
1086 else if (lti< 96) return ( 2*lti-64 )*1e-3;
1087 else if (lti< 128) return ( 4*lti-256 )*1e-3;
1088 else if (lti< 160) return ( 8*lti-768 )*1e-3;
1089 else if (lti< 192) return ( 16*lti-2048 )*1e-3;
1090 else if (lti< 224) return ( 32*lti-5120 )*1e-3;
1091 else if (lti< 256) return ( 64*lti-12288 )*1e-3;
1092 else if (lti< 288) return ( 128*lti-28672 )*1e-3;
1093 else if (lti< 320) return ( 256*lti-65536 )*1e-3;
1094 else if (lti< 352) return ( 512*lti-147456 )*1e-3;
1095 else if (lti< 384) return ( 1024*lti-327680 )*1e-3;
1096 else if (lti< 416) return ( 2048*lti-720896 )*1e-3;
1097 else if (lti< 448) return ( 4096*lti-1572864 )*1e-3;
1098 else if (lti< 480) return ( 8192*lti-3407872 )*1e-3;
1099 else if (lti< 512) return ( 16384*lti-7340032 )*1e-3;
1100 else if (lti< 544) return ( 32768*lti-15728640 )*1e-3;
1101 else if (lti< 576) return ( 65536*lti-33554432 )*1e-3;
1102 else if (lti< 608) return ( 131072*lti-71303168 )*1e-3;
1103 else if (lti< 640) return ( 262144*lti-150994944 )*1e-3;
1104 else if (lti< 672) return ( 524288*lti-318767104 )*1e-3;
1105 else if (lti< 704) return (1048576*lti-671088640 )*1e-3;
1106 else if (lti==704) return (2097152*lti-1409286144)*1e-3;
1107 else return ( -1.0 );
1108 }
1109 else {
1110 return -1.0;
1111 };
1112};
Note: See TracBrowser for help on using the repository browser.