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

Last change on this file since 1049 was 1025, checked in by zdenek, 16 years ago

Zdenek Lukes: added logic for decoding of messages 20/21 from RTCM2.3 streams
File size: 8.5 KB
Line 
1#include <math.h>
2#include <sstream>
3#include <iomanip>
4
5#include "ephemeris.h"
6#include "timeutils.h"
7
8using namespace std;
9
10bool t_eph::isNewerThan(const t_eph* eph) const {
11 if (_GPSweek > eph->_GPSweek ||
12 (_GPSweek == eph->_GPSweek && _GPSweeks > eph->_GPSweeks)) {
13 return true;
14 }
15 else {
16 return false;
17 }
18}
19
20void t_ephGPS::set(const gpsephemeris* ee) {
21 ostringstream prn;
22 prn << 'G' << setfill('0') << setw(2) << ee->satellite;
23
24 _prn = prn.str();
25
26 // TODO: check if following two lines are correct
27 _GPSweek = ee->GPSweek;
28 _GPSweeks = ee->TOE;
29
30 _TOW = ee->TOW;
31 _TOC = ee->TOC;
32 _TOE = ee->TOE;
33 _IODE = ee->IODE;
34 _IODC = ee->IODC;
35
36 _clock_bias = ee->clock_bias ;
37 _clock_drift = ee->clock_drift ;
38 _clock_driftrate = ee->clock_driftrate;
39
40 _Crs = ee->Crs;
41 _Delta_n = ee->Delta_n;
42 _M0 = ee->M0;
43 _Cuc = ee->Cuc;
44 _e = ee->e;
45 _Cus = ee->Cus;
46 _sqrt_A = ee->sqrt_A;
47 _Cic = ee->Cic;
48 _OMEGA0 = ee->OMEGA0;
49 _Cis = ee->Cis;
50 _i0 = ee->i0;
51 _Crc = ee->Crc;
52 _omega = ee->omega;
53 _OMEGADOT = ee->OMEGADOT;
54 _IDOT = ee->IDOT;
55
56 _TGD = ee->TGD;
57}
58
59void t_ephGPS::set(int prn,
60 int GPSWeek,
61 double toc, double toe, double tot,
62 double IODE, double IODC,
63 double clock_bias, double clock_drift, double clock_driftrate,
64 double OMEGA0, double OMEGADOT,
65 double i0, double IDOT,
66 double omega,
67 double M0, double Delta_n,
68 double sqrt_A,
69 double e,
70 double Crc, double Crs,
71 double Cic, double Cis,
72 double Cuc, double Cus,
73 double TGD,
74 int /*health*/) {
75 ostringstream prnstr;
76 prnstr << 'G' << setfill('0') << setw(2) << prn;
77
78 _prn = prnstr.str();
79
80 _GPSweek = GPSWeek;
81 _GPSweeks = toe;
82
83 _TOC = toc;
84 _TOE = toe;
85 _TOW = tot;
86 _IODE = IODE;
87 _IODC = IODC;
88 _clock_bias = clock_bias;
89 _clock_drift = clock_drift;
90 _clock_driftrate = clock_driftrate;
91 _Crs = Crs;
92 _Delta_n = Delta_n;
93 _M0 = M0;
94 _Cuc = Cuc;
95 _e = e;
96 _Cus = Cus;
97 _sqrt_A = sqrt_A;
98 _Cic = Cic;
99 _OMEGA0 = OMEGA0;
100 _Cis = Cis;
101 _i0 = i0;
102 _Crc = Crc;
103 _omega = omega;
104 _OMEGADOT = OMEGADOT;
105 _IDOT = IDOT;
106 _TGD = TGD;
107 //_health = health;
108}
109
110// Compute GPS Satellite Position
111////////////////////////////////////////////////////////////////////////////
112void t_ephGPS::position(int GPSweek, double GPSweeks,
113 double* xc,
114 double* vv) const {
115
116 const static double secPerWeek = 7 * 86400.0;
117 const static double omegaEarth = 7292115.1467e-11;
118 const static double gmWGS = 398.6005e12;
119
120 memset(xc, 0, 4*sizeof(double));
121 memset(vv, 0, 3*sizeof(double));
122
123 double a0 = _sqrt_A * _sqrt_A;
124 if (a0 == 0) {
125 return;
126 }
127
128 double n0 = sqrt(gmWGS/(a0*a0*a0));
129 double tk = GPSweeks - _TOE;
130 if (GPSweek != _GPSweek) {
131 tk += (GPSweek - _GPSweek) * secPerWeek;
132 }
133 double n = n0 + _Delta_n;
134 double M = _M0 + n*tk;
135 double E = M;
136 double E_last;
137 do {
138 E_last = E;
139 E = M + _e*sin(E);
140 } while ( fabs(E-E_last)*a0 > 0.001 );
141 double v = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
142 double u0 = v + _omega;
143 double sin2u0 = sin(2*u0);
144 double cos2u0 = cos(2*u0);
145 double r = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
146 double i = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
147 double u = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
148 double xp = r*cos(u);
149 double yp = r*sin(u);
150 double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
151 omegaEarth*_TOE;
152
153 double sinom = sin(OM);
154 double cosom = cos(OM);
155 double sini = sin(i);
156 double cosi = cos(i);
157 xc[0] = xp*cosom - yp*cosi*sinom;
158 xc[1] = xp*sinom + yp*cosi*cosom;
159 xc[2] = yp*sini;
160
161 double tc = GPSweeks - _TOC;
162 if (GPSweek != _GPSweek) {
163 tc += (GPSweek - _GPSweek) * secPerWeek;
164 }
165 xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc
166 - 4.442807633e-10 * _e * sqrt(a0) *sin(E);
167
168 // Velocity
169 // --------
170 double tanv2 = tan(v/2);
171 double dEdM = 1 / (1 - _e*cos(E));
172 double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
173 * dEdM * n;
174 double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
175 double dotom = _OMEGADOT - omegaEarth;
176 double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
177 double dotr = a0 * _e*sin(E) * dEdM * n
178 + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
179 double dotx = dotr*cos(u) - r*sin(u)*dotu;
180 double doty = dotr*sin(u) + r*cos(u)*dotu;
181
182 vv[0] = cosom *dotx - cosi*sinom *doty // dX / dr
183 - xp*sinom*dotom - yp*cosi*cosom*dotom // dX / dOMEGA
184 + yp*sini*sinom*doti; // dX / di
185
186 vv[1] = sinom *dotx + cosi*cosom *doty
187 + xp*cosom*dotom - yp*cosi*sinom*dotom
188 - yp*sini*cosom*doti;
189
190 vv[2] = sini *doty + yp*cosi *doti;
191}
192
193
194void t_ephGPS::print(std::ostream& out) const {
195 double toc_mjd = gpjd(_TOC, _GPSweek);
196 long toc_y, toc_m;
197 int toc_d;
198 double toc_dd;
199 jmt(toc_mjd, toc_y, toc_m, toc_dd);
200 toc_d = static_cast<int>(toc_dd);
201
202 int toc_hour = static_cast<int>( _TOC/3600 );
203 int toc_min = static_cast<int>( (_TOC/3600 - toc_hour)*60 );
204 double toc_sec = _TOC - toc_hour*3600 - toc_min*60;
205 toc_hour = toc_hour % 24;
206
207 char tmps[20];
208 int year;
209
210 year = toc_y;
211 if (year>2000)
212 year-=2000;
213 else
214 year-=1900;
215
216
217 out << _prn.substr(1,2);
218 sprintf(tmps,"%02d", year); out << setw(3) << tmps;
219 out << setw(3) << toc_m;
220 out << setw(3) << toc_d;
221 out << setw(3) << toc_hour;
222 out << setw(3) << toc_min;
223 sprintf(tmps,"%.1f", toc_sec); out << setw(5) << tmps;
224 sprintf(tmps,"%.12E", _clock_bias); out << setw(19) << tmps;
225 sprintf(tmps,"%.12E", _clock_drift); out << setw(19) << tmps;
226 sprintf(tmps,"%.12E", _clock_driftrate); out << setw(19) << tmps;
227 out << endl;
228 out << " ";
229 sprintf(tmps,"%.12E", (double)_IODE); out << setw(19) << tmps;
230 sprintf(tmps,"%.12E", _Crs); out << setw(19) << tmps;
231 sprintf(tmps,"%.12E", _Delta_n); out << setw(19) << tmps;
232 sprintf(tmps,"%.12E", _M0); out << setw(19) << tmps;
233 out << endl;
234 out << " ";
235 sprintf(tmps,"%.12E", _Cuc); out << setw(19) << tmps;
236 sprintf(tmps,"%.12E", _e); out << setw(19) << tmps;
237 sprintf(tmps,"%.12E", _Cus); out << setw(19) << tmps;
238 sprintf(tmps,"%.12E", _sqrt_A); out << setw(19) << tmps;
239 out << endl;
240 out << " ";
241 sprintf(tmps,"%.12E", _TOE); out << setw(19) << tmps;
242 sprintf(tmps,"%.12E", _Cic); out << setw(19) << tmps;
243 sprintf(tmps,"%.12E", _OMEGA0); out << setw(19) << tmps;
244 sprintf(tmps,"%.12E", _Cis); out << setw(19) << tmps;
245 out << endl;
246 out << " ";
247 sprintf(tmps,"%.12E", _i0); out << setw(19) << tmps;
248 sprintf(tmps,"%.12E", _Crc); out << setw(19) << tmps;
249 sprintf(tmps,"%.12E", _omega); out << setw(19) << tmps;
250 sprintf(tmps,"%.12E", _OMEGADOT); out << setw(19) << tmps;
251 out << endl;
252 out << " ";
253 sprintf(tmps,"%.12E", _IDOT); out << setw(19) << tmps;
254 sprintf(tmps,"%.12E", 0.0); out << setw(19) << tmps;
255 sprintf(tmps,"%.12E", (double)_GPSweek); out << setw(19) << tmps;
256 sprintf(tmps,"%.12E", 0.0); out << setw(19) << tmps;
257 out << endl;
258 out << " ";
259 sprintf(tmps,"%.12E", 0.0); out << setw(19) << tmps;
260 sprintf(tmps,"%.12E", 0.0); out << setw(19) << tmps;
261 sprintf(tmps,"%.12E", _TGD); out << setw(19) << tmps;
262 sprintf(tmps,"%.12E", (double)_IODC); out << setw(19) << tmps;
263 out << endl;
264 out << " ";
265 sprintf(tmps,"%.12E", _TOE); out << setw(19) << tmps;
266 sprintf(tmps,"%.12E", 0.0); out << setw(19) << tmps;
267 sprintf(tmps,"%.12E", 0.0); out << setw(19) << tmps;
268 sprintf(tmps,"%.12E", 0.0); out << setw(19) << tmps;
269 out << endl;
270}
Note: See TracBrowser for help on using the repository browser.