source: ntrip/trunk/clock_and_orbit/clock_orbit_rtcm.c@ 2421

Last change on this file since 2421 was 2421, checked in by stoecker, 12 years ago

update of SSR messages

  • Property svn:executable set to *
File size: 32.3 KB
Line 
1/* Programheader
2
3 Name: clock_orbit_rtcm.c
4 Project: RTCM3
5 Version: $Id: clock_orbit_rtcm.c,v 1.9 2010/02/22 13:42:26 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(1, 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 int nums, left, start = 0;
245 nums = co->NumberOfGPSSat;
246 if(nums > 28) /* split block when more than 28 sats */
247 {
248 left = nums - 28;
249 nums = 28;
250 }
251 else
252 {
253 left = 0;
254 }
255 while(nums)
256 {
257 INITBLOCK
258 T_MESSAGE_NUMBER(COTYPE_GPSCOMBINED)
259 T_GPS_EPOCH_TIME(co->GPSEpochTime)
260 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
261 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi || */ left ? 1 : 0)
262 --mmi;
263 T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
264 T_RESERVED4
265 T_NO_OF_SATELLITES(nums)
266 for(i = start; i < start+nums; ++i)
267 {
268 T_GPS_SATELLITE_ID(co->Sat[i].ID)
269 T_GPS_IODE(co->Sat[i].IOD)
270 T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
271 T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
272 T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
273 T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
274 T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
275 T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
276 T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
277 T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
278 T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)
279 }
280 ENDBLOCK
281 start += nums;
282 nums = left;
283 left = 0;
284 }
285 }
286 if(gpshr)
287 {
288 INITBLOCK
289 T_MESSAGE_NUMBER(COTYPE_GPSHR)
290 T_GPS_EPOCH_TIME(co->GPSEpochTime)
291 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
292 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
293 --mmi;
294 T_RESERVED5
295 T_NO_OF_SATELLITES(co->NumberOfGPSSat)
296 for(i = 0; i < co->NumberOfGPSSat; ++i)
297 {
298 T_GPS_SATELLITE_ID(co->Sat[i].ID)
299 T_HR_CLOCK_CORRECTION(co->Sat[i].hrclock)
300 }
301 ENDBLOCK
302 }
303 if(gpsur)
304 {
305 INITBLOCK
306 T_MESSAGE_NUMBER(COTYPE_GPSURA)
307 T_GPS_EPOCH_TIME(co->GPSEpochTime)
308 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
309 --mmi;
310 T_RESERVED5
311 T_NO_OF_SATELLITES(co->NumberOfGPSSat)
312 for(i = 0; i < co->NumberOfGPSSat; ++i)
313 {
314 T_GPS_SATELLITE_ID(co->Sat[i].ID)
315 T_SSR_URA(ValueToURA(co->Sat[i].UserRangeAccuracy))
316 }
317 ENDBLOCK
318 }
319 if(gloor)
320 {
321 INITBLOCK
322 T_MESSAGE_NUMBER(COTYPE_GLONASSORBIT)
323 T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
324 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
325 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
326 --mmi;
327 T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
328 T_RESERVED4
329 T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
330 for(i = CLOCKORBIT_NUMGPS;
331 i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
332 {
333 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
334 T_GLONASS_IOD(co->Sat[i].IOD)
335 T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
336 T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
337 T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
338 T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
339 T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
340 T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
341 }
342 ENDBLOCK
343 }
344 if(glocl)
345 {
346 INITBLOCK
347 T_MESSAGE_NUMBER(COTYPE_GLONASSCLOCK)
348 T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
349 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
350 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
351 --mmi;
352 T_RESERVED5
353 T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
354 for(i = CLOCKORBIT_NUMGPS;
355 i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
356 {
357 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
358 T_GLONASS_IOD(co->Sat[i].IOD)
359 T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
360 T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
361 T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)
362 }
363 ENDBLOCK
364 }
365 if(gloco)
366 {
367 INITBLOCK
368 T_MESSAGE_NUMBER(COTYPE_GLONASSCOMBINED)
369 T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
370 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
371 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
372 --mmi;
373 T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
374 T_RESERVED4
375 T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
376 for(i = CLOCKORBIT_NUMGPS;
377 i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
378 {
379 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
380 T_GLONASS_IOD(co->Sat[i].IOD)
381 T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
382 T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
383 T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
384 T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
385 T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
386 T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
387 T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
388 T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
389 T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)
390 }
391 ENDBLOCK
392 }
393 if(glohr)
394 {
395 INITBLOCK
396 T_MESSAGE_NUMBER(COTYPE_GLONASSHR)
397 T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
398 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
399 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
400 --mmi;
401 T_RESERVED5
402 T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
403 for(i = CLOCKORBIT_NUMGPS;
404 i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
405 {
406 T_GPS_SATELLITE_ID(co->Sat[i].ID)
407 T_HR_CLOCK_CORRECTION(co->Sat[i].hrclock)
408 }
409 ENDBLOCK
410 }
411 if(glour)
412 {
413 INITBLOCK
414 T_MESSAGE_NUMBER(COTYPE_GLONASSURA)
415 T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
416 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
417 --mmi;
418 T_RESERVED5
419 T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
420 for(i = CLOCKORBIT_NUMGPS;
421 i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
422 {
423 T_GPS_SATELLITE_ID(co->Sat[i].ID)
424 T_SSR_URA(ValueToURA(co->Sat[i].UserRangeAccuracy))
425 }
426 ENDBLOCK
427 }
428
429 return ressize;
430}
431
432size_t MakeBias(const struct Bias *b, enum BiasType type,
433int moremessagesfollow, char *buffer, size_t size)
434{
435 int gps=0, glo=0, mmi, i, j;
436
437 STARTDATA
438
439 if(b->NumberOfGPSSat && (type == BTYPE_AUTO || type == BTYPE_GPS))
440 gps = 1;
441 if(b->NumberOfGLONASSSat && (type == BTYPE_AUTO || type == BTYPE_GLONASS))
442 glo = 1;
443
444 mmi = gps+glo; /* required for multimessage */
445 if(!moremessagesfollow) --mmi;
446
447 if(gps)
448 {
449 INITBLOCK
450 T_MESSAGE_NUMBER(BTYPE_GPS)
451 T_GPS_EPOCH_TIME(b->GPSEpochTime)
452 T_SSR_UPDATE_INTERVAL(b->UpdateInterval)
453 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
454 --mmi;
455 T_RESERVED5
456 T_NO_OF_SATELLITES(b->NumberOfGPSSat)
457 for(i = 0; i < b->NumberOfGPSSat; ++i)
458 {
459 T_GPS_SATELLITE_ID(b->Sat[i].ID)
460 T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
461 for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
462 {
463 T_GPS_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type)
464 T_CODE_BIAS(b->Sat[i].Biases[j].Bias)
465 }
466 }
467 ENDBLOCK
468 }
469 if(glo)
470 {
471 INITBLOCK
472 T_MESSAGE_NUMBER(BTYPE_GLONASS)
473 T_GPS_EPOCH_TIME(b->GLONASSEpochTime)
474 T_SSR_UPDATE_INTERVAL(b->UpdateInterval)
475 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
476 --mmi;
477 T_RESERVED5
478 T_NO_OF_SATELLITES(b->NumberOfGLONASSSat)
479 for(i = CLOCKORBIT_NUMGPS;
480 i < CLOCKORBIT_NUMGPS+b->NumberOfGLONASSSat; ++i)
481 {
482 T_GLONASS_SATELLITE_ID(b->Sat[i].ID)
483 T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
484 for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
485 {
486 T_GLONASS_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type)
487 T_CODE_BIAS(b->Sat[i].Biases[j].Bias)
488 }
489 }
490 ENDBLOCK
491 }
492
493 return ressize;
494}
495
496#endif /* NOENCODE */
497
498#ifndef NODECODE
499
500#define DECODESTART \
501 int numbits=0; \
502 uint64_t bitbuffer=0;
503
504#define LOADBITS(a) \
505{ \
506 while((a) > numbits) \
507 { \
508 if(!size--) return GCOBR_SHORTMESSAGE; \
509 bitbuffer = (bitbuffer<<8)|((unsigned char)*(buffer++)); \
510 numbits += 8; \
511 } \
512}
513
514/* extract bits from data stream
515 b = variable to store result, a = number of bits */
516#define GETBITS(b, a) \
517{ \
518 LOADBITS(a) \
519 b = (bitbuffer<<(64-numbits))>>(64-(a)); \
520 numbits -= (a); \
521}
522
523/* extract signed floating value from data stream
524 b = variable to store result, a = number of bits */
525#define GETFLOATSIGN(b, a, c) \
526{ \
527 LOADBITS(a) \
528 b = ((double)(((int64_t)(bitbuffer<<(64-numbits)))>>(64-(a))))*(c); \
529 numbits -= (a); \
530}
531
532#define SKIPBITS(b) { LOADBITS(b) numbits -= (b); }
533
534/* standard values */
535#define G_HEADER(a) GETBITS(a,8)
536#define G_RESERVEDH(a) GETBITS(a,6)
537#define G_SIZE(a) GETBITS(a, 10)
538#define G_MESSAGE_NUMBER(a) GETBITS(a, 12) /* DF002 */
539#define G_RESERVED4 SKIPBITS(4) /* DF001 */
540#define G_RESERVED5 SKIPBITS(5) /* DF001 */
541#define G_GPS_SATELLITE_ID(a) GETBITS(a, 6) /* DF068 */
542#define G_GPS_IODE(a) GETBITS(a, 8) /* DF071 */
543#define G_GLONASS_IOD(a) GETBITS(a, 8) /* DF237 */
544
545/* defined values */
546#define G_DELTA_RADIAL(a) GETFLOATSIGN(a, 22, 1/10000.0)
547#define G_DELTA_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0)
548#define G_DELTA_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0)
549#define G_DELTA_DOT_RADIAL(a) GETFLOATSIGN(a, 21, 1/1000000.0)
550#define G_DELTA_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0)
551#define G_DELTA_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0)
552
553#define G_SATELLITE_REFERENCE_DATUM(a) GETBITS(a, 1)
554#define G_DELTA_CLOCK_C0(a) GETFLOATSIGN(a, 22, 1/10000.0)
555#define G_DELTA_CLOCK_C1(a) GETFLOATSIGN(a, 21, 1/1000000.0)
556#define G_DELTA_CLOCK_C2(a) GETFLOATSIGN(a, 27, 1/50000000.0)
557#define G_NO_OF_CODE_BIASES(a) GETBITS(a, 5)
558#define G_GPS_SIGNAL_IDENTIFIER(a) GETBITS(a, 5)
559#define G_GLONASS_SIGNAL_IDENTIFIER(a) GETBITS(a, 5)
560#define G_CODE_BIAS(a) GETFLOATSIGN(a, 14, 1/100.0)
561#define G_GLONASS_SATELLITE_ID(a) GETBITS(a, 5)
562
563#define G_GPS_EPOCH_TIME(a, b) {int temp; GETBITS(temp, 20) \
564 if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
565#define G_GLONASS_EPOCH_TIME(a, b) {int temp; GETBITS(temp, 17) \
566 if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
567#define G_NO_OF_SATELLITES(a) GETBITS(a, 6)
568#define G_MULTIPLE_MESSAGE_INDICATOR(a) GETBITS(a, 1)
569#define G_SSR_URA(a) {int temp; GETBITS(temp, 6) \
570 (a) = URAToValue(temp);}
571#define G_HR_CLOCK_CORRECTION(a) GETFLOATSIGN(a, 22, 1/10000.0)
572#define G_SSR_UPDATE_INTERVAL(a) GETBITS(a, 4)
573
574enum GCOB_RETURN GetClockOrbitBias(struct ClockOrbit *co, struct Bias *b,
575const char *buffer, size_t size, int *bytesused)
576{
577 int type, mmi=0, i, j, h, rs, nums, pos, id;
578 size_t sizeofrtcmblock;
579 const char *blockstart = buffer;
580 DECODESTART
581
582 if(size < 7)
583 return GCOBR_SHORTBUFFER;
584
585#ifdef DEBUG
586fprintf(stderr, "GetClockOrbitBias START: size %d, numbits %d\n",size, numbits);
587#endif
588
589 G_HEADER(h)
590 G_RESERVEDH(rs)
591 G_SIZE(sizeofrtcmblock);
592
593 if((unsigned char)h != 0xD3 || rs)
594 return GCOBR_UNKNOWNDATA;
595 if(size < sizeofrtcmblock + 3) /* 3 header bytes already removed */
596 return GCOBR_MESSAGEEXCEEDSBUFFER;
597 if(CRC24(sizeofrtcmblock+3, (const unsigned char *) blockstart) !=
598 (uint32_t)((((unsigned char)buffer[sizeofrtcmblock])<<16)|
599 (((unsigned char)buffer[sizeofrtcmblock+1])<<8)|
600 (((unsigned char)buffer[sizeofrtcmblock+2]))))
601 return GCOBR_CRCMISMATCH;
602 size = sizeofrtcmblock; /* reduce size, so overflows are detected */
603
604 G_MESSAGE_NUMBER(type)
605#ifdef DEBUG
606fprintf(stderr, "type %d size %d\n",type,sizeofrtcmblock);
607#endif
608 switch(type)
609 {
610 case COTYPE_GPSORBIT:
611 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
612 co->messageType = COTYPE_GPSORBIT;
613 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
614 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
615 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
616 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
617 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
618 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
619 G_RESERVED4
620 G_NO_OF_SATELLITES(nums)
621 co->OrbitDataSupplied |= 1;
622#ifdef DEBUG
623fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->GPSEpochTime,
624co->UpdateInterval,mmi,co->NumberOfGPSSat,nums, co->SatRefDatum);
625#endif
626 for(i = 0; i < nums; ++i)
627 {
628 G_GPS_SATELLITE_ID(id)
629 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
630 ;
631 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
632 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
633 co->Sat[pos].ID = id;
634
635 G_GPS_IODE(co->Sat[pos].IOD)
636 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
637 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
638 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
639 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
640 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
641 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
642#ifdef DEBUG
643fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f\n",
644co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
645co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
646co->Sat[pos].Orbit.DotDeltaRadial,
647co->Sat[pos].Orbit.DotDeltaAlongTrack,
648co->Sat[pos].Orbit.DotDeltaCrossTrack);
649#endif
650 }
651 break;
652 case COTYPE_GPSCLOCK:
653 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
654 co->messageType = COTYPE_GPSCLOCK;
655 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
656 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
657 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
658 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
659 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
660 G_RESERVED5
661 G_NO_OF_SATELLITES(nums)
662 co->ClockDataSupplied |= 1;
663#ifdef DEBUG
664fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GPSEpochTime,
665co->UpdateInterval,mmi,co->NumberOfGPSSat,nums);
666#endif
667 for(i = 0; i < nums; ++i)
668 {
669 G_GPS_SATELLITE_ID(id)
670 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
671 ;
672 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
673 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
674 co->Sat[pos].ID = id;
675
676 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
677 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
678 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
679#ifdef DEBUG
680fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
681co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
682co->Sat[pos].Clock.DeltaA2);
683#endif
684 }
685 break;
686 case COTYPE_GPSCOMBINED:
687 if(!co) return -5;
688 co->messageType = COTYPE_GPSCOMBINED;
689 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
690 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
691 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
692 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
693 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
694 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
695 G_RESERVED4
696 G_NO_OF_SATELLITES(nums)
697 co->OrbitDataSupplied |= 1;
698 co->ClockDataSupplied |= 1;
699 for(i = 0; i < nums; ++i)
700 {
701 G_GPS_SATELLITE_ID(id)
702 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
703 ;
704 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
705 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
706 co->Sat[pos].ID = id;
707
708 G_GPS_IODE(co->Sat[pos].IOD)
709 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
710 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
711 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
712 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
713 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
714 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
715 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
716 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
717 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
718 }
719 break;
720 case COTYPE_GPSURA:
721 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
722 co->messageType = COTYPE_GPSURA;
723 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
724 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
725 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
726 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
727 G_RESERVED5
728 G_NO_OF_SATELLITES(nums)
729 co->URADataSupplied |= 1;
730 for(i = 0; i < nums; ++i)
731 {
732 G_GPS_SATELLITE_ID(id)
733 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
734 ;
735 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
736 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
737 co->Sat[pos].ID = id;
738
739 G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
740 }
741 break;
742 case COTYPE_GPSHR:
743 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
744 co->messageType = COTYPE_GPSHR;
745 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
746 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
747 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
748 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
749 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
750 G_RESERVED5
751 G_NO_OF_SATELLITES(nums)
752 co->HRDataSupplied |= 1;
753 for(i = 0; i < nums; ++i)
754 {
755 G_GPS_SATELLITE_ID(id)
756 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
757 ;
758 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
759 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
760 co->Sat[pos].ID = id;
761
762 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
763 }
764 break;
765 case COTYPE_GLONASSORBIT:
766 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
767 co->messageType = COTYPE_GLONASSORBIT;
768 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
769 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
770 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
771 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
772 G_RESERVED4
773 G_NO_OF_SATELLITES(nums)
774 co->OrbitDataSupplied |= 2;
775#ifdef DEBUG
776fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->GLONASSEpochTime,
777co->UpdateInterval,mmi,co->NumberOfGLONASSSat,nums, co->SatRefDatum);
778#endif
779 for(i = 0; i < nums; ++i)
780 {
781 G_GLONASS_SATELLITE_ID(id)
782 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
783 ;
784 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
785 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
786 co->Sat[pos].ID = id;
787
788 G_GLONASS_IOD(co->Sat[pos].IOD)
789 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
790 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
791 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
792 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
793 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
794 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
795#ifdef DEBUG
796fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f\n",
797co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
798co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
799co->Sat[pos].Orbit.DotDeltaRadial,
800co->Sat[pos].Orbit.DotDeltaAlongTrack,
801co->Sat[pos].Orbit.DotDeltaCrossTrack);
802#endif
803 }
804 break;
805 case COTYPE_GLONASSCLOCK:
806 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
807 co->messageType = COTYPE_GLONASSCLOCK;
808 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
809 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
810 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
811 G_RESERVED5
812 G_NO_OF_SATELLITES(nums)
813 co->ClockDataSupplied |= 2;
814#ifdef DEBUG
815fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GLONASSEpochTime,
816co->UpdateInterval,mmi,co->NumberOfGLONASSSat,nums);
817#endif
818 for(i = 0; i < nums; ++i)
819 {
820 G_GLONASS_SATELLITE_ID(id)
821 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
822 ;
823 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
824 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
825 co->Sat[pos].ID = id;
826
827 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
828 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
829 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
830#ifdef DEBUG
831fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
832co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
833co->Sat[pos].Clock.DeltaA2);
834#endif
835 }
836 break;
837 case COTYPE_GLONASSCOMBINED:
838 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
839 co->messageType = COTYPE_GLONASSCOMBINED;
840 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
841 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
842 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
843 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
844 G_RESERVED4
845 G_NO_OF_SATELLITES(nums)
846 co->OrbitDataSupplied |= 2;
847 co->ClockDataSupplied |= 2;
848 for(i = 0; i < nums; ++i)
849 {
850 G_GLONASS_SATELLITE_ID(id)
851 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
852 ;
853 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
854 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
855 co->Sat[pos].ID = id;
856
857 G_GLONASS_IOD(co->Sat[pos].IOD)
858 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
859 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
860 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
861 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
862 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
863 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
864 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
865 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
866 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
867 }
868 break;
869 case COTYPE_GLONASSURA:
870 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
871 co->messageType = COTYPE_GLONASSURA;
872 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
873 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
874 G_RESERVED5
875 G_NO_OF_SATELLITES(nums)
876 co->URADataSupplied |= 2;
877 for(i = 0; i < nums; ++i)
878 {
879 G_GLONASS_SATELLITE_ID(id)
880 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
881 ;
882 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
883 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
884 co->Sat[pos].ID = id;
885
886 G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
887 }
888 break;
889 case COTYPE_GLONASSHR:
890 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
891 co->messageType = COTYPE_GLONASSHR;
892 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
893 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
894 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
895 G_RESERVED5
896 G_NO_OF_SATELLITES(nums)
897 co->HRDataSupplied |= 2;
898 for(i = 0; i < nums; ++i)
899 {
900 G_GLONASS_SATELLITE_ID(id)
901 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
902 ;
903 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
904 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
905 co->Sat[pos].ID = id;
906
907 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
908 }
909 break;
910 case BTYPE_GPS:
911 if(!b) return GCOBR_NOBIASPARAMETER;
912 b->messageType = BTYPE_GPS;
913 G_GPS_EPOCH_TIME(b->GPSEpochTime, b->NumberOfGPSSat)
914 G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
915 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
916 G_RESERVED5
917 G_NO_OF_SATELLITES(nums)
918 for(i = 0; i < nums; ++i)
919 {
920 G_GPS_SATELLITE_ID(id)
921 for(pos = 0; pos < b->NumberOfGPSSat && b->Sat[pos].ID != id; ++pos)
922 ;
923 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
924 else if(pos == b->NumberOfGPSSat) ++b->NumberOfGPSSat;
925 b->Sat[pos].ID = id;
926
927 G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases)
928 for(j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j)
929 {
930 G_GPS_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
931 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
932 }
933 }
934 break;
935 case BTYPE_GLONASS:
936 if(!b) return GCOBR_NOBIASPARAMETER;
937 b->messageType = BTYPE_GLONASS;
938 G_GLONASS_EPOCH_TIME(b->GLONASSEpochTime, b->NumberOfGLONASSSat)
939 G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
940 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
941 G_RESERVED5
942 G_NO_OF_SATELLITES(nums)
943 for(i = 0; i < nums; ++i)
944 {
945 G_GLONASS_SATELLITE_ID(id)
946 for(pos = CLOCKORBIT_NUMGPS; pos < b->NumberOfGLONASSSat && b->Sat[pos].ID != id; ++pos)
947 ;
948 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
949 else if(pos == b->NumberOfGLONASSSat) ++b->NumberOfGLONASSSat;
950 b->Sat[pos].ID = id;
951
952 G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases)
953 for(j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j)
954 {
955 G_GLONASS_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
956 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
957 }
958 }
959 break;
960 default:
961 if(bytesused)
962 *bytesused = sizeofrtcmblock+6;
963 return GCOBR_UNKNOWNTYPE;
964 }
965#ifdef DEBUG
966for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type)
967 numbits += 8;
968fprintf(stderr, "numbits left %d\n",numbits);
969#endif
970 if(bytesused)
971 *bytesused = sizeofrtcmblock+6;
972 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
973}
974#endif /* NODECODE */
Note: See TracBrowser for help on using the repository browser.