source: ntrip/trunk/BNS/bnsutils.cpp@ 10181

Last change on this file since 10181 was 3045, checked in by mervart, 14 years ago
File size: 5.7 KB
Line 
1/* -------------------------------------------------------------------------
2 * BKG NTRIP Server
3 * -------------------------------------------------------------------------
4 *
5 * Class: bncutils
6 *
7 * Purpose: Auxiliary Functions
8 *
9 * Author: L. Mervart
10 *
11 * Created: 08-Apr-2008
12 *
13 * Changes:
14 *
15 * -----------------------------------------------------------------------*/
16
17#include <ctime>
18#include <math.h>
19
20#include <QRegExp>
21#include <QStringList>
22#include <QDateTime>
23
24#include "bnsutils.h"
25
26using namespace std;
27
28namespace BNS {
29
30//
31////////////////////////////////////////////////////////////////////////////
32void expandEnvVar(QString& str) {
33
34 QRegExp rx("(\\$\\{.+\\})");
35
36 if (rx.indexIn(str) != -1) {
37 QStringListIterator it(rx.capturedTexts());
38 if (it.hasNext()) {
39 QString rxStr = it.next();
40 QString envVar = rxStr.mid(2,rxStr.length()-3);
41 str.replace(rxStr, qgetenv(envVar.toAscii()));
42 }
43 }
44
45}
46
47//
48////////////////////////////////////////////////////////////////////////////
49QDateTime dateAndTimeFromGPSweek(int GPSWeek, double GPSWeeks) {
50
51 static const QDate zeroEpoch(1980, 1, 6);
52
53 QDate date(zeroEpoch);
54 QTime time(0,0,0,0);
55
56 int weekDays = int(GPSWeeks) / 86400;
57 date = date.addDays( GPSWeek * 7 + weekDays );
58 time = time.addMSecs( int( (GPSWeeks - 86400 * weekDays) * 1e3 ) );
59
60 return QDateTime(date,time,Qt::UTC);
61}
62
63//
64////////////////////////////////////////////////////////////////////////////
65void GPSweekFromDateAndTime(const QDateTime& dateTime,
66 int& GPSWeek, double& GPSWeeks) {
67
68 static const QDateTime zeroEpoch(QDate(1980, 1, 6),QTime(),Qt::UTC);
69
70 GPSWeek = zeroEpoch.daysTo(dateTime) / 7;
71
72 int weekDay = dateTime.date().dayOfWeek() + 1; // Qt: Monday = 1
73 if (weekDay > 7) weekDay = 1;
74
75 GPSWeeks = (weekDay - 1) * 86400.0
76 - dateTime.time().msecsTo(QTime()) / 1e3;
77}
78
79//
80////////////////////////////////////////////////////////////////////////////
81void currentGPSWeeks(int& week, double& sec) {
82
83 QDateTime currDateTime = QDateTime::currentDateTime().toUTC();
84 QDate currDate = currDateTime.date();
85 QTime currTime = currDateTime.time();
86
87 week = int( (double(currDate.toJulianDay()) - 2444244.5) / 7 );
88
89 sec = (currDate.dayOfWeek() % 7) * 24.0 * 3600.0 +
90 currTime.hour() * 3600.0 +
91 currTime.minute() * 60.0 +
92 currTime.second() +
93 currTime.msec() / 1000.0;
94}
95
96//
97////////////////////////////////////////////////////////////////////////////
98void mjdFromDateAndTime(const QDateTime& dateTime, int& mjd, double& dayfrac) {
99
100 static const QDate zeroDate(1858, 11, 17);
101
102 mjd = zeroDate.daysTo(dateTime.date());
103
104 dayfrac = (dateTime.time().hour() +
105 (dateTime.time().minute() +
106 (dateTime.time().second() +
107 dateTime.time().msec() / 1000.0) / 60.0) / 60.0) / 24.0;
108}
109
110// Transformation xyz --> radial, along track, out-of-plane
111////////////////////////////////////////////////////////////////////////////
112void XYZ_to_RSW(const ColumnVector& rr, const ColumnVector& vv,
113 const ColumnVector& xyz, ColumnVector& rsw) {
114
115 ColumnVector along = vv / vv.norm_Frobenius();
116 ColumnVector cross = crossproduct(rr, vv); cross /= cross.norm_Frobenius();
117 ColumnVector radial = crossproduct(along, cross);
118
119 rsw.ReSize(3);
120 rsw(1) = DotProduct(xyz, radial);
121 rsw(2) = DotProduct(xyz, along);
122 rsw(3) = DotProduct(xyz, cross);
123}
124
125// Fourth order Runge-Kutta numerical integrator for ODEs
126////////////////////////////////////////////////////////////////////////////
127ColumnVector rungeKutta4(
128 double xi, // the initial x-value
129 const ColumnVector& yi, // vector of the initial y-values
130 double dx, // the step size for the integration
131 double* acc, // aditional acceleration
132 ColumnVector (*der)(double x, const ColumnVector& y, double* acc)
133 // A pointer to a function that computes the
134 // derivative of a function at a point (x,y)
135 ) {
136
137 ColumnVector k1 = der(xi , yi , acc) * dx;
138 ColumnVector k2 = der(xi+dx/2.0, yi+k1/2.0, acc) * dx;
139 ColumnVector k3 = der(xi+dx/2.0, yi+k2/2.0, acc) * dx;
140 ColumnVector k4 = der(xi+dx , yi+k3 , acc) * dx;
141
142 ColumnVector yf = yi + k1/6.0 + k2/3.0 + k3/3.0 + k4/6.0;
143
144 return yf;
145}
146
147//
148////////////////////////////////////////////////////////////////////////////
149QByteArray waitForLine(QTcpSocket* socket) {
150
151 QByteArray line;
152
153 while (true) {
154 char ch;
155 if (socket->getChar(&ch)) {
156 line += ch;
157 if (ch == '\n') {
158 break;
159 }
160 }
161 else {
162 if (socket->state() != QAbstractSocket::ConnectedState) {
163 return "";
164 }
165 socket->waitForReadyRead(10);
166 }
167 }
168 return line;
169}
170
171//
172////////////////////////////////////////////////////////////////////////////
173double djul(int jj, int mm, double tt) {
174 int ii, kk;
175 double djul ;
176
177 if( mm <= 2 ) {
178 jj = jj - 1;
179 mm = mm + 12;
180 }
181
182 ii = jj/100;
183 kk = 2 - ii + ii/4;
184 djul = (365.25*jj - fmod( 365.25*jj, 1.0 )) - 679006.0;
185 djul = djul + floor( 30.6001*(mm + 1) ) + tt + kk;
186 return djul;
187}
188
189void jdgp(double tjul, double & second, int & nweek) {
190 double deltat;
191
192 deltat = tjul - 44244.0 ;
193
194 // current gps week
195
196 nweek = (int) floor(deltat/7.0);
197
198 // seconds past midnight of last weekend
199
200 second = (deltat - (nweek)*7.0)*86400.0;
201
202}
203
204void GPSweekFromYMDhms(int year, int month, int day, int hour, int min,
205 double sec, int& GPSWeek, double& GPSWeeks) {
206
207 double mjd = djul(year, month, day);
208
209 jdgp(mjd, GPSWeeks, GPSWeek);
210 GPSWeeks += hour * 3600.0 + min * 60.0 + sec;
211}
212
213} // namespace BNS
Note: See TracBrowser for help on using the repository browser.