source: ntrip/trunk/BNC/RTCM3/clock_orbit_rtcm.c@ 2447

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

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