source: ntrip/trunk/BNS/RTCM/clock_orbit_rtcm.c@ 1865

Last change on this file since 1865 was 1865, checked in by mervart, 15 years ago

* empty log message *
  • Property svn:executable set to *
File size: 34.7 KB
Line 
1/* Programheader
2
3 Name: clock_orbit_rtcm.c
4 Project: RTCM3
5 Version: $Id: clock_orbit_rtcm.c,v 1.15 2009/07/06 05:42:44 mervart Exp $
6 Authors: Dirk Stöcker
7 Description: state space approach for RTCM3
8*/
9
10#include <stdio.h>
11#include <string.h>
12#include <stdint.h>
13#include "clock_orbit_rtcm.h"
14
15static uint32_t CRC24(long size, const unsigned char *buf)
16{
17 uint32_t crc = 0;
18 int i;
19
20 while(size--)
21 {
22 crc ^= (*buf++) << (16);
23 for(i = 0; i < 8; i++)
24 {
25 crc <<= 1;
26 if(crc & 0x1000000)
27 crc ^= 0x01864cfb;
28 }
29 }
30 return crc;
31}
32
33/* NOTE: These defines are interlinked with below functions and directly modify
34the values. This may not be optimized in terms of final program code size but
35should be optimized in terms of speed.
36
37modified variables are:
38- everything defined in STARTDATA (only use ressize outside of the defines,
39 others are private)
40- buffer
41- size
42*/
43
44#ifndef NOENCODE
45#define STOREBITS \
46 while(numbits >= 8) \
47 { \
48 if(!size) return 0; \
49 *(buffer++) = bitbuffer>>(numbits-8); \
50 numbits -= 8; \
51 ++ressize; \
52 --size; \
53 }
54
55#define ADDBITS(a, b) \
56 { \
57 bitbuffer = (bitbuffer<<(a))|((b)&((1<<a)-1)); \
58 numbits += (a); \
59 STOREBITS \
60 }
61
62#define STARTDATA \
63 size_t ressize=0; \
64 char *blockstart; \
65 int numbits; \
66 uint64_t bitbuffer=0;
67
68#define INITBLOCK \
69 numbits = 0; \
70 blockstart = buffer; \
71 ADDBITS(8, 0xD3) \
72 ADDBITS(6, 0) \
73 ADDBITS(10, 0)
74
75#define ENDBLOCK \
76 if(numbits) { ADDBITS((8-numbits), 0) } \
77 { \
78 int len = buffer-blockstart-3; \
79 blockstart[1] |= len>>8; \
80 blockstart[2] = len; \
81 if(len > 1023) \
82 return 0; \
83 len = CRC24(len+3, (const unsigned char *) blockstart); \
84 ADDBITS(24, len) \
85 }
86
87#define SCALEADDBITS(a, b, c) ADDBITS(a, (int64_t)(b*c))
88
89/* standard values */
90#define T_MESSAGE_NUMBER(a) ADDBITS(12, a) /* DF002 */
91#define T_RESERVED5 ADDBITS(5, 0) /* DF001 */
92#define T_GPS_SATELLITE_ID(a) ADDBITS(6, a) /* DF068 */
93#define T_GPS_IODE(a) ADDBITS(8, a) /* DF071 */
94#define T_GLONASS_IOD(a) ADDBITS(8, a) /* DF237 */
95
96/* defined values */
97#define T_DELTA_RADIAL(a) SCALEADDBITS(22, 10000.0, a)
98#define T_DELTA_ALONG_TRACK(a) SCALEADDBITS(20, 2500.0, a)
99#define T_DELTA_CROSS_TRACK(a) SCALEADDBITS(20, 2500.0, a)
100#define T_DELTA_DOT_RADIAL(a) SCALEADDBITS(21, 1000000.0, a)
101#define T_DELTA_DOT_ALONG_TRACK(a) SCALEADDBITS(19, 250000.0, a)
102#define T_DELTA_DOT_CROSS_TRACK(a) SCALEADDBITS(19, 250000.0, a)
103#define T_DELTA_DOT_DOT_RADIAL(a) SCALEADDBITS(27, 50000000.0, a)
104#define T_DELTA_DOT_DOT_ALONG_TRACK(a) SCALEADDBITS(25, 12500000.0, a)
105#define T_DELTA_DOT_DOT_CROSS_TRACK(a) SCALEADDBITS(25, 12500000.0, a)
106#define T_SATELLITE_REFERENCE_POINT(a) ADDBITS(1, a)
107
108#define T_SATELLITE_REFERENCE_DATUM(a) ADDBITS(1, a)
109#define T_DELTA_CLOCK_C0(a) SCALEADDBITS(22, 10000.0, a)
110#define T_DELTA_CLOCK_C1(a) SCALEADDBITS(21, 1000000.0, a)
111#define T_DELTA_CLOCK_C2(a) SCALEADDBITS(27, 50000000.0, a)
112#define T_NO_OF_CODE_BIASES(a) ADDBITS(5, a)
113#define T_GPS_SIGNAL_IDENTIFIER(a) ADDBITS(5, a)
114#define T_GLONASS_SIGNAL_IDENTIFIER(a) ADDBITS(5, a)
115#define T_GALILEO_SIGNAL_IDENTIFIER(a) ADDBITS(5, a)
116#define T_CODE_BIAS(a) SCALEADDBITS(14, 100.0, a)
117#define T_GLONASS_SATELLITE_ID(a) ADDBITS(5, a)
118
119#define T_GPS_EPOCH_TIME(a) ADDBITS(20, a)
120#define T_GLONASS_EPOCH_TIME(a) ADDBITS(17, a)
121#define T_NO_OF_SATELLITES(a) ADDBITS(6, a)
122#define T_MULTIPLE_MESSAGE_INDICATOR(a) ADDBITS(1, a)
123#define T_SSR_URA(a) ADDBITS(4, a)
124#define T_HR_CLOCK_CORRECTION(a) SCALEADDBITS(22, 10000.0, a)
125#define T_SSR_UPDATE_INTERVAL(a) ADDBITS(4, a)
126
127size_t MakeClockOrbit(const struct ClockOrbit *co, enum ClockOrbitType type,
128int moremessagesfollow, char *buffer, size_t size)
129{
130 int gpshr=0, gpsur=0, gpsor=0, gpscl=0, gpsco=0, glohr=0, glour=0, gloor=0,
131 glocl=0, gloco=0, mmi, i;
132
133 STARTDATA
134
135 if(co->NumberOfGPSSat && co->HRDataSupplied
136 && (type == COTYPE_AUTO || type == COTYPE_GPSHR))
137 gpshr = 1;
138 if(co->NumberOfGPSSat && co->URADataSupplied
139 && (type == COTYPE_AUTO || type == COTYPE_GPSURA))
140 gpsur = 1;
141 if(co->NumberOfGPSSat && co->OrbitDataSupplied
142 && (type == COTYPE_AUTO || type == COTYPE_GPSORBIT))
143 gpsor = 1;
144 if(co->NumberOfGPSSat && co->ClockDataSupplied
145 && (type == COTYPE_AUTO || type == COTYPE_GPSCLOCK))
146 gpscl = 1;
147 if(co->NumberOfGPSSat && co->ClockDataSupplied && co->OrbitDataSupplied
148 && (type == COTYPE_AUTO || type == COTYPE_GPSCOMBINED)
149 /*&& co->NumberOfGPSSat <= 28*/)
150 {
151 gpsco = 1; gpsor = 0; gpscl = 0;
152 }
153 if(co->NumberOfGLONASSSat && co->HRDataSupplied
154 && (type == COTYPE_AUTO || type == COTYPE_GLONASSHR))
155 glohr = 1;
156 if(co->NumberOfGLONASSSat && co->URADataSupplied
157 && (type == COTYPE_AUTO || type == COTYPE_GLONASSURA))
158 glour = 1;
159 if(co->NumberOfGLONASSSat && co->OrbitDataSupplied
160 && (type == COTYPE_AUTO || type == COTYPE_GLONASSORBIT))
161 gloor = 1;
162 if(co->NumberOfGLONASSSat && co->ClockDataSupplied
163 && (type == COTYPE_AUTO || type == COTYPE_GLONASSCLOCK))
164 glocl = 1;
165 if(co->NumberOfGLONASSSat && co->ClockDataSupplied && co->OrbitDataSupplied
166 && (type == COTYPE_AUTO || type == COTYPE_GLONASSCOMBINED))
167 {
168 gloco = 1; gloor = 0; glocl = 0;
169 }
170
171 mmi = gpshr+gpsur+gpsor+gpscl+gpsco+glohr+glour+gloor+glocl+gloco; /* required for multimessage */
172 if(!moremessagesfollow) --mmi;
173
174 if(gpsor)
175 {
176 INITBLOCK
177 T_MESSAGE_NUMBER(COTYPE_GPSORBIT)
178 T_GPS_EPOCH_TIME(co->GPSEpochTime)
179 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
180 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
181 --mmi;
182 T_RESERVED5
183 T_NO_OF_SATELLITES(co->NumberOfGPSSat)
184 for(i = 0; i < co->NumberOfGPSSat; ++i)
185 {
186 T_GPS_SATELLITE_ID(co->Sat[i].ID)
187 T_GPS_IODE(co->Sat[i].IOD)
188 T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
189 T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
190 T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
191 T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
192 T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
193 T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
194 T_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
195 T_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
196 T_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
197 T_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
198 T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
199 }
200 ENDBLOCK
201 }
202 if(gpscl)
203 {
204 INITBLOCK
205 T_MESSAGE_NUMBER(COTYPE_GPSCLOCK)
206 T_GPS_EPOCH_TIME(co->GPSEpochTime)
207 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
208 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
209 --mmi;
210 T_RESERVED5
211 T_NO_OF_SATELLITES(co->NumberOfGPSSat)
212 for(i = 0; i < co->NumberOfGPSSat; ++i)
213 {
214 T_GPS_SATELLITE_ID(co->Sat[i].ID)
215 T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
216 T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
217 T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)
218 }
219 ENDBLOCK
220 }
221 if(gpsco)
222 {
223 int nums, left, start = 0;
224 nums = co->NumberOfGPSSat;
225 if(nums > 28) /* split block when more than 28 sats */
226 {
227 left = nums - 28;
228 nums = 28;
229 }
230 else {
231 left = 0;
232 }
233 while(nums)
234 {
235 INITBLOCK
236 T_MESSAGE_NUMBER(COTYPE_GPSCOMBINED)
237 T_GPS_EPOCH_TIME(co->GPSEpochTime)
238 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
239 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi || */ left ? 1 : 0)
240 --mmi;
241 T_RESERVED5
242 T_NO_OF_SATELLITES(nums)
243 for(i = start; i < start+nums; ++i)
244 {
245 T_GPS_SATELLITE_ID(co->Sat[i].ID)
246 T_GPS_IODE(co->Sat[i].IOD)
247 T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
248 T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
249 T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
250 T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
251 T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
252 T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
253 T_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
254 T_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
255 T_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
256 T_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
257 T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
258 T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
259 T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
260 T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)
261 }
262 ENDBLOCK
263 start += nums;
264 nums = left;
265 left = 0;
266 }
267 }
268 if(gpshr)
269 {
270 INITBLOCK
271 T_MESSAGE_NUMBER(COTYPE_GPSHR)
272 T_GPS_EPOCH_TIME(co->GPSEpochTime)
273 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
274 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
275 --mmi;
276 T_RESERVED5
277 T_NO_OF_SATELLITES(co->NumberOfGPSSat)
278 for(i = 0; i < co->NumberOfGPSSat; ++i)
279 {
280 T_GPS_SATELLITE_ID(co->Sat[i].ID)
281 T_HR_CLOCK_CORRECTION(co->Sat[i].hrclock)
282 }
283 ENDBLOCK
284 }
285 if(gpsur)
286 {
287 INITBLOCK
288 T_MESSAGE_NUMBER(COTYPE_GPSURA)
289 T_GPS_EPOCH_TIME(co->GPSEpochTime)
290 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
291 --mmi;
292 T_RESERVED5
293 T_NO_OF_SATELLITES(co->NumberOfGPSSat)
294 for(i = 0; i < co->NumberOfGPSSat; ++i)
295 {
296 T_GPS_SATELLITE_ID(co->Sat[i].ID)
297 T_SSR_URA(co->Sat[i].URA)
298 }
299 ENDBLOCK
300 }
301 if(gloor)
302 {
303 INITBLOCK
304 T_MESSAGE_NUMBER(COTYPE_GLONASSORBIT)
305 T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
306 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
307 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
308 --mmi;
309 T_RESERVED5
310 T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
311 for(i = CLOCKORBIT_NUMGPS;
312 i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
313 {
314 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
315 T_GLONASS_IOD(co->Sat[i].IOD)
316 T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
317 T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
318 T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
319 T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
320 T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
321 T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
322 T_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
323 T_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
324 T_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
325 T_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
326 T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
327 }
328 ENDBLOCK
329 }
330 if(glocl)
331 {
332 INITBLOCK
333 T_MESSAGE_NUMBER(COTYPE_GLONASSCLOCK)
334 T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
335 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
336 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
337 --mmi;
338 T_RESERVED5
339 T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
340 for(i = CLOCKORBIT_NUMGPS;
341 i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
342 {
343 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
344 T_GLONASS_IOD(co->Sat[i].IOD)
345 T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
346 T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
347 T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)
348 }
349 ENDBLOCK
350 }
351 if(gloco)
352 {
353 INITBLOCK
354 T_MESSAGE_NUMBER(COTYPE_GLONASSCOMBINED)
355 T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
356 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
357 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
358 --mmi;
359 T_RESERVED5
360 T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
361 for(i = CLOCKORBIT_NUMGPS;
362 i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
363 {
364 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
365 T_GLONASS_IOD(co->Sat[i].IOD)
366 T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
367 T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
368 T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
369 T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
370 T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
371 T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
372 T_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
373 T_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
374 T_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
375 T_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
376 T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
377 T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
378 T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
379 T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)
380 }
381 ENDBLOCK
382 }
383 if(glohr)
384 {
385 INITBLOCK
386 T_MESSAGE_NUMBER(COTYPE_GLONASSHR)
387 T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
388 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
389 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
390 --mmi;
391 T_RESERVED5
392 T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
393 for(i = CLOCKORBIT_NUMGPS;
394 i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
395 {
396 T_GPS_SATELLITE_ID(co->Sat[i].ID)
397 T_HR_CLOCK_CORRECTION(co->Sat[i].hrclock)
398 }
399 ENDBLOCK
400 }
401 if(glour)
402 {
403 INITBLOCK
404 T_MESSAGE_NUMBER(COTYPE_GLONASSURA)
405 T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
406 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
407 --mmi;
408 T_RESERVED5
409 T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
410 for(i = CLOCKORBIT_NUMGPS;
411 i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
412 {
413 T_GPS_SATELLITE_ID(co->Sat[i].ID)
414 T_SSR_URA(co->Sat[i].URA)
415 }
416 ENDBLOCK
417 }
418
419 return ressize;
420}
421
422size_t MakeBias(const struct Bias *b, enum BiasType type,
423int moremessagesfollow, char *buffer, size_t size)
424{
425 int gps, glo, mmi, i, j;
426
427 STARTDATA
428
429 if(b->NumberOfGPSSat && (type == BTYPE_AUTO || type == BTYPE_GPS))
430 gps = 1;
431 if(b->NumberOfGLONASSSat && (type == BTYPE_AUTO || type == BTYPE_GLONASS))
432 glo = 1;
433
434 mmi = gps+glo; /* required for multimessage */
435 if(!moremessagesfollow) --mmi;
436
437 if(gps)
438 {
439 INITBLOCK
440 T_MESSAGE_NUMBER(BTYPE_GPS)
441 T_GPS_EPOCH_TIME(b->GPSEpochTime)
442 T_SSR_UPDATE_INTERVAL(b->UpdateInterval)
443 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
444 --mmi;
445 T_RESERVED5
446 T_NO_OF_SATELLITES(b->NumberOfGPSSat)
447 for(i = 0; i < b->NumberOfGPSSat; ++i)
448 {
449 T_GPS_SATELLITE_ID(b->Sat[i].ID)
450 T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
451 for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
452 {
453 T_GPS_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type)
454 T_CODE_BIAS(b->Sat[i].Biases[j].Bias)
455 }
456 }
457 ENDBLOCK
458 }
459 if(glo)
460 {
461 INITBLOCK
462 T_MESSAGE_NUMBER(BTYPE_GLONASS)
463 T_GPS_EPOCH_TIME(b->GLONASSEpochTime)
464 T_SSR_UPDATE_INTERVAL(b->UpdateInterval)
465 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
466 --mmi;
467 T_RESERVED5
468 T_NO_OF_SATELLITES(b->NumberOfGLONASSSat)
469 for(i = CLOCKORBIT_NUMGPS;
470 i < CLOCKORBIT_NUMGPS+b->NumberOfGLONASSSat; ++i)
471 {
472 T_GLONASS_SATELLITE_ID(b->Sat[i].ID)
473 T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
474 for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
475 {
476 T_GLONASS_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type)
477 T_CODE_BIAS(b->Sat[i].Biases[j].Bias)
478 }
479 }
480 ENDBLOCK
481 }
482
483 return ressize;
484}
485
486#endif /* NOENCODE */
487
488#ifndef NODECODE
489
490#define DECODESTART \
491 int numbits=0; \
492 uint64_t bitbuffer=0;
493
494#define LOADBITS(a) \
495{ \
496 while((a) > numbits) \
497 { \
498 if(!size--) return GCOBR_SHORTMESSAGE; \
499 bitbuffer = (bitbuffer<<8)|((unsigned char)*(buffer++)); \
500 numbits += 8; \
501 } \
502}
503
504/* extract bits from data stream
505 b = variable to store result, a = number of bits */
506#define GETBITS(b, a) \
507{ \
508 LOADBITS(a) \
509 b = (bitbuffer<<(64-numbits))>>(64-(a)); \
510 numbits -= (a); \
511}
512
513/* extract signed floating value from data stream
514 b = variable to store result, a = number of bits */
515#define GETFLOATSIGN(b, a, c) \
516{ \
517 LOADBITS(a) \
518 b = ((double)(((int64_t)(bitbuffer<<(64-numbits)))>>(64-(a))))*(c); \
519 numbits -= (a); \
520}
521
522#define SKIPBITS(b) { LOADBITS(b) numbits -= (b); }
523
524/* standard values */
525#define G_HEADER(a) GETBITS(a,8)
526#define G_RESERVEDH(a) GETBITS(a,6)
527#define G_SIZE(a) GETBITS(a, 10)
528#define G_MESSAGE_NUMBER(a) GETBITS(a, 12) /* DF002 */
529#define G_RESERVED5 SKIPBITS(5) /* DF001 */
530#define G_GPS_SATELLITE_ID(a) GETBITS(a, 6) /* DF068 */
531#define G_GPS_IODE(a) GETBITS(a, 8) /* DF071 */
532#define G_GLONASS_IOD(a) GETBITS(a, 8) /* DF237 */
533
534/* defined values */
535#define G_DELTA_RADIAL(a) GETFLOATSIGN(a, 22, 1/10000.0)
536#define G_DELTA_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0)
537#define G_DELTA_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0)
538#define G_DELTA_DOT_RADIAL(a) GETFLOATSIGN(a, 21, 1/1000000.0)
539#define G_DELTA_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0)
540#define G_DELTA_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0)
541#define G_DELTA_DOT_DOT_RADIAL(a) GETFLOATSIGN(a, 27, 1/50000000.0)
542#define G_DELTA_DOT_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 25, 1/12500000.0)
543#define G_DELTA_DOT_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 25, 1/12500000.0)
544#define G_SATELLITE_REFERENCE_POINT(a) GETBITS(a, 1)
545
546#define G_SATELLITE_REFERENCE_DATUM(a) GETBITS(a, 1)
547#define G_DELTA_CLOCK_C0(a) GETFLOATSIGN(a, 22, 1/10000.0)
548#define G_DELTA_CLOCK_C1(a) GETFLOATSIGN(a, 21, 1/1000000.0)
549#define G_DELTA_CLOCK_C2(a) GETFLOATSIGN(a, 27, 1/50000000.0)
550#define G_NO_OF_CODE_BIASES(a) GETBITS(a, 5)
551#define G_GPS_SIGNAL_IDENTIFIER(a) GETBITS(a, 5)
552#define G_GLONASS_SIGNAL_IDENTIFIER(a) GETBITS(a, 5)
553#define G_GALILEO_SIGNAL_IDENTIFIER(a) GETBITS(a, 5)
554#define G_CODE_BIAS(a) GETFLOATSIGN(a, 14, 1/100.0)
555#define G_GLONASS_SATELLITE_ID(a) GETBITS(a, 5)
556
557#define G_GPS_EPOCH_TIME(a, b) {int temp; GETBITS(temp, 20) \
558 if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
559#define G_GLONASS_EPOCH_TIME(a, b) {int temp; GETBITS(temp, 17) \
560 if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
561#define G_NO_OF_SATELLITES(a) GETBITS(a, 6)
562#define G_MULTIPLE_MESSAGE_INDICATOR(a) GETBITS(a, 1)
563#define G_SSR_URA(a) GETBITS(a, 4)
564#define G_HR_CLOCK_CORRECTION(a) GETFLOATSIGN(a, 22, 1/10000.0)
565#define G_SSR_UPDATE_INTERVAL(a) GETBITS(a, 4)
566
567enum GCOB_RETURN GetClockOrbitBias(struct ClockOrbit *co, struct Bias *b,
568const char *buffer, size_t size, int *bytesused)
569{
570 int type, mmi=0, i, j, h, rs, nums, pos, id;
571 size_t sizeofrtcmblock;
572 const char *blockstart = buffer;
573 DECODESTART
574
575 if(size < 7)
576 return GCOBR_SHORTBUFFER;
577
578#ifdef DEBUG
579fprintf(stderr, "GetClockOrbitBias START: size %d, numbits %d\n",size, numbits);
580#endif
581
582 G_HEADER(h)
583 G_RESERVEDH(rs)
584 G_SIZE(sizeofrtcmblock);
585
586 if((unsigned char)h != 0xD3 || rs)
587 return GCOBR_UNKNOWNDATA;
588 if(size < sizeofrtcmblock + 3) /* 3 header bytes already removed */
589 return GCOBR_MESSAGEEXCEEDSBUFFER;
590 if(CRC24(sizeofrtcmblock+3, (const unsigned char *) blockstart) !=
591 (uint32_t)((((unsigned char)buffer[sizeofrtcmblock])<<16)|
592 (((unsigned char)buffer[sizeofrtcmblock+1])<<8)|
593 (((unsigned char)buffer[sizeofrtcmblock+2]))))
594 return GCOBR_CRCMISMATCH;
595 size = sizeofrtcmblock; /* reduce size, so overflows are detected */
596
597 G_MESSAGE_NUMBER(type)
598#ifdef DEBUG
599fprintf(stderr, "type %d size %d\n",type,sizeofrtcmblock);
600#endif
601 switch(type)
602 {
603 case COTYPE_GPSORBIT:
604 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
605 co->messageType = COTYPE_GPSORBIT;
606 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
607 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
608 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
609 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
610 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
611 G_RESERVED5
612 G_NO_OF_SATELLITES(nums)
613 co->OrbitDataSupplied |= 1;
614#ifdef DEBUG
615fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GPSEpochTime,
616co->UpdateInterval,mmi,co->NumberOfGPSSat,nums);
617#endif
618 for(i = 0; i < nums; ++i)
619 {
620 G_GPS_SATELLITE_ID(id)
621 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
622 ;
623 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
624 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
625 co->Sat[pos].ID = id;
626
627 G_GPS_IODE(co->Sat[pos].IOD)
628 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
629 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
630 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
631 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
632 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
633 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
634 G_DELTA_DOT_DOT_RADIAL(co->Sat[pos].Orbit.DotDotDeltaRadial)
635 G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDotDeltaAlongTrack)
636 G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDotDeltaCrossTrack)
637 G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
638 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
639#ifdef DEBUG
640fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f rp %d rd %d\n",
641co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
642co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
643co->Sat[pos].Orbit.DotDeltaRadial,
644co->Sat[pos].Orbit.DotDeltaAlongTrack,
645co->Sat[pos].Orbit.DotDeltaCrossTrack,
646co->Sat[pos].Orbit.DotDotDeltaRadial,
647co->Sat[pos].Orbit.DotDotDeltaAlongTrack,
648co->Sat[pos].Orbit.DotDotDeltaCrossTrack,
649co->SatRefPoint,
650co->SatRefDatum);
651#endif
652 }
653 break;
654 case COTYPE_GPSCLOCK:
655 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
656 co->messageType = COTYPE_GPSCLOCK;
657 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
658 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
659 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
660 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
661 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
662 G_RESERVED5
663 G_NO_OF_SATELLITES(nums)
664 co->ClockDataSupplied |= 1;
665#ifdef DEBUG
666fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GPSEpochTime,
667co->UpdateInterval,mmi,co->NumberOfGPSSat,nums);
668#endif
669 for(i = 0; i < nums; ++i)
670 {
671 G_GPS_SATELLITE_ID(id)
672 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
673 ;
674 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
675 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
676 co->Sat[pos].ID = id;
677
678 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
679 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
680 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
681#ifdef DEBUG
682fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
683co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
684co->Sat[pos].Clock.DeltaA2);
685#endif
686 }
687 break;
688 case COTYPE_GPSCOMBINED:
689 if(!co) return -5;
690 co->messageType = COTYPE_GPSCOMBINED;
691 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
692 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
693 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
694 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
695 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
696 G_RESERVED5
697 G_NO_OF_SATELLITES(nums)
698 co->OrbitDataSupplied |= 1;
699 co->ClockDataSupplied |= 1;
700 for(i = 0; i < nums; ++i)
701 {
702 G_GPS_SATELLITE_ID(id)
703 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
704 ;
705 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
706 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
707 co->Sat[pos].ID = id;
708
709 G_GPS_IODE(co->Sat[pos].IOD)
710 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
711 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
712 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
713 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
714 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
715 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
716 G_DELTA_DOT_DOT_RADIAL(co->Sat[pos].Orbit.DotDotDeltaRadial)
717 G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDotDeltaAlongTrack)
718 G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDotDeltaCrossTrack)
719 G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
720 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
721 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
722 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
723 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
724 }
725 break;
726 case COTYPE_GPSURA:
727 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
728 co->messageType = COTYPE_GPSURA;
729 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
730 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
731 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
732 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
733 G_RESERVED5
734 G_NO_OF_SATELLITES(nums)
735 co->URADataSupplied |= 1;
736 for(i = 0; i < nums; ++i)
737 {
738 G_GPS_SATELLITE_ID(id)
739 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
740 ;
741 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
742 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
743 co->Sat[pos].ID = id;
744
745 G_SSR_URA(co->Sat[pos].URA)
746 }
747 break;
748 case COTYPE_GPSHR:
749 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
750 co->messageType = COTYPE_GPSHR;
751 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
752 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
753 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
754 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
755 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
756 G_RESERVED5
757 G_NO_OF_SATELLITES(nums)
758 co->HRDataSupplied |= 1;
759 for(i = 0; i < nums; ++i)
760 {
761 G_GPS_SATELLITE_ID(id)
762 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
763 ;
764 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
765 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
766 co->Sat[pos].ID = id;
767
768 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
769 }
770 break;
771 case COTYPE_GLONASSORBIT:
772 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
773 co->messageType = COTYPE_GLONASSORBIT;
774 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
775 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
776 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
777 G_RESERVED5
778 G_NO_OF_SATELLITES(nums)
779 co->OrbitDataSupplied |= 2;
780#ifdef DEBUG
781fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GLONASSEpochTime,
782co->UpdateInterval,mmi,co->NumberOfGLONASSSat,nums);
783#endif
784 for(i = 0; i < nums; ++i)
785 {
786 G_GLONASS_SATELLITE_ID(id)
787 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
788 ;
789 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
790 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
791 co->Sat[pos].ID = id;
792
793 G_GLONASS_IOD(co->Sat[pos].IOD)
794 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
795 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
796 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
797 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
798 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
799 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
800 G_DELTA_DOT_DOT_RADIAL(co->Sat[pos].Orbit.DotDotDeltaRadial)
801 G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDotDeltaAlongTrack)
802 G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDotDeltaCrossTrack)
803 G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
804 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
805#ifdef DEBUG
806fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f rp %d rd %d\n",
807co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
808co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
809co->Sat[pos].Orbit.DotDeltaRadial,
810co->Sat[pos].Orbit.DotDeltaAlongTrack,
811co->Sat[pos].Orbit.DotDeltaCrossTrack,
812co->Sat[pos].Orbit.DotDotDeltaRadial,
813co->Sat[pos].Orbit.DotDotDeltaAlongTrack,
814co->Sat[pos].Orbit.DotDotDeltaCrossTrack,
815co->SatRefPoint,
816co->SatRefDatum);
817#endif
818 }
819 break;
820 case COTYPE_GLONASSCLOCK:
821 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
822 co->messageType = COTYPE_GLONASSCLOCK;
823 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
824 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
825 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
826 G_RESERVED5
827 G_NO_OF_SATELLITES(nums)
828 co->ClockDataSupplied |= 2;
829#ifdef DEBUG
830fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GLONASSEpochTime,
831co->UpdateInterval,mmi,co->NumberOfGLONASSSat,nums);
832#endif
833 for(i = 0; i < nums; ++i)
834 {
835 G_GLONASS_SATELLITE_ID(id)
836 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
837 ;
838 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
839 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
840 co->Sat[pos].ID = id;
841
842 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
843 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
844 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
845#ifdef DEBUG
846fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
847co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
848co->Sat[pos].Clock.DeltaA2);
849#endif
850 }
851 break;
852 case COTYPE_GLONASSCOMBINED:
853 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
854 co->messageType = COTYPE_GLONASSCOMBINED;
855 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
856 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
857 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
858 G_RESERVED5
859 G_NO_OF_SATELLITES(nums)
860 co->OrbitDataSupplied |= 2;
861 co->ClockDataSupplied |= 2;
862 for(i = 0; i < nums; ++i)
863 {
864 G_GLONASS_SATELLITE_ID(id)
865 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
866 ;
867 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
868 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
869 co->Sat[pos].ID = id;
870
871 G_GLONASS_IOD(co->Sat[pos].IOD)
872 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
873 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
874 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
875 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
876 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
877 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
878 G_DELTA_DOT_DOT_RADIAL(co->Sat[pos].Orbit.DotDotDeltaRadial)
879 G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDotDeltaAlongTrack)
880 G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDotDeltaCrossTrack)
881 G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
882 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
883 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
884 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
885 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
886 }
887 break;
888 case COTYPE_GLONASSURA:
889 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
890 co->messageType = COTYPE_GLONASSURA;
891 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
892 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
893 G_RESERVED5
894 G_NO_OF_SATELLITES(nums)
895 co->URADataSupplied |= 2;
896 for(i = 0; i < nums; ++i)
897 {
898 G_GLONASS_SATELLITE_ID(id)
899 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
900 ;
901 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
902 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
903 co->Sat[pos].ID = id;
904
905 G_SSR_URA(co->Sat[pos].URA)
906 }
907 break;
908 case COTYPE_GLONASSHR:
909 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
910 co->messageType = COTYPE_GLONASSHR;
911 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
912 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
913 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
914 G_RESERVED5
915 G_NO_OF_SATELLITES(nums)
916 co->HRDataSupplied |= 2;
917 for(i = 0; i < nums; ++i)
918 {
919 G_GLONASS_SATELLITE_ID(id)
920 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
921 ;
922 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
923 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
924 co->Sat[pos].ID = id;
925
926 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
927 }
928 break;
929 case BTYPE_GPS:
930 if(!b) return GCOBR_NOBIASPARAMETER;
931 b->messageType = BTYPE_GPS;
932 G_GPS_EPOCH_TIME(b->GPSEpochTime, b->NumberOfGPSSat)
933 G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
934 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
935 G_RESERVED5
936 G_NO_OF_SATELLITES(nums)
937 for(i = 0; i < nums; ++i)
938 {
939 G_GPS_SATELLITE_ID(id)
940 for(pos = 0; pos < b->NumberOfGPSSat && b->Sat[pos].ID != id; ++pos)
941 ;
942 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
943 else if(pos == b->NumberOfGPSSat) ++b->NumberOfGPSSat;
944 b->Sat[pos].ID = id;
945
946 G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases)
947 for(j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j)
948 {
949 G_GPS_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
950 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
951 }
952 }
953 break;
954 case BTYPE_GLONASS:
955 if(!b) return GCOBR_NOBIASPARAMETER;
956 b->messageType = BTYPE_GLONASS;
957 G_GLONASS_EPOCH_TIME(b->GLONASSEpochTime, b->NumberOfGLONASSSat)
958 G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
959 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
960 G_RESERVED5
961 G_NO_OF_SATELLITES(nums)
962 for(i = 0; i < nums; ++i)
963 {
964 G_GLONASS_SATELLITE_ID(id)
965 for(pos = CLOCKORBIT_NUMGPS; pos < b->NumberOfGLONASSSat && b->Sat[pos].ID != id; ++pos)
966 ;
967 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
968 else if(pos == b->NumberOfGLONASSSat) ++b->NumberOfGLONASSSat;
969 b->Sat[pos].ID = id;
970
971 G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases)
972 for(j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j)
973 {
974 G_GLONASS_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
975 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
976 }
977 }
978 break;
979 default:
980 if(bytesused)
981 *bytesused = sizeofrtcmblock+6;
982 return GCOBR_UNKNOWNTYPE;
983 }
984#ifdef DEBUG
985for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type)
986 numbits += 8;
987fprintf(stderr, "numbits left %d\n",numbits);
988#endif
989 if(bytesused)
990 *bytesused = sizeofrtcmblock+6;
991 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
992}
993#endif /* NODECODE */
Note: See TracBrowser for help on using the repository browser.