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

Last change on this file since 6134 was 5853, checked in by mervart, 10 years ago
File size: 11.1 KB
RevLine 
[5852]1
2#include "ephEncoder.h"
3using namespace std;
4
5// Returns CRC24
6////////////////////////////////////////////////////////////////////////////
7static unsigned long CRC24(long size, const unsigned char *buf) {
8 unsigned long crc = 0;
9 int ii;
10 while (size--) {
11 crc ^= (*buf++) << (16);
12 for(ii = 0; ii < 8; ii++) {
13 crc <<= 1;
14 if (crc & 0x1000000) {
15 crc ^= 0x01864cfb;
16 }
17 }
18 }
19 return crc;
20}
21
22// build up RTCM3 for GPS
23////////////////////////////////////////////////////////////////////////////
24#define GPSTOINT(type, value) static_cast<type>(round(value))
25
26#define GPSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
27 |(GPSTOINT(long long,b)&((1ULL<<a)-1)); \
28 numbits += (a); \
29 while(numbits >= 8) { \
30 buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
31
32#define GPSADDBITSFLOAT(a,b,c) {long long i = GPSTOINT(long long,(b)/(c)); \
33 GPSADDBITS(a,i)};
34
35int t_ephEncoder::RTCM3(const t_ephGPS& eph, unsigned char *buffer) {
36
37 unsigned char *startbuffer = buffer;
38 buffer= buffer+3;
39 int size = 0;
40 int numbits = 0;
41 unsigned long long bitbuffer = 0;
42 if (eph._ura <= 2.40){
43 eph._ura = 0;
44 }
45 else if (eph._ura <= 3.40){
46 eph._ura = 1;
47 }
48 else if (eph._ura <= 6.85){
49 eph._ura = 2;
50 }
51 else if (eph._ura <= 9.65){
52 eph._ura = 3;
53 }
54 else if (eph._ura <= 13.65){
55 eph._ura = 4;
56 }
57 else if (eph._ura <= 24.00){
58 eph._ura = 5;
59 }
60 else if (eph._ura <= 48.00){
61 eph._ura = 6;
62 }
63 else if (eph._ura <= 96.00){
64 eph._ura = 7;
65 }
66 else if (eph._ura <= 192.00){
67 eph._ura = 8;
68 }
69 else if (eph._ura <= 384.00){
70 eph._ura = 9;
71 }
72 else if (eph._ura <= 768.00){
73 eph._ura = 10;
74 }
75 else if (eph._ura <= 1536.00){
76 eph._ura = 11;
77 }
78 else if (eph._ura <= 1536.00){
79 eph._ura = 12;
80 }
81 else if (eph._ura <= 2072.00){
82 eph._ura = 13;
83 }
84 else if (eph._ura <= 6144.00){
85 eph._ura = 14;
86 }
87 else{
88 eph._ura = 15;
89 }
90
91 GPSADDBITS(12, 1019)
92 GPSADDBITS(6,eph._prn.number())
93 GPSADDBITS(10, eph._TOC.gpsw())
94 GPSADDBITS(4, eph._ura)
95 GPSADDBITS(2,eph._L2Codes)
96 GPSADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)
97 /static_cast<double>(1<<13))
98 GPSADDBITS(8, eph._IODE)
99 GPSADDBITS(16, static_cast<int>(eph._TOC.gpssec())>>4)
100 GPSADDBITSFLOAT(8, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)
101 /static_cast<double>(1<<25))
102 GPSADDBITSFLOAT(16, eph._clock_drift, 1.0/static_cast<double>(1<<30)
103 /static_cast<double>(1<<13))
104 GPSADDBITSFLOAT(22, eph._clock_bias, 1.0/static_cast<double>(1<<30)
105 /static_cast<double>(1<<1))
106 GPSADDBITS(10, eph._IODC)
107 GPSADDBITSFLOAT(16, eph._Crs, 1.0/static_cast<double>(1<<5))
108 GPSADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)
109 /static_cast<double>(1<<13))
110 GPSADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
111 GPSADDBITSFLOAT(16, eph._Cuc, 1.0/static_cast<double>(1<<29))
112 GPSADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
113 GPSADDBITSFLOAT(16, eph._Cus, 1.0/static_cast<double>(1<<29))
114 GPSADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
115 GPSADDBITS(16, static_cast<int>(eph._TOEsec)>>4)
116 GPSADDBITSFLOAT(16, eph._Cic, 1.0/static_cast<double>(1<<29))
117 GPSADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)
118 /static_cast<double>(1<<1))
119 GPSADDBITSFLOAT(16, eph._Cis, 1.0/static_cast<double>(1<<29))
120 GPSADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
121 GPSADDBITSFLOAT(16, eph._Crc, 1.0/static_cast<double>(1<<5))
122 GPSADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)
123 /static_cast<double>(1<<1))
124 GPSADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)
125 /static_cast<double>(1<<13))
126 GPSADDBITSFLOAT(8, eph._TGD, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<1))
127 GPSADDBITS(6, eph._health)
128 GPSADDBITS(1, eph._L2PFlag)
129 GPSADDBITS(1, 0) /* GPS fit interval */
130
131 startbuffer[0]=0xD3;
132 startbuffer[1]=(size >> 8);
133 startbuffer[2]=size;
134 unsigned long i = CRC24(size+3, startbuffer);
135 buffer[size++] = i >> 16;
136 buffer[size++] = i >> 8;
137 buffer[size++] = i;
138 size += 3;
139 return size;
140}
141
142// build up RTCM3 for GLONASS
143////////////////////////////////////////////////////////////////////////////
144#define GLONASSTOINT(type, value) static_cast<type>(round(value))
145
146#define GLONASSADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
147 |(GLONASSTOINT(long long,b)&((1ULL<<(a))-1)); \
148 numbits += (a); \
149 while(numbits >= 8) { \
150 buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
151#define GLONASSADDBITSFLOATM(a,b,c) {int s; long long i; \
152 if(b < 0.0) \
153 { \
154 s = 1; \
155 i = GLONASSTOINT(long long,(-b)/(c)); \
156 if(!i) s = 0; \
157 } \
158 else \
159 { \
160 s = 0; \
161 i = GLONASSTOINT(long long,(b)/(c)); \
162 } \
163 GLONASSADDBITS(1,s) \
164 GLONASSADDBITS(a-1,i)}
165
166int t_ephEncoder::RTCM3(const t_ephGlo& eph, unsigned char *buffer)
167{
168
169 int size = 0;
170 int numbits = 0;
171 long long bitbuffer = 0;
172 unsigned char *startbuffer = buffer;
173 buffer= buffer+3;
174
175 GLONASSADDBITS(12, 1020)
[5853]176 GLONASSADDBITS(6, eph._prn.number())
177 GLONASSADDBITS(5, 7+eph._frequency_number)
[5852]178 GLONASSADDBITS(1, 0)
179 GLONASSADDBITS(1, 0)
180 GLONASSADDBITS(2, 0)
[5853]181 eph._tki=eph._tki+3*60*60;
182 GLONASSADDBITS(5, static_cast<int>(eph._tki)/(60*60))
183 GLONASSADDBITS(6, (static_cast<int>(eph._tki)/60)%60)
184 GLONASSADDBITS(1, (static_cast<int>(eph._tki)/30)%30)
185 GLONASSADDBITS(1, eph._health)
[5852]186 GLONASSADDBITS(1, 0)
[5853]187 unsigned long long timeofday = (static_cast<int>(eph._tt.gpssec()+3*60*60-eph._gps_utc)%86400);
[5852]188 GLONASSADDBITS(7, timeofday/60/15)
[5853]189 GLONASSADDBITSFLOATM(24, eph._x_velocity*1000, 1000.0/static_cast<double>(1<<20))
190 GLONASSADDBITSFLOATM(27, eph._x_pos*1000, 1000.0/static_cast<double>(1<<11))
191 GLONASSADDBITSFLOATM(5, eph._x_acceleration*1000, 1000.0/static_cast<double>(1<<30))
192 GLONASSADDBITSFLOATM(24, eph._y_velocity*1000, 1000.0/static_cast<double>(1<<20))
193 GLONASSADDBITSFLOATM(27, eph._y_pos*1000, 1000.0/static_cast<double>(1<<11))
194 GLONASSADDBITSFLOATM(5, eph._y_acceleration*1000, 1000.0/static_cast<double>(1<<30))
195 GLONASSADDBITSFLOATM(24, eph._z_velocity*1000, 1000.0/static_cast<double>(1<<20))
196 GLONASSADDBITSFLOATM(27,eph._z_pos*1000, 1000.0/static_cast<double>(1<<11))
197 GLONASSADDBITSFLOATM(5, eph._z_acceleration*1000, 1000.0/static_cast<double>(1<<30))
[5852]198 GLONASSADDBITS(1, 0)
[5853]199 GLONASSADDBITSFLOATM(11, eph._gamma, 1.0/static_cast<double>(1<<30)
[5852]200 /static_cast<double>(1<<10))
201 GLONASSADDBITS(2, 0) /* GLONASS-M P */
202 GLONASSADDBITS(1, 0) /* GLONASS-M ln(3) */
[5853]203 GLONASSADDBITSFLOATM(22, eph._tau, 1.0/static_cast<double>(1<<30))
[5852]204 GLONASSADDBITS(5, 0) /* GLONASS-M delta tau */
[5853]205 GLONASSADDBITS(5, eph._E)
[5852]206 GLONASSADDBITS(1, 0) /* GLONASS-M P4 */
207 GLONASSADDBITS(4, 0) /* GLONASS-M FT */
208 GLONASSADDBITS(11, 0) /* GLONASS-M NT */
209 GLONASSADDBITS(2, 0) /* GLONASS-M active? */
210 GLONASSADDBITS(1, 0) /* GLONASS additional data */
211 GLONASSADDBITS(11, 0) /* GLONASS NA */
212 GLONASSADDBITS(32, 0) /* GLONASS tau C */
213 GLONASSADDBITS(5, 0) /* GLONASS-M N4 */
214 GLONASSADDBITS(22, 0) /* GLONASS-M tau GPS */
215 GLONASSADDBITS(1, 0) /* GLONASS-M ln(5) */
216 GLONASSADDBITS(7, 0) /* Reserved */
217
218 startbuffer[0]=0xD3;
219 startbuffer[1]=(size >> 8);
220 startbuffer[2]=size;
221 unsigned long i = CRC24(size+3, startbuffer);
222 buffer[size++] = i >> 16;
223 buffer[size++] = i >> 8;
224 buffer[size++] = i;
225 size += 3;
226 return size;
227}
228
229// build up RTCM3 for Galileo
230////////////////////////////////////////////////////////////////////////////
231#define GALILEOTOINT(type, value) static_cast<type>(round(value))
232
233#define GALILEOADDBITS(a, b) {bitbuffer = (bitbuffer<<(a)) \
234 |(GALILEOTOINT(long long,b)&((1LL<<a)-1)); \
235 numbits += (a); \
236 while(numbits >= 8) { \
237 buffer[size++] = bitbuffer>>(numbits-8);numbits -= 8;}}
238#define GALILEOADDBITSFLOAT(a,b,c) {long long i = GALILEOTOINT(long long,(b)/(c)); \
239 GALILEOADDBITS(a,i)};
240
241int t_ephEncoder::RTCM3(const t_ephGal& eph, unsigned char *buffer) {
242 int size = 0;
243 int numbits = 0;
244 long long bitbuffer = 0;
245 unsigned char *startbuffer = buffer;
246 buffer= buffer+3;
247
[5853]248 bool inav = ( (eph._flags & GALEPHF_INAV) == GALEPHF_INAV );
[5852]249
250 GALILEOADDBITS(12, inav ? 1046 : 1045)
[5853]251 GALILEOADDBITS(6, eph._prn.number())
252 GALILEOADDBITS(12, eph._TOC.gpsw())
253 GALILEOADDBITS(10, eph._IODnav)
254 GALILEOADDBITS(8, eph._SISA)
255 GALILEOADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)
[5852]256 /static_cast<double>(1<<13))
[5853]257 GALILEOADDBITS(14, eph._TOC.gpssec()/60)
258 GALILEOADDBITSFLOAT(6, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)
[5852]259 /static_cast<double>(1<<29))
[5853]260 GALILEOADDBITSFLOAT(21, eph._clock_drift, 1.0/static_cast<double>(1<<30)
[5852]261 /static_cast<double>(1<<16))
[5853]262 GALILEOADDBITSFLOAT(31, eph._clock_bias, 1.0/static_cast<double>(1<<30)
[5852]263 /static_cast<double>(1<<4))
[5853]264 GALILEOADDBITSFLOAT(16, eph._Crs, 1.0/static_cast<double>(1<<5))
265 GALILEOADDBITSFLOAT(16, eph._Delta_n, M_PI/static_cast<double>(1<<30)
[5852]266 /static_cast<double>(1<<13))
[5853]267 GALILEOADDBITSFLOAT(32, eph._M0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
268 GALILEOADDBITSFLOAT(16, eph._Cuc, 1.0/static_cast<double>(1<<29))
269 GALILEOADDBITSFLOAT(32, eph._e, 1.0/static_cast<double>(1<<30)/static_cast<double>(1<<3))
270 GALILEOADDBITSFLOAT(16, eph._Cus, 1.0/static_cast<double>(1<<29))
271 GALILEOADDBITSFLOAT(32, eph._sqrt_A, 1.0/static_cast<double>(1<<19))
272 GALILEOADDBITS(14, eph._TOEsec/60)
273 GALILEOADDBITSFLOAT(16, eph._Cic, 1.0/static_cast<double>(1<<29))
274 GALILEOADDBITSFLOAT(32, eph._OMEGA0, M_PI/static_cast<double>(1<<30)
[5852]275 /static_cast<double>(1<<1))
[5853]276 GALILEOADDBITSFLOAT(16, eph._Cis, 1.0/static_cast<double>(1<<29))
277 GALILEOADDBITSFLOAT(32, eph._i0, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<1))
278 GALILEOADDBITSFLOAT(16, eph._Crc, 1.0/static_cast<double>(1<<5))
279 GALILEOADDBITSFLOAT(32, eph._omega, M_PI/static_cast<double>(1<<30)
[5852]280 /static_cast<double>(1<<1))
[5853]281 GALILEOADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)
[5852]282 /static_cast<double>(1<<13))
[5853]283 GALILEOADDBITSFLOAT(10, eph._BGD_1_5A, 1.0/static_cast<double>(1<<30)
[5852]284 /static_cast<double>(1<<2))
285 if(inav)
286 {
[5853]287 GALILEOADDBITSFLOAT(10, eph._BGD_1_5B, 1.0/static_cast<double>(1<<30)
[5852]288 /static_cast<double>(1<<2))
[5853]289 GALILEOADDBITS(2, static_cast<int>(eph._E5bHS))
290 GALILEOADDBITS(1, eph._flags & GALEPHF_E5BDINVALID)
[5852]291 }
292 else
293 {
[5853]294 GALILEOADDBITS(2, static_cast<int>(eph._E5aHS))
295 GALILEOADDBITS(1, eph._flags & GALEPHF_E5ADINVALID)
[5852]296 }
[5853]297 //// eph._TOEsec = 0.9999E9;
298 GALILEOADDBITS(20, eph._TOEsec)
[5852]299
300 GALILEOADDBITS(inav ? 1 : 3, 0)
301
302 startbuffer[0]=0xD3;
303 startbuffer[1]=(size >> 8);
304 startbuffer[2]=size;
305 unsigned long i = CRC24(size+3, startbuffer);
306 buffer[size++] = i >> 16;
307 buffer[size++] = i >> 8;
308 buffer[size++] = i;
309 size += 3;
310 return size;
311}
312
Note: See TracBrowser for help on using the repository browser.