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

Last change on this file since 8801 was 8801, checked in by stuerze, 13 months ago

IRNSS support is added in RTCM3 decoder, RTCM signal mapping IDs for GLONASS and BDS are updated/extended

File size: 29.8 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.norm_Frobenius();
320  ColumnVector cross  = crossproduct(rr, vv); cross /= cross.norm_Frobenius();
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.norm_Frobenius();
337  ColumnVector cross  = crossproduct(rr, vv); cross /= cross.norm_Frobenius();
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
810double accuracyFromIndex(int index, t_eph::e_type type) {
811double accuracy = -1.0;
812
813  if (type == t_eph::GPS ||
814      type == t_eph::BDS ||
815      type == t_eph::SBAS||
816      type == t_eph::QZSS) {
817    if ((index >= 0) && (index <= 6)) {
818      if (index == 3) {
819        accuracy =  ceil(10.0 * pow(2.0, (double(index) / 2.0) + 1.0)) / 10.0;
820      }
821      else {
822        accuracy = floor(10.0 * pow(2.0, (double(index) / 2.0) + 1.0)) / 10.0;
823      }
824    }
825    else if ((index > 6) && (index <= 15)) {
826      accuracy = (10.0 * pow(2.0, (double(index) - 2.0))) / 10.0;
827    }
828    else {
829      accuracy = 8192.0;
830    }
831  }
832  else if (type == t_eph::Galileo) {
833    if ((index >= 0) && (index <= 49)) {
834      accuracy = (double(index) / 100.0);
835    }
836    else if ((index > 49) && (index <= 74)) {
837      accuracy = (50.0 + (double(index) - 50.0) * 2.0) / 100.0;
838    }
839    else if ((index > 74) && (index <= 99)) {
840      accuracy = 1.0 + (double(index) - 75.0) * 0.04;
841    }
842    else if ((index > 99) && (index <= 125)) {
843      accuracy = 2.0 + (double(index) - 100.0) * 0.16;
844    }
845    else {
846      accuracy = -1.0;
847    }
848  }
849  else if (type == t_eph::IRNSS) {
850    if ((index >= 0) && (index <= 6)) {
851      if      (index == 1) {
852        accuracy = 2.8;
853      }
854      else if (index == 3) {
855        accuracy = 5.7;
856      }
857      else if (index == 5) {
858        accuracy = 11.3;
859      }
860      else {
861        accuracy = pow(2, 1 + index / 2);
862      }
863    }
864    else if ((index > 6) && (index <= 15)) {
865      accuracy = pow(2, index - 2);
866    }
867  }
868  return accuracy;
869}
870
871int indexFromAccuracy(double accuracy, t_eph::e_type type) {
872
873  if (type == t_eph::GPS || type == t_eph::BDS || type == t_eph::SBAS
874      || type == t_eph::QZSS) {
875
876    if (accuracy <= 2.40) {
877      return 0;
878    }
879    else if (accuracy <= 3.40) {
880      return 1;
881    }
882    else if (accuracy <= 4.85) {
883      return 2;
884    }
885    else if (accuracy <= 6.85) {
886      return 3;
887    }
888    else if (accuracy <= 9.65) {
889      return 4;
890    }
891    else if (accuracy <= 13.65) {
892      return 5;
893    }
894    else if (accuracy <= 24.00) {
895      return 6;
896    }
897    else if (accuracy <= 48.00) {
898      return 7;
899    }
900    else if (accuracy <= 96.00) {
901      return 8;
902    }
903    else if (accuracy <= 192.00) {
904      return 9;
905    }
906    else if (accuracy <= 384.00) {
907      return 10;
908    }
909    else if (accuracy <= 768.00) {
910      return 11;
911    }
912    else if (accuracy <= 1536.00) {
913      return 12;
914    }
915    else if (accuracy <= 3072.00) {
916      return 13;
917    }
918    else if (accuracy <= 6144.00) {
919      return 14;
920    }
921    else {
922      return 15;
923    }
924  }
925
926  if (type == t_eph::Galileo) {
927
928    if (accuracy <= 0.49) {
929      return int(ceil(accuracy * 100.0));
930    }
931    else if (accuracy <= 0.98) {
932      return int(50.0 + (((accuracy * 100.0) - 50) / 2.0));
933    }
934    else if (accuracy <= 2.0) {
935      return int(75.0 + ((accuracy - 1.0) / 0.04));
936    }
937    else if (accuracy <= 6.0) {
938      return int(100.0 + ((accuracy - 2.0) / 0.16));
939    }
940    else {
941      return 255;
942    }
943  }
944
945  return (type == t_eph::Galileo) ? 255 : 15;
946}
947
948// Returns CRC24
949////////////////////////////////////////////////////////////////////////////
950unsigned long CRC24(long size, const unsigned char *buf) {
951  unsigned long crc = 0;
952  int ii;
953  while (size--) {
954    crc ^= (*buf++) << (16);
955    for(ii = 0; ii < 8; ii++) {
956      crc <<= 1;
957      if (crc & 0x1000000) {
958        crc ^= 0x01864cfb;
959      }
960    }
961  }
962  return crc;
963}
964
965// Convert RTCM3 lock-time indicator to lock time in seconds
966////////////////////////////////////////////////////////////////////////////
967double lti2sec(int type, int lti) {
968
969  if ( (type>=1001 && type<=1004) ||
970       (type>=1009 && type<=1012)    ) { // RTCM3 msg 100[1...4] and 10[09...12]
971         if (lti<   0) return  -1;
972    else if (lti<  24) return   1*lti;      // [  0   1   23]
973    else if (lti<  48) return   2*lti-24;   // [ 24   2   70]
974    else if (lti<  72) return   4*lti-120;  // [ 72   4  164]
975    else if (lti<  96) return   8*lti-408;  // [168   8  352]
976    else if (lti< 120) return  16*lti-1176; // [360  16  728]
977    else if (lti< 127) return  32*lti-3096; // [744  32  905]
978    else if (lti==127) return  937;
979    else               return  -1;
980  }
981  else if (type%10==2 || type%10==3 ||
982           type%10==4 || type%10==5) {  // RTCM3 MSM-2/-3/-4/-5
983    switch(lti) {
984      case( 0) : return      0;
985      case( 1) : return     32e-3;
986      case( 2) : return     64e-3;
987      case( 3) : return    128e-3;
988      case( 4) : return    256e-3;
989      case( 5) : return    512e-3;
990      case( 6) : return   1024e-3;
991      case( 7) : return   2048e-3;
992      case( 8) : return   4096e-3;
993      case( 9) : return   8192e-3;
994      case(10) : return  16384e-3;
995      case(11) : return  32768e-3;
996      case(12) : return  65536e-3;
997      case(13) : return 131072e-3;
998      case(14) : return 262144e-3;
999      case(15) : return 524288e-3;
1000      default  : return     -1;
1001    };
1002  }
1003  else if (type%10==6 || type%10==7) {  // RTCM3 MSM-6 and MSM-7
1004         if (lti<   0) return (     -1               );
1005    else if (lti<  64) return (      1*lti           )*1e-3;
1006    else if (lti<  96) return (      2*lti-64        )*1e-3;
1007    else if (lti< 128) return (      4*lti-256       )*1e-3;
1008    else if (lti< 160) return (      8*lti-768       )*1e-3;
1009    else if (lti< 192) return (     16*lti-2048      )*1e-3;
1010    else if (lti< 224) return (     32*lti-5120      )*1e-3;
1011    else if (lti< 256) return (     64*lti-12288     )*1e-3;
1012    else if (lti< 288) return (    128*lti-28672     )*1e-3;
1013    else if (lti< 320) return (    256*lti-65536     )*1e-3;
1014    else if (lti< 352) return (    512*lti-147456    )*1e-3;
1015    else if (lti< 384) return (   1024*lti-327680    )*1e-3;
1016    else if (lti< 416) return (   2048*lti-720896    )*1e-3;
1017    else if (lti< 448) return (   4096*lti-1572864   )*1e-3;
1018    else if (lti< 480) return (   8192*lti-3407872   )*1e-3;
1019    else if (lti< 512) return (  16384*lti-7340032   )*1e-3;
1020    else if (lti< 544) return (  32768*lti-15728640  )*1e-3;
1021    else if (lti< 576) return (  65536*lti-33554432  )*1e-3;
1022    else if (lti< 608) return ( 131072*lti-71303168  )*1e-3;
1023    else if (lti< 640) return ( 262144*lti-150994944 )*1e-3;
1024    else if (lti< 672) return ( 524288*lti-318767104 )*1e-3;
1025    else if (lti< 704) return (1048576*lti-671088640 )*1e-3;
1026    else if (lti==704) return (2097152*lti-1409286144)*1e-3;
1027    else               return (     -1               );
1028  }
1029  else {
1030    return -1;
1031  };
1032};
Note: See TracBrowser for help on using the repository browser.