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

Last change on this file since 2074 was 1842, checked in by stoecker, 16 years ago

fixed a set of decoding bugs
  • Property svn:executable set to *
File size: 34.2 KB
Line 
1/* Programheader
2
3 Name: clock_orbit_rtcm.c
4 Project: RTCM3
5 Version: $Id: clock_orbit_rtcm.c,v 1.15 2009/05/14 13:24:13 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#ifdef DEBUG
576fprintf(stderr, "GetClockOrbitBias START: size %d, numbits %d\n",size, numbits);
577#endif
578
579 G_HEADER(h)
580 G_RESERVEDH(rs)
581 G_SIZE(sizeofrtcmblock);
582
583 if((unsigned char)h != 0xD3 || rs)
584 return GCOBR_UNKNOWNDATA;
585 if(size < sizeofrtcmblock + 3) /* 3 header bytes already removed */
586 return GCOBR_MESSAGEEXCEEDSBUFFER;
587 if(CRC24(sizeofrtcmblock+3, (const unsigned char *) blockstart) !=
588 (uint32_t)((((unsigned char)buffer[sizeofrtcmblock])<<16)|
589 (((unsigned char)buffer[sizeofrtcmblock+1])<<8)|
590 (((unsigned char)buffer[sizeofrtcmblock+2]))))
591 return GCOBR_CRCMISMATCH;
592 size = sizeofrtcmblock; /* reduce size, so overflows are detected */
593
594 G_MESSAGE_NUMBER(type)
595#ifdef DEBUG
596fprintf(stderr, "type %d size %d\n",type,sizeofrtcmblock);
597#endif
598 switch(type)
599 {
600 case COTYPE_GPSORBIT:
601 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
602 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
603 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
604 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
605 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
606 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
607 G_RESERVED5
608 G_NO_OF_SATELLITES(nums)
609 co->OrbitDataSupplied |= 1;
610#ifdef DEBUG
611fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GPSEpochTime,
612co->UpdateInterval,mmi,co->NumberOfGPSSat,nums);
613#endif
614 for(i = 0; i < nums; ++i)
615 {
616 G_GPS_SATELLITE_ID(id)
617 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
618 ;
619 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
620 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
621 co->Sat[pos].ID = id;
622
623 G_GPS_IODE(co->Sat[pos].IOD)
624 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
625 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
626 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
627 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
628 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
629 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
630 G_DELTA_DOT_DOT_RADIAL(co->Sat[pos].Orbit.DotDotDeltaRadial)
631 G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDotDeltaAlongTrack)
632 G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDotDeltaCrossTrack)
633 G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
634 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
635#ifdef DEBUG
636fprintf(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",
637co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
638co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
639co->Sat[pos].Orbit.DotDeltaRadial,
640co->Sat[pos].Orbit.DotDeltaAlongTrack,
641co->Sat[pos].Orbit.DotDeltaCrossTrack,
642co->Sat[pos].Orbit.DotDotDeltaRadial,
643co->Sat[pos].Orbit.DotDotDeltaAlongTrack,
644co->Sat[pos].Orbit.DotDotDeltaCrossTrack,
645co->SatRefPoint,
646co->SatRefDatum);
647#endif
648 }
649 break;
650 case COTYPE_GPSCLOCK:
651 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
652 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
653 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
654 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
655 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
656 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
657 G_RESERVED5
658 G_NO_OF_SATELLITES(nums)
659 co->ClockDataSupplied |= 1;
660#ifdef DEBUG
661fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GPSEpochTime,
662co->UpdateInterval,mmi,co->NumberOfGPSSat,nums);
663#endif
664 for(i = 0; i < nums; ++i)
665 {
666 G_GPS_SATELLITE_ID(id)
667 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
668 ;
669 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
670 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
671 co->Sat[pos].ID = id;
672
673 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
674 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
675 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
676#ifdef DEBUG
677fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
678co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
679co->Sat[pos].Clock.DeltaA2);
680#endif
681 }
682 break;
683 case COTYPE_GPSCOMBINED:
684 if(!co) return -5;
685 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
686 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
687 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
688 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
689 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
690 G_RESERVED5
691 G_NO_OF_SATELLITES(nums)
692 co->OrbitDataSupplied |= 1;
693 co->ClockDataSupplied |= 1;
694 for(i = 0; i < nums; ++i)
695 {
696 G_GPS_SATELLITE_ID(id)
697 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
698 ;
699 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
700 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
701 co->Sat[pos].ID = id;
702
703 G_GPS_IODE(co->Sat[pos].IOD)
704 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
705 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
706 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
707 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
708 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
709 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
710 G_DELTA_DOT_DOT_RADIAL(co->Sat[pos].Orbit.DotDotDeltaRadial)
711 G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDotDeltaAlongTrack)
712 G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDotDeltaCrossTrack)
713 G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
714 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
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 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
723 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
724 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
725 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
726 G_RESERVED5
727 G_NO_OF_SATELLITES(nums)
728 co->URADataSupplied |= 1;
729 for(i = 0; i < nums; ++i)
730 {
731 G_GPS_SATELLITE_ID(id)
732 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
733 ;
734 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
735 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
736 co->Sat[pos].ID = id;
737
738 G_SSR_URA(co->Sat[pos].URA)
739 }
740 break;
741 case COTYPE_GPSHR:
742 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
743 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
744 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
745 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
746 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
747 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
748 G_RESERVED5
749 G_NO_OF_SATELLITES(nums)
750 co->HRDataSupplied |= 1;
751 for(i = 0; i < nums; ++i)
752 {
753 G_GPS_SATELLITE_ID(id)
754 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
755 ;
756 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
757 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
758 co->Sat[pos].ID = id;
759
760 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
761 }
762 break;
763 case COTYPE_GLONASSORBIT:
764 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
765 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
766 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
767 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
768 G_RESERVED5
769 G_NO_OF_SATELLITES(nums)
770 co->OrbitDataSupplied |= 2;
771#ifdef DEBUG
772fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GLONASSEpochTime,
773co->UpdateInterval,mmi,co->NumberOfGLONASSSat,nums);
774#endif
775 for(i = 0; i < nums; ++i)
776 {
777 G_GLONASS_SATELLITE_ID(id)
778 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
779 ;
780 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
781 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
782 co->Sat[pos].ID = id;
783
784 G_GLONASS_IOD(co->Sat[pos].IOD)
785 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
786 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
787 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
788 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
789 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
790 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
791 G_DELTA_DOT_DOT_RADIAL(co->Sat[pos].Orbit.DotDotDeltaRadial)
792 G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDotDeltaAlongTrack)
793 G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDotDeltaCrossTrack)
794 G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
795 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
796#ifdef DEBUG
797fprintf(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",
798co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
799co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
800co->Sat[pos].Orbit.DotDeltaRadial,
801co->Sat[pos].Orbit.DotDeltaAlongTrack,
802co->Sat[pos].Orbit.DotDeltaCrossTrack,
803co->Sat[pos].Orbit.DotDotDeltaRadial,
804co->Sat[pos].Orbit.DotDotDeltaAlongTrack,
805co->Sat[pos].Orbit.DotDotDeltaCrossTrack,
806co->SatRefPoint,
807co->SatRefDatum);
808#endif
809 }
810 break;
811 case COTYPE_GLONASSCLOCK:
812 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
813 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
814 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
815 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
816 G_RESERVED5
817 G_NO_OF_SATELLITES(nums)
818 co->ClockDataSupplied |= 2;
819#ifdef DEBUG
820fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GLONASSEpochTime,
821co->UpdateInterval,mmi,co->NumberOfGLONASSSat,nums);
822#endif
823 for(i = 0; i < nums; ++i)
824 {
825 G_GLONASS_SATELLITE_ID(id)
826 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
827 ;
828 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
829 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
830 co->Sat[pos].ID = id;
831
832 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
833 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
834 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
835#ifdef DEBUG
836fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
837co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
838co->Sat[pos].Clock.DeltaA2);
839#endif
840 }
841 break;
842 case COTYPE_GLONASSCOMBINED:
843 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
844 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
845 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
846 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
847 G_RESERVED5
848 G_NO_OF_SATELLITES(nums)
849 co->OrbitDataSupplied |= 2;
850 co->ClockDataSupplied |= 2;
851 for(i = 0; i < nums; ++i)
852 {
853 G_GLONASS_SATELLITE_ID(id)
854 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
855 ;
856 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
857 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
858 co->Sat[pos].ID = id;
859
860 G_GLONASS_IOD(co->Sat[pos].IOD)
861 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
862 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
863 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
864 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
865 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
866 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
867 G_DELTA_DOT_DOT_RADIAL(co->Sat[pos].Orbit.DotDotDeltaRadial)
868 G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDotDeltaAlongTrack)
869 G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDotDeltaCrossTrack)
870 G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
871 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
872 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
873 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
874 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
875 }
876 break;
877 case COTYPE_GLONASSURA:
878 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
879 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
880 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
881 G_RESERVED5
882 G_NO_OF_SATELLITES(nums)
883 co->URADataSupplied |= 2;
884 for(i = 0; i < nums; ++i)
885 {
886 G_GLONASS_SATELLITE_ID(id)
887 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
888 ;
889 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
890 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
891 co->Sat[pos].ID = id;
892
893 G_SSR_URA(co->Sat[pos].URA)
894 }
895 break;
896 case COTYPE_GLONASSHR:
897 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
898 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
899 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
900 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
901 G_RESERVED5
902 G_NO_OF_SATELLITES(nums)
903 co->HRDataSupplied |= 2;
904 for(i = 0; i < nums; ++i)
905 {
906 G_GLONASS_SATELLITE_ID(id)
907 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
908 ;
909 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
910 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
911 co->Sat[pos].ID = id;
912
913 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
914 }
915 break;
916 case BTYPE_GPS:
917 if(!b) return GCOBR_NOBIASPARAMETER;
918 G_GPS_EPOCH_TIME(b->GPSEpochTime, b->NumberOfGPSSat)
919 G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
920 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
921 G_RESERVED5
922 G_NO_OF_SATELLITES(nums)
923 for(i = 0; i < nums; ++i)
924 {
925 G_GPS_SATELLITE_ID(id)
926 for(pos = 0; pos < b->NumberOfGPSSat && b->Sat[pos].ID != id; ++pos)
927 ;
928 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
929 else if(pos == b->NumberOfGPSSat) ++b->NumberOfGPSSat;
930 b->Sat[pos].ID = id;
931
932 G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases)
933 for(j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j)
934 {
935 G_GPS_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
936 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
937 }
938 }
939 break;
940 case BTYPE_GLONASS:
941 if(!b) return GCOBR_NOBIASPARAMETER;
942 G_GLONASS_EPOCH_TIME(b->GLONASSEpochTime, b->NumberOfGLONASSSat)
943 G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
944 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
945 G_RESERVED5
946 G_NO_OF_SATELLITES(nums)
947 for(i = 0; i < nums; ++i)
948 {
949 G_GLONASS_SATELLITE_ID(id)
950 for(pos = CLOCKORBIT_NUMGPS; pos < b->NumberOfGLONASSSat && b->Sat[pos].ID != id; ++pos)
951 ;
952 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
953 else if(pos == b->NumberOfGLONASSSat) ++b->NumberOfGLONASSSat;
954 b->Sat[pos].ID = id;
955
956 G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases)
957 for(j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j)
958 {
959 G_GLONASS_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
960 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
961 }
962 }
963 break;
964 default:
965 if(bytesused)
966 *bytesused = sizeofrtcmblock+6;
967 return GCOBR_UNKNOWNTYPE;
968 }
969#ifdef DEBUG
970for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type)
971 numbits += 8;
972fprintf(stderr, "numbits left %d\n",numbits);
973#endif
974 if(bytesused)
975 *bytesused = sizeofrtcmblock+6;
976 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
977}
978#endif /* NODECODE */
Note: See TracBrowser for help on using the repository browser.