source: ntrip/trunk/BNC/RTCM3/ephemeris.cpp@ 2221

Last change on this file since 2221 was 2221, checked in by mervart, 14 years ago

* empty log message *
File size: 11.5 KB
Line 
1#include <math.h>
2#include <sstream>
3#include <iomanip>
4#include <cstring>
5
6#include "ephemeris.h"
7#include "bncutils.h"
8#include "timeutils.h"
9
10using namespace std;
11
12bool t_eph::isNewerThan(const t_eph* eph) const {
13 if (_GPSweek > eph->_GPSweek ||
14 (_GPSweek == eph->_GPSweek && _GPSweeks > eph->_GPSweeks)) {
15 return true;
16 }
17 else {
18 return false;
19 }
20}
21
22void t_ephGPS::set(const gpsephemeris* ee) {
23 ostringstream prn;
24 prn << 'G' << setfill('0') << setw(2) << ee->satellite;
25
26 _prn = prn.str();
27
28 // TODO: check if following two lines are correct
29 _GPSweek = ee->GPSweek;
30 _GPSweeks = ee->TOE;
31
32 _TOW = ee->TOW;
33 _TOC = ee->TOC;
34 _TOE = ee->TOE;
35 _IODE = ee->IODE;
36 _IODC = ee->IODC;
37
38 _clock_bias = ee->clock_bias ;
39 _clock_drift = ee->clock_drift ;
40 _clock_driftrate = ee->clock_driftrate;
41
42 _Crs = ee->Crs;
43 _Delta_n = ee->Delta_n;
44 _M0 = ee->M0;
45 _Cuc = ee->Cuc;
46 _e = ee->e;
47 _Cus = ee->Cus;
48 _sqrt_A = ee->sqrt_A;
49 _Cic = ee->Cic;
50 _OMEGA0 = ee->OMEGA0;
51 _Cis = ee->Cis;
52 _i0 = ee->i0;
53 _Crc = ee->Crc;
54 _omega = ee->omega;
55 _OMEGADOT = ee->OMEGADOT;
56 _IDOT = ee->IDOT;
57
58 _TGD = ee->TGD;
59}
60
61void t_ephGPS::set(int prn,
62 int GPSWeek,
63 double toc, double toe, double tot,
64 double IODE, double IODC,
65 double clock_bias, double clock_drift, double clock_driftrate,
66 double OMEGA0, double OMEGADOT,
67 double i0, double IDOT,
68 double omega,
69 double M0, double Delta_n,
70 double sqrt_A,
71 double e,
72 double Crc, double Crs,
73 double Cic, double Cis,
74 double Cuc, double Cus,
75 double TGD,
76 int /*health*/) {
77 ostringstream prnstr;
78 prnstr << 'G' << setfill('0') << setw(2) << prn;
79
80 _prn = prnstr.str();
81
82 _GPSweek = GPSWeek;
83 _GPSweeks = toe;
84
85 _TOC = toc;
86 _TOE = toe;
87 _TOW = tot;
88 _IODE = IODE;
89 _IODC = IODC;
90 _clock_bias = clock_bias;
91 _clock_drift = clock_drift;
92 _clock_driftrate = clock_driftrate;
93 _Crs = Crs;
94 _Delta_n = Delta_n;
95 _M0 = M0;
96 _Cuc = Cuc;
97 _e = e;
98 _Cus = Cus;
99 _sqrt_A = sqrt_A;
100 _Cic = Cic;
101 _OMEGA0 = OMEGA0;
102 _Cis = Cis;
103 _i0 = i0;
104 _Crc = Crc;
105 _omega = omega;
106 _OMEGADOT = OMEGADOT;
107 _IDOT = IDOT;
108 _TGD = TGD;
109 //_health = health;
110}
111
112// Compute GPS Satellite Position
113////////////////////////////////////////////////////////////////////////////
114void t_ephGPS::position(int GPSweek, double GPSweeks,
115 double* xc,
116 double* vv) const {
117
118 static const double secPerWeek = 7 * 86400.0;
119 static const double omegaEarth = 7292115.1467e-11;
120 static const double gmWGS = 398.6005e12;
121
122 memset(xc, 0, 4*sizeof(double));
123 memset(vv, 0, 3*sizeof(double));
124
125 double a0 = _sqrt_A * _sqrt_A;
126 if (a0 == 0) {
127 return;
128 }
129
130 double n0 = sqrt(gmWGS/(a0*a0*a0));
131 double tk = GPSweeks - _TOE;
132 if (GPSweek != _GPSweek) {
133 tk += (GPSweek - _GPSweek) * secPerWeek;
134 }
135 double n = n0 + _Delta_n;
136 double M = _M0 + n*tk;
137 double E = M;
138 double E_last;
139 do {
140 E_last = E;
141 E = M + _e*sin(E);
142 } while ( fabs(E-E_last)*a0 > 0.001 );
143 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
144 double u0 = v + _omega;
145 double sin2u0 = sin(2*u0);
146 double cos2u0 = cos(2*u0);
147 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
148 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
149 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
150 double xp = r*cos(u);
151 double yp = r*sin(u);
152 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
153 omegaEarth*_TOE;
154
155 double sinom = sin(OM);
156 double cosom = cos(OM);
157 double sini = sin(i);
158 double cosi = cos(i);
159 xc[0] = xp*cosom - yp*cosi*sinom;
160 xc[1] = xp*sinom + yp*cosi*cosom;
161 xc[2] = yp*sini;
162
163 double tc = GPSweeks - _TOC;
164 if (GPSweek != _GPSweek) {
165 tc += (GPSweek - _GPSweek) * secPerWeek;
166 }
167 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc
168 - 4.442807633e-10 * _e * sqrt(a0) *sin(E);
169
170 // Velocity
171 // --------
172 double tanv2 = tan(v/2);
173 double dEdM = 1 / (1 - _e*cos(E));
174 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
175 * dEdM * n;
176 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
177 double dotom = _OMEGADOT - omegaEarth;
178 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
179 double dotr = a0 * _e*sin(E) * dEdM * n
180 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
181 double dotx = dotr*cos(u) - r*sin(u)*dotu;
182 double doty = dotr*sin(u) + r*cos(u)*dotu;
183
184 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
185 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
186 + yp*sini*sinom*doti; // dX / di
187
188 vv[1] = sinom *dotx + cosi*cosom *doty
189 + xp*cosom*dotom - yp*cosi*sinom*dotom
190 - yp*sini*cosom*doti;
191
192 vv[2] = sini *doty + yp*cosi *doti;
193}
194
195
196void t_ephGPS::print(std::ostream& out) const {
197 double toc_mjd = gpjd(_TOC, _GPSweek);
198 long toc_y, toc_m;
199 int toc_d;
200 double toc_dd;
201 jmt(toc_mjd, toc_y, toc_m, toc_dd);
202 toc_d = static_cast<int>(toc_dd);
203
204 int toc_hour = static_cast<int>( _TOC/3600 );
205 int toc_min = static_cast<int>( (_TOC/3600 - toc_hour)*60 );
206 double toc_sec = _TOC - toc_hour*3600 - toc_min*60;
207 toc_hour = toc_hour % 24;
208
209 char tmps[20];
210 int year;
211
212 year = toc_y;
213 if (year>2000)
214 year-=2000;
215 else
216 year-=1900;
217
218
219 out << _prn.substr(1,2);
220 sprintf(tmps,"%02d", year); out << setw(3) << tmps;
221 out << setw(3) << toc_m;
222 out << setw(3) << toc_d;
223 out << setw(3) << toc_hour;
224 out << setw(3) << toc_min;
225 sprintf(tmps,"%.1f", toc_sec); out << setw(5) << tmps;
226 sprintf(tmps,"%.12E", _clock_bias); out << setw(19) << tmps;
227 sprintf(tmps,"%.12E", _clock_drift); out << setw(19) << tmps;
228 sprintf(tmps,"%.12E", _clock_driftrate); out << setw(19) << tmps;
229 out << endl;
230 out << " ";
231 sprintf(tmps,"%.12E", (double)_IODE); out << setw(19) << tmps;
232 sprintf(tmps,"%.12E", _Crs); out << setw(19) << tmps;
233 sprintf(tmps,"%.12E", _Delta_n); out << setw(19) << tmps;
234 sprintf(tmps,"%.12E", _M0); out << setw(19) << tmps;
235 out << endl;
236 out << " ";
237 sprintf(tmps,"%.12E", _Cuc); out << setw(19) << tmps;
238 sprintf(tmps,"%.12E", _e); out << setw(19) << tmps;
239 sprintf(tmps,"%.12E", _Cus); out << setw(19) << tmps;
240 sprintf(tmps,"%.12E", _sqrt_A); out << setw(19) << tmps;
241 out << endl;
242 out << " ";
243 sprintf(tmps,"%.12E", _TOE); out << setw(19) << tmps;
244 sprintf(tmps,"%.12E", _Cic); out << setw(19) << tmps;
245 sprintf(tmps,"%.12E", _OMEGA0); out << setw(19) << tmps;
246 sprintf(tmps,"%.12E", _Cis); out << setw(19) << tmps;
247 out << endl;
248 out << " ";
249 sprintf(tmps,"%.12E", _i0); out << setw(19) << tmps;
250 sprintf(tmps,"%.12E", _Crc); out << setw(19) << tmps;
251 sprintf(tmps,"%.12E", _omega); out << setw(19) << tmps;
252 sprintf(tmps,"%.12E", _OMEGADOT); out << setw(19) << tmps;
253 out << endl;
254 out << " ";
255 sprintf(tmps,"%.12E", _IDOT); out << setw(19) << tmps;
256 sprintf(tmps,"%.12E", 0.0); out << setw(19) << tmps;
257 sprintf(tmps,"%.12E", (double)_GPSweek); out << setw(19) << tmps;
258 sprintf(tmps,"%.12E", 0.0); out << setw(19) << tmps;
259 out << endl;
260 out << " ";
261 sprintf(tmps,"%.12E", 0.0); out << setw(19) << tmps;
262 sprintf(tmps,"%.12E", 0.0); out << setw(19) << tmps;
263 sprintf(tmps,"%.12E", _TGD); out << setw(19) << tmps;
264 sprintf(tmps,"%.12E", (double)_IODC); out << setw(19) << tmps;
265 out << endl;
266 out << " ";
267 sprintf(tmps,"%.12E", _TOE); out << setw(19) << tmps;
268 sprintf(tmps,"%.12E", 0.0); out << setw(19) << tmps;
269 sprintf(tmps,"%.12E", 0.0); out << setw(19) << tmps;
270 sprintf(tmps,"%.12E", 0.0); out << setw(19) << tmps;
271 out << endl;
272}
273
274// Derivative of the state vector using a simple force model (static)
275////////////////////////////////////////////////////////////////////////////
276ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv) {
277
278 // State vector components
279 // -----------------------
280 ColumnVector rr = xv.rows(1,3);
281 ColumnVector vv = xv.rows(4,6);
282
283 // Acceleration
284 // ------------
285 static const double GM = 398.60044e12;
286 static const double AE = 6378136.0;
287 static const double OMEGA = 7292115.e-11;
288 static const double C20 = -1082.63e-6;
289
290 double rho = rr.norm_Frobenius();
291 double t1 = -GM/(rho*rho*rho);
292 double t2 = 3.0/2.0 * C20 * (GM*AE*AE) / (rho*rho*rho*rho*rho);
293 double t3 = OMEGA * OMEGA;
294 double t4 = 2.0 * OMEGA;
295 double z2 = rr(3) * rr(3);
296
297 // Vector of derivatives
298 // ---------------------
299 ColumnVector va(6);
300 va(1) = vv(1);
301 va(2) = vv(2);
302 va(3) = vv(3);
303 va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2);
304 va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1);
305 va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3);
306
307 return va;
308}
309
310// Compute Glonass Satellite Position (virtual)
311////////////////////////////////////////////////////////////////////////////
312void t_ephGlo::position(int GPSweek, double GPSweeks,
313 double* xc, double* vv) const {
314
315 static const double secPerWeek = 7 * 86400.0;
316 static const double nominalStep = 10.0;
317
318 memset(xc, 0, 4*sizeof(double));
319 memset(vv, 0, 3*sizeof(double));
320
321 double dtPos = GPSweeks - _tt;
322 if (GPSweek != _GPSweek) {
323 dtPos += (GPSweek - _GPSweek) * secPerWeek;
324 }
325
326 int nSteps = int(fabs(dtPos) / nominalStep) + 1;
327 double step = dtPos / nSteps;
328
329 for (int ii = 1; ii <= nSteps; ii++) {
330 _xv = rungeKutta4(_tt, _xv, step, glo_deriv);
331 _tt += step;
332 }
333
334 // Position and Velocity
335 // ---------------------
336 xc[0] = _xv(1);
337 xc[1] = _xv(2);
338 xc[2] = _xv(3);
339
340 vv[0] = _xv(4);
341 vv[1] = _xv(5);
342 vv[2] = _xv(6);
343
344 // Clock Correction
345 // ----------------
346 double dtClk = GPSweeks - _GPSweeks;
347 if (GPSweek != _GPSweek) {
348 dtClk += (GPSweek - _GPSweek) * secPerWeek;
349 }
350 xc[3] = -_tau + _gamma * dtClk;
351}
352
353// IOD of Glonass Ephemeris (virtual)
354////////////////////////////////////////////////////////////////////////////
355int t_ephGlo::IOD() const {
356
357 bool old = false;
358
359 if (old) { // 5 LSBs of iod are equal to 5 LSBs of tb
360 unsigned int tb = int(fmod(_GPSweeks,86400.0)); //sec of day
361 const int shift = sizeof(tb) * 8 - 5;
362 unsigned int iod = tb << shift;
363 return (iod >> shift);
364 }
365 else {
366 return int(fmod(_tki, 3600)) / 30;
367 }
368}
369
370// Print Glonass Ephemeris (virtual)
371////////////////////////////////////////////////////////////////////////////
372void t_ephGlo::print(std::ostream& out) const {
373
374}
375
376// Set Glonass Ephemeris
377////////////////////////////////////////////////////////////////////////////
378void t_ephGlo::set(const glonassephemeris* ee) {
379
380}
Note: See TracBrowser for help on using the repository browser.