source: ntrip/trunk/clock_and_orbit/lib/clock_orbit_rtcm.c@ 3593

Last change on this file since 3593 was 3593, checked in by mervart, 12 years ago

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