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

Last change on this file since 1840 was 1840, checked in by stoecker, 13 years ago

updated

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