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

Last change on this file since 8184 was 8184, checked in by stuerze, 16 months ago

RTCM MT 1020 ephemeris parameter are now completed

File size: 14.7 KB
Line 
1
2#include "ephEncoder.h"
3
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;
16  int ura = indexFromAccuracy(eph._ura, eph.type());
17  if (eph.type() == t_eph::QZSS) {
18    GPSADDBITS(12, 1044)
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 */
57  }
58  else {
59    GPSADDBITS(12, 1019)
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))
95    GPSADDBITS(6, eph._health)
96    GPSADDBITS(1, eph._L2PFlag)
97    GPSADDBITS(1, eph._fitInterval)
98  }
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)
123  GLONASSADDBITS(6, eph._prn.number())
124  GLONASSADDBITS(5, 7+eph._frequency_number)
125  GLONASSADDBITS(1, eph._almanac_health)
126  GLONASSADDBITS(1, eph._almanac_health_availablility_indicator)
127  GLONASSADDBITS(2, eph._P1)
128  int tki = static_cast<int>(eph._tki)+3*60*60;
129  if (tki > 86400) {tki -= 86400;}
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)
133  GLONASSADDBITS(1, eph._health)
134  GLONASSADDBITS(1, eph._P2)
135  unsigned long long timeofday = (static_cast<int>(eph._tt.gpssec()+3*60*60-eph._gps_utc)%86400);
136  GLONASSADDBITS(7, timeofday/60/15)
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))
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) */
150  GLONASSADDBITSFLOATM(22, eph._tau, 1.0/static_cast<double>(1<<30))
151  GLONASSADDBITSFLOATM(5, eph._M_delta_tau, 1.0/static_cast<double>(1<<30)) /* GLONASS-M delta tau */
152  GLONASSADDBITS(5, eph._E)
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) */
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
185  int SISA = indexFromAccuracy(eph._SISA, eph.type());
186
187  GALILEOADDBITS(12, eph._inav ? 1046 : 1045)
188  GALILEOADDBITS(6, eph._prn.number())
189  GALILEOADDBITS(12, eph._TOC.galw())
190  GALILEOADDBITS(10, eph._IODnav)
191  GALILEOADDBITS(8, SISA)
192  GALILEOADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)
193  /static_cast<double>(1<<13))
194  GALILEOADDBITS(14, eph._TOC.gpssec()/60)
195  GALILEOADDBITSFLOAT(6, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)
196  /static_cast<double>(1<<29))
197  GALILEOADDBITSFLOAT(21, eph._clock_drift, 1.0/static_cast<double>(1<<30)
198  /static_cast<double>(1<<16))
199  GALILEOADDBITSFLOAT(31, eph._clock_bias, 1.0/static_cast<double>(1<<30)
200  /static_cast<double>(1<<4))
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)
203  /static_cast<double>(1<<13))
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)
212  /static_cast<double>(1<<1))
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)
217  /static_cast<double>(1<<1))
218  GALILEOADDBITSFLOAT(24, eph._OMEGADOT, M_PI/static_cast<double>(1<<30)
219  /static_cast<double>(1<<13))
220  GALILEOADDBITSFLOAT(10, eph._BGD_1_5A, 1.0/static_cast<double>(1<<30)
221  /static_cast<double>(1<<2))
222  if(eph._inav)
223  {
224    GALILEOADDBITSFLOAT(10, eph._BGD_1_5B, 1.0/static_cast<double>(1<<30)
225    /static_cast<double>(1<<2))
226    GALILEOADDBITS(2, static_cast<int>(eph._E5bHS))
227    GALILEOADDBITS(1, eph._e5bDataInValid ? 1 : 0)
228    GALILEOADDBITS(2, static_cast<int>(eph._E1_bHS))
229    GALILEOADDBITS(1, eph._e1DataInValid ? 1 : 0)
230  }
231  else
232  {
233    GALILEOADDBITS(2, static_cast<int>(eph._E5aHS))
234    GALILEOADDBITS(1,  eph._e5aDataInValid ? 1 : 0)
235  }
236
237  GALILEOADDBITS((eph._inav ? 2 : 7), 0)
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
250// build up RTCM3 for SBAS
251////////////////////////////////////////////////////////////////////////////
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
259  int ura = indexFromAccuracy(eph._ura, eph.type());
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)
264  SBASADDBITS(4, ura)
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))
276  SBASADDBITS(2,0);
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;
287}
288
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
298  int URA = indexFromAccuracy(eph._URA, eph.type());
299  BDSADDBITS(12, RTCM3ID_BDS)
300  BDSADDBITS(6, eph._prn.number())
301  BDSADDBITS(13, eph._TOC.bdsw() - 1356.0)
302  BDSADDBITS(4, URA);
303  BDSADDBITSFLOAT(14, eph._IDOT, M_PI/static_cast<double>(1<<30)/static_cast<double>(1<<13))
304  BDSADDBITS(5, eph._AODE)
305  BDSADDBITS(17, static_cast<int>(eph._TOC.bdssec())>>3)
306  BDSADDBITSFLOAT(11, eph._clock_driftrate, 1.0/static_cast<double>(1<<30)
307      /static_cast<double>(1<<30)/static_cast<double>(1<<6))
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))
318  BDSADDBITS(17, static_cast<int>(eph._TOE.bdssec())>>3)
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))
323  BDSADDBITSFLOAT(18, eph._Crc, 1.0/static_cast<double>(1<<6))
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)
329  BDSADDBITS(1, 0) /* reserved bit, fill up 8 bits */
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;
340}
341
Note: See TracBrowser for help on using the repository browser.