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

Last change on this file since 6630 was 6627, checked in by stoecker, 10 years ago

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