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

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

use correct error code
  • 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 3728 2012-03-15 13:49:35Z 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_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
388 T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
389 T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)
390 }
391 ENDBLOCK
392 }
393 if(gloco)
394 {
395 INITBLOCK
396 T_MESSAGE_NUMBER(COTYPE_GLONASSCOMBINED)
397 T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
398 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
399 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
400 --mmi;
401 T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
402 T_SSR_IOD(co->SSRIOD)
403 T_SSR_PROVIDER_ID(co->SSRProviderID)
404 T_SSR_SOLUTION_ID(co->SSRSolutionID)
405 T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
406 for(i = CLOCKORBIT_NUMGPS;
407 i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
408 {
409 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
410 T_GLONASS_IOD(co->Sat[i].IOD)
411 T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
412 T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
413 T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
414 T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
415 T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
416 T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
417 T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
418 T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
419 T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)
420 }
421 ENDBLOCK
422 }
423 if(glohr)
424 {
425 INITBLOCK
426 T_MESSAGE_NUMBER(COTYPE_GLONASSHR)
427 T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
428 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
429 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
430 --mmi;
431 T_SSR_IOD(co->SSRIOD)
432 T_SSR_PROVIDER_ID(co->SSRProviderID)
433 T_SSR_SOLUTION_ID(co->SSRSolutionID)
434 T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
435 for(i = CLOCKORBIT_NUMGPS;
436 i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
437 {
438 T_GPS_SATELLITE_ID(co->Sat[i].ID)
439 T_HR_CLOCK_CORRECTION(co->Sat[i].hrclock)
440 }
441 ENDBLOCK
442 }
443 if(glour)
444 {
445 INITBLOCK
446 T_MESSAGE_NUMBER(COTYPE_GLONASSURA)
447 T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
448 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
449 --mmi;
450 T_SSR_IOD(co->SSRIOD)
451 T_SSR_PROVIDER_ID(co->SSRProviderID)
452 T_SSR_SOLUTION_ID(co->SSRSolutionID)
453 T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
454 for(i = CLOCKORBIT_NUMGPS;
455 i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
456 {
457 T_GPS_SATELLITE_ID(co->Sat[i].ID)
458 T_SSR_URA(ValueToURA(co->Sat[i].UserRangeAccuracy))
459 }
460 ENDBLOCK
461 }
462
463 return ressize;
464}
465
466size_t MakeBias(const struct Bias *b, enum BiasType type,
467int moremessagesfollow, char *buffer, size_t size)
468{
469 int gps=0, glo=0, mmi, i, j;
470
471 STARTDATA
472
473 if(b->NumberOfGPSSat && (type == BTYPE_AUTO || type == BTYPE_GPS))
474 gps = 1;
475 if(b->NumberOfGLONASSSat && (type == BTYPE_AUTO || type == BTYPE_GLONASS))
476 glo = 1;
477
478 mmi = gps+glo; /* required for multimessage */
479 if(!moremessagesfollow) --mmi;
480
481 if(gps)
482 {
483 INITBLOCK
484 T_MESSAGE_NUMBER(BTYPE_GPS)
485 T_GPS_EPOCH_TIME(b->GPSEpochTime)
486 T_SSR_UPDATE_INTERVAL(b->UpdateInterval)
487 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
488 --mmi;
489 T_SSR_IOD(b->SSRIOD)
490 T_SSR_PROVIDER_ID(b->SSRProviderID)
491 T_SSR_SOLUTION_ID(b->SSRSolutionID)
492 T_NO_OF_SATELLITES(b->NumberOfGPSSat)
493 for(i = 0; i < b->NumberOfGPSSat; ++i)
494 {
495 T_GPS_SATELLITE_ID(b->Sat[i].ID)
496 T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
497 for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
498 {
499 T_GPS_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type)
500 T_CODE_BIAS(b->Sat[i].Biases[j].Bias)
501 }
502 }
503 ENDBLOCK
504 }
505 if(glo)
506 {
507 INITBLOCK
508 T_MESSAGE_NUMBER(BTYPE_GLONASS)
509 T_GLONASS_EPOCH_TIME(b->GLONASSEpochTime)
510 T_SSR_UPDATE_INTERVAL(b->UpdateInterval)
511 T_MULTIPLE_MESSAGE_INDICATOR(/*mmi ? 1 :*/0)
512 --mmi;
513 T_SSR_IOD(b->SSRIOD)
514 T_SSR_PROVIDER_ID(b->SSRProviderID)
515 T_SSR_SOLUTION_ID(b->SSRSolutionID)
516 T_NO_OF_SATELLITES(b->NumberOfGLONASSSat)
517 for(i = CLOCKORBIT_NUMGPS;
518 i < CLOCKORBIT_NUMGPS+b->NumberOfGLONASSSat; ++i)
519 {
520 T_GLONASS_SATELLITE_ID(b->Sat[i].ID)
521 T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
522 for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
523 {
524 T_GLONASS_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type)
525 T_CODE_BIAS(b->Sat[i].Biases[j].Bias)
526 }
527 }
528 ENDBLOCK
529 }
530
531 return ressize;
532}
533
534#endif /* NOENCODE */
535
536#ifndef NODECODE
537
538#define DECODESTART \
539 int numbits=0; \
540 uint64_t bitbuffer=0;
541
542#define LOADBITS(a) \
543{ \
544 while((a) > numbits) \
545 { \
546 if(!size--) return GCOBR_SHORTMESSAGE; \
547 bitbuffer = (bitbuffer<<8)|((unsigned char)*(buffer++)); \
548 numbits += 8; \
549 } \
550}
551
552/* extract bits from data stream
553 b = variable to store result, a = number of bits */
554#define GETBITS(b, a) \
555{ \
556 LOADBITS(a) \
557 b = (bitbuffer<<(64-numbits))>>(64-(a)); \
558 numbits -= (a); \
559}
560
561/* extract signed floating value from data stream
562 b = variable to store result, a = number of bits */
563#define GETFLOATSIGN(b, a, c) \
564{ \
565 LOADBITS(a) \
566 b = ((double)(((int64_t)(bitbuffer<<(64-numbits)))>>(64-(a))))*(c); \
567 numbits -= (a); \
568}
569
570#define SKIPBITS(b) { LOADBITS(b) numbits -= (b); }
571
572/* standard values */
573#define G_HEADER(a) GETBITS(a,8)
574#define G_RESERVEDH(a) GETBITS(a,6)
575#define G_SIZE(a) GETBITS(a, 10)
576#define G_MESSAGE_NUMBER(a) GETBITS(a, 12) /* DF002 */
577#define G_GPS_SATELLITE_ID(a) GETBITS(a, 6) /* DF068 */
578#define G_GPS_IODE(a) GETBITS(a, 8) /* DF071 */
579#define G_GLONASS_IOD(a) GETBITS(a, 8) /* DF237 */
580
581/* defined values */
582#define G_DELTA_RADIAL(a) GETFLOATSIGN(a, 22, 1/10000.0)
583#define G_DELTA_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0)
584#define G_DELTA_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0)
585#define G_DELTA_DOT_RADIAL(a) GETFLOATSIGN(a, 21, 1/1000000.0)
586#define G_DELTA_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0)
587#define G_DELTA_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0)
588
589#define G_SATELLITE_REFERENCE_DATUM(a) GETBITS(a, 1)
590#define G_DELTA_CLOCK_C0(a) GETFLOATSIGN(a, 22, 1/10000.0)
591#define G_DELTA_CLOCK_C1(a) GETFLOATSIGN(a, 21, 1/1000000.0)
592#define G_DELTA_CLOCK_C2(a) GETFLOATSIGN(a, 27, 1/50000000.0)
593#define G_NO_OF_CODE_BIASES(a) GETBITS(a, 5)
594#define G_GPS_SIGNAL_IDENTIFIER(a) GETBITS(a, 5)
595#define G_GLONASS_SIGNAL_IDENTIFIER(a) GETBITS(a, 5)
596#define G_CODE_BIAS(a) GETFLOATSIGN(a, 14, 1/100.0)
597#define G_GLONASS_SATELLITE_ID(a) GETBITS(a, 5)
598
599#define G_GPS_EPOCH_TIME(a, b) {int temp; GETBITS(temp, 20) \
600 if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
601#define G_GLONASS_EPOCH_TIME(a, b) {int temp; GETBITS(temp, 17) \
602 if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
603#define G_NO_OF_SATELLITES(a) GETBITS(a, 6)
604#define G_MULTIPLE_MESSAGE_INDICATOR(a) GETBITS(a, 1)
605#define G_SSR_URA(a) {int temp; GETBITS(temp, 6) \
606 (a) = URAToValue(temp);}
607#define G_HR_CLOCK_CORRECTION(a) GETFLOATSIGN(a, 22, 1/10000.0)
608#define G_SSR_UPDATE_INTERVAL(a) GETBITS(a, 4)
609
610#define G_SSR_IOD(a) GETBITS(a, 4)
611#define G_SSR_PROVIDER_ID(a) GETBITS(a, 16)
612#define G_SSR_SOLUTION_ID(a) GETBITS(a, 4)
613
614enum GCOB_RETURN GetClockOrbitBias(struct ClockOrbit *co, struct Bias *b,
615const char *buffer, size_t size, int *bytesused)
616{
617 int type, mmi=0, i, j, h, rs, nums, pos, id;
618 size_t sizeofrtcmblock;
619 const char *blockstart = buffer;
620 DECODESTART
621
622 if(size < 7)
623 return GCOBR_SHORTBUFFER;
624
625#ifdef DEBUG
626fprintf(stderr, "GetClockOrbitBias START: size %d, numbits %d\n",size, numbits);
627#endif
628
629 G_HEADER(h)
630 G_RESERVEDH(rs)
631 G_SIZE(sizeofrtcmblock);
632
633 if((unsigned char)h != 0xD3 || rs)
634 return GCOBR_UNKNOWNDATA;
635 if(size < sizeofrtcmblock + 3) /* 3 header bytes already removed */
636 return GCOBR_MESSAGEEXCEEDSBUFFER;
637 if(CRC24(sizeofrtcmblock+3, (const unsigned char *) blockstart) !=
638 (uint32_t)((((unsigned char)buffer[sizeofrtcmblock])<<16)|
639 (((unsigned char)buffer[sizeofrtcmblock+1])<<8)|
640 (((unsigned char)buffer[sizeofrtcmblock+2]))))
641 return GCOBR_CRCMISMATCH;
642 size = sizeofrtcmblock; /* reduce size, so overflows are detected */
643
644 G_MESSAGE_NUMBER(type)
645#ifdef DEBUG
646fprintf(stderr, "type %d size %d\n",type,sizeofrtcmblock);
647#endif
648 switch(type)
649 {
650 case COTYPE_GPSORBIT:
651 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
652 co->messageType = COTYPE_GPSORBIT;
653 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
654 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
655 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
656 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
657 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
658 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
659 G_SSR_IOD(co->SSRIOD)
660 G_SSR_PROVIDER_ID(co->SSRProviderID)
661 G_SSR_SOLUTION_ID(co->SSRSolutionID)
662 G_NO_OF_SATELLITES(nums)
663 co->OrbitDataSupplied |= 1;
664#ifdef DEBUG
665fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->GPSEpochTime,
666co->UpdateInterval,mmi,co->NumberOfGPSSat,nums, co->SatRefDatum);
667#endif
668 for(i = 0; i < nums; ++i)
669 {
670 G_GPS_SATELLITE_ID(id)
671 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
672 ;
673 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
674 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
675 co->Sat[pos].ID = id;
676
677 G_GPS_IODE(co->Sat[pos].IOD)
678 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
679 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
680 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
681 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
682 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
683 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
684#ifdef DEBUG
685fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f\n",
686co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
687co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
688co->Sat[pos].Orbit.DotDeltaRadial,
689co->Sat[pos].Orbit.DotDeltaAlongTrack,
690co->Sat[pos].Orbit.DotDeltaCrossTrack);
691#endif
692 }
693 break;
694 case COTYPE_GPSCLOCK:
695 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
696 co->messageType = COTYPE_GPSCLOCK;
697 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
698 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
699 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
700 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
701 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
702 G_SSR_IOD(co->SSRIOD)
703 G_SSR_PROVIDER_ID(co->SSRProviderID)
704 G_SSR_SOLUTION_ID(co->SSRSolutionID)
705 G_NO_OF_SATELLITES(nums)
706 co->ClockDataSupplied |= 1;
707#ifdef DEBUG
708fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GPSEpochTime,
709co->UpdateInterval,mmi,co->NumberOfGPSSat,nums);
710#endif
711 for(i = 0; i < nums; ++i)
712 {
713 G_GPS_SATELLITE_ID(id)
714 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
715 ;
716 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
717 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
718 co->Sat[pos].ID = id;
719
720 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
721 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
722 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
723#ifdef DEBUG
724fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
725co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
726co->Sat[pos].Clock.DeltaA2);
727#endif
728 }
729 break;
730 case COTYPE_GPSCOMBINED:
731 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
732 co->messageType = COTYPE_GPSCOMBINED;
733 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
734 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
735 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
736 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
737 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
738 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
739 G_SSR_IOD(co->SSRIOD)
740 G_SSR_PROVIDER_ID(co->SSRProviderID)
741 G_SSR_SOLUTION_ID(co->SSRSolutionID)
742 G_NO_OF_SATELLITES(nums)
743 co->OrbitDataSupplied |= 1;
744 co->ClockDataSupplied |= 1;
745 for(i = 0; i < nums; ++i)
746 {
747 G_GPS_SATELLITE_ID(id)
748 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
749 ;
750 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
751 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
752 co->Sat[pos].ID = id;
753
754 G_GPS_IODE(co->Sat[pos].IOD)
755 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
756 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
757 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
758 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
759 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
760 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
761 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
762 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
763 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
764 }
765 break;
766 case COTYPE_GPSURA:
767 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
768 co->messageType = COTYPE_GPSURA;
769 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
770 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
771 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
772 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
773 G_SSR_IOD(co->SSRIOD)
774 G_SSR_PROVIDER_ID(co->SSRProviderID)
775 G_SSR_SOLUTION_ID(co->SSRSolutionID)
776 G_NO_OF_SATELLITES(nums)
777 co->URADataSupplied |= 1;
778 for(i = 0; i < nums; ++i)
779 {
780 G_GPS_SATELLITE_ID(id)
781 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
782 ;
783 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
784 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
785 co->Sat[pos].ID = id;
786
787 G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
788 }
789 break;
790 case COTYPE_GPSHR:
791 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
792 co->messageType = COTYPE_GPSHR;
793 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
794 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
795 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
796 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
797 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
798 G_SSR_IOD(co->SSRIOD)
799 G_SSR_PROVIDER_ID(co->SSRProviderID)
800 G_SSR_SOLUTION_ID(co->SSRSolutionID)
801 G_NO_OF_SATELLITES(nums)
802 co->HRDataSupplied |= 1;
803 for(i = 0; i < nums; ++i)
804 {
805 G_GPS_SATELLITE_ID(id)
806 for(pos = 0; pos < co->NumberOfGPSSat && co->Sat[pos].ID != id; ++pos)
807 ;
808 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
809 else if(pos == co->NumberOfGPSSat) ++co->NumberOfGPSSat;
810 co->Sat[pos].ID = id;
811
812 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
813 }
814 break;
815 case COTYPE_GLONASSORBIT:
816 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
817 co->messageType = COTYPE_GLONASSORBIT;
818 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
819 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
820 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
821 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
822 G_SSR_IOD(co->SSRIOD)
823 G_SSR_PROVIDER_ID(co->SSRProviderID)
824 G_SSR_SOLUTION_ID(co->SSRSolutionID)
825 G_NO_OF_SATELLITES(nums)
826 co->OrbitDataSupplied |= 2;
827#ifdef DEBUG
828fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->GLONASSEpochTime,
829co->UpdateInterval,mmi,co->NumberOfGLONASSSat,nums, co->SatRefDatum);
830#endif
831 for(i = 0; i < nums; ++i)
832 {
833 G_GLONASS_SATELLITE_ID(id)
834 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
835 ;
836 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
837 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
838 co->Sat[pos].ID = id;
839
840 G_GLONASS_IOD(co->Sat[pos].IOD)
841 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
842 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
843 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
844 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
845 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
846 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
847#ifdef DEBUG
848fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f\n",
849co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
850co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
851co->Sat[pos].Orbit.DotDeltaRadial,
852co->Sat[pos].Orbit.DotDeltaAlongTrack,
853co->Sat[pos].Orbit.DotDeltaCrossTrack);
854#endif
855 }
856 break;
857 case COTYPE_GLONASSCLOCK:
858 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
859 co->messageType = COTYPE_GLONASSCLOCK;
860 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
861 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
862 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
863 G_SSR_IOD(co->SSRIOD)
864 G_SSR_PROVIDER_ID(co->SSRProviderID)
865 G_SSR_SOLUTION_ID(co->SSRSolutionID)
866 G_NO_OF_SATELLITES(nums)
867 co->ClockDataSupplied |= 2;
868#ifdef DEBUG
869fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->GLONASSEpochTime,
870co->UpdateInterval,mmi,co->NumberOfGLONASSSat,nums);
871#endif
872 for(i = 0; i < nums; ++i)
873 {
874 G_GLONASS_SATELLITE_ID(id)
875 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
876 ;
877 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
878 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
879 co->Sat[pos].ID = id;
880
881 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
882 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
883 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
884#ifdef DEBUG
885fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
886co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
887co->Sat[pos].Clock.DeltaA2);
888#endif
889 }
890 break;
891 case COTYPE_GLONASSCOMBINED:
892 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
893 co->messageType = COTYPE_GLONASSCOMBINED;
894 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
895 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
896 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
897 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
898 G_SSR_IOD(co->SSRIOD)
899 G_SSR_PROVIDER_ID(co->SSRProviderID)
900 G_SSR_SOLUTION_ID(co->SSRSolutionID)
901 G_NO_OF_SATELLITES(nums)
902 co->OrbitDataSupplied |= 2;
903 co->ClockDataSupplied |= 2;
904 for(i = 0; i < nums; ++i)
905 {
906 G_GLONASS_SATELLITE_ID(id)
907 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
908 ;
909 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
910 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
911 co->Sat[pos].ID = id;
912
913 G_GLONASS_IOD(co->Sat[pos].IOD)
914 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
915 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
916 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
917 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
918 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
919 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
920 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
921 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
922 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
923 }
924 break;
925 case COTYPE_GLONASSURA:
926 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
927 co->messageType = COTYPE_GLONASSURA;
928 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
929 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
930 G_SSR_IOD(co->SSRIOD)
931 G_SSR_PROVIDER_ID(co->SSRProviderID)
932 G_SSR_SOLUTION_ID(co->SSRSolutionID)
933 G_NO_OF_SATELLITES(nums)
934 co->URADataSupplied |= 2;
935 for(i = 0; i < nums; ++i)
936 {
937 G_GLONASS_SATELLITE_ID(id)
938 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
939 ;
940 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
941 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
942 co->Sat[pos].ID = id;
943
944 G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
945 }
946 break;
947 case COTYPE_GLONASSHR:
948 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
949 co->messageType = COTYPE_GLONASSHR;
950 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
951 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
952 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
953 G_SSR_IOD(co->SSRIOD)
954 G_SSR_PROVIDER_ID(co->SSRProviderID)
955 G_SSR_SOLUTION_ID(co->SSRSolutionID)
956 G_NO_OF_SATELLITES(nums)
957 co->HRDataSupplied |= 2;
958 for(i = 0; i < nums; ++i)
959 {
960 G_GLONASS_SATELLITE_ID(id)
961 for(pos = CLOCKORBIT_NUMGPS; pos < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat && co->Sat[pos].ID != id; ++pos)
962 ;
963 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
964 else if(pos == CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat) ++co->NumberOfGLONASSSat;
965 co->Sat[pos].ID = id;
966
967 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
968 }
969 break;
970 case BTYPE_GPS:
971 if(!b) return GCOBR_NOBIASPARAMETER;
972 b->messageType = BTYPE_GPS;
973 G_GPS_EPOCH_TIME(b->GPSEpochTime, b->NumberOfGPSSat)
974 G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
975 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
976 G_SSR_IOD(b->SSRIOD)
977 G_SSR_PROVIDER_ID(b->SSRProviderID)
978 G_SSR_SOLUTION_ID(b->SSRSolutionID)
979 G_NO_OF_SATELLITES(nums)
980 for(i = 0; i < nums; ++i)
981 {
982 G_GPS_SATELLITE_ID(id)
983 for(pos = 0; pos < b->NumberOfGPSSat && b->Sat[pos].ID != id; ++pos)
984 ;
985 if(pos >= CLOCKORBIT_NUMGPS) return GCOBR_DATAMISMATCH;
986 else if(pos == b->NumberOfGPSSat) ++b->NumberOfGPSSat;
987 b->Sat[pos].ID = id;
988
989 G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases)
990 for(j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j)
991 {
992 G_GPS_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
993 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
994 }
995 }
996 break;
997 case BTYPE_GLONASS:
998 if(!b) return GCOBR_NOBIASPARAMETER;
999 b->messageType = BTYPE_GLONASS;
1000 G_GLONASS_EPOCH_TIME(b->GLONASSEpochTime, b->NumberOfGLONASSSat)
1001 G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
1002 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1003 G_SSR_IOD(b->SSRIOD)
1004 G_SSR_PROVIDER_ID(b->SSRProviderID)
1005 G_SSR_SOLUTION_ID(b->SSRSolutionID)
1006 G_NO_OF_SATELLITES(nums)
1007 for(i = 0; i < nums; ++i)
1008 {
1009 G_GLONASS_SATELLITE_ID(id)
1010 for(pos = CLOCKORBIT_NUMGPS; pos < b->NumberOfGLONASSSat && b->Sat[pos].ID != id; ++pos)
1011 ;
1012 if(pos >= CLOCKORBIT_NUMGPS+CLOCKORBIT_NUMGLONASS) return GCOBR_DATAMISMATCH;
1013 else if(pos == b->NumberOfGLONASSSat) ++b->NumberOfGLONASSSat;
1014 b->Sat[pos].ID = id;
1015
1016 G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases)
1017 for(j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j)
1018 {
1019 G_GLONASS_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
1020 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
1021 }
1022 }
1023 break;
1024 default:
1025 if(bytesused)
1026 *bytesused = sizeofrtcmblock+6;
1027 return GCOBR_UNKNOWNTYPE;
1028 }
1029#ifdef DEBUG
1030for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type)
1031 numbits += 8;
1032fprintf(stderr, "numbits left %d\n",numbits);
1033#endif
1034 if(bytesused)
1035 *bytesused = sizeofrtcmblock+6;
1036 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
1037}
1038#endif /* NODECODE */
Note: See TracBrowser for help on using the repository browser.