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

Last change on this file since 6812 was 6812, checked in by stoecker, 9 years ago

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