source: ntrip/trunk/BNC/src/RTCM3/ephEncoder.cpp@ 7481

Last change on this file since 7481 was 7053, checked in by stuerze, 10 years ago

CRC24 calculation as well as some definitions required for ephemeris encoding were shifted to have them also somewhere else available

File size: 14.3 KB
Line 
1
2#include "ephEncoder.h"
3
4using namespace std;
5
6
7// build up RTCM3 for GPS
8////////////////////////////////////////////////////////////////////////////
9int t_ephEncoder::RTCM3(const t_ephGPS& eph, unsigned char *buffer) {
10
11 unsigned char *startbuffer = buffer;
12 buffer= buffer+3;
13 int size = 0;
14 int numbits = 0;
15 unsigned long long bitbuffer = 0;
16 int ura = indexFromAccuracy(eph._ura, eph.type());
17 if (eph.type() == t_eph::QZSS) {
18 GPSADDBITS(12, 1044)
19 GPSADDBITS(4,eph._prn.number())
20 GPSADDBITS(16, static_cast<int>(eph._TOC.gpssec())>>4)
21 GPSADDBITSFLOAT(8, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)
22 /static_cast<double>(1<<25))
23 GPSADDBITSFLOAT(16, eph._clock_drift, 1.0/static_cast<double>(1<<30)
24 /static_cast<double>(1<<13))
25 GPSADDBITSFLOAT(22, eph._clock_bias, 1.0/static_cast<double>(1<<30)
26 /static_cast<double>(1<<1))
27 GPSADDBITS(8, eph._IODE)
28 GPSADDBITSFLOAT(16, eph._Crs, 1.0/static_cast<double>(1<<5))
29 GPSADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)
30 /static_cast<double>(1<<13))
31 GPSADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
32 GPSADDBITSFLOAT(16, eph._Cuc, 1.0/static_cast<double>(1<<29))
33 GPSADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
34 GPSADDBITSFLOAT(16, eph._Cus, 1.0/static_cast<double>(1<<29))
35 GPSADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
36 GPSADDBITS(16, static_cast<int>(eph._TOEsec)>>4)
37 GPSADDBITSFLOAT(16, eph._Cic, 1.0/static_cast<double>(1<<29))
38 GPSADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)
39 /static_cast<double>(1<<1))
40 GPSADDBITSFLOAT(16, eph._Cis, 1.0/static_cast<double>(1<<29))
41 GPSADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
42 GPSADDBITSFLOAT(16, eph._Crc, 1.0/static_cast<double>(1<<5))
43 GPSADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)
44 /static_cast<double>(1<<1))
45 GPSADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)
46 /static_cast<double>(1<<13))
47 GPSADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)
48 /static_cast<double>(1<<13))
49 GPSADDBITS(2,eph._L2Codes)
50 GPSADDBITS(10, eph._TOC.gpsw())
51 GPSADDBITS(4, ura)
52 GPSADDBITS(6, eph._health)
53 GPSADDBITSFLOAT(8, eph._TGD, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
54 GPSADDBITS(10, eph._IODC)
55 GPSADDBITS(1, eph._fitInterval)
56 GPSADDBITS(3, 0) /* padding */
57 }
58 else {
59 GPSADDBITS(12, 1019)
60 GPSADDBITS(6,eph._prn.number())
61 GPSADDBITS(10, eph._TOC.gpsw())
62 GPSADDBITS(4, ura)
63 GPSADDBITS(2,eph._L2Codes)
64 GPSADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)
65 /static_cast<double>(1<<13))
66 GPSADDBITS(8, eph._IODE)
67 GPSADDBITS(16, static_cast<int>(eph._TOC.gpssec())>>4)
68 GPSADDBITSFLOAT(8, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)
69 /static_cast<double>(1<<25))
70 GPSADDBITSFLOAT(16, eph._clock_drift, 1.0/static_cast<double>(1<<30)
71 /static_cast<double>(1<<13))
72 GPSADDBITSFLOAT(22, eph._clock_bias, 1.0/static_cast<double>(1<<30)
73 /static_cast<double>(1<<1))
74 GPSADDBITS(10, eph._IODC)
75 GPSADDBITSFLOAT(16, eph._Crs, 1.0/static_cast<double>(1<<5))
76 GPSADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)
77 /static_cast<double>(1<<13))
78 GPSADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
79 GPSADDBITSFLOAT(16, eph._Cuc, 1.0/static_cast<double>(1<<29))
80 GPSADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
81 GPSADDBITSFLOAT(16, eph._Cus, 1.0/static_cast<double>(1<<29))
82 GPSADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
83 GPSADDBITS(16, static_cast<int>(eph._TOEsec)>>4)
84 GPSADDBITSFLOAT(16, eph._Cic, 1.0/static_cast<double>(1<<29))
85 GPSADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)
86 /static_cast<double>(1<<1))
87 GPSADDBITSFLOAT(16, eph._Cis, 1.0/static_cast<double>(1<<29))
88 GPSADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
89 GPSADDBITSFLOAT(16, eph._Crc, 1.0/static_cast<double>(1<<5))
90 GPSADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)
91 /static_cast<double>(1<<1))
92 GPSADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)
93 /static_cast<double>(1<<13))
94 GPSADDBITSFLOAT(8, eph._TGD, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
95 GPSADDBITS(6, eph._health)
96 GPSADDBITS(1, eph._L2PFlag)
97 GPSADDBITS(1, eph._fitInterval)
98 }
99
100 startbuffer[0]=0xD3;
101 startbuffer[1]=(size >> 8);
102 startbuffer[2]=size;
103 unsigned long i = CRC24(size+3, startbuffer);
104 buffer[size++] = i >> 16;
105 buffer[size++] = i >> 8;
106 buffer[size++] = i;
107 size += 3;
108 return size;
109}
110
111// build up RTCM3 for GLONASS
112////////////////////////////////////////////////////////////////////////////
113int t_ephEncoder::RTCM3(const t_ephGlo& eph, unsigned char *buffer)
114{
115
116 int size = 0;
117 int numbits = 0;
118 long long bitbuffer = 0;
119 unsigned char *startbuffer = buffer;
120 buffer= buffer+3;
121
122 GLONASSADDBITS(12, 1020)
123 GLONASSADDBITS(6, eph._prn.number())
124 GLONASSADDBITS(5, 7+eph._frequency_number)
125 GLONASSADDBITS(1, 0)
126 GLONASSADDBITS(1, 0)
127 GLONASSADDBITS(2, 0)
128 eph._tki=eph._tki+3*60*60;
129 GLONASSADDBITS(5, static_cast<int>(eph._tki)/(60*60))
130 GLONASSADDBITS(6, (static_cast<int>(eph._tki)/60)%60)
131 GLONASSADDBITS(1, (static_cast<int>(eph._tki)/30)%30)
132 GLONASSADDBITS(1, eph._health)
133 GLONASSADDBITS(1, 0)
134 unsigned long long timeofday = (static_cast<int>(eph._tt.gpssec()+3*60*60-eph._gps_utc)%86400);
135 GLONASSADDBITS(7, timeofday/60/15)
136 GLONASSADDBITSFLOATM(24, eph._x_velocity*1000, 1000.0/static_cast<double>(1<<20))
137 GLONASSADDBITSFLOATM(27, eph._x_pos*1000, 1000.0/static_cast<double>(1<<11))
138 GLONASSADDBITSFLOATM(5, eph._x_acceleration*1000, 1000.0/static_cast<double>(1<<30))
139 GLONASSADDBITSFLOATM(24, eph._y_velocity*1000, 1000.0/static_cast<double>(1<<20))
140 GLONASSADDBITSFLOATM(27, eph._y_pos*1000, 1000.0/static_cast<double>(1<<11))
141 GLONASSADDBITSFLOATM(5, eph._y_acceleration*1000, 1000.0/static_cast<double>(1<<30))
142 GLONASSADDBITSFLOATM(24, eph._z_velocity*1000, 1000.0/static_cast<double>(1<<20))
143 GLONASSADDBITSFLOATM(27,eph._z_pos*1000, 1000.0/static_cast<double>(1<<11))
144 GLONASSADDBITSFLOATM(5, eph._z_acceleration*1000, 1000.0/static_cast<double>(1<<30))
145 GLONASSADDBITS(1, 0)
146 GLONASSADDBITSFLOATM(11, eph._gamma, 1.0/static_cast<double>(1<<30)
147 /static_cast<double>(1<<10))
148 GLONASSADDBITS(2, 0) /* GLONASS-M P */
149 GLONASSADDBITS(1, 0) /* GLONASS-M ln(3) */
150 GLONASSADDBITSFLOATM(22, eph._tau, 1.0/static_cast<double>(1<<30))
151 GLONASSADDBITS(5, 0) /* GLONASS-M delta tau */
152 GLONASSADDBITS(5, eph._E)
153 GLONASSADDBITS(1, 0) /* GLONASS-M P4 */
154 GLONASSADDBITS(4, 0) /* GLONASS-M FT */
155 GLONASSADDBITS(11, 0) /* GLONASS-M NT */
156 GLONASSADDBITS(2, 0) /* GLONASS-M active? */
157 GLONASSADDBITS(1, 0) /* GLONASS additional data */
158 GLONASSADDBITS(11, 0) /* GLONASS NA */
159 GLONASSADDBITS(32, 0) /* GLONASS tau C */
160 GLONASSADDBITS(5, 0) /* GLONASS-M N4 */
161 GLONASSADDBITS(22, 0) /* GLONASS-M tau GPS */
162 GLONASSADDBITS(1, 0) /* GLONASS-M ln(5) */
163 GLONASSADDBITS(7, 0) /* Reserved */
164
165 startbuffer[0]=0xD3;
166 startbuffer[1]=(size >> 8);
167 startbuffer[2]=size;
168 unsigned long i = CRC24(size+3, startbuffer);
169 buffer[size++] = i >> 16;
170 buffer[size++] = i >> 8;
171 buffer[size++] = i;
172 size += 3;
173 return size;
174}
175
176// build up RTCM3 for Galileo
177////////////////////////////////////////////////////////////////////////////
178int t_ephEncoder::RTCM3(const t_ephGal& eph, unsigned char *buffer) {
179 int size = 0;
180 int numbits = 0;
181 long long bitbuffer = 0;
182 unsigned char *startbuffer = buffer;
183 buffer= buffer+3;
184
185 int SISA = indexFromAccuracy(eph._SISA, eph.type());
186
187 GALILEOADDBITS(12, eph._inav ? 1046 : 1045)
188 GALILEOADDBITS(6, eph._prn.number())
189 GALILEOADDBITS(12, eph._TOC.galw())
190 GALILEOADDBITS(10, eph._IODnav)
191 GALILEOADDBITS(8, SISA)
192 GALILEOADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)
193 /static_cast<double>(1<<13))
194 GALILEOADDBITS(14, eph._TOC.gpssec()/60)
195 GALILEOADDBITSFLOAT(6, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)
196 /static_cast<double>(1<<29))
197 GALILEOADDBITSFLOAT(21, eph._clock_drift, 1.0/static_cast<double>(1<<30)
198 /static_cast<double>(1<<16))
199 GALILEOADDBITSFLOAT(31, eph._clock_bias, 1.0/static_cast<double>(1<<30)
200 /static_cast<double>(1<<4))
201 GALILEOADDBITSFLOAT(16, eph._Crs, 1.0/static_cast<double>(1<<5))
202 GALILEOADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)
203 /static_cast<double>(1<<13))
204 GALILEOADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
205 GALILEOADDBITSFLOAT(16, eph._Cuc, 1.0/static_cast<double>(1<<29))
206 GALILEOADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
207 GALILEOADDBITSFLOAT(16, eph._Cus, 1.0/static_cast<double>(1<<29))
208 GALILEOADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
209 GALILEOADDBITS(14, eph._TOEsec/60)
210 GALILEOADDBITSFLOAT(16, eph._Cic, 1.0/static_cast<double>(1<<29))
211 GALILEOADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)
212 /static_cast<double>(1<<1))
213 GALILEOADDBITSFLOAT(16, eph._Cis, 1.0/static_cast<double>(1<<29))
214 GALILEOADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
215 GALILEOADDBITSFLOAT(16, eph._Crc, 1.0/static_cast<double>(1<<5))
216 GALILEOADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)
217 /static_cast<double>(1<<1))
218 GALILEOADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)
219 /static_cast<double>(1<<13))
220 GALILEOADDBITSFLOAT(10, eph._BGD_1_5A, 1.0/static_cast<double>(1<<30)
221 /static_cast<double>(1<<2))
222 if(eph._inav)
223 {
224 GALILEOADDBITSFLOAT(10, eph._BGD_1_5B, 1.0/static_cast<double>(1<<30)
225 /static_cast<double>(1<<2))
226 GALILEOADDBITS(2, static_cast<int>(eph._E5bHS))
227 GALILEOADDBITS(1, eph._e5bDataInValid ? 1 : 0)
228 GALILEOADDBITS(2, static_cast<int>(eph._E1_bHS))
229 GALILEOADDBITS(1, eph._e1DataInValid ? 1 : 0)
230 }
231 else
232 {
233 GALILEOADDBITS(2, static_cast<int>(eph._E5aHS))
234 GALILEOADDBITS(1, eph._e5aDataInValid ? 1 : 0)
235 }
236
237 GALILEOADDBITS((eph._inav ? 2 : 7), 0)
238
239 startbuffer[0]=0xD3;
240 startbuffer[1]=(size >> 8);
241 startbuffer[2]=size;
242 unsigned long i = CRC24(size+3, startbuffer);
243 buffer[size++] = i >> 16;
244 buffer[size++] = i >> 8;
245 buffer[size++] = i;
246 size += 3;
247 return size;
248}
249
250// build up RTCM3 for SBAS
251////////////////////////////////////////////////////////////////////////////
252int t_ephEncoder::RTCM3(const t_ephSBAS& eph, unsigned char* buffer) {
253 int size = 0;
254 int numbits = 0;
255 long long bitbuffer = 0;
256 unsigned char *startbuffer = buffer;
257 buffer= buffer+3;
258
259 int ura = indexFromAccuracy(eph._ura, eph.type());
260 SBASADDBITS(12, 1043)
261 SBASADDBITS(6, eph._prn.number()-20)
262 SBASADDBITS(8, eph._IODN)
263 SBASADDBITS(13, static_cast<int>(eph._TOC.daysec())>>4)
264 SBASADDBITS(4, ura)
265 SBASADDBITSFLOAT(30, eph._x_pos, 0.08)
266 SBASADDBITSFLOAT(30, eph._y_pos, 0.08)
267 SBASADDBITSFLOAT(25, eph._z_pos, 0.4)
268 SBASADDBITSFLOAT(17, eph._x_velocity, 0.000625)
269 SBASADDBITSFLOAT(17, eph._y_velocity, 0.000625)
270 SBASADDBITSFLOAT(18, eph._z_velocity, 0.004)
271 SBASADDBITSFLOAT(10, eph._x_acceleration, 0.0000125)
272 SBASADDBITSFLOAT(10, eph._y_acceleration, 0.0000125)
273 SBASADDBITSFLOAT(10, eph._z_acceleration, 0.0000625)
274 SBASADDBITSFLOAT(12, eph._agf0, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
275 SBASADDBITSFLOAT(8, eph._agf1, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<10))
276 SBASADDBITS(2,0);
277
278 startbuffer[0]=0xD3;
279 startbuffer[1]=(size >> 8);
280 startbuffer[2]=size;
281 unsigned long i = CRC24(size+3, startbuffer);
282 buffer[size++] = i >> 16;
283 buffer[size++] = i >> 8;
284 buffer[size++] = i;
285 size += 3;
286 return size;
287}
288
289// build up RTCM3 for BDS
290////////////////////////////////////////////////////////////////////////////
291int t_ephEncoder::RTCM3(const t_ephBDS& eph, unsigned char* buffer) {
292 int size = 0;
293 int numbits = 0;
294 long long bitbuffer = 0;
295 unsigned char *startbuffer = buffer;
296 buffer= buffer+3;
297
298 int URA = indexFromAccuracy(eph._URA, eph.type());
299 BDSADDBITS(12, RTCM3ID_BDS)
300 BDSADDBITS(6, eph._prn.number())
301 BDSADDBITS(13, eph._TOC.bdsw() - 1356.0)
302 BDSADDBITS(4, URA);
303 BDSADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
304 BDSADDBITS(5, eph._AODE)
305 BDSADDBITS(17, static_cast<int>(eph._TOC.bdssec())>>3)
306 BDSADDBITSFLOAT(11, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)
307 /static_cast<double>(1<<30)/static_cast<double>(1<<6))
308 BDSADDBITSFLOAT(22, eph._clock_drift, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<20))
309 BDSADDBITSFLOAT(24, eph._clock_bias, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
310 BDSADDBITS(5, eph._AODC)
311 BDSADDBITSFLOAT(18, eph._Crs, 1.0/static_cast<double>(1<<6))
312 BDSADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
313 BDSADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
314 BDSADDBITSFLOAT(18, eph._Cuc, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
315 BDSADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
316 BDSADDBITSFLOAT(18, eph._Cus, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
317 BDSADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
318 BDSADDBITS(17, static_cast<int>(eph._TOE.bdssec())>>3)
319 BDSADDBITSFLOAT(18, eph._Cic, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
320 BDSADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
321 BDSADDBITSFLOAT(18, eph._Cis, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
322 BDSADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
323 BDSADDBITSFLOAT(18, eph._Crc, 1.0/static_cast<double>(1<<6))
324 BDSADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
325 BDSADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
326 BDSADDBITSFLOAT(10, eph._TGD1, 0.0000000001)
327 BDSADDBITSFLOAT(10, eph._TGD2, 0.0000000001)
328 BDSADDBITS(1, eph._SatH1)
329 BDSADDBITS(1, 0) /* reserved bit, fill up 8 bits */
330
331 startbuffer[0]=0xD3;
332 startbuffer[1]=(size >> 8);
333 startbuffer[2]=size;
334 unsigned long i = CRC24(size+3, startbuffer);
335 buffer[size++] = i >> 16;
336 buffer[size++] = i >> 8;
337 buffer[size++] = i;
338 size += 3;
339 return size;
340}
341
Note: See TracBrowser for help on using the repository browser.