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

Last change on this file since 5753 was 5753, checked in by mervart, 8 years ago
File size: 16.6 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 "bncutils.h"
50#include "bnccore.h"
51
52using namespace std;
53
54//
55////////////////////////////////////////////////////////////////////////////
56void expandEnvVar(QString& str) {
57
58 QRegExp rx("(\\$\\{.+\\})");
59
60 if (rx.indexIn(str) != -1) {
61 QStringListIterator it(rx.capturedTexts());
62 if (it.hasNext()) {
63 QString rxStr = it.next();
64 QString envVar = rxStr.mid(2,rxStr.length()-3);
65 str.replace(rxStr, qgetenv(envVar.toAscii()));
66 }
67 }
68
69}
70
71//
72////////////////////////////////////////////////////////////////////////////
73QDateTime dateAndTimeFromGPSweek(int GPSWeek, double GPSWeeks) {
74
75 static const QDate zeroEpoch(1980, 1, 6);
76
77 QDate date(zeroEpoch);
78 QTime time(0,0,0,0);
79
80 int weekDays = int(GPSWeeks) / 86400;
81 date = date.addDays( GPSWeek * 7 + weekDays );
82 time = time.addMSecs( int( (GPSWeeks - 86400 * weekDays) * 1e3 ) );
83
84 return QDateTime(date,time);
85}
86
87//
88////////////////////////////////////////////////////////////////////////////
89void currentGPSWeeks(int& week, double& sec) {
90
91 QDateTime currDateTimeGPS;
92
93 if ( BNC_CORE->_currentDateAndTimeGPS ) {
94 currDateTimeGPS = *(BNC_CORE->_currentDateAndTimeGPS);
95 }
96 else {
97 currDateTimeGPS = QDateTime::currentDateTime().toUTC();
98 QDate hlp = currDateTimeGPS.date();
99 currDateTimeGPS = currDateTimeGPS.addSecs(gnumleap(hlp.year(),
100 hlp.month(), hlp.day()));
101 }
102
103 QDate currDateGPS = currDateTimeGPS.date();
104 QTime currTimeGPS = currDateTimeGPS.time();
105
106 week = int( (double(currDateGPS.toJulianDay()) - 2444244.5) / 7 );
107
108 sec = (currDateGPS.dayOfWeek() % 7) * 24.0 * 3600.0 +
109 currTimeGPS.hour() * 3600.0 +
110 currTimeGPS.minute() * 60.0 +
111 currTimeGPS.second() +
112 currTimeGPS.msec() / 1000.0;
113}
114
115//
116////////////////////////////////////////////////////////////////////////////
117QDateTime currentDateAndTimeGPS() {
118 if ( BNC_CORE->_currentDateAndTimeGPS ) {
119 return *(BNC_CORE->_currentDateAndTimeGPS);
120 }
121 else {
122 int GPSWeek;
123 double GPSWeeks;
124 currentGPSWeeks(GPSWeek, GPSWeeks);
125 return dateAndTimeFromGPSweek(GPSWeek, GPSWeeks);
126 }
127}
128
129//
130////////////////////////////////////////////////////////////////////////////
131QByteArray ggaString(const QByteArray& latitude,
132 const QByteArray& longitude,
133 const QByteArray& height) {
134
135 double lat = strtod(latitude,NULL);
136 double lon = strtod(longitude,NULL);
137 double hei = strtod(height,NULL);
138
139 const char* flagN="N";
140 const char* flagE="E";
141 if (lon >180.) {lon=(lon-360.)*(-1.); flagE="W";}
142 if ((lon < 0.) && (lon >= -180.)) {lon=lon*(-1.); flagE="W";}
143 if (lon < -180.) {lon=(lon+360.); flagE="E";}
144 if (lat < 0.) {lat=lat*(-1.); flagN="S";}
145 QTime ttime(QDateTime::currentDateTime().toUTC().time());
146 int lat_deg = (int)lat;
147 double lat_min=(lat-lat_deg)*60.;
148 int lon_deg = (int)lon;
149 double lon_min=(lon-lon_deg)*60.;
150 int hh = 0 , mm = 0;
151 double ss = 0.0;
152 hh=ttime.hour();
153 mm=ttime.minute();
154 ss=(double)ttime.second()+0.001*ttime.msec();
155 QString gga;
156 gga += "GPGGA,";
157 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'));
158 gga += QString("%1%2,").arg((int)lat_deg,2, 10, QLatin1Char('0')).arg(lat_min, 7, 'f', 4, QLatin1Char('0'));
159 gga += flagN;
160 gga += QString(",%1%2,").arg((int)lon_deg,3, 10, QLatin1Char('0')).arg(lon_min, 7, 'f', 4, QLatin1Char('0'));
161 gga += flagE + QString(",1,05,1.00");
162 gga += QString(",%1,").arg(hei, 2, 'f', 1);
163 gga += QString("M,10.000,M,,");
164 int xori;
165 char XOR = 0;
166 char *Buff =gga.toAscii().data();
167 int iLen = strlen(Buff);
168 for (xori = 0; xori < iLen; xori++) {
169 XOR ^= (char)Buff[xori];
170 }
171 gga = "$" + gga + QString("*%1").arg(XOR, 2, 16, QLatin1Char('0'));
172
173 return gga.toAscii();
174}
175
176//
177////////////////////////////////////////////////////////////////////////////
178void RSW_to_XYZ(const ColumnVector& rr, const ColumnVector& vv,
179 const ColumnVector& rsw, ColumnVector& xyz) {
180
181 ColumnVector along = vv / vv.norm_Frobenius();
182 ColumnVector cross = crossproduct(rr, vv); cross /= cross.norm_Frobenius();
183 ColumnVector radial = crossproduct(along, cross);
184
185 Matrix RR(3,3);
186 RR.Column(1) = radial;
187 RR.Column(2) = along;
188 RR.Column(3) = cross;
189
190 xyz = RR * rsw;
191}
192
193// Transformation xyz --> radial, along track, out-of-plane
194////////////////////////////////////////////////////////////////////////////
195void XYZ_to_RSW(const ColumnVector& rr, const ColumnVector& vv,
196 const ColumnVector& xyz, ColumnVector& rsw) {
197
198 ColumnVector along = vv / vv.norm_Frobenius();
199 ColumnVector cross = crossproduct(rr, vv); cross /= cross.norm_Frobenius();
200 ColumnVector radial = crossproduct(along, cross);
201
202 rsw.ReSize(3);
203 rsw(1) = DotProduct(xyz, radial);
204 rsw(2) = DotProduct(xyz, along);
205 rsw(3) = DotProduct(xyz, cross);
206}
207
208// Rectangular Coordinates -> Ellipsoidal Coordinates
209////////////////////////////////////////////////////////////////////////////
210t_irc xyz2ell(const double* XYZ, double* Ell) {
211
212 const double bell = t_CST::aell*(1.0-1.0/t_CST::fInv) ;
213 const double e2 = (t_CST::aell*t_CST::aell-bell*bell)/(t_CST::aell*t_CST::aell) ;
214 const double e2c = (t_CST::aell*t_CST::aell-bell*bell)/(bell*bell) ;
215
216 double nn, ss, zps, hOld, phiOld, theta, sin3, cos3;
217
218 ss = sqrt(XYZ[0]*XYZ[0]+XYZ[1]*XYZ[1]) ;
219 zps = XYZ[2]/ss ;
220 theta = atan( (XYZ[2]*t_CST::aell) / (ss*bell) );
221 sin3 = sin(theta) * sin(theta) * sin(theta);
222 cos3 = cos(theta) * cos(theta) * cos(theta);
223
224 // Closed formula
225 Ell[0] = atan( (XYZ[2] + e2c * bell * sin3) / (ss - e2 * t_CST::aell * cos3) );
226 Ell[1] = atan2(XYZ[1],XYZ[0]) ;
227 nn = t_CST::aell/sqrt(1.0-e2*sin(Ell[0])*sin(Ell[0])) ;
228 Ell[2] = ss / cos(Ell[0]) - nn;
229
230 const int MAXITER = 100;
231 for (int ii = 1; ii <= MAXITER; ii++) {
232 nn = t_CST::aell/sqrt(1.0-e2*sin(Ell[0])*sin(Ell[0])) ;
233 hOld = Ell[2] ;
234 phiOld = Ell[0] ;
235 Ell[2] = ss/cos(Ell[0])-nn ;
236 Ell[0] = atan(zps/(1.0-e2*nn/(nn+Ell[2]))) ;
237 if ( fabs(phiOld-Ell[0]) <= 1.0e-11 && fabs(hOld-Ell[2]) <= 1.0e-5 ) {
238 return success;
239 }
240 }
241
242 return failure;
243}
244
245// Rectangular Coordinates -> North, East, Up Components
246////////////////////////////////////////////////////////////////////////////
247void xyz2neu(const double* Ell, const double* xyz, double* neu) {
248
249 double sinPhi = sin(Ell[0]);
250 double cosPhi = cos(Ell[0]);
251 double sinLam = sin(Ell[1]);
252 double cosLam = cos(Ell[1]);
253
254 neu[0] = - sinPhi*cosLam * xyz[0]
255 - sinPhi*sinLam * xyz[1]
256 + cosPhi * xyz[2];
257
258 neu[1] = - sinLam * xyz[0]
259 + cosLam * xyz[1];
260
261 neu[2] = + cosPhi*cosLam * xyz[0]
262 + cosPhi*sinLam * xyz[1]
263 + sinPhi * xyz[2];
264}
265
266// North, East, Up Components -> Rectangular Coordinates
267////////////////////////////////////////////////////////////////////////////
268void neu2xyz(const double* Ell, const double* neu, double* xyz) {
269
270 double sinPhi = sin(Ell[0]);
271 double cosPhi = cos(Ell[0]);
272 double sinLam = sin(Ell[1]);
273 double cosLam = cos(Ell[1]);
274
275 xyz[0] = - sinPhi*cosLam * neu[0]
276 - sinLam * neu[1]
277 + cosPhi*cosLam * neu[2];
278
279 xyz[1] = - sinPhi*sinLam * neu[0]
280 + cosLam * neu[1]
281 + cosPhi*sinLam * neu[2];
282
283 xyz[2] = + cosPhi * neu[0]
284 + sinPhi * neu[2];
285}
286
287// Round to nearest integer
288////////////////////////////////////////////////////////////////////////////
289double nint(double val) {
290 return ((val < 0.0) ? -floor(fabs(val)+0.5) : floor(val+0.5));
291}
292
293// Jacobian XYZ --> NEU
294////////////////////////////////////////////////////////////////////////////
295void jacobiXYZ_NEU(const double* Ell, Matrix& jacobi) {
296
297 Tracer tracer("jacobiXYZ_NEU");
298
299 double sinPhi = sin(Ell[0]);
300 double cosPhi = cos(Ell[0]);
301 double sinLam = sin(Ell[1]);
302 double cosLam = cos(Ell[1]);
303
304 jacobi(1,1) = - sinPhi * cosLam;
305 jacobi(1,2) = - sinPhi * sinLam;
306 jacobi(1,3) = cosPhi;
307
308 jacobi(2,1) = - sinLam;
309 jacobi(2,2) = cosLam;
310 jacobi(2,3) = 0.0;
311
312 jacobi(3,1) = cosPhi * cosLam;
313 jacobi(3,2) = cosPhi * sinLam;
314 jacobi(3,3) = sinPhi;
315}
316
317// Jacobian Ell --> XYZ
318////////////////////////////////////////////////////////////////////////////
319void jacobiEll_XYZ(const double* Ell, Matrix& jacobi) {
320
321 Tracer tracer("jacobiEll_XYZ");
322
323 double sinPhi = sin(Ell[0]);
324 double cosPhi = cos(Ell[0]);
325 double sinLam = sin(Ell[1]);
326 double cosLam = cos(Ell[1]);
327 double hh = Ell[2];
328
329 double bell = t_CST::aell*(1.0-1.0/t_CST::fInv);
330 double e2 = (t_CST::aell*t_CST::aell-bell*bell)/(t_CST::aell*t_CST::aell) ;
331 double nn = t_CST::aell/sqrt(1.0-e2*sinPhi*sinPhi) ;
332
333 jacobi(1,1) = -(nn+hh) * sinPhi * cosLam;
334 jacobi(1,2) = -(nn+hh) * cosPhi * sinLam;
335 jacobi(1,3) = cosPhi * cosLam;
336
337 jacobi(2,1) = -(nn+hh) * sinPhi * sinLam;
338 jacobi(2,2) = (nn+hh) * cosPhi * cosLam;
339 jacobi(2,3) = cosPhi * sinLam;
340
341 jacobi(3,1) = (nn*(1.0-e2)+hh) * cosPhi;
342 jacobi(3,2) = 0.0;
343 jacobi(3,3) = sinPhi;
344}
345
346// Covariance Matrix in NEU
347////////////////////////////////////////////////////////////////////////////
348void covariXYZ_NEU(const SymmetricMatrix& QQxyz, const double* Ell,
349 SymmetricMatrix& Qneu) {
350
351 Tracer tracer("covariXYZ_NEU");
352
353 Matrix CC(3,3);
354 jacobiXYZ_NEU(Ell, CC);
355 Qneu << CC * QQxyz * CC.t();
356}
357
358// Covariance Matrix in XYZ
359////////////////////////////////////////////////////////////////////////////
360void covariNEU_XYZ(const SymmetricMatrix& QQneu, const double* Ell,
361 SymmetricMatrix& Qxyz) {
362
363 Tracer tracer("covariNEU_XYZ");
364
365 Matrix CC(3,3);
366 jacobiXYZ_NEU(Ell, CC);
367 Qxyz << CC.t() * QQneu * CC;
368}
369
370// Fourth order Runge-Kutta numerical integrator for ODEs
371////////////////////////////////////////////////////////////////////////////
372ColumnVector rungeKutta4(
373 double xi, // the initial x-value
374 const ColumnVector& yi, // vector of the initial y-values
375 double dx, // the step size for the integration
376 double* acc, // aditional acceleration
377 ColumnVector (*der)(double x, const ColumnVector& y, double* acc)
378 // A pointer to a function that computes the
379 // derivative of a function at a point (x,y)
380 ) {
381
382 ColumnVector k1 = der(xi , yi , acc) * dx;
383 ColumnVector k2 = der(xi+dx/2.0, yi+k1/2.0, acc) * dx;
384 ColumnVector k3 = der(xi+dx/2.0, yi+k2/2.0, acc) * dx;
385 ColumnVector k4 = der(xi+dx , yi+k3 , acc) * dx;
386
387 ColumnVector yf = yi + k1/6.0 + k2/3.0 + k3/3.0 + k4/6.0;
388
389 return yf;
390}
391
392//
393////////////////////////////////////////////////////////////////////////////
394double djul(int jj, int mm, double tt) {
395 int ii, kk;
396 double djul ;
397 if( mm <= 2 ) {
398 jj = jj - 1;
399 mm = mm + 12;
400 }
401 ii = jj/100;
402 kk = 2 - ii + ii/4;
403 djul = (365.25*jj - fmod( 365.25*jj, 1.0 )) - 679006.0;
404 djul = djul + floor( 30.6001*(mm + 1) ) + tt + kk;
405 return djul;
406}
407
408//
409////////////////////////////////////////////////////////////////////////////
410void jdgp(double tjul, double & second, int & nweek) {
411 double deltat;
412 deltat = tjul - 44244.0 ;
413 // current gps week
414 nweek = (int) floor(deltat/7.0);
415 // seconds past midnight of last weekend
416 second = (deltat - (nweek)*7.0)*86400.0;
417}
418
419//
420////////////////////////////////////////////////////////////////////////////
421void GPSweekFromDateAndTime(const QDateTime& dateTime,
422 int& GPSWeek, double& GPSWeeks) {
423
424 static const QDateTime zeroEpoch(QDate(1980, 1, 6),QTime(),Qt::UTC);
425
426 GPSWeek = zeroEpoch.daysTo(dateTime) / 7;
427
428 int weekDay = dateTime.date().dayOfWeek() + 1; // Qt: Monday = 1
429 if (weekDay > 7) weekDay = 1;
430
431 GPSWeeks = (weekDay - 1) * 86400.0
432 - dateTime.time().msecsTo(QTime()) / 1e3;
433}
434
435//
436////////////////////////////////////////////////////////////////////////////
437void GPSweekFromYMDhms(int year, int month, int day, int hour, int min,
438 double sec, int& GPSWeek, double& GPSWeeks) {
439
440 double mjd = djul(year, month, day);
441
442 jdgp(mjd, GPSWeeks, GPSWeek);
443 GPSWeeks += hour * 3600.0 + min * 60.0 + sec;
444}
445
446//
447////////////////////////////////////////////////////////////////////////////
448void mjdFromDateAndTime(const QDateTime& dateTime, int& mjd, double& dayfrac) {
449
450 static const QDate zeroDate(1858, 11, 17);
451
452 mjd = zeroDate.daysTo(dateTime.date());
453
454 dayfrac = (dateTime.time().hour() +
455 (dateTime.time().minute() +
456 (dateTime.time().second() +
457 dateTime.time().msec() / 1000.0) / 60.0) / 60.0) / 24.0;
458}
459
460//
461////////////////////////////////////////////////////////////////////////////
462bool findInVector(const vector<QString>& vv, const QString& str) {
463 std::vector<QString>::const_iterator it;
464 for (it = vv.begin(); it != vv.end(); ++it) {
465 if ( (*it) == str) {
466 return true;
467 }
468 }
469 return false;
470}
471
472//
473////////////////////////////////////////////////////////////////////////////
474int readInt(const QString& str, int pos, int len, int& value) {
475 bool ok;
476 value = str.mid(pos, len).toInt(&ok);
477 return ok ? 0 : 1;
478}
479
480//
481////////////////////////////////////////////////////////////////////////////
482int readDbl(const QString& str, int pos, int len, double& value) {
483 QString hlp = str.mid(pos, len);
484 for (int ii = 0; ii < hlp.length(); ii++) {
485 if (hlp[ii]=='D' || hlp[ii]=='d' || hlp[ii] == 'E') {
486 hlp[ii]='e';
487 }
488 }
489 bool ok;
490 value = hlp.toDouble(&ok);
491 return ok ? 0 : 1;
492}
493
494// Topocentrical Distance and Elevation
495////////////////////////////////////////////////////////////////////////////
496void topos(double xRec, double yRec, double zRec,
497 double xSat, double ySat, double zSat,
498 double& rho, double& eleSat, double& azSat) {
499
500 double dx[3];
501 dx[0] = xSat-xRec;
502 dx[1] = ySat-yRec;
503 dx[2] = zSat-zRec;
504
505 rho = sqrt( dx[0]*dx[0] + dx[1]*dx[1] + dx[2]*dx[2] );
506
507 double xyzRec[3];
508 xyzRec[0] = xRec;
509 xyzRec[1] = yRec;
510 xyzRec[2] = zRec;
511
512 double Ell[3];
513 double neu[3];
514 xyz2ell(xyzRec, Ell);
515 xyz2neu(Ell, dx, neu);
516
517 eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
518 if (neu[2] < 0) {
519 eleSat *= -1.0;
520 }
521
522 azSat = atan2(neu[1], neu[0]);
523}
524
525// Degrees -> degrees, minutes, seconds
526////////////////////////////////////////////////////////////////////////////
527void deg2DMS(double decDeg, int& deg, int& min, double& sec) {
528 int sgn = (decDeg < 0.0 ? -1 : 1);
529 deg = sgn * static_cast<int>(decDeg);
530 min = static_cast<int>((decDeg - deg)*60);
531 sec = (decDeg - deg - min/60.0) * 3600.0;
532}
533
534//
535////////////////////////////////////////////////////////////////////////////
536QString fortranFormat(double value, int width, int prec) {
537 int expo = value == 0.0 ? 0 : log10(fabs(value));
538 double mant = value == 0.0 ? 0 : value / pow(10, expo);
539 if (fabs(mant) >= 1.0) {
540 mant /= 10.0;
541 expo += 1;
542 }
543 if (expo >= 0) {
544 return QString("%1e+%2").arg(mant, width-4, 'f', prec).arg(expo, 2, 10, QChar('0'));
545 }
546 else {
547 return QString("%1e-%2").arg(mant, width-4, 'f', prec).arg(-expo, 2, 10, QChar('0'));
548 }
549}
Note: See TracBrowser for help on using the repository browser.