source: ntrip/branches/BNC_2.12/src/RTCM3/ephEncoder.cpp@ 8677

Last change on this file since 8677 was 8209, checked in by stuerze, 7 years ago

bug with respect to BDS week fixed

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