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

Last change on this file since 6814 was 6814, checked in by stuerze, 11 years ago

QZSS message type added in GPS ephEncoder

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