source: ntrip/trunk/BNC/bncutils.cpp@ 2530

Last change on this file since 2530 was 2530, checked in by mervart, 14 years ago
File size: 8.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 "bncapp.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 ( ((bncApp*) qApp)->_currentDateAndTimeGPS ) {
94 currDateTimeGPS = *(((bncApp*) qApp)->_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 ( ((bncApp*) qApp)->_currentDateAndTimeGPS ) {
119 return *(((bncApp*) qApp)->_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// Rectangular Coordinates -> Ellipsoidal Coordinates
194////////////////////////////////////////////////////////////////////////////
195t_irc xyz2ell(const double* XYZ, double* Ell) {
196
197 const double bell = t_CST::aell*(1.0-1.0/t_CST::fInv) ;
198 const double e2 = (t_CST::aell*t_CST::aell-bell*bell)/(t_CST::aell*t_CST::aell) ;
199 const double e2c = (t_CST::aell*t_CST::aell-bell*bell)/(bell*bell) ;
200
201 double nn, ss, zps, hOld, phiOld, theta, sin3, cos3;
202
203 ss = sqrt(XYZ[0]*XYZ[0]+XYZ[1]*XYZ[1]) ;
204 zps = XYZ[2]/ss ;
205 theta = atan( (XYZ[2]*t_CST::aell) / (ss*bell) );
206 sin3 = sin(theta) * sin(theta) * sin(theta);
207 cos3 = cos(theta) * cos(theta) * cos(theta);
208
209 // Closed formula
210 Ell[0] = atan( (XYZ[2] + e2c * bell * sin3) / (ss - e2 * t_CST::aell * cos3) );
211 Ell[1] = atan2(XYZ[1],XYZ[0]) ;
212 nn = t_CST::aell/sqrt(1.0-e2*sin(Ell[0])*sin(Ell[0])) ;
213 Ell[2] = ss / cos(Ell[0]) - nn;
214
215 const int MAXITER = 100;
216 for (int ii = 1; ii <= MAXITER; ii++) {
217 nn = t_CST::aell/sqrt(1.0-e2*sin(Ell[0])*sin(Ell[0])) ;
218 hOld = Ell[2] ;
219 phiOld = Ell[0] ;
220 Ell[2] = ss/cos(Ell[0])-nn ;
221 Ell[0] = atan(zps/(1.0-e2*nn/(nn+Ell[2]))) ;
222 if ( fabs(phiOld-Ell[0]) <= 1.0e-11 && fabs(hOld-Ell[2]) <= 1.0e-5 ) {
223 return success;
224 }
225 }
226
227 return failure;
228}
229
230// Rectangular Coordinates -> North, East, Up Components
231////////////////////////////////////////////////////////////////////////////
232void xyz2neu(const double* Ell, const double* xyz, double* neu) {
233
234 double sinPhi = sin(Ell[0]);
235 double cosPhi = cos(Ell[0]);
236 double sinLam = sin(Ell[1]);
237 double cosLam = cos(Ell[1]);
238
239 neu[0] = - sinPhi*cosLam * xyz[0]
240 - sinPhi*sinLam * xyz[1]
241 + cosPhi * xyz[2];
242
243 neu[1] = - sinLam * xyz[0]
244 + cosLam * xyz[1];
245
246 neu[2] = + cosPhi*cosLam * xyz[0]
247 + cosPhi*sinLam * xyz[1]
248 + sinPhi * xyz[2];
249}
250
251// Fourth order Runge-Kutta numerical integrator for ODEs
252////////////////////////////////////////////////////////////////////////////
253ColumnVector rungeKutta4(
254 double xi, // the initial x-value
255 const ColumnVector& yi, // vector of the initial y-values
256 double dx, // the step size for the integration
257 ColumnVector (*der)(double x, const ColumnVector& y)
258 // A pointer to a function that computes the
259 // derivative of a function at a point (x,y)
260 ) {
261
262 ColumnVector k1 = der(xi , yi ) * dx;
263 ColumnVector k2 = der(xi+dx/2.0, yi+k1/2.0) * dx;
264 ColumnVector k3 = der(xi+dx/2.0, yi+k2/2.0) * dx;
265 ColumnVector k4 = der(xi+dx , yi+k3 ) * dx;
266
267 ColumnVector yf = yi + k1/6.0 + k2/3.0 + k3/3.0 + k4/6.0;
268
269 return yf;
270}
271
Note: See TracBrowser for help on using the repository browser.