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

Last change on this file since 6212 was 6139, checked in by mervart, 10 years ago
File size: 11.2 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 GPSADDBITS(6,eph._prn.number())
98 GPSADDBITS(10, eph._TOC.gpsw())
99 GPSADDBITS(4, eph._ura)
100 GPSADDBITS(2,eph._L2Codes)
101 GPSADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)
102 /static_cast<double>(1<<13))
103 GPSADDBITS(8, eph._IODE)
104 GPSADDBITS(16, static_cast<int>(eph._TOC.gpssec())>>4)
105 GPSADDBITSFLOAT(8, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)
106 /static_cast<double>(1<<25))
107 GPSADDBITSFLOAT(16, eph._clock_drift, 1.0/static_cast<double>(1<<30)
108 /static_cast<double>(1<<13))
109 GPSADDBITSFLOAT(22, eph._clock_bias, 1.0/static_cast<double>(1<<30)
110 /static_cast<double>(1<<1))
111 GPSADDBITS(10, eph._IODC)
112 GPSADDBITSFLOAT(16, eph._Crs, 1.0/static_cast<double>(1<<5))
113 GPSADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)
114 /static_cast<double>(1<<13))
115 GPSADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
116 GPSADDBITSFLOAT(16, eph._Cuc, 1.0/static_cast<double>(1<<29))
117 GPSADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
118 GPSADDBITSFLOAT(16, eph._Cus, 1.0/static_cast<double>(1<<29))
119 GPSADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
120 GPSADDBITS(16, static_cast<int>(eph._TOEsec)>>4)
121 GPSADDBITSFLOAT(16, eph._Cic, 1.0/static_cast<double>(1<<29))
122 GPSADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)
123 /static_cast<double>(1<<1))
124 GPSADDBITSFLOAT(16, eph._Cis, 1.0/static_cast<double>(1<<29))
125 GPSADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
126 GPSADDBITSFLOAT(16, eph._Crc, 1.0/static_cast<double>(1<<5))
127 GPSADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)
128 /static_cast<double>(1<<1))
129 GPSADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)
130 /static_cast<double>(1<<13))
131 GPSADDBITSFLOAT(8, eph._TGD, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
132 GPSADDBITS(6, eph._health)
133 GPSADDBITS(1, eph._L2PFlag)
134 GPSADDBITS(1, 0) /* GPS fit interval */
135
136 startbuffer[0]=0xD3;
137 startbuffer[1]=(size >> 8);
138 startbuffer[2]=size;
139 unsigned long i = CRC24(size+3, startbuffer);
140 buffer[size++] = i >> 16;
141 buffer[size++] = i >> 8;
142 buffer[size++] = i;
143 size += 3;
144 return size;
145}
146
147// build up RTCM3 for GLONASS
148////////////////////////////////////////////////////////////////////////////
149#define GLONASSTOINT(type, value) static_cast<type>(round(value))
150
151#define GLONASSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
152 |(GLONASSTOINT(long long,b)&((1ULL<<(a))-1)); \
153 numbits += (a); \
154 while(numbits >= 8) { \
155 buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
156#define GLONASSADDBITSFLOATM(a,b,c) {int s; long long i; \
157 if(b < 0.0) \
158 { \
159 s = 1; \
160 i = GLONASSTOINT(long long,(-b)/(c)); \
161 if(!i) s = 0; \
162 } \
163 else \
164 { \
165 s = 0; \
166 i = GLONASSTOINT(long long,(b)/(c)); \
167 } \
168 GLONASSADDBITS(1,s) \
169 GLONASSADDBITS(a-1,i)}
170
171int t_ephEncoder::RTCM3(const t_ephGlo& eph, unsigned char *buffer)
172{
173
174 int size = 0;
175 int numbits = 0;
176 long long bitbuffer = 0;
177 unsigned char *startbuffer = buffer;
178 buffer= buffer+3;
179
180 GLONASSADDBITS(12, 1020)
181 GLONASSADDBITS(6, eph._prn.number())
182 GLONASSADDBITS(5, 7+eph._frequency_number)
183 GLONASSADDBITS(1, 0)
184 GLONASSADDBITS(1, 0)
185 GLONASSADDBITS(2, 0)
186 eph._tki=eph._tki+3*60*60;
187 GLONASSADDBITS(5, static_cast<int>(eph._tki)/(60*60))
188 GLONASSADDBITS(6, (static_cast<int>(eph._tki)/60)%60)
189 GLONASSADDBITS(1, (static_cast<int>(eph._tki)/30)%30)
190 GLONASSADDBITS(1, eph._health)
191 GLONASSADDBITS(1, 0)
192 unsigned long long timeofday = (static_cast<int>(eph._tt.gpssec()+3*60*60-eph._gps_utc)%86400);
193 GLONASSADDBITS(7, timeofday/60/15)
194 GLONASSADDBITSFLOATM(24, eph._x_velocity*1000, 1000.0/static_cast<double>(1<<20))
195 GLONASSADDBITSFLOATM(27, eph._x_pos*1000, 1000.0/static_cast<double>(1<<11))
196 GLONASSADDBITSFLOATM(5, eph._x_acceleration*1000, 1000.0/static_cast<double>(1<<30))
197 GLONASSADDBITSFLOATM(24, eph._y_velocity*1000, 1000.0/static_cast<double>(1<<20))
198 GLONASSADDBITSFLOATM(27, eph._y_pos*1000, 1000.0/static_cast<double>(1<<11))
199 GLONASSADDBITSFLOATM(5, eph._y_acceleration*1000, 1000.0/static_cast<double>(1<<30))
200 GLONASSADDBITSFLOATM(24, eph._z_velocity*1000, 1000.0/static_cast<double>(1<<20))
201 GLONASSADDBITSFLOATM(27,eph._z_pos*1000, 1000.0/static_cast<double>(1<<11))
202 GLONASSADDBITSFLOATM(5, eph._z_acceleration*1000, 1000.0/static_cast<double>(1<<30))
203 GLONASSADDBITS(1, 0)
204 GLONASSADDBITSFLOATM(11, eph._gamma, 1.0/static_cast<double>(1<<30)
205 /static_cast<double>(1<<10))
206 GLONASSADDBITS(2, 0) /* GLONASS-M P */
207 GLONASSADDBITS(1, 0) /* GLONASS-M ln(3) */
208 GLONASSADDBITSFLOATM(22, eph._tau, 1.0/static_cast<double>(1<<30))
209 GLONASSADDBITS(5, 0) /* GLONASS-M delta tau */
210 GLONASSADDBITS(5, eph._E)
211 GLONASSADDBITS(1, 0) /* GLONASS-M P4 */
212 GLONASSADDBITS(4, 0) /* GLONASS-M FT */
213 GLONASSADDBITS(11, 0) /* GLONASS-M NT */
214 GLONASSADDBITS(2, 0) /* GLONASS-M active? */
215 GLONASSADDBITS(1, 0) /* GLONASS additional data */
216 GLONASSADDBITS(11, 0) /* GLONASS NA */
217 GLONASSADDBITS(32, 0) /* GLONASS tau C */
218 GLONASSADDBITS(5, 0) /* GLONASS-M N4 */
219 GLONASSADDBITS(22, 0) /* GLONASS-M tau GPS */
220 GLONASSADDBITS(1, 0) /* GLONASS-M ln(5) */
221 GLONASSADDBITS(7, 0) /* Reserved */
222
223 startbuffer[0]=0xD3;
224 startbuffer[1]=(size >> 8);
225 startbuffer[2]=size;
226 unsigned long i = CRC24(size+3, startbuffer);
227 buffer[size++] = i >> 16;
228 buffer[size++] = i >> 8;
229 buffer[size++] = i;
230 size += 3;
231 return size;
232}
233
234// build up RTCM3 for Galileo
235////////////////////////////////////////////////////////////////////////////
236#define GALILEOTOINT(type, value) static_cast<type>(round(value))
237
238#define GALILEOADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
239 |(GALILEOTOINT(long long,b)&((1LL<<a)-1)); \
240 numbits += (a); \
241 while(numbits >= 8) { \
242 buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
243#define GALILEOADDBITSFLOAT(a,b,c) {long long i = GALILEOTOINT(long long,(b)/(c)); \
244 GALILEOADDBITS(a,i)};
245
246int t_ephEncoder::RTCM3(const t_ephGal& eph, unsigned char *buffer) {
247 int size = 0;
248 int numbits = 0;
249 long long bitbuffer = 0;
250 unsigned char *startbuffer = buffer;
251 buffer= buffer+3;
252
253 bool inav = ( (eph._flags & GALEPHF_INAV) == GALEPHF_INAV );
254
255 GALILEOADDBITS(12, inav ? 1046 : 1045)
256 GALILEOADDBITS(6, eph._prn.number())
257 GALILEOADDBITS(12, eph._TOC.gpsw())
258 GALILEOADDBITS(10, eph._IODnav)
259 GALILEOADDBITS(8, eph._SISA)
260 GALILEOADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)
261 /static_cast<double>(1<<13))
262 GALILEOADDBITS(14, eph._TOC.gpssec()/60)
263 GALILEOADDBITSFLOAT(6, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)
264 /static_cast<double>(1<<29))
265 GALILEOADDBITSFLOAT(21, eph._clock_drift, 1.0/static_cast<double>(1<<30)
266 /static_cast<double>(1<<16))
267 GALILEOADDBITSFLOAT(31, eph._clock_bias, 1.0/static_cast<double>(1<<30)
268 /static_cast<double>(1<<4))
269 GALILEOADDBITSFLOAT(16, eph._Crs, 1.0/static_cast<double>(1<<5))
270 GALILEOADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)
271 /static_cast<double>(1<<13))
272 GALILEOADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
273 GALILEOADDBITSFLOAT(16, eph._Cuc, 1.0/static_cast<double>(1<<29))
274 GALILEOADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
275 GALILEOADDBITSFLOAT(16, eph._Cus, 1.0/static_cast<double>(1<<29))
276 GALILEOADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
277 GALILEOADDBITS(14, eph._TOEsec/60)
278 GALILEOADDBITSFLOAT(16, eph._Cic, 1.0/static_cast<double>(1<<29))
279 GALILEOADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)
280 /static_cast<double>(1<<1))
281 GALILEOADDBITSFLOAT(16, eph._Cis, 1.0/static_cast<double>(1<<29))
282 GALILEOADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
283 GALILEOADDBITSFLOAT(16, eph._Crc, 1.0/static_cast<double>(1<<5))
284 GALILEOADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)
285 /static_cast<double>(1<<1))
286 GALILEOADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)
287 /static_cast<double>(1<<13))
288 GALILEOADDBITSFLOAT(10, eph._BGD_1_5A, 1.0/static_cast<double>(1<<30)
289 /static_cast<double>(1<<2))
290 if(inav)
291 {
292 GALILEOADDBITSFLOAT(10, eph._BGD_1_5B, 1.0/static_cast<double>(1<<30)
293 /static_cast<double>(1<<2))
294 GALILEOADDBITS(2, static_cast<int>(eph._E5bHS))
295 GALILEOADDBITS(1, eph._flags & GALEPHF_E5BDINVALID)
296 }
297 else
298 {
299 GALILEOADDBITS(2, static_cast<int>(eph._E5aHS))
300 GALILEOADDBITS(1, eph._flags & GALEPHF_E5ADINVALID)
301 }
302 //// eph._TOEsec = 0.9999E9;
303 GALILEOADDBITS(20, eph._TOEsec)
304
305 GALILEOADDBITS(inav ? 1 : 3, 0)
306
307 startbuffer[0]=0xD3;
308 startbuffer[1]=(size >> 8);
309 startbuffer[2]=size;
310 unsigned long i = CRC24(size+3, startbuffer);
311 buffer[size++] = i >> 16;
312 buffer[size++] = i >> 8;
313 buffer[size++] = i;
314 size += 3;
315 return size;
316}
317
Note: See TracBrowser for help on using the repository browser.