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

Last change on this file since 5070 was 5070, checked in by mervart, 9 years ago
File size: 13.4 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// Fourth order Runge-Kutta numerical integrator for ODEs
288////////////////////////////////////////////////////////////////////////////
289ColumnVector rungeKutta4(
290 double xi, // the initial x-value
291 const ColumnVector& yi, // vector of the initial y-values
292 double dx, // the step size for the integration
293 double* acc, // aditional acceleration
294 ColumnVector (*der)(double x, const ColumnVector& y, double* acc)
295 // A pointer to a function that computes the
296 // derivative of a function at a point (x,y)
297 ) {
298
299 ColumnVector k1 = der(xi , yi , acc) * dx;
300 ColumnVector k2 = der(xi+dx/2.0, yi+k1/2.0, acc) * dx;
301 ColumnVector k3 = der(xi+dx/2.0, yi+k2/2.0, acc) * dx;
302 ColumnVector k4 = der(xi+dx , yi+k3 , acc) * dx;
303
304 ColumnVector yf = yi + k1/6.0 + k2/3.0 + k3/3.0 + k4/6.0;
305
306 return yf;
307}
308
309//
310////////////////////////////////////////////////////////////////////////////
311double djul(int jj, int mm, double tt) {
312 int ii, kk;
313 double djul ;
314 if( mm <= 2 ) {
315 jj = jj - 1;
316 mm = mm + 12;
317 }
318 ii = jj/100;
319 kk = 2 - ii + ii/4;
320 djul = (365.25*jj - fmod( 365.25*jj, 1.0 )) - 679006.0;
321 djul = djul + floor( 30.6001*(mm + 1) ) + tt + kk;
322 return djul;
323}
324
325//
326////////////////////////////////////////////////////////////////////////////
327void jdgp(double tjul, double & second, int & nweek) {
328 double deltat;
329 deltat = tjul - 44244.0 ;
330 // current gps week
331 nweek = (int) floor(deltat/7.0);
332 // seconds past midnight of last weekend
333 second = (deltat - (nweek)*7.0)*86400.0;
334}
335
336//
337////////////////////////////////////////////////////////////////////////////
338void GPSweekFromDateAndTime(const QDateTime& dateTime,
339 int& GPSWeek, double& GPSWeeks) {
340
341 static const QDateTime zeroEpoch(QDate(1980, 1, 6),QTime(),Qt::UTC);
342
343 GPSWeek = zeroEpoch.daysTo(dateTime) / 7;
344
345 int weekDay = dateTime.date().dayOfWeek() + 1; // Qt: Monday = 1
346 if (weekDay > 7) weekDay = 1;
347
348 GPSWeeks = (weekDay - 1) * 86400.0
349 - dateTime.time().msecsTo(QTime()) / 1e3;
350}
351
352//
353////////////////////////////////////////////////////////////////////////////
354void GPSweekFromYMDhms(int year, int month, int day, int hour, int min,
355 double sec, int& GPSWeek, double& GPSWeeks) {
356
357 double mjd = djul(year, month, day);
358
359 jdgp(mjd, GPSWeeks, GPSWeek);
360 GPSWeeks += hour * 3600.0 + min * 60.0 + sec;
361}
362
363//
364////////////////////////////////////////////////////////////////////////////
365void mjdFromDateAndTime(const QDateTime& dateTime, int& mjd, double& dayfrac) {
366
367 static const QDate zeroDate(1858, 11, 17);
368
369 mjd = zeroDate.daysTo(dateTime.date());
370
371 dayfrac = (dateTime.time().hour() +
372 (dateTime.time().minute() +
373 (dateTime.time().second() +
374 dateTime.time().msec() / 1000.0) / 60.0) / 60.0) / 24.0;
375}
376
377//
378////////////////////////////////////////////////////////////////////////////
379bool findInVector(const vector<QString>& vv, const QString& str) {
380 std::vector<QString>::const_iterator it;
381 for (it = vv.begin(); it != vv.end(); ++it) {
382 if ( (*it) == str) {
383 return true;
384 }
385 }
386 return false;
387}
388
389//
390////////////////////////////////////////////////////////////////////////////
391int readInt(const QString& str, int pos, int len, int& value) {
392 bool ok;
393 value = str.mid(pos, len).toInt(&ok);
394 return ok ? 0 : 1;
395}
396
397//
398////////////////////////////////////////////////////////////////////////////
399int readDbl(const QString& str, int pos, int len, double& value) {
400 QString hlp = str.mid(pos, len);
401 for (int ii = 0; ii < hlp.length(); ii++) {
402 if (hlp[ii]=='D' || hlp[ii]=='d' || hlp[ii] == 'E') {
403 hlp[ii]='e';
404 }
405 }
406 bool ok;
407 value = hlp.toDouble(&ok);
408 return ok ? 0 : 1;
409}
410
411// Topocentrical Distance and Elevation
412////////////////////////////////////////////////////////////////////////////
413void topos(double xRec, double yRec, double zRec,
414 double xSat, double ySat, double zSat,
415 double& rho, double& eleSat, double& azSat) {
416
417 double dx[3];
418 dx[0] = xSat-xRec;
419 dx[1] = ySat-yRec;
420 dx[2] = zSat-zRec;
421
422 rho = sqrt( dx[0]*dx[0] + dx[1]*dx[1] + dx[2]*dx[2] );
423
424 double xyzRec[3];
425 xyzRec[0] = xRec;
426 xyzRec[1] = yRec;
427 xyzRec[2] = zRec;
428
429 double Ell[3];
430 double neu[3];
431 xyz2ell(xyzRec, Ell);
432 xyz2neu(Ell, dx, neu);
433
434 eleSat = acos( sqrt(neu[0]*neu[0] + neu[1]*neu[1]) / rho );
435 if (neu[2] < 0) {
436 eleSat *= -1.0;
437 }
438
439 azSat = atan2(neu[1], neu[0]);
440}
Note: See TracBrowser for help on using the repository browser.