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

Last change on this file since 6807 was 6799, checked in by stuerze, 10 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.