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

Last change on this file since 6799 was 6799, checked in by stuerze, 9 years ago

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