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

Last change on this file since 3511 was 3511, checked in by stoecker, 12 years ago

update to final standard
  • Property svn:executable set to *
  • Property svn:keywords set to Id
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 3511 2011-11-25 11:16:34Z stoecker $
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_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
883 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
884 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
885#ifdef DEBUG
886fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
887co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
888co->Sat[pos].Clock.DeltaA2);
889#endif
890 }
891 break;
892 case COTYPE_GLONASSCOMBINED:
893 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
894 co->messageType = COTYPE_GLONASSCOMBINED;
895 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
896 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
897 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
898 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
899 G_SSR_IOD(co->SSRIOD)
900 G_SSR_PROVIDER_ID(co->SSRProviderID)
901 G_SSR_SOLUTION_ID(co->SSRSolutionID)
902 G_NO_OF_SATELLITES(nums)
903 co->OrbitDataSupplied |= 2;
904 co->ClockDataSupplied |= 2;
905 for(i = 0; i < nums; ++i)
906 {
907 G_GLONASS_SATELLITE_ID(id)
908 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
909 ;
910 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
911 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
912 co->Sat[pos].ID = id;
913
914 G_GLONASS_IOD(co->Sat[pos].IOD)
915 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
916 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
917 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
918 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
919 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
920 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
921 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
922 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
923 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
924 }
925 break;
926 case COTYPE_GLONASSURA:
927 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
928 co->messageType = COTYPE_GLONASSURA;
929 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
930 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
931 G_SSR_IOD(co->SSRIOD)
932 G_SSR_PROVIDER_ID(co->SSRProviderID)
933 G_SSR_SOLUTION_ID(co->SSRSolutionID)
934 G_NO_OF_SATELLITES(nums)
935 co->URADataSupplied |= 2;
936 for(i = 0; i < nums; ++i)
937 {
938 G_GLONASS_SATELLITE_ID(id)
939 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
940 ;
941 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
942 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
943 co->Sat[pos].ID = id;
944
945 G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
946 }
947 break;
948 case COTYPE_GLONASSHR:
949 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
950 co->messageType = COTYPE_GLONASSHR;
951 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
952 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
953 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
954 G_SSR_IOD(co->SSRIOD)
955 G_SSR_PROVIDER_ID(co->SSRProviderID)
956 G_SSR_SOLUTION_ID(co->SSRSolutionID)
957 G_NO_OF_SATELLITES(nums)
958 co->HRDataSupplied |= 2;
959 for(i = 0; i < nums; ++i)
960 {
961 G_GLONASS_SATELLITE_ID(id)
962 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
963 ;
964 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
965 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
966 co->Sat[pos].ID = id;
967
968 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
969 }
970 break;
971 case BTYPE_GPS:
972 if(!b) return GCOBR_NOBIASPARAMETER;
973 b->messageType = BTYPE_GPS;
974 G_GPS_EPOCH_TIME(b->GPSEpochTime, b->NumberOfGPSSat)
975 G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
976 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
977 G_SSR_IOD(b->SSRIOD)
978 G_SSR_PROVIDER_ID(b->SSRProviderID)
979 G_SSR_SOLUTION_ID(b->SSRSolutionID)
980 G_NO_OF_SATELLITES(nums)
981 for(i = 0; i < nums; ++i)
982 {
983 G_GPS_SATELLITE_ID(id)
984 for(pos = 0; pos < b->NumberOfGPSSat && b->Sat[pos].ID != id; ++pos)
985 ;
986 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
987 else if(pos == b->NumberOfGPSSat) ++b->NumberOfGPSSat;
988 b->Sat[pos].ID = id;
989
990 G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases)
991 for(j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j)
992 {
993 G_GPS_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
994 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
995 }
996 }
997 break;
998 case BTYPE_GLONASS:
999 if(!b) return GCOBR_NOBIASPARAMETER;
1000 b->messageType = BTYPE_GLONASS;
1001 G_GLONASS_EPOCH_TIME(b->GLONASSEpochTime, b->NumberOfGLONASSSat)
1002 G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
1003 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1004 G_SSR_IOD(b->SSRIOD)
1005 G_SSR_PROVIDER_ID(b->SSRProviderID)
1006 G_SSR_SOLUTION_ID(b->SSRSolutionID)
1007 G_NO_OF_SATELLITES(nums)
1008 for(i = 0; i < nums; ++i)
1009 {
1010 G_GLONASS_SATELLITE_ID(id)
1011 for(pos = CLOCKORBIT_NUMGPS; pos < b->NumberOfGLONASSSat && b->Sat[pos].ID != id; ++pos)
1012 ;
1013 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
1014 else if(pos == b->NumberOfGLONASSSat) ++b->NumberOfGLONASSSat;
1015 b->Sat[pos].ID = id;
1016
1017 G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases)
1018 for(j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j)
1019 {
1020 G_GLONASS_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
1021 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
1022 }
1023 }
1024 break;
1025 default:
1026 if(bytesused)
1027 *bytesused = sizeofrtcmblock+6;
1028 return GCOBR_UNKNOWNTYPE;
1029 }
1030#ifdef DEBUG
1031for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type)
1032 numbits += 8;
1033fprintf(stderr, "numbits left %d\n",numbits);
1034#endif
1035 if(bytesused)
1036 *bytesused = sizeofrtcmblock+6;
1037 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
1038}
1039#endif /* NODECODE */
Note: See TracBrowser for help on using the repository browser.