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

Last change on this file since 6305 was 6207, checked in by stoecker, 10 years ago

fix bias loops
  • Property svn:executable set to *
  • Property svn:keywords set to Id
File size: 43.8 KB
Line 
1/* Programheader
2
3 Name: clock_orbit_rtcm.c
4 Project: RTCM3
5 Version: $Id: clock_orbit_rtcm.c 6207 2014-09-25 08:59: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)(c > 0 ? b*c+0.5 : b*c-0.5))
93
94#define MPI 3.141592653589793
95
96/* GPS macros also used for other systems when matching! */
97#define T_MESSAGE_NUMBER(a) ADDBITS(12, a) /* DF002 */
98#define T_GPS_SATELLITE_ID(a) ADDBITS(6, a) /* DF068 */
99#define T_QZSS_SATELLITE_ID(a) ADDBITS(4, a) /* DF249 */
100#define T_GLONASS_SATELLITE_ID(a) ADDBITS(5, a)
101
102#define T_GPS_IODE(a) ADDBITS(8, a) /* DF071 */
103#define T_GLONASS_IOD(a) ADDBITS(8, a) /* DF237 */
104#define T_GALILEO_IOD(a) ADDBITS(10, a) /* DF459 */
105#define T_SBAS_T0MOD(a) ADDBITS(9, (a/16)) /* DF468 */
106#define T_SBAS_IODCRC(a) ADDBITS(24, a) /* DF469 */
107#define T_BDS_TOEMOD(a) ADDBITS(10, (a/8)) /* DF470 */
108#define T_BDS_IODCRC(a) ADDBITS(24, a) /* DF471 */
109
110#define T_DELTA_RADIAL(a) SCALEADDBITS(22, 10000.0, a)
111#define T_DELTA_ALONG_TRACK(a) SCALEADDBITS(20, 2500.0, a)
112#define T_DELTA_CROSS_TRACK(a) SCALEADDBITS(20, 2500.0, a)
113#define T_DELTA_DOT_RADIAL(a) SCALEADDBITS(21, 1000000.0, a)
114#define T_DELTA_DOT_ALONG_TRACK(a) SCALEADDBITS(19, 250000.0, a)
115#define T_DELTA_DOT_CROSS_TRACK(a) SCALEADDBITS(19, 250000.0, a)
116
117#define T_SATELLITE_REFERENCE_DATUM(a) ADDBITS(1, a)
118#define T_DELTA_CLOCK_C0(a) SCALEADDBITS(22, 10000.0, a)
119#define T_DELTA_CLOCK_C1(a) SCALEADDBITS(21, 1000000.0, a)
120#define T_DELTA_CLOCK_C2(a) SCALEADDBITS(27, 50000000.0, a)
121#define T_NO_OF_CODE_BIASES(a) ADDBITS(5, a)
122#define T_NO_OF_PHASE_BIASES(a) ADDBITS(5, a)
123#define T_SIGNAL_IDENTIFIER(a) ADDBITS(5, a)
124#define T_CODE_BIAS(a) SCALEADDBITS(14, 100.0, a)
125#define T_YAW_ANGLE(a) SCALEADDBITS(9, 256.0/MPI, a)
126#define T_YAW_RATE(a) SCALEADDBITS(8, 8192.0/MPI, a)
127#define T_PHASE_BIAS(a) SCALEADDBITS(20, 10000.0, a)
128
129#define T_GPS_EPOCH_TIME(a) ADDBITS(20, a)
130#define T_GLONASS_EPOCH_TIME(a) ADDBITS(17, a)
131#define T_NO_OF_SATELLITES(a) ADDBITS(6, a)
132#define T_MULTIPLE_MESSAGE_INDICATOR(a) ADDBITS(1, a)
133#define T_DISPERSIVE_BIAS_INDICATOR(a) ADDBITS(1, a)
134#define T_MW_CONSISTENCY_INDICATOR(a) ADDBITS(1, a)
135#define T_INTEGER_INDICATOR(a) ADDBITS(1, a)
136#define T_WIDE_LANE_INDICATOR(a) ADDBITS(2, a)
137#define T_DISCONTINUITY_COUNTER(a) ADDBITS(4, a)
138#define T_SSR_URA(a) ADDBITS(6, a)
139#define T_HR_CLOCK_CORRECTION(a) SCALEADDBITS(22, 10000.0, a)
140#define T_SSR_UPDATE_INTERVAL(a) ADDBITS(4, a)
141
142#define T_SSR_IOD(a) ADDBITS(4, a)
143#define T_SSR_PROVIDER_ID(a) ADDBITS(16, a)
144#define T_SSR_SOLUTION_ID(a) ADDBITS(4, a)
145
146#define T_NO_IONO_LAYERS(a) ADDBITS(2, a-1)
147#define T_VTEC_QUALITY_INDICATOR(a) SCALEADDBITS(9, 20.0, a)
148#define T_IONO_COEFF(a) SCALEADDBITS(16, 200.0, a)
149#define T_IONO_DEGREE(a) ADDBITS(4, a-1)
150#define T_IONO_ORDER(a) ADDBITS(4, a-1)
151#define T_IONO_HEIGHT(a) SCALEADDBITS(8, 1/10000.0, a)
152
153static double URAToValue(int ura)
154{
155 int urac, urav;
156 urac = ura >> 3;
157 urav = ura & 7;
158 if(!ura)
159 return 0;
160 else if(ura == 63)
161 return SSR_MAXURA;
162 return (pow(3,urac)*(1.0+urav/4.0)-1.0)/1000.0;
163}
164
165static int ValueToURA(double val)
166{
167 int ura;
168 if(!val)
169 return 0;
170 else if(val > 5.4665)
171 return 63;
172 for(ura = 1; ura < 63 && val > URAToValue(ura); ++ura)
173 ;
174 return ura;
175}
176
177static const enum ClockOrbitType corbase[CLOCKORBIT_SATNUM] =
178{
179 COBBASE_GPS, COBBASE_GLONASS, COBBASE_GALILEO, COBBASE_QZSS, COBBASE_SBAS,
180 COBBASE_BDS
181};
182static const enum COR_OFFSETS satoffset[CLOCKORBIT_SATNUM+1] =
183{
184 CLOCKORBIT_OFFSETGPS, CLOCKORBIT_OFFSETGLONASS, CLOCKORBIT_OFFSETGALILEO,
185 CLOCKORBIT_OFFSETQZSS, CLOCKORBIT_OFFSETSBAS, CLOCKORBIT_OFFSETBDS,
186 CLOCKORBIT_COUNTSAT
187};
188
189size_t MakeClockOrbit(const struct ClockOrbit *co, enum ClockOrbitType type,
190int moremessagesfollow, char *buffer, size_t size)
191{
192 unsigned int status[CLOCKORBIT_SATNUM][COBOFS_NUM], i, s;
193
194 memset(status, 0, sizeof(status));
195 STARTDATA
196
197 for(s = 0; s < CLOCKORBIT_SATNUM; ++s)
198 {
199 for(i = 0; i < COBOFS_NUM; ++i)
200 {
201 if(co->NumberOfSat[s] && (type == COTYPE_AUTO
202 || type == corbase[s]+i) && (co->Supplied[i] || (i <= COBOFS_CLOCK
203 && co->Supplied[COBOFS_COMBINED]) || (i == COBOFS_COMBINED
204 && co->Supplied[COBOFS_ORBIT] && co->Supplied[COBOFS_CLOCK])))
205 {
206 status[s][i] = 1;
207 if(i == COBOFS_COMBINED)
208 {
209 status[s][COBOFS_ORBIT] = status[s][COBOFS_CLOCK] = 0;
210 } /* disable single blocks for combined type */
211 } /* check for data */
212 } /* iterate over RTCM data types */
213 } /* iterate over satellite systems */
214
215 for(s = 0; s < CLOCKORBIT_SATNUM; ++s)
216 {
217 if(status[s][COBOFS_ORBIT])
218 {
219 INITBLOCK
220 T_MESSAGE_NUMBER(corbase[s]+COBOFS_ORBIT)
221 switch(s)
222 {
223 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
224 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
225 case CLOCKORBIT_SATBDS:
226 T_GPS_EPOCH_TIME(co->EpochTime[s])
227 break;
228 case CLOCKORBIT_SATGLONASS:
229 T_GLONASS_EPOCH_TIME(co->EpochTime[s])
230 break;
231 }
232 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
233 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
234 T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
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->NumberOfSat[s])
239 for(i = satoffset[s]; i < satoffset[s]+co->NumberOfSat[s]; ++i)
240 {
241 switch(s)
242 {
243 case CLOCKORBIT_SATGPS:
244 T_GPS_SATELLITE_ID(co->Sat[i].ID)
245 T_GPS_IODE(co->Sat[i].IOD)
246 break;
247 case CLOCKORBIT_SATGLONASS:
248 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
249 T_GLONASS_IOD(co->Sat[i].IOD)
250 break;
251 case CLOCKORBIT_SATGALILEO:
252 T_GPS_SATELLITE_ID(co->Sat[i].ID)
253 T_GALILEO_IOD(co->Sat[i].IOD)
254 break;
255 case CLOCKORBIT_SATQZSS:
256 T_QZSS_SATELLITE_ID(co->Sat[i].ID)
257 T_GPS_IODE(co->Sat[i].IOD)
258 break;
259 case CLOCKORBIT_SATSBAS:
260 T_GPS_SATELLITE_ID(co->Sat[i].ID)
261 T_SBAS_T0MOD(co->Sat[i].toe)
262 T_SBAS_IODCRC(co->Sat[i].IOD)
263 break;
264 case CLOCKORBIT_SATBDS:
265 T_GPS_SATELLITE_ID(co->Sat[i].ID)
266 T_BDS_TOEMOD(co->Sat[i].toe)
267 T_BDS_IODCRC(co->Sat[i].IOD)
268 break;
269 }
270 T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
271 T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
272 T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
273 T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
274 T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
275 T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
276 }
277 ENDBLOCK
278 }
279 if(status[s][COBOFS_CLOCK])
280 {
281 INITBLOCK
282 T_MESSAGE_NUMBER(corbase[s]+COBOFS_CLOCK)
283 switch(s)
284 {
285 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
286 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
287 case CLOCKORBIT_SATBDS:
288 T_GPS_EPOCH_TIME(co->EpochTime[s])
289 break;
290 case CLOCKORBIT_SATGLONASS:
291 T_GLONASS_EPOCH_TIME(co->EpochTime[s])
292 break;
293 }
294 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
295 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
296 T_SSR_IOD(co->SSRIOD)
297 T_SSR_PROVIDER_ID(co->SSRProviderID)
298 T_SSR_SOLUTION_ID(co->SSRSolutionID)
299 T_NO_OF_SATELLITES(co->NumberOfSat[s])
300 for(i = satoffset[s]; i < satoffset[s]+co->NumberOfSat[s]; ++i)
301 {
302 switch(s)
303 {
304 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
305 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
306 T_GPS_SATELLITE_ID(co->Sat[i].ID)
307 break;
308 case CLOCKORBIT_SATQZSS:
309 T_QZSS_SATELLITE_ID(co->Sat[i].ID)
310 break;
311 case CLOCKORBIT_SATGLONASS:
312 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
313 break;
314 }
315 T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
316 T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
317 T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)
318 }
319 ENDBLOCK
320 }
321 if(status[s][COBOFS_COMBINED])
322 {
323#ifdef SPLITBLOCK
324 int nums = co->NumberOfSat[s];
325 int left, start = satoffset[s];
326 if(nums > 28) /* split block when more than 28 sats */
327 {
328 left = nums - 28;
329 nums = 28;
330 }
331 else
332 {
333 left = 0;
334 }
335 while(nums)
336 {
337#endif
338 INITBLOCK
339 T_MESSAGE_NUMBER(corbase[s]+COBOFS_COMBINED)
340 switch(s)
341 {
342 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
343 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
344 case CLOCKORBIT_SATBDS:
345 T_GPS_EPOCH_TIME(co->EpochTime[s])
346 break;
347 case CLOCKORBIT_SATGLONASS:
348 T_GLONASS_EPOCH_TIME(co->EpochTime[s])
349 break;
350 }
351 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
352#ifdef SPLITBLOCK
353 T_MULTIPLE_MESSAGE_INDICATOR((moremessagesfollow || left) ? 1 : 0)
354#else
355 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
356#endif
357 T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
358 T_SSR_IOD(co->SSRIOD)
359 T_SSR_PROVIDER_ID(co->SSRProviderID)
360 T_SSR_SOLUTION_ID(co->SSRSolutionID)
361#ifdef SPLITBLOCK
362 T_NO_OF_SATELLITES(nums)
363 for(i = start; i < start+nums; ++i)
364#else
365 T_NO_OF_SATELLITES(co->NumberOfSat[s])
366 for(i = satoffset[s]; i < satoffset[s]+co->NumberOfSat[s]; ++i)
367#endif
368 {
369 switch(s)
370 {
371 case CLOCKORBIT_SATGPS:
372 T_GPS_SATELLITE_ID(co->Sat[i].ID)
373 T_GPS_IODE(co->Sat[i].IOD)
374 break;
375 case CLOCKORBIT_SATGLONASS:
376 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
377 T_GLONASS_IOD(co->Sat[i].IOD)
378 break;
379 case CLOCKORBIT_SATGALILEO:
380 T_GPS_SATELLITE_ID(co->Sat[i].ID)
381 T_GALILEO_IOD(co->Sat[i].IOD)
382 break;
383 case CLOCKORBIT_SATQZSS:
384 T_QZSS_SATELLITE_ID(co->Sat[i].ID)
385 T_GPS_IODE(co->Sat[i].IOD)
386 break;
387 case CLOCKORBIT_SATSBAS:
388 T_GPS_SATELLITE_ID(co->Sat[i].ID)
389 T_SBAS_T0MOD(co->Sat[i].toe)
390 T_SBAS_IODCRC(co->Sat[i].IOD)
391 break;
392 case CLOCKORBIT_SATBDS:
393 T_GPS_SATELLITE_ID(co->Sat[i].ID)
394 T_BDS_TOEMOD(co->Sat[i].toe)
395 T_BDS_IODCRC(co->Sat[i].IOD)
396 break;
397 }
398 T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
399 T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
400 T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
401 T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
402 T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
403 T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
404 T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
405 T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
406 T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)
407 }
408 ENDBLOCK
409#ifdef SPLITBLOCK
410 start += nums;
411 nums = left;
412 left = 0;
413 }
414#endif
415 }
416 if(status[s][COBOFS_HR])
417 {
418 INITBLOCK
419 T_MESSAGE_NUMBER(corbase[s]+COBOFS_HR)
420 switch(s)
421 {
422 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
423 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
424 case CLOCKORBIT_SATBDS:
425 T_GPS_EPOCH_TIME(co->EpochTime[s])
426 break;
427 case CLOCKORBIT_SATGLONASS:
428 T_GLONASS_EPOCH_TIME(co->EpochTime[s])
429 break;
430 }
431 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
432 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
433 T_SSR_IOD(co->SSRIOD)
434 T_SSR_PROVIDER_ID(co->SSRProviderID)
435 T_SSR_SOLUTION_ID(co->SSRSolutionID)
436 T_NO_OF_SATELLITES(co->NumberOfSat[s])
437 for(i = satoffset[s]; i < satoffset[s]+co->NumberOfSat[s]; ++i)
438 {
439 switch(s)
440 {
441 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
442 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
443 T_GPS_SATELLITE_ID(co->Sat[i].ID)
444 break;
445 case CLOCKORBIT_SATQZSS:
446 T_QZSS_SATELLITE_ID(co->Sat[i].ID)
447 break;
448 case CLOCKORBIT_SATGLONASS:
449 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
450 break;
451 }
452 T_HR_CLOCK_CORRECTION(co->Sat[i].hrclock)
453 }
454 ENDBLOCK
455 }
456 if(status[s][COBOFS_URA])
457 {
458 INITBLOCK
459 T_MESSAGE_NUMBER(corbase[s]+COBOFS_URA)
460 switch(s)
461 {
462 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
463 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
464 case CLOCKORBIT_SATBDS:
465 T_GPS_EPOCH_TIME(co->EpochTime[s])
466 break;
467 case CLOCKORBIT_SATGLONASS:
468 T_GLONASS_EPOCH_TIME(co->EpochTime[s])
469 break;
470 }
471 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
472 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
473 T_SSR_IOD(co->SSRIOD)
474 T_SSR_PROVIDER_ID(co->SSRProviderID)
475 T_SSR_SOLUTION_ID(co->SSRSolutionID)
476 T_NO_OF_SATELLITES(co->NumberOfSat[s])
477 for(i = satoffset[s]; i < satoffset[s]+co->NumberOfSat[s]; ++i)
478 {
479 switch(s)
480 {
481 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
482 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
483 T_GPS_SATELLITE_ID(co->Sat[i].ID)
484 break;
485 case CLOCKORBIT_SATQZSS:
486 T_QZSS_SATELLITE_ID(co->Sat[i].ID)
487 break;
488 case CLOCKORBIT_SATGLONASS:
489 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
490 break;
491 }
492 T_SSR_URA(ValueToURA(co->Sat[i].UserRangeAccuracy))
493 }
494 ENDBLOCK
495 }
496 }
497 return ressize;
498}
499
500size_t MakeCodeBias(const struct CodeBias *b, enum CodeBiasType type,
501int moremessagesfollow, char *buffer, size_t size)
502{
503 unsigned int s, i, j;
504
505 STARTDATA
506
507 for(s = 0; s < CLOCKORBIT_SATNUM; ++s)
508 {
509 if(b->NumberOfSat[s] && (type == BTYPE_AUTO || type == corbase[s]+COBOFS_BIAS))
510 {
511 INITBLOCK
512 T_MESSAGE_NUMBER(corbase[s]+COBOFS_BIAS)
513 switch(s)
514 {
515 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
516 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
517 case CLOCKORBIT_SATBDS:
518 T_GPS_EPOCH_TIME(b->EpochTime[s])
519 break;
520 case CLOCKORBIT_SATGLONASS:
521 T_GLONASS_EPOCH_TIME(b->EpochTime[s])
522 break;
523 }
524 T_SSR_UPDATE_INTERVAL(b->UpdateInterval)
525 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
526 T_SSR_IOD(b->SSRIOD)
527 T_SSR_PROVIDER_ID(b->SSRProviderID)
528 T_SSR_SOLUTION_ID(b->SSRSolutionID)
529 T_NO_OF_SATELLITES(b->NumberOfSat[s])
530 for(i = satoffset[s]; i < satoffset[s]+b->NumberOfSat[s]; ++i)
531 {
532 switch(s)
533 {
534 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
535 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
536 T_GPS_SATELLITE_ID(b->Sat[i].ID)
537 break;
538 case CLOCKORBIT_SATQZSS:
539 T_QZSS_SATELLITE_ID(b->Sat[i].ID)
540 break;
541 case CLOCKORBIT_SATGLONASS:
542 T_GLONASS_SATELLITE_ID(b->Sat[i].ID)
543 break;
544 }
545 T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
546 for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
547 {
548 T_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type)
549 T_CODE_BIAS(b->Sat[i].Biases[j].Bias)
550 }
551 }
552 ENDBLOCK
553 }
554 }
555 return ressize;
556}
557
558size_t MakePhaseBias(const struct PhaseBias *b, enum PhaseBiasType type,
559int moremessagesfollow, char *buffer, size_t size)
560{
561 unsigned int s, i, j;
562
563 STARTDATA
564
565 for(s = 0; s < CLOCKORBIT_SATNUM; ++s)
566 {
567 if(b->NumberOfSat[s] && (type == PBTYPE_AUTO || type == corbase[s]+COBOFS_BIAS))
568 {
569 INITBLOCK
570 T_MESSAGE_NUMBER(corbase[s]+COBOFS_BIAS)
571 switch(s)
572 {
573 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
574 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
575 case CLOCKORBIT_SATBDS:
576 T_GPS_EPOCH_TIME(b->EpochTime[s])
577 break;
578 case CLOCKORBIT_SATGLONASS:
579 T_GLONASS_EPOCH_TIME(b->EpochTime[s])
580 break;
581 }
582 T_SSR_UPDATE_INTERVAL(b->UpdateInterval)
583 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
584 T_SSR_IOD(b->SSRIOD)
585 T_SSR_PROVIDER_ID(b->SSRProviderID)
586 T_SSR_SOLUTION_ID(b->SSRSolutionID)
587 T_DISPERSIVE_BIAS_INDICATOR(b->DispersiveBiasConsistencyIndicator ? 1 : 0)
588 T_MW_CONSISTENCY_INDICATOR(b->MWConsistencyIndicator ? 1 : 0)
589 T_NO_OF_SATELLITES(b->NumberOfSat[CLOCKORBIT_SATGPS])
590 for(i = satoffset[s]; i < satoffset[s]+b->NumberOfSat[s]; ++i)
591 {
592 switch(s)
593 {
594 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
595 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
596 T_GPS_SATELLITE_ID(b->Sat[i].ID)
597 break;
598 case CLOCKORBIT_SATQZSS:
599 T_QZSS_SATELLITE_ID(b->Sat[i].ID)
600 break;
601 case CLOCKORBIT_SATGLONASS:
602 T_GLONASS_SATELLITE_ID(b->Sat[i].ID)
603 break;
604 }
605 T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfPhaseBiases)
606 T_YAW_ANGLE(b->Sat[i].YawAngle)
607 T_YAW_RATE(b->Sat[i].YawRate)
608 for(j = 0; j < b->Sat[i].NumberOfPhaseBiases; ++j)
609 {
610 T_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type)
611 T_INTEGER_INDICATOR(b->Sat[i].Biases[j].SignalIntegerIndicator ? 1 : 0)
612 T_WIDE_LANE_INDICATOR(b->Sat[i].Biases[j].SignalsWideLaneIntegerIndicator ? 1 : 0)
613 T_DISCONTINUITY_COUNTER(b->Sat[i].Biases[j].SignalDiscontinuityCounter)
614 T_PHASE_BIAS(b->Sat[i].Biases[j].Bias)
615 }
616 }
617 ENDBLOCK
618 }
619 }
620 return ressize;
621}
622
623size_t MakeVTEC(const struct VTEC *v, int moremessagesfollow, char *buffer,
624size_t size)
625{
626 unsigned int l, o, d;
627
628 STARTDATA
629 INITBLOCK
630 T_MESSAGE_NUMBER(VTEC_BASE)
631
632 T_GPS_EPOCH_TIME(v->EpochTime)
633 T_SSR_UPDATE_INTERVAL(v->UpdateInterval)
634 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
635 T_SSR_IOD(v->SSRIOD)
636 T_SSR_PROVIDER_ID(v->SSRProviderID)
637 T_SSR_SOLUTION_ID(v->SSRSolutionID)
638 T_VTEC_QUALITY_INDICATOR(v->Quality)
639 T_NO_IONO_LAYERS(v->NumLayers)
640 for(l = 0; l < v->NumLayers; ++l)
641 {
642 T_IONO_HEIGHT(v->Layers[l].Height)
643 T_IONO_DEGREE(v->Layers[l].Degree)
644 T_IONO_ORDER(v->Layers[l].Order)
645 for(o = 0; o <= v->Layers[l].Order; ++o)
646 {
647 for(d = o; d <= v->Layers[l].Degree; ++d)
648 {
649 T_IONO_COEFF(v->Layers[l].Cosinus[d][o])
650 }
651 }
652 for(o = 1; o <= v->Layers[l].Order; ++o)
653 {
654 for(d = o; d <= v->Layers[l].Degree; ++d)
655 {
656 T_IONO_COEFF(v->Layers[l].Sinus[d][o])
657 }
658 }
659 }
660 ENDBLOCK
661 return ressize;
662}
663#endif /* NOENCODE */
664
665#ifndef NODECODE
666
667#define DECODESTART \
668 int numbits=0; \
669 uint64_t bitbuffer=0;
670
671#define LOADBITS(a) \
672{ \
673 while((a) > numbits) \
674 { \
675 if(!size--) return GCOBR_SHORTMESSAGE; \
676 bitbuffer = (bitbuffer<<8)|((unsigned char)*(buffer++)); \
677 numbits += 8; \
678 } \
679}
680
681/* extract bits from data stream
682 b = variable to store result, a = number of bits */
683#define GETBITS(b, a) \
684{ \
685 LOADBITS(a) \
686 b = (bitbuffer<<(64-numbits))>>(64-(a)); \
687 numbits -= (a); \
688}
689
690/* extract bits from data stream
691 b = variable to store result, a = number of bits */
692#define GETBITSFACTOR(b, a, c) \
693{ \
694 LOADBITS(a) \
695 b = ((bitbuffer<<(64-numbits))>>(64-(a)))*(c); \
696 numbits -= (a); \
697}
698
699/* extract signed floating value from data stream
700 b = variable to store result, a = number of bits */
701#define GETFLOATSIGN(b, a, c) \
702{ \
703 LOADBITS(a) \
704 b = ((double)(((int64_t)(bitbuffer<<(64-numbits)))>>(64-(a))))*(c); \
705 numbits -= (a); \
706}
707
708/* extract floating value from data stream
709 b = variable to store result, a = number of bits, c = scale factor */
710#define GETFLOAT(b, a, c) \
711{ \
712 LOADBITS(a) \
713 b = ((double)((bitbuffer<<(sizeof(bitbuffer)*8-numbits))>>(sizeof(bitbuffer)*8-(a))))*(c); \
714 numbits -= (a); \
715}
716
717#define SKIPBITS(b) { LOADBITS(b) numbits -= (b); }
718
719/* GPS macros also used for other systems when matching! */
720#define G_HEADER(a) GETBITS(a,8)
721#define G_RESERVEDH(a) GETBITS(a,6)
722#define G_SIZE(a) GETBITS(a, 10)
723#define G_MESSAGE_NUMBER(a) GETBITS(a, 12) /* DF002 */
724#define G_GPS_SATELLITE_ID(a) GETBITS(a, 6) /* DF068 */
725#define G_QZSS_SATELLITE_ID(a) GETBITS(a, 4) /* DF249 */
726#define G_GLONASS_SATELLITE_ID(a) GETBITS(a, 5)
727
728#define G_GPS_IODE(a) GETBITS(a, 8) /* DF071 */
729#define G_GLONASS_IOD(a) GETBITS(a, 8) /* DF237 */
730#define G_GALILEO_IOD(a) GETBITS(a, 10) /* DF459 */
731#define G_SBAS_T0MOD(a) GETBITSFACTOR(a, 9, 16) /* DF468 */
732#define G_SBAS_IODCRC(a) GETBITS(a, 24) /* DF469 */
733#define G_BDS_TOEMOD(a) GETBITSFACTOR(a, 10, 8) /* DF470 */
734#define G_BDS_IODCRC(a) GETBITS(a, 24) /* DF471 */
735
736/* defined values */
737#define G_DELTA_RADIAL(a) GETFLOATSIGN(a, 22, 1/10000.0)
738#define G_DELTA_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0)
739#define G_DELTA_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0)
740#define G_DELTA_DOT_RADIAL(a) GETFLOATSIGN(a, 21, 1/1000000.0)
741#define G_DELTA_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0)
742#define G_DELTA_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0)
743
744#define G_SATELLITE_REFERENCE_DATUM(a) GETBITS(a, 1)
745#define G_DELTA_CLOCK_C0(a) GETFLOATSIGN(a, 22, 1/10000.0)
746#define G_DELTA_CLOCK_C1(a) GETFLOATSIGN(a, 21, 1/1000000.0)
747#define G_DELTA_CLOCK_C2(a) GETFLOATSIGN(a, 27, 1/50000000.0)
748#define G_NO_OF_CODE_BIASES(a) GETBITS(a, 5)
749#define G_NO_OF_PHASE_BIASES(a) GETBITS(a, 5)
750#define G_SIGNAL_IDENTIFIER(a) GETBITS(a, 5)
751#define G_CODE_BIAS(a) GETFLOATSIGN(a, 14, 1/100.0)
752#define G_YAW_ANGLE(a) GETFLOAT(a, 9, MPI/256.0)
753#define G_YAW_RATE(a) GETFLOATSIGN(a, 8, MPI/8192.0)
754#define G_PHASE_BIAS(a) GETFLOATSIGN(a, 20, 1/10000.0)
755
756#define G_GPS_EPOCH_TIME(a, b) {unsigned int temp; GETBITS(temp, 20) \
757 if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
758#define G_GLONASS_EPOCH_TIME(a, b) {unsigned int temp; GETBITS(temp, 17) \
759 if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
760#define G_EPOCH_TIME(a) GETBITS(a, 20)
761#define G_NO_OF_SATELLITES(a) GETBITS(a, 6)
762#define G_MULTIPLE_MESSAGE_INDICATOR(a) GETBITS(a, 1)
763#define G_DISPERSIVE_BIAS_INDICATOR(a) GETBITS(a, 1)
764#define G_MW_CONSISTENCY_INDICATOR(a) GETBITS(a, 1)
765#define G_INTEGER_INDICATOR(a) GETBITS(a, 1)
766#define G_WIDE_LANE_INDICATOR(a) GETBITS(a, 2)
767#define G_DISCONTINUITY_COUNTER(a) GETBITS(a, 4)
768#define G_SSR_URA(a) {int temp; GETBITS(temp, 6) \
769 (a) = URAToValue(temp);}
770#define G_HR_CLOCK_CORRECTION(a) GETFLOATSIGN(a, 22, 1/10000.0)
771#define G_SSR_UPDATE_INTERVAL(a) GETBITS(a, 4)
772
773#define G_SSR_IOD(a) GETBITS(a, 4)
774#define G_SSR_PROVIDER_ID(a) GETBITS(a, 16)
775#define G_SSR_SOLUTION_ID(a) GETBITS(a, 4)
776
777#define G_NO_IONO_LAYERS(a) {unsigned int temp; GETBITS(temp, 2) a = temp+1; }
778#define G_VTEC_QUALITY_INDICATOR(a) GETFLOAT(a, 9, 1/20.0)
779#define G_IONO_COEFF(a) GETFLOATSIGN(a, 16,1/200.0)
780#define G_IONO_DEGREE(a) {unsigned int temp; GETBITS(temp, 4) a = temp+1; }
781#define G_IONO_ORDER(a) {unsigned int temp; GETBITS(temp, 4) a = temp+1; }
782#define G_IONO_HEIGHT(a) GETFLOAT(a, 8 , 10000.0)
783
784enum GCOB_RETURN GetSSR(struct ClockOrbit *co, struct CodeBias *b, struct VTEC *v,
785struct PhaseBias *pb, const char *buffer, size_t size, int *bytesused)
786{
787 int mmi=0, h, rs;
788 unsigned int type, pos, i, j, s, nums, id;
789 size_t sizeofrtcmblock;
790 const char *blockstart = buffer;
791 DECODESTART
792
793 if(size < 7)
794 return GCOBR_SHORTBUFFER;
795
796#ifdef DEBUG
797 fprintf(stderr, "GetClockOrbitBias START: size %d, numbits %d\n",size, numbits);
798#endif
799
800 G_HEADER(h)
801 G_RESERVEDH(rs)
802 G_SIZE(sizeofrtcmblock);
803
804 if((unsigned char)h != 0xD3 || rs)
805 return GCOBR_UNKNOWNDATA;
806 if(size < sizeofrtcmblock + 3) /* 3 header bytes already removed */
807 return GCOBR_MESSAGEEXCEEDSBUFFER;
808 if(CRC24(sizeofrtcmblock+3, (const unsigned char *) blockstart) !=
809 (uint32_t)((((unsigned char)buffer[sizeofrtcmblock])<<16)|
810 (((unsigned char)buffer[sizeofrtcmblock+1])<<8)|
811 (((unsigned char)buffer[sizeofrtcmblock+2]))))
812 return GCOBR_CRCMISMATCH;
813 size = sizeofrtcmblock; /* reduce size, so overflows are detected */
814
815 G_MESSAGE_NUMBER(type)
816#ifdef DEBUG
817fprintf(stderr, "type %d size %d\n",type,sizeofrtcmblock);
818#endif
819 if(bytesused)
820 *bytesused = sizeofrtcmblock+6;
821 if(type == VTEC_BASE)
822 {
823 unsigned int l, o, d;
824 if(!v) return GCOBR_NOVTECPARAMETER;
825 memset(v, 0, sizeof(*v));
826 G_EPOCH_TIME(v->EpochTime)
827 G_SSR_UPDATE_INTERVAL(v->UpdateInterval)
828 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
829 G_SSR_IOD(v->SSRIOD)
830 G_SSR_PROVIDER_ID(v->SSRProviderID)
831 G_SSR_SOLUTION_ID(v->SSRSolutionID)
832 G_VTEC_QUALITY_INDICATOR(v->Quality)
833 G_NO_IONO_LAYERS(v->NumLayers)
834 for(l = 0; l < v->NumLayers; ++l)
835 {
836 G_IONO_HEIGHT(v->Layers[l].Height)
837 G_IONO_DEGREE(v->Layers[l].Degree)
838 G_IONO_ORDER(v->Layers[l].Order)
839 for(o = 0; o <= v->Layers[l].Order; ++o)
840 {
841 for(d = o; d <= v->Layers[l].Degree; ++d)
842 {
843 G_IONO_COEFF(v->Layers[l].Cosinus[d][o])
844 }
845 }
846 for(o = 1; o <= v->Layers[l].Order; ++o)
847 {
848 for(d = o; d <= v->Layers[l].Degree; ++d)
849 {
850 G_IONO_COEFF(v->Layers[l].Sinus[d][o])
851 }
852 }
853 }
854#ifdef DEBUG
855 for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type)
856 numbits += 8;
857 fprintf(stderr, "numbits left %d\n",numbits);
858#endif
859 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
860 }
861 for(s = CLOCKORBIT_SATNUM; s-- > 0;)
862 {
863 if(type == PBTYPE_BASE+s)
864 {
865 if(!pb) return GCOBR_NOPHASEBIASPARAMETER;
866 pb->messageType = type;
867 switch(s)
868 {
869 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
870 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
871 case CLOCKORBIT_SATBDS:
872 G_GPS_EPOCH_TIME(pb->EpochTime[s], pb->NumberOfSat[s])
873 break;
874 case CLOCKORBIT_SATGLONASS:
875 G_GLONASS_EPOCH_TIME(pb->EpochTime[s], pb->NumberOfSat[s])
876 break;
877 }
878 G_SSR_UPDATE_INTERVAL(pb->UpdateInterval)
879 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
880 G_SSR_IOD(pb->SSRIOD)
881 G_SSR_PROVIDER_ID(pb->SSRProviderID)
882 G_SSR_SOLUTION_ID(pb->SSRSolutionID)
883 G_DISPERSIVE_BIAS_INDICATOR(pb->DispersiveBiasConsistencyIndicator)
884 G_MW_CONSISTENCY_INDICATOR(pb->MWConsistencyIndicator)
885 G_NO_OF_SATELLITES(nums)
886 for(i = 0; i < nums; ++i)
887 {
888 switch(s)
889 {
890 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
891 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
892 G_GPS_SATELLITE_ID(id)
893 break;
894 case CLOCKORBIT_SATQZSS:
895 G_QZSS_SATELLITE_ID(id)
896 break;
897 case CLOCKORBIT_SATGLONASS:
898 G_GLONASS_SATELLITE_ID(id)
899 break;
900 }
901 for(pos = satoffset[s]; pos < satoffset[s]+pb->NumberOfSat[s] && pb->Sat[pos].ID != id; ++pos)
902 ;
903 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
904 else if(pos == pb->NumberOfSat[s] + satoffset[s]) ++pb->NumberOfSat[s];
905 pb->Sat[pos].ID = id;
906
907 G_NO_OF_PHASE_BIASES(pb->Sat[pos].NumberOfPhaseBiases)
908 G_YAW_ANGLE(pb->Sat[pos].YawAngle)
909 G_YAW_RATE(pb->Sat[pos].YawRate)
910 for(j = 0; j < pb->Sat[pos].NumberOfPhaseBiases; ++j)
911 {
912 G_SIGNAL_IDENTIFIER(pb->Sat[pos].Biases[j].Type)
913 G_INTEGER_INDICATOR(pb->Sat[pos].Biases[j].SignalIntegerIndicator)
914 G_WIDE_LANE_INDICATOR(pb->Sat[pos].Biases[j].SignalsWideLaneIntegerIndicator)
915 G_DISCONTINUITY_COUNTER(pb->Sat[pos].Biases[j].SignalDiscontinuityCounter)
916 G_PHASE_BIAS(pb->Sat[pos].Biases[j].Bias)
917 }
918 }
919#ifdef DEBUG
920 for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type)
921 numbits += 8;
922 fprintf(stderr, "numbits left %d\n",numbits);
923#endif
924 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
925 }
926 else if(type >= corbase[s])
927 {
928 switch(type-corbase[s])
929 {
930 case COBOFS_ORBIT:
931 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
932 co->messageType = type;
933 switch(s)
934 {
935 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
936 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
937 case CLOCKORBIT_SATBDS:
938 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
939 break;
940 case CLOCKORBIT_SATGLONASS:
941 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
942 break;
943 }
944 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
945 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
946 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
947 G_SSR_IOD(co->SSRIOD)
948 G_SSR_PROVIDER_ID(co->SSRProviderID)
949 G_SSR_SOLUTION_ID(co->SSRSolutionID)
950 G_NO_OF_SATELLITES(nums)
951 co->Supplied[COBOFS_ORBIT] |= 1;
952#ifdef DEBUG
953 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->EpochTime[s],
954 co->UpdateInterval,mmi,co->NumberOfSat[s],nums, co->SatRefDatum);
955#endif
956 for(i = 0; i < nums; ++i)
957 {
958 switch(s)
959 {
960 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
961 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
962 G_GPS_SATELLITE_ID(id)
963 break;
964 case CLOCKORBIT_SATQZSS:
965 G_QZSS_SATELLITE_ID(id)
966 break;
967 case CLOCKORBIT_SATGLONASS:
968 G_GLONASS_SATELLITE_ID(id)
969 break;
970 }
971 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
972 ;
973 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
974 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
975 co->Sat[pos].ID = id;
976
977 switch(s)
978 {
979 case CLOCKORBIT_SATGPS:
980 case CLOCKORBIT_SATQZSS:
981 G_GPS_IODE(co->Sat[pos].IOD)
982 break;
983 case CLOCKORBIT_SATGLONASS:
984 G_GLONASS_IOD(co->Sat[pos].IOD)
985 break;
986 case CLOCKORBIT_SATGALILEO:
987 G_GALILEO_IOD(co->Sat[pos].IOD)
988 break;
989 case CLOCKORBIT_SATSBAS:
990 G_SBAS_T0MOD(co->Sat[pos].toe)
991 G_SBAS_IODCRC(co->Sat[pos].IOD)
992 break;
993 case CLOCKORBIT_SATBDS:
994 G_BDS_TOEMOD(co->Sat[pos].toe)
995 G_BDS_IODCRC(co->Sat[pos].IOD)
996 break;
997 }
998 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
999 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
1000 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
1001 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
1002 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
1003 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
1004#ifdef DEBUG
1005 fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f\n",
1006 co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
1007 co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
1008 co->Sat[pos].Orbit.DotDeltaRadial,
1009 co->Sat[pos].Orbit.DotDeltaAlongTrack,
1010 co->Sat[pos].Orbit.DotDeltaCrossTrack);
1011#endif
1012 }
1013 break;
1014 case COBOFS_CLOCK:
1015 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
1016 co->messageType = type;
1017 switch(s)
1018 {
1019 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1020 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1021 case CLOCKORBIT_SATBDS:
1022 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1023 break;
1024 case CLOCKORBIT_SATGLONASS:
1025 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1026 break;
1027 }
1028 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1029 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1030 G_SSR_IOD(co->SSRIOD)
1031 G_SSR_PROVIDER_ID(co->SSRProviderID)
1032 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1033 G_NO_OF_SATELLITES(nums)
1034 co->Supplied[COBOFS_CLOCK] |= 1;
1035#ifdef DEBUG
1036 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->EpochTime[s],
1037 co->UpdateInterval,mmi,co->NumberOfSat[s],nums);
1038#endif
1039 for(i = 0; i < nums; ++i)
1040 {
1041 switch(s)
1042 {
1043 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1044 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1045 G_GPS_SATELLITE_ID(id)
1046 break;
1047 case CLOCKORBIT_SATQZSS:
1048 G_QZSS_SATELLITE_ID(id)
1049 break;
1050 case CLOCKORBIT_SATGLONASS:
1051 G_GLONASS_SATELLITE_ID(id)
1052 break;
1053 }
1054 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
1055 ;
1056 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1057 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
1058 co->Sat[pos].ID = id;
1059
1060 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
1061 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
1062 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
1063#ifdef DEBUG
1064 fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
1065 co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
1066 co->Sat[pos].Clock.DeltaA2);
1067#endif
1068 }
1069 break;
1070 case COBOFS_COMBINED:
1071 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
1072 co->messageType = type;
1073 switch(s)
1074 {
1075 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1076 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1077 case CLOCKORBIT_SATBDS:
1078 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1079 break;
1080 case CLOCKORBIT_SATGLONASS:
1081 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1082 break;
1083 }
1084 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1085 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1086 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
1087 G_SSR_IOD(co->SSRIOD)
1088 G_SSR_PROVIDER_ID(co->SSRProviderID)
1089 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1090 G_NO_OF_SATELLITES(nums)
1091 co->Supplied[COBOFS_ORBIT] |= 1;
1092 co->Supplied[COBOFS_CLOCK] |= 1;
1093 for(i = 0; i < nums; ++i)
1094 {
1095 switch(s)
1096 {
1097 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1098 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1099 G_GPS_SATELLITE_ID(id)
1100 break;
1101 case CLOCKORBIT_SATQZSS:
1102 G_QZSS_SATELLITE_ID(id)
1103 break;
1104 case CLOCKORBIT_SATGLONASS:
1105 G_GLONASS_SATELLITE_ID(id)
1106 break;
1107 }
1108 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
1109 ;
1110 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1111 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
1112 co->Sat[pos].ID = id;
1113
1114 switch(s)
1115 {
1116 case CLOCKORBIT_SATGPS:
1117 case CLOCKORBIT_SATQZSS:
1118 G_GPS_IODE(co->Sat[pos].IOD)
1119 break;
1120 case CLOCKORBIT_SATGLONASS:
1121 G_GLONASS_IOD(co->Sat[pos].IOD)
1122 break;
1123 case CLOCKORBIT_SATGALILEO:
1124 G_GALILEO_IOD(co->Sat[pos].IOD)
1125 break;
1126 case CLOCKORBIT_SATSBAS:
1127 G_SBAS_T0MOD(co->Sat[pos].toe)
1128 G_SBAS_IODCRC(co->Sat[pos].IOD)
1129 break;
1130 case CLOCKORBIT_SATBDS:
1131 G_BDS_TOEMOD(co->Sat[pos].toe)
1132 G_BDS_IODCRC(co->Sat[pos].IOD)
1133 break;
1134 }
1135 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
1136 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
1137 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
1138 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
1139 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
1140 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
1141 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
1142 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
1143 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
1144 }
1145 break;
1146 case COBOFS_URA:
1147 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
1148 co->messageType = type;
1149 switch(s)
1150 {
1151 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1152 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1153 case CLOCKORBIT_SATBDS:
1154 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1155 break;
1156 case CLOCKORBIT_SATGLONASS:
1157 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1158 break;
1159 }
1160 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1161 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1162 G_SSR_IOD(co->SSRIOD)
1163 G_SSR_PROVIDER_ID(co->SSRProviderID)
1164 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1165 G_NO_OF_SATELLITES(nums)
1166 co->Supplied[COBOFS_URA] |= 1;
1167 for(i = 0; i < nums; ++i)
1168 {
1169 switch(s)
1170 {
1171 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1172 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1173 G_GPS_SATELLITE_ID(id)
1174 break;
1175 case CLOCKORBIT_SATQZSS:
1176 G_QZSS_SATELLITE_ID(id)
1177 break;
1178 case CLOCKORBIT_SATGLONASS:
1179 G_GLONASS_SATELLITE_ID(id)
1180 break;
1181 }
1182 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
1183 ;
1184 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1185 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
1186 co->Sat[pos].ID = id;
1187
1188 G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
1189 }
1190 break;
1191 case COBOFS_HR:
1192 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
1193 co->messageType = type;
1194 switch(s)
1195 {
1196 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1197 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1198 case CLOCKORBIT_SATBDS:
1199 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1200 break;
1201 case CLOCKORBIT_SATGLONASS:
1202 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1203 break;
1204 }
1205 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1206 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1207 G_SSR_IOD(co->SSRIOD)
1208 G_SSR_PROVIDER_ID(co->SSRProviderID)
1209 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1210 G_NO_OF_SATELLITES(nums)
1211 co->Supplied[COBOFS_HR] |= 1;
1212 for(i = 0; i < nums; ++i)
1213 {
1214 switch(s)
1215 {
1216 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1217 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1218 G_GPS_SATELLITE_ID(id)
1219 break;
1220 case CLOCKORBIT_SATQZSS:
1221 G_QZSS_SATELLITE_ID(id)
1222 break;
1223 case CLOCKORBIT_SATGLONASS:
1224 G_GLONASS_SATELLITE_ID(id)
1225 break;
1226 }
1227 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
1228 ;
1229 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1230 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
1231 co->Sat[pos].ID = id;
1232
1233 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
1234 }
1235 break;
1236 case COBOFS_BIAS:
1237 if(!b) return GCOBR_NOCODEBIASPARAMETER;
1238 b->messageType = type;
1239 switch(s)
1240 {
1241 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1242 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1243 case CLOCKORBIT_SATBDS:
1244 G_GPS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s])
1245 break;
1246 case CLOCKORBIT_SATGLONASS:
1247 G_GLONASS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s])
1248 break;
1249 }
1250 G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
1251 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1252 G_SSR_IOD(b->SSRIOD)
1253 G_SSR_PROVIDER_ID(b->SSRProviderID)
1254 G_SSR_SOLUTION_ID(b->SSRSolutionID)
1255 G_NO_OF_SATELLITES(nums)
1256 for(i = 0; i < nums; ++i)
1257 {
1258 switch(s)
1259 {
1260 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1261 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1262 G_GPS_SATELLITE_ID(id)
1263 break;
1264 case CLOCKORBIT_SATQZSS:
1265 G_QZSS_SATELLITE_ID(id)
1266 break;
1267 case CLOCKORBIT_SATGLONASS:
1268 G_GLONASS_SATELLITE_ID(id)
1269 break;
1270 }
1271 for(pos = satoffset[s]; pos < satoffset[s]+b->NumberOfSat[s] && b->Sat[pos].ID != id; ++pos)
1272 ;
1273 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1274 else if(pos == b->NumberOfSat[s] + satoffset[s]) ++b->NumberOfSat[s];
1275 b->Sat[pos].ID = id;
1276
1277 G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases)
1278 for(j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j)
1279 {
1280 G_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
1281 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
1282 }
1283 }
1284 break;
1285 default:
1286 continue;
1287 }
1288#ifdef COR_LATENCY
1289 if(s == CLOCKORBIT_SATGPS && type-corbase[s] != COBOFS_BIAS)
1290 {
1291 co->epochGPS[co->epochSize] = co->EpochTime[s];
1292 if(co->epochSize < COR_LATENCYCOUNT)
1293 ++co->epochSize;
1294 }
1295#endif
1296#ifdef DEBUG
1297 for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type)
1298 numbits += 8;
1299 fprintf(stderr, "numbits left %d\n",numbits);
1300#endif
1301 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
1302 }
1303 }
1304 return GCOBR_UNKNOWNTYPE;
1305}
1306#endif /* NODECODE */
Note: See TracBrowser for help on using the repository browser.