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

Last change on this file since 6205 was 6205, checked in by stoecker, 8 years ago

fix bitcount for phase biases

  • Property svn:executable set to *
  • Property svn:keywords set to Id
File size: 43.6 KB
Line 
1/* Programheader
2
3 Name: clock_orbit_rtcm.c
4 Project: RTCM3
5 Version: $Id: clock_orbit_rtcm.c 6205 2014-09-25 06:42: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(1, 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 }
920 else if(type >= corbase[s])
921 {
922 switch(type-corbase[s])
923 {
924 case COBOFS_ORBIT:
925 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
926 co->messageType = type;
927 switch(s)
928 {
929 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
930 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
931 case CLOCKORBIT_SATBDS:
932 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
933 break;
934 case CLOCKORBIT_SATGLONASS:
935 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
936 break;
937 }
938 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
939 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
940 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
941 G_SSR_IOD(co->SSRIOD)
942 G_SSR_PROVIDER_ID(co->SSRProviderID)
943 G_SSR_SOLUTION_ID(co->SSRSolutionID)
944 G_NO_OF_SATELLITES(nums)
945 co->Supplied[COBOFS_ORBIT] |= 1;
946#ifdef DEBUG
947 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->EpochTime[s],
948 co->UpdateInterval,mmi,co->NumberOfSat[s],nums, co->SatRefDatum);
949#endif
950 for(i = 0; i < nums; ++i)
951 {
952 switch(s)
953 {
954 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
955 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
956 G_GPS_SATELLITE_ID(id)
957 break;
958 case CLOCKORBIT_SATQZSS:
959 G_QZSS_SATELLITE_ID(id)
960 break;
961 case CLOCKORBIT_SATGLONASS:
962 G_GLONASS_SATELLITE_ID(id)
963 break;
964 }
965 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
966 ;
967 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
968 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
969 co->Sat[pos].ID = id;
970
971 switch(s)
972 {
973 case CLOCKORBIT_SATGPS:
974 case CLOCKORBIT_SATQZSS:
975 G_GPS_IODE(co->Sat[pos].IOD)
976 break;
977 case CLOCKORBIT_SATGLONASS:
978 G_GLONASS_IOD(co->Sat[pos].IOD)
979 break;
980 case CLOCKORBIT_SATGALILEO:
981 G_GALILEO_IOD(co->Sat[pos].IOD)
982 break;
983 case CLOCKORBIT_SATSBAS:
984 G_SBAS_T0MOD(co->Sat[pos].toe)
985 G_SBAS_IODCRC(co->Sat[pos].IOD)
986 break;
987 case CLOCKORBIT_SATBDS:
988 G_BDS_TOEMOD(co->Sat[pos].toe)
989 G_BDS_IODCRC(co->Sat[pos].IOD)
990 break;
991 }
992 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
993 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
994 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
995 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
996 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
997 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
998#ifdef DEBUG
999 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",
1000 co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
1001 co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
1002 co->Sat[pos].Orbit.DotDeltaRadial,
1003 co->Sat[pos].Orbit.DotDeltaAlongTrack,
1004 co->Sat[pos].Orbit.DotDeltaCrossTrack);
1005#endif
1006 }
1007 break;
1008 case COBOFS_CLOCK:
1009 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
1010 co->messageType = type;
1011 switch(s)
1012 {
1013 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1014 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1015 case CLOCKORBIT_SATBDS:
1016 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1017 break;
1018 case CLOCKORBIT_SATGLONASS:
1019 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1020 break;
1021 }
1022 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1023 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1024 G_SSR_IOD(co->SSRIOD)
1025 G_SSR_PROVIDER_ID(co->SSRProviderID)
1026 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1027 G_NO_OF_SATELLITES(nums)
1028 co->Supplied[COBOFS_CLOCK] |= 1;
1029#ifdef DEBUG
1030 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->EpochTime[s],
1031 co->UpdateInterval,mmi,co->NumberOfSat[s],nums);
1032#endif
1033 for(i = 0; i < nums; ++i)
1034 {
1035 switch(s)
1036 {
1037 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1038 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1039 G_GPS_SATELLITE_ID(id)
1040 break;
1041 case CLOCKORBIT_SATQZSS:
1042 G_QZSS_SATELLITE_ID(id)
1043 break;
1044 case CLOCKORBIT_SATGLONASS:
1045 G_GLONASS_SATELLITE_ID(id)
1046 break;
1047 }
1048 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
1049 ;
1050 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1051 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
1052 co->Sat[pos].ID = id;
1053
1054 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
1055 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
1056 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
1057#ifdef DEBUG
1058 fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
1059 co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
1060 co->Sat[pos].Clock.DeltaA2);
1061#endif
1062 }
1063 break;
1064 case COBOFS_COMBINED:
1065 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
1066 co->messageType = type;
1067 switch(s)
1068 {
1069 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1070 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1071 case CLOCKORBIT_SATBDS:
1072 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1073 break;
1074 case CLOCKORBIT_SATGLONASS:
1075 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1076 break;
1077 }
1078 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1079 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1080 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
1081 G_SSR_IOD(co->SSRIOD)
1082 G_SSR_PROVIDER_ID(co->SSRProviderID)
1083 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1084 G_NO_OF_SATELLITES(nums)
1085 co->Supplied[COBOFS_ORBIT] |= 1;
1086 co->Supplied[COBOFS_CLOCK] |= 1;
1087 for(i = 0; i < nums; ++i)
1088 {
1089 switch(s)
1090 {
1091 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1092 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1093 G_GPS_SATELLITE_ID(id)
1094 break;
1095 case CLOCKORBIT_SATQZSS:
1096 G_QZSS_SATELLITE_ID(id)
1097 break;
1098 case CLOCKORBIT_SATGLONASS:
1099 G_GLONASS_SATELLITE_ID(id)
1100 break;
1101 }
1102 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
1103 ;
1104 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1105 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
1106 co->Sat[pos].ID = id;
1107
1108 switch(s)
1109 {
1110 case CLOCKORBIT_SATGPS:
1111 case CLOCKORBIT_SATQZSS:
1112 G_GPS_IODE(co->Sat[pos].IOD)
1113 break;
1114 case CLOCKORBIT_SATGLONASS:
1115 G_GLONASS_IOD(co->Sat[pos].IOD)
1116 break;
1117 case CLOCKORBIT_SATGALILEO:
1118 G_GALILEO_IOD(co->Sat[pos].IOD)
1119 break;
1120 case CLOCKORBIT_SATSBAS:
1121 G_SBAS_T0MOD(co->Sat[pos].toe)
1122 G_SBAS_IODCRC(co->Sat[pos].IOD)
1123 break;
1124 case CLOCKORBIT_SATBDS:
1125 G_BDS_TOEMOD(co->Sat[pos].toe)
1126 G_BDS_IODCRC(co->Sat[pos].IOD)
1127 break;
1128 }
1129 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
1130 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
1131 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
1132 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
1133 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
1134 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
1135 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
1136 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
1137 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
1138 }
1139 break;
1140 case COBOFS_URA:
1141 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
1142 co->messageType = type;
1143 switch(s)
1144 {
1145 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1146 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1147 case CLOCKORBIT_SATBDS:
1148 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1149 break;
1150 case CLOCKORBIT_SATGLONASS:
1151 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1152 break;
1153 }
1154 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1155 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1156 G_SSR_IOD(co->SSRIOD)
1157 G_SSR_PROVIDER_ID(co->SSRProviderID)
1158 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1159 G_NO_OF_SATELLITES(nums)
1160 co->Supplied[COBOFS_URA] |= 1;
1161 for(i = 0; i < nums; ++i)
1162 {
1163 switch(s)
1164 {
1165 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1166 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1167 G_GPS_SATELLITE_ID(id)
1168 break;
1169 case CLOCKORBIT_SATQZSS:
1170 G_QZSS_SATELLITE_ID(id)
1171 break;
1172 case CLOCKORBIT_SATGLONASS:
1173 G_GLONASS_SATELLITE_ID(id)
1174 break;
1175 }
1176 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
1177 ;
1178 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1179 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
1180 co->Sat[pos].ID = id;
1181
1182 G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
1183 }
1184 break;
1185 case COBOFS_HR:
1186 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
1187 co->messageType = type;
1188 switch(s)
1189 {
1190 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1191 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1192 case CLOCKORBIT_SATBDS:
1193 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1194 break;
1195 case CLOCKORBIT_SATGLONASS:
1196 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1197 break;
1198 }
1199 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1200 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1201 G_SSR_IOD(co->SSRIOD)
1202 G_SSR_PROVIDER_ID(co->SSRProviderID)
1203 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1204 G_NO_OF_SATELLITES(nums)
1205 co->Supplied[COBOFS_HR] |= 1;
1206 for(i = 0; i < nums; ++i)
1207 {
1208 switch(s)
1209 {
1210 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1211 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1212 G_GPS_SATELLITE_ID(id)
1213 break;
1214 case CLOCKORBIT_SATQZSS:
1215 G_QZSS_SATELLITE_ID(id)
1216 break;
1217 case CLOCKORBIT_SATGLONASS:
1218 G_GLONASS_SATELLITE_ID(id)
1219 break;
1220 }
1221 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
1222 ;
1223 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1224 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
1225 co->Sat[pos].ID = id;
1226
1227 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
1228 }
1229 break;
1230 case COBOFS_BIAS:
1231 if(!b) return GCOBR_NOCODEBIASPARAMETER;
1232 b->messageType = type;
1233 switch(s)
1234 {
1235 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1236 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1237 case CLOCKORBIT_SATBDS:
1238 G_GPS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s])
1239 break;
1240 case CLOCKORBIT_SATGLONASS:
1241 G_GLONASS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s])
1242 break;
1243 }
1244 G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
1245 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1246 G_SSR_IOD(b->SSRIOD)
1247 G_SSR_PROVIDER_ID(b->SSRProviderID)
1248 G_SSR_SOLUTION_ID(b->SSRSolutionID)
1249 G_NO_OF_SATELLITES(nums)
1250 for(i = 0; i < nums; ++i)
1251 {
1252 switch(s)
1253 {
1254 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1255 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1256 G_GPS_SATELLITE_ID(id)
1257 break;
1258 case CLOCKORBIT_SATQZSS:
1259 G_QZSS_SATELLITE_ID(id)
1260 break;
1261 case CLOCKORBIT_SATGLONASS:
1262 G_GLONASS_SATELLITE_ID(id)
1263 break;
1264 }
1265 for(pos = satoffset[s]; pos < satoffset[s]+b->NumberOfSat[s] && b->Sat[pos].ID != id; ++pos)
1266 ;
1267 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1268 else if(pos == b->NumberOfSat[s] + satoffset[s]) ++b->NumberOfSat[s];
1269 b->Sat[pos].ID = id;
1270
1271 G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases)
1272 for(j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j)
1273 {
1274 G_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
1275 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
1276 }
1277 }
1278 break;
1279 default:
1280 return GCOBR_UNKNOWNTYPE;
1281 }
1282#ifdef COR_LATENCY
1283 if(s == CLOCKORBIT_SATGPS && type-corbase[s] != COBOFS_BIAS)
1284 {
1285 co->epochGPS[co->epochSize] = co->EpochTime[s];
1286 if(co->epochSize < COR_LATENCYCOUNT)
1287 ++co->epochSize;
1288 }
1289#endif
1290#ifdef DEBUG
1291 for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type)
1292 numbits += 8;
1293 fprintf(stderr, "numbits left %d\n",numbits);
1294#endif
1295 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
1296 }
1297 }
1298 return GCOBR_UNKNOWNTYPE;
1299}
1300#endif /* NODECODE */
Note: See TracBrowser for help on using the repository browser.