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

Last change on this file since 1629 was 1239, checked in by weber, 16 years ago

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