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

Last change on this file since 2379 was 2323, checked in by stoecker, 15 years ago

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