source: ntrip/trunk/BNS/RTCM/clock_orbit_rtcm.c@ 1864

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

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