source: ntrip/trunk/BNS/bnseph.cpp@ 1054

Last change on this file since 1054 was 978, checked in by mervart, 16 years ago

* empty log message *

File size: 10.5 KB
Line 
1/* -------------------------------------------------------------------------
2 * BKG NTRIP Server
3 * -------------------------------------------------------------------------
4 *
5 * Class: bnseph
6 *
7 * Purpose: Retrieve broadcast ephemeris from BNC
8 *
9 * Author: L. Mervart
10 *
11 * Created: 29-Mar-2008
12 *
13 * Changes:
14 *
15 * -----------------------------------------------------------------------*/
16
17#include <iostream>
18
19#include "bnseph.h"
20#include "bnsutils.h"
21
22using namespace std;
23
24// Constructor
25////////////////////////////////////////////////////////////////////////////
26t_bnseph::t_bnseph(QObject* parent) : QThread(parent) {
27
28 this->setTerminationEnabled(true);
29
30 _socket = 0;
31}
32
33// Destructor
34////////////////////////////////////////////////////////////////////////////
35t_bnseph::~t_bnseph() {
36 delete _socket;
37}
38
39// Connect the Socket
40////////////////////////////////////////////////////////////////////////////
41void t_bnseph::reconnect() {
42
43 delete _socket;
44
45 QSettings settings;
46 QString host = settings.value("ephHost").toString();
47 if (host.isEmpty()) {
48 host = "localhost";
49 }
50 int port = settings.value("ephPort").toInt();
51
52 _socket = new QTcpSocket();
53 _socket->connectToHost(host, port);
54
55 const int timeOut = 10*1000; // 10 seconds
56 if (!_socket->waitForConnected(timeOut)) {
57 emit(newMessage("bnseph::run Connect Timeout"));
58 }
59}
60
61// Start
62////////////////////////////////////////////////////////////////////////////
63void t_bnseph::run() {
64
65 emit(newMessage("bnseph::run Start"));
66
67 while (true) {
68 if (_socket == 0 || _socket->state() != QAbstractSocket::ConnectedState) {
69 reconnect();
70 }
71 if (_socket && _socket->state() == QAbstractSocket::ConnectedState) {
72 readEph();
73 }
74 else {
75 msleep(10);
76 }
77 }
78}
79
80// Read One Ephemeris
81////////////////////////////////////////////////////////////////////////////
82void t_bnseph::readEph() {
83
84 t_eph* eph = 0;
85 QByteArray line = waitForLine(_socket);
86
87 QTextStream in(line);
88 QString prn;
89
90 in >> prn;
91
92 int numlines = 0;
93 if (prn.indexOf('R') != -1) {
94 eph = new t_ephGlo();
95 numlines = 4;
96 }
97 else {
98 eph = new t_ephGPS();
99 numlines = 8;
100 }
101
102 QStringList lines;
103 lines << line;
104
105 for (int ii = 2; ii <= numlines; ii++) {
106 QByteArray line = waitForLine(_socket);
107 lines << line;
108 }
109
110 eph->read(lines);
111
112 emit(newEph(eph));
113}
114
115// Compare Time
116////////////////////////////////////////////////////////////////////////////
117bool t_eph::isNewerThan(const t_eph* eph) const {
118 if (_GPSweek > eph->_GPSweek ||
119 (_GPSweek == eph->_GPSweek && _GPSweeks > eph->_GPSweeks)) {
120 return true;
121 }
122 else {
123 return false;
124 }
125}
126
127// Read GPS Ephemeris
128////////////////////////////////////////////////////////////////////////////
129void t_ephGPS::read(const QStringList& lines) {
130
131 for (int ii = 1; ii <= lines.size(); ii++) {
132 QTextStream in(lines.at(ii-1).toAscii());
133
134 if (ii == 1) {
135 double year, month, day, hour, minute, second;
136 in >> _prn >> year >> month >> day >> hour >> minute >> second
137 >> _clock_bias >> _clock_drift >> _clock_driftrate;
138
139 if (year < 100) year += 2000;
140
141 QDateTime dateTime(QDate(int(year), int(month), int(day)),
142 QTime(int(hour), int(minute), int(second)), Qt::UTC);
143
144 GPSweekFromDateAndTime(dateTime, _GPSweek, _GPSweeks);
145 _TOC = _GPSweeks;
146 }
147 else if (ii == 2) {
148 in >> _IODE >> _Crs >> _Delta_n >> _M0;
149 }
150 else if (ii == 3) {
151 in >> _Cuc >> _e >> _Cus >> _sqrt_A;
152 }
153 else if (ii == 4) {
154 in >> _TOE >> _Cic >> _OMEGA0 >> _Cis;
155 }
156 else if (ii == 5) {
157 in >> _i0 >> _Crc >> _omega >> _OMEGADOT;
158 }
159 else if (ii == 6) {
160 in >> _IDOT;
161 }
162 else if (ii == 7) {
163 double hlp, health;
164 in >> hlp >> health >> _TGD >> _IODC;
165 }
166 else if (ii == 8) {
167 in >> _TOW;
168 }
169 }
170}
171
172// Compute GPS Satellite Position
173////////////////////////////////////////////////////////////////////////////
174void t_ephGPS::position(int GPSweek, double GPSweeks, ColumnVector& xc,
175 ColumnVector& vv) const {
176
177 const static double secPerWeek = 7 * 86400.0;
178 const static double omegaEarth = 7292115.1467e-11;
179 const static double gmWGS = 398.6005e12;
180
181 if (xc.Nrows() < 4) {
182 xc.ReSize(4);
183 }
184 xc = 0.0;
185
186 if (vv.Nrows() < 3) {
187 vv.ReSize(3);
188 }
189 vv = 0.0;
190
191 double a0 = _sqrt_A * _sqrt_A;
192 if (a0 == 0) {
193 return;
194 }
195
196 double n0 = sqrt(gmWGS/(a0*a0*a0));
197 double tk = GPSweeks - _TOE;
198 if (GPSweek != _GPSweek) {
199 tk += (GPSweek - _GPSweek) * secPerWeek;
200 }
201 double n = n0 + _Delta_n;
202 double M = _M0 + n*tk;
203 double E = M;
204 double E_last;
205 do {
206 E_last = E;
207 E = M + _e*sin(E);
208 } while ( fabs(E-E_last)*a0 > 0.001 );
209 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
210 double u0 = v + _omega;
211 double sin2u0 = sin(2*u0);
212 double cos2u0 = cos(2*u0);
213 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
214 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
215 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
216 double xp = r*cos(u);
217 double yp = r*sin(u);
218 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
219 omegaEarth*_TOE;
220
221 double sinom = sin(OM);
222 double cosom = cos(OM);
223 double sini = sin(i);
224 double cosi = cos(i);
225 xc(1) = xp*cosom - yp*cosi*sinom;
226 xc(2) = xp*sinom + yp*cosi*cosom;
227 xc(3) = yp*sini;
228
229 double tc = GPSweeks - _TOC;
230 if (GPSweek != _GPSweek) {
231 tc += (GPSweek - _GPSweek) * secPerWeek;
232 }
233 xc(4) = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc
234 - 4.442807633e-10 * _e * sqrt(a0) *sin(E);
235
236 // Velocity
237 // --------
238 double tanv2 = tan(v/2);
239 double dEdM = 1 / (1 - _e*cos(E));
240 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
241 * dEdM * n;
242 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
243 double dotom = _OMEGADOT - omegaEarth;
244 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
245 double dotr = a0 * _e*sin(E) * dEdM * n
246 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
247 double dotx = dotr*cos(u) - r*sin(u)*dotu;
248 double doty = dotr*sin(u) + r*cos(u)*dotu;
249
250 vv(1) = cosom *dotx - cosi*sinom *doty // dX / dr
251 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
252 + yp*sini*sinom*doti; // dX / di
253
254 vv(2) = sinom *dotx + cosi*cosom *doty
255 + xp*cosom*dotom - yp*cosi*sinom*dotom
256 - yp*sini*cosom*doti;
257
258 vv(3) = sini *doty + yp*cosi *doti;
259}
260
261// Read Glonass Ephemeris
262////////////////////////////////////////////////////////////////////////////
263void t_ephGlo::read(const QStringList& lines) {
264
265 for (int ii = 1; ii <= lines.size(); ii++) {
266 QTextStream in(lines.at(ii-1).toAscii());
267
268 if (ii == 1) {
269 double year, month, day, hour, minute, second;
270 in >> _prn >> year >> month >> day >> hour >> minute >> second
271 >> _tau >> _gamma;
272
273 _tau = -_tau;
274
275 if (year < 100) year += 2000;
276
277 QDateTime dateTime(QDate(int(year), int(month), int(day)),
278 QTime(int(hour), int(minute), int(second)), Qt::UTC);
279
280 GPSweekFromDateAndTime(dateTime, _GPSweek, _GPSweeks);
281
282 //// beg test
283 //// _gps_utc = 14.0;
284 //// end test
285
286 _GPSweeks += _gps_utc;
287 }
288 else if (ii == 2) {
289 in >>_x_pos >> _x_velocity >> _x_acceleration >> _health;
290 }
291 else if (ii == 3) {
292 in >>_y_pos >> _y_velocity >> _y_acceleration >> _frequency_number;
293 }
294 else if (ii == 4) {
295 in >>_z_pos >> _z_velocity >> _z_acceleration >> _E;
296 }
297 }
298
299 // Initialize status vector
300 // ------------------------
301 _tt = _GPSweeks;
302
303 _xv(1) = _x_pos * 1.e3;
304 _xv(2) = _y_pos * 1.e3;
305 _xv(3) = _z_pos * 1.e3;
306 _xv(4) = _x_velocity * 1.e3;
307 _xv(5) = _y_velocity * 1.e3;
308 _xv(6) = _z_velocity * 1.e3;
309}
310
311// Derivative of the state vector using a simple force model (static)
312////////////////////////////////////////////////////////////////////////////
313ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv) {
314
315 // State vector components
316 // -----------------------
317 ColumnVector rr = xv.rows(1,3);
318 ColumnVector vv = xv.rows(4,6);
319
320 // Acceleration
321 // ------------
322 const static double GM = 398.60044e12;
323 const static double AE = 6378136.0;
324 const static double OMEGA = 7292115.e-11;
325 const static double C20 = -1082.63e-6;
326
327 double rho = rr.norm_Frobenius();
328 double t1 = -GM/(rho*rho*rho);
329 double t2 = 3.0/2.0 * C20 * (GM*AE*AE) / (rho*rho*rho*rho*rho);
330 double t3 = OMEGA * OMEGA;
331 double t4 = 2.0 * OMEGA;
332 double z2 = rr(3) * rr(3);
333
334 // Vector of derivatives
335 // ---------------------
336 ColumnVector va(6);
337 va(1) = vv(1);
338 va(2) = vv(2);
339 va(3) = vv(3);
340 va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2);
341 va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1);
342 va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3);
343
344 return va;
345}
346
347// Compute Glonass Satellite Position
348////////////////////////////////////////////////////////////////////////////
349void t_ephGlo::position(int GPSweek, double GPSweeks, ColumnVector& xc,
350 ColumnVector& vv) const {
351
352 const static double secPerWeek = 7 * 86400.0;
353 const static double nominalStep = 10.0;
354
355 double dtPos = GPSweeks - _tt;
356 if (GPSweek != _GPSweek) {
357 dtPos += (GPSweek - _GPSweek) * secPerWeek;
358 }
359
360 int nSteps = int(fabs(dtPos) / nominalStep) + 1;
361 double step = dtPos / nSteps;
362
363 for (int ii = 1; ii <= nSteps; ii++) {
364 _xv = rungeKutta4(_tt, _xv, step, glo_deriv);
365 _tt += step;
366 }
367
368 // Position and Velocity
369 // ---------------------
370 xc(1) = _xv(1);
371 xc(2) = _xv(2);
372 xc(3) = _xv(3);
373
374 vv(1) = _xv(4);
375 vv(2) = _xv(5);
376 vv(3) = _xv(6);
377
378 // Clock Correction
379 // ----------------
380 double dtClk = GPSweeks - _GPSweeks;
381 if (GPSweek != _GPSweek) {
382 dtClk += (GPSweek - _GPSweek) * secPerWeek;
383 }
384 xc(4) = -_tau + _gamma * dtClk;
385}
386
387// Glonass IOD
388////////////////////////////////////////////////////////////////////////////
389int t_ephGlo::IOD() const {
390 //// return int(fmod(_GPSweeks,86400.0)) / 600;
391
392 //// unsigned int tb = int(fmod(_GPSweeks,86400.0)) * 1000; // msec of day
393 unsigned int tb = int(fmod(_GPSweeks,86400.0)); //sec of day
394
395 // 5 LSBs of iod are equal to 5 LSBs of tb, remaining bits are zero
396 // ----------------------------------------------------------------
397 const int shift = sizeof(tb) * 8 - 5;
398 unsigned int iod = tb << shift;
399 return (iod >> shift);
400}
Note: See TracBrowser for help on using the repository browser.