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

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

SSR update
  • Property svn:executable set to *
  • Property svn:keywords set to Id
File size: 43.5 KB
RevLine 
[956]1/* Programheader
2
3 Name: clock_orbit_rtcm.c
4 Project: RTCM3
[3495]5 Version: $Id: clock_orbit_rtcm.c 5664 2014-05-26 15:09:28Z stoecker $
[956]6 Authors: Dirk Stöcker
7 Description: state space approach for RTCM3
8*/
9
[2421]10#include <math.h>
[956]11#include <stdio.h>
12#include <string.h>
[2323]13#ifndef sparc
[956]14#include <stdint.h>
[2323]15#else
16#include <sys/types.h>
17#endif
[956]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; \
[1826]86 if(len > 1023) \
87 return 0; \
[956]88 len = CRC24(len+3, (const unsigned char *) blockstart); \
89 ADDBITS(24, len) \
90 }
91
[5664]92#define SCALEADDBITS(a, b, c) ADDBITS(a, (int64_t)(c > 0 ? b*c+0.5 : b*c-0.5))
[956]93
[5664]94#define MPI 3.141592653589793
[956]95
[5664]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
[1808]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)
[1581]116
[956]117#define T_SATELLITE_REFERENCE_DATUM(a) ADDBITS(1, a)
[1808]118#define T_DELTA_CLOCK_C0(a) SCALEADDBITS(22, 10000.0, a)
119#define T_DELTA_CLOCK_C1(a) SCALEADDBITS(21, 1000000.0, a)
[1581]120#define T_DELTA_CLOCK_C2(a) SCALEADDBITS(27, 50000000.0, a)
[956]121#define T_NO_OF_CODE_BIASES(a) ADDBITS(5, a)
[5664]122#define T_NO_OF_PHASE_BIASES(a) ADDBITS(5, a)
123#define T_SIGNAL_IDENTIFIER(a) ADDBITS(5, a)
[1808]124#define T_CODE_BIAS(a) SCALEADDBITS(14, 100.0, a)
[5664]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)
[956]128
[1581]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)
[5664]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)
[2421]138#define T_SSR_URA(a) ADDBITS(6, a)
[1808]139#define T_HR_CLOCK_CORRECTION(a) SCALEADDBITS(22, 10000.0, a)
[1581]140#define T_SSR_UPDATE_INTERVAL(a) ADDBITS(4, a)
[956]141
[3511]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
[5664]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
[2421]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
[5664]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
[956]189size_t MakeClockOrbit(const struct ClockOrbit *co, enum ClockOrbitType type,
190int moremessagesfollow, char *buffer, size_t size)
191{
[5664]192 unsigned int status[CLOCKORBIT_SATNUM][COBOFS_NUM], i, s;
[1819]193
[5664]194 memset(status, 0, sizeof(status));
[956]195 STARTDATA
196
[5664]197 for(s = 0; s < CLOCKORBIT_SATNUM; ++s)
[956]198 {
[5664]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 */
[956]214
[5664]215 for(s = 0; s < CLOCKORBIT_SATNUM; ++s)
[956]216 {
[5664]217 if(status[s][COBOFS_ORBIT])
[956]218 {
[1840]219 INITBLOCK
[5664]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 }
[1840]232 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
[5664]233 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
[2421]234 T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
[3511]235 T_SSR_IOD(co->SSRIOD)
236 T_SSR_PROVIDER_ID(co->SSRProviderID)
237 T_SSR_SOLUTION_ID(co->SSRSolutionID)
[5664]238 T_NO_OF_SATELLITES(co->NumberOfSat[s])
239 for(i = satoffset[s]; i < satoffset[s]+co->NumberOfSat[s]; ++i)
[1840]240 {
[5664]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 }
[1840]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)
[5664]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 }
[1840]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
[5664]320 }
321 if(status[s][COBOFS_COMBINED])
322 {
[2432]323#ifdef SPLITBLOCK
[5664]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 {
[2432]337#endif
[5664]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
[1581]415 }
[5664]416 if(status[s][COBOFS_HR])
[1581]417 {
[5664]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
[1581]455 }
[5664]456 if(status[s][COBOFS_URA])
[956]457 {
[5664]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_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
472 T_SSR_IOD(co->SSRIOD)
473 T_SSR_PROVIDER_ID(co->SSRProviderID)
474 T_SSR_SOLUTION_ID(co->SSRSolutionID)
475 T_NO_OF_SATELLITES(co->NumberOfSat[s])
476 for(i = satoffset[s]; i < satoffset[s]+co->NumberOfSat[s]; ++i)
477 {
478 switch(s)
479 {
480 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
481 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
482 T_GPS_SATELLITE_ID(co->Sat[i].ID)
483 break;
484 case CLOCKORBIT_SATQZSS:
485 T_QZSS_SATELLITE_ID(co->Sat[i].ID)
486 break;
487 case CLOCKORBIT_SATGLONASS:
488 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
489 break;
490 }
491 T_SSR_URA(ValueToURA(co->Sat[i].UserRangeAccuracy))
492 }
493 ENDBLOCK
[956]494 }
495 }
[5664]496 return ressize;
497}
498
499size_t MakeCodeBias(const struct CodeBias *b, enum CodeBiasType type,
500int moremessagesfollow, char *buffer, size_t size)
501{
502 unsigned int s, i, j;
503
504 STARTDATA
505
506 for(s = 0; s < CLOCKORBIT_SATNUM; ++s)
[956]507 {
[5664]508 if(b->NumberOfSat[s] && (type == BTYPE_AUTO || type == corbase[s]+COBOFS_BIAS))
[956]509 {
[5664]510 INITBLOCK
511 T_MESSAGE_NUMBER(corbase[s]+COBOFS_BIAS)
512 switch(s)
513 {
514 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
515 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
516 case CLOCKORBIT_SATBDS:
517 T_GPS_EPOCH_TIME(b->EpochTime[s])
518 break;
519 case CLOCKORBIT_SATGLONASS:
520 T_GLONASS_EPOCH_TIME(b->EpochTime[s])
521 break;
522 }
523 T_SSR_UPDATE_INTERVAL(b->UpdateInterval)
524 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
525 T_SSR_IOD(b->SSRIOD)
526 T_SSR_PROVIDER_ID(b->SSRProviderID)
527 T_SSR_SOLUTION_ID(b->SSRSolutionID)
528 T_NO_OF_SATELLITES(b->NumberOfSat[s])
529 for(i = satoffset[s]; i < satoffset[s]+b->NumberOfSat[s]; ++i)
530 {
531 switch(s)
532 {
533 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
534 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
535 T_GPS_SATELLITE_ID(b->Sat[i].ID)
536 break;
537 case CLOCKORBIT_SATQZSS:
538 T_QZSS_SATELLITE_ID(b->Sat[i].ID)
539 break;
540 case CLOCKORBIT_SATGLONASS:
541 T_GLONASS_SATELLITE_ID(b->Sat[i].ID)
542 break;
543 }
544 T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
545 for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
546 {
547 T_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type)
548 T_CODE_BIAS(b->Sat[i].Biases[j].Bias)
549 }
550 }
551 ENDBLOCK
[956]552 }
553 }
[5664]554 return ressize;
555}
556
557size_t MakePhaseBias(const struct PhaseBias *b, enum PhaseBiasType type,
558int moremessagesfollow, char *buffer, size_t size)
559{
560 unsigned int s, i, j;
561
562 STARTDATA
563
564 for(s = 0; s < CLOCKORBIT_SATNUM; ++s)
[956]565 {
[5664]566 if(b->NumberOfSat[s] && (type == PBTYPE_AUTO || type == corbase[s]+COBOFS_BIAS))
[956]567 {
[5664]568 INITBLOCK
569 T_MESSAGE_NUMBER(corbase[s]+COBOFS_BIAS)
570 switch(s)
571 {
572 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
573 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
574 case CLOCKORBIT_SATBDS:
575 T_GPS_EPOCH_TIME(b->EpochTime[s])
576 break;
577 case CLOCKORBIT_SATGLONASS:
578 T_GLONASS_EPOCH_TIME(b->EpochTime[s])
579 break;
580 }
581 T_SSR_UPDATE_INTERVAL(b->UpdateInterval)
582 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
583 T_SSR_IOD(b->SSRIOD)
584 T_SSR_PROVIDER_ID(b->SSRProviderID)
585 T_SSR_SOLUTION_ID(b->SSRSolutionID)
586 T_DISPERSIVE_BIAS_INDICATOR(b->DispersiveBiasConsistencyIndicator ? 1 : 0)
587 T_MW_CONSISTENCY_INDICATOR(b->MWConsistencyIndicator ? 1 : 0)
588 T_NO_OF_SATELLITES(b->NumberOfSat[CLOCKORBIT_SATGPS])
589 for(i = satoffset[s]; i < satoffset[s]+b->NumberOfSat[s]; ++i)
590 {
591 switch(s)
592 {
593 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
594 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
595 T_GPS_SATELLITE_ID(b->Sat[i].ID)
596 break;
597 case CLOCKORBIT_SATQZSS:
598 T_QZSS_SATELLITE_ID(b->Sat[i].ID)
599 break;
600 case CLOCKORBIT_SATGLONASS:
601 T_GLONASS_SATELLITE_ID(b->Sat[i].ID)
602 break;
603 }
604 T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfPhaseBiases)
605 T_YAW_ANGLE(b->Sat[i].YawAngle)
606 T_YAW_RATE(b->Sat[i].YawRate)
607 for(j = 0; j < b->Sat[i].NumberOfPhaseBiases; ++j)
608 {
609 T_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type)
610 T_INTEGER_INDICATOR(b->Sat[i].Biases[j].SignalIntegerIndicator ? 1 : 0)
611 T_WIDE_LANE_INDICATOR(b->Sat[i].Biases[j].SignalsWideLaneIntegerIndicator ? 1 : 0)
612 T_DISCONTINUITY_COUNTER(b->Sat[i].Biases[j].SignalDiscontinuityCounter)
613 T_PHASE_BIAS(b->Sat[i].Biases[j].Bias)
614 }
615 }
616 ENDBLOCK
[956]617 }
618 }
619 return ressize;
620}
621
[5664]622size_t MakeVTEC(const struct VTEC *v, int moremessagesfollow, char *buffer,
623size_t size)
[956]624{
[5664]625 unsigned int l, o, d;
[1819]626
[956]627 STARTDATA
[5664]628 INITBLOCK
629 T_MESSAGE_NUMBER(VTEC_BASE)
[956]630
[5664]631 T_GPS_EPOCH_TIME(v->EpochTime)
632 T_SSR_UPDATE_INTERVAL(v->UpdateInterval)
633 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
634 T_SSR_IOD(v->SSRIOD)
635 T_SSR_PROVIDER_ID(v->SSRProviderID)
636 T_SSR_SOLUTION_ID(v->SSRSolutionID)
637 T_VTEC_QUALITY_INDICATOR(v->Quality)
638 T_NO_IONO_LAYERS(v->NumLayers)
639 for(l = 0; l < v->NumLayers; ++l)
[956]640 {
[5664]641 T_IONO_HEIGHT(v->Layers[l].Height)
642 T_IONO_DEGREE(v->Layers[l].Degree)
643 T_IONO_ORDER(v->Layers[l].Order)
644 for(o = 0; o <= v->Layers[l].Order; ++o)
[956]645 {
[5664]646 for(d = o; d <= v->Layers[l].Degree; ++d)
[956]647 {
[5664]648 T_IONO_COEFF(v->Layers[l].Cosinus[d][o])
[956]649 }
650 }
[5664]651 for(o = 1; o <= v->Layers[l].Order; ++o)
[956]652 {
[5664]653 for(d = o; d <= v->Layers[l].Degree; ++d)
[956]654 {
[5664]655 T_IONO_COEFF(v->Layers[l].Sinus[d][o])
[956]656 }
657 }
658 }
[5664]659 ENDBLOCK
[956]660 return ressize;
661}
662#endif /* NOENCODE */
663
664#ifndef NODECODE
665
666#define DECODESTART \
667 int numbits=0; \
668 uint64_t bitbuffer=0;
669
670#define LOADBITS(a) \
671{ \
672 while((a) > numbits) \
673 { \
[1808]674 if(!size--) return GCOBR_SHORTMESSAGE; \
[956]675 bitbuffer = (bitbuffer<<8)|((unsigned char)*(buffer++)); \
676 numbits += 8; \
677 } \
678}
679
680/* extract bits from data stream
681 b = variable to store result, a = number of bits */
682#define GETBITS(b, a) \
683{ \
684 LOADBITS(a) \
685 b = (bitbuffer<<(64-numbits))>>(64-(a)); \
686 numbits -= (a); \
687}
688
[5664]689/* extract bits from data stream
690 b = variable to store result, a = number of bits */
691#define GETBITSFACTOR(b, a, c) \
692{ \
693 LOADBITS(a) \
694 b = ((bitbuffer<<(64-numbits))>>(64-(a)))*(c); \
695 numbits -= (a); \
696}
697
[956]698/* extract signed floating value from data stream
699 b = variable to store result, a = number of bits */
700#define GETFLOATSIGN(b, a, c) \
701{ \
702 LOADBITS(a) \
703 b = ((double)(((int64_t)(bitbuffer<<(64-numbits)))>>(64-(a))))*(c); \
704 numbits -= (a); \
705}
706
[5664]707/* extract floating value from data stream
708 b = variable to store result, a = number of bits, c = scale factor */
709#define GETFLOAT(b, a, c) \
710{ \
711 LOADBITS(a) \
712 b = ((double)((bitbuffer<<(sizeof(bitbuffer)*8-numbits))>>(sizeof(bitbuffer)*8-(a))))*(c); \
713 numbits -= (a); \
714}
715
[956]716#define SKIPBITS(b) { LOADBITS(b) numbits -= (b); }
717
[5664]718/* GPS macros also used for other systems when matching! */
[956]719#define G_HEADER(a) GETBITS(a,8)
720#define G_RESERVEDH(a) GETBITS(a,6)
721#define G_SIZE(a) GETBITS(a, 10)
[5664]722#define G_MESSAGE_NUMBER(a) GETBITS(a, 12) /* DF002 */
723#define G_GPS_SATELLITE_ID(a) GETBITS(a, 6) /* DF068 */
724#define G_QZSS_SATELLITE_ID(a) GETBITS(a, 4) /* DF249 */
725#define G_GLONASS_SATELLITE_ID(a) GETBITS(a, 5)
[956]726
[5664]727#define G_GPS_IODE(a) GETBITS(a, 8) /* DF071 */
728#define G_GLONASS_IOD(a) GETBITS(a, 8) /* DF237 */
729#define G_GALILEO_IOD(a) GETBITS(a, 10) /* DF459 */
730#define G_SBAS_T0MOD(a) GETBITSFACTOR(a, 9, 16) /* DF468 */
731#define G_SBAS_IODCRC(a) GETBITS(a, 24) /* DF469 */
732#define G_BDS_TOEMOD(a) GETBITSFACTOR(a, 10, 8) /* DF470 */
733#define G_BDS_IODCRC(a) GETBITS(a, 24) /* DF471 */
734
[956]735/* defined values */
[1581]736#define G_DELTA_RADIAL(a) GETFLOATSIGN(a, 22, 1/10000.0)
[1808]737#define G_DELTA_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0)
738#define G_DELTA_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0)
[1581]739#define G_DELTA_DOT_RADIAL(a) GETFLOATSIGN(a, 21, 1/1000000.0)
[1808]740#define G_DELTA_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0)
741#define G_DELTA_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0)
[1581]742
743#define G_SATELLITE_REFERENCE_DATUM(a) GETBITS(a, 1)
744#define G_DELTA_CLOCK_C0(a) GETFLOATSIGN(a, 22, 1/10000.0)
745#define G_DELTA_CLOCK_C1(a) GETFLOATSIGN(a, 21, 1/1000000.0)
746#define G_DELTA_CLOCK_C2(a) GETFLOATSIGN(a, 27, 1/50000000.0)
747#define G_NO_OF_CODE_BIASES(a) GETBITS(a, 5)
[5664]748#define G_NO_OF_PHASE_BIASES(a) GETBITS(a, 5)
749#define G_SIGNAL_IDENTIFIER(a) GETBITS(a, 5)
[1581]750#define G_CODE_BIAS(a) GETFLOATSIGN(a, 14, 1/100.0)
[5664]751#define G_YAW_ANGLE(a) GETFLOAT(a, 9, MPI/256.0)
752#define G_YAW_RATE(a) GETFLOATSIGN(a, 8, MPI/8192.0)
753#define G_PHASE_BIAS(a) GETFLOATSIGN(a, 20, 1/10000.0)
[1581]754
[5664]755#define G_GPS_EPOCH_TIME(a, b) {unsigned int temp; GETBITS(temp, 20) \
[956]756 if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
[5664]757#define G_GLONASS_EPOCH_TIME(a, b) {unsigned int temp; GETBITS(temp, 17) \
[956]758 if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
[5664]759#define G_EPOCH_TIME(a) GETBITS(a, 20)
[1840]760#define G_NO_OF_SATELLITES(a) GETBITS(a, 6)
[1581]761#define G_MULTIPLE_MESSAGE_INDICATOR(a) GETBITS(a, 1)
[5664]762#define G_DISPERSIVE_BIAS_INDICATOR(a) GETBITS(a, 1)
763#define G_MW_CONSISTENCY_INDICATOR(a) GETBITS(a, 1)
764#define G_INTEGER_INDICATOR(a) GETBITS(a, 1)
765#define G_WIDE_LANE_INDICATOR(a) GETBITS(a, 1)
766#define G_DISCONTINUITY_COUNTER(a) GETBITS(a, 4)
[2421]767#define G_SSR_URA(a) {int temp; GETBITS(temp, 6) \
768 (a) = URAToValue(temp);}
[1581]769#define G_HR_CLOCK_CORRECTION(a) GETFLOATSIGN(a, 22, 1/10000.0)
770#define G_SSR_UPDATE_INTERVAL(a) GETBITS(a, 4)
771
[3511]772#define G_SSR_IOD(a) GETBITS(a, 4)
773#define G_SSR_PROVIDER_ID(a) GETBITS(a, 16)
774#define G_SSR_SOLUTION_ID(a) GETBITS(a, 4)
775
[5664]776#define G_NO_IONO_LAYERS(a) {unsigned int temp; GETBITS(temp, 2) a = temp+1; }
777#define G_VTEC_QUALITY_INDICATOR(a) GETFLOAT(a, 9, 1/20.0)
778#define G_IONO_COEFF(a) GETFLOATSIGN(a, 16,1/200.0)
779#define G_IONO_DEGREE(a) {unsigned int temp; GETBITS(temp, 4) a = temp+1; }
780#define G_IONO_ORDER(a) {unsigned int temp; GETBITS(temp, 4) a = temp+1; }
781#define G_IONO_HEIGHT(a) GETFLOAT(a, 8 , 10000.0)
782
783enum GCOB_RETURN GetSSR(struct ClockOrbit *co, struct CodeBias *b, struct VTEC *v,
784struct PhaseBias *pb, const char *buffer, size_t size, int *bytesused)
[956]785{
[5664]786 int mmi=0, h, rs;
787 unsigned int type, pos, i, j, s, nums, id;
[1819]788 size_t sizeofrtcmblock;
[956]789 const char *blockstart = buffer;
790 DECODESTART
791
792 if(size < 7)
793 return GCOBR_SHORTBUFFER;
794
[1842]795#ifdef DEBUG
[5664]796 fprintf(stderr, "GetClockOrbitBias START: size %d, numbits %d\n",size, numbits);
[1842]797#endif
798
[956]799 G_HEADER(h)
800 G_RESERVEDH(rs)
801 G_SIZE(sizeofrtcmblock);
802
803 if((unsigned char)h != 0xD3 || rs)
804 return GCOBR_UNKNOWNDATA;
805 if(size < sizeofrtcmblock + 3) /* 3 header bytes already removed */
806 return GCOBR_MESSAGEEXCEEDSBUFFER;
807 if(CRC24(sizeofrtcmblock+3, (const unsigned char *) blockstart) !=
[1819]808 (uint32_t)((((unsigned char)buffer[sizeofrtcmblock])<<16)|
[5664]809 (((unsigned char)buffer[sizeofrtcmblock+1])<<8)|
810 (((unsigned char)buffer[sizeofrtcmblock+2]))))
[956]811 return GCOBR_CRCMISMATCH;
[1808]812 size = sizeofrtcmblock; /* reduce size, so overflows are detected */
[956]813
814 G_MESSAGE_NUMBER(type)
[1808]815#ifdef DEBUG
816fprintf(stderr, "type %d size %d\n",type,sizeofrtcmblock);
817#endif
[5664]818 if(bytesused)
819 *bytesused = sizeofrtcmblock+6;
820 if(type == VTEC_BASE)
[956]821 {
[5664]822 unsigned int l, o, d;
823 if(!v) return GCOBR_NOVTECPARAMETER;
824 memset(v, 0, sizeof(*v));
825 G_EPOCH_TIME(v->EpochTime)
826 G_SSR_UPDATE_INTERVAL(v->UpdateInterval)
[956]827 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
[5664]828 G_SSR_IOD(v->SSRIOD)
829 G_SSR_PROVIDER_ID(v->SSRProviderID)
830 G_SSR_SOLUTION_ID(v->SSRSolutionID)
831 G_VTEC_QUALITY_INDICATOR(v->Quality)
832 G_NO_IONO_LAYERS(v->NumLayers)
833 for(l = 0; l < v->NumLayers; ++l)
[956]834 {
[5664]835 G_IONO_HEIGHT(v->Layers[l].Height)
836 G_IONO_DEGREE(v->Layers[l].Degree)
837 G_IONO_ORDER(v->Layers[l].Order)
838 for(o = 0; o <= v->Layers[l].Order; ++o)
839 {
840 for(d = o; d <= v->Layers[l].Degree; ++d)
841 {
842 G_IONO_COEFF(v->Layers[l].Cosinus[d][o])
843 }
844 }
845 for(o = 1; o <= v->Layers[l].Order; ++o)
846 {
847 for(d = o; d <= v->Layers[l].Degree; ++d)
848 {
849 G_IONO_COEFF(v->Layers[l].Sinus[d][o])
850 }
851 }
[956]852 }
[1842]853#ifdef DEBUG
[5664]854 for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type)
855 numbits += 8;
856 fprintf(stderr, "numbits left %d\n",numbits);
[1842]857#endif
[5664]858 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
859 }
860 for(s = CLOCKORBIT_SATNUM; s-- > 0;)
861 {
862 if(type == PBTYPE_BASE+s)
[956]863 {
[5664]864 if(!pb) return GCOBR_NOPHASEBIASPARAMETER;
865 pb->messageType = type;
866 switch(s)
867 {
868 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
869 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
870 case CLOCKORBIT_SATBDS:
871 G_GPS_EPOCH_TIME(pb->EpochTime[s], pb->NumberOfSat[s])
872 break;
873 case CLOCKORBIT_SATGLONASS:
874 G_GLONASS_EPOCH_TIME(pb->EpochTime[s], pb->NumberOfSat[s])
875 break;
876 }
877 G_SSR_UPDATE_INTERVAL(pb->UpdateInterval)
878 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
879 G_SSR_IOD(pb->SSRIOD)
880 G_SSR_PROVIDER_ID(pb->SSRProviderID)
881 G_SSR_SOLUTION_ID(pb->SSRSolutionID)
882 G_DISPERSIVE_BIAS_INDICATOR(pb->DispersiveBiasConsistencyIndicator)
883 G_MW_CONSISTENCY_INDICATOR(pb->MWConsistencyIndicator)
884 G_NO_OF_SATELLITES(nums)
885 for(i = 0; i < nums; ++i)
886 {
887 switch(s)
888 {
889 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
890 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
891 G_GPS_SATELLITE_ID(id)
892 break;
893 case CLOCKORBIT_SATQZSS:
894 G_QZSS_SATELLITE_ID(id)
895 break;
896 case CLOCKORBIT_SATGLONASS:
897 G_GLONASS_SATELLITE_ID(id)
898 break;
899 }
900 for(pos = satoffset[s]; pos < satoffset[s]+pb->NumberOfSat[s] && pb->Sat[pos].ID != id; ++pos)
901 ;
902 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
903 else if(pos == pb->NumberOfSat[s] + satoffset[s]) ++pb->NumberOfSat[s];
904 pb->Sat[pos].ID = id;
[1840]905
[5664]906 G_NO_OF_PHASE_BIASES(pb->Sat[pos].NumberOfPhaseBiases)
907 G_YAW_ANGLE(pb->Sat[pos].YawAngle)
908 G_YAW_RATE(pb->Sat[pos].YawRate)
909 for(j = 0; j < pb->Sat[pos].NumberOfPhaseBiases; ++j)
910 {
911 G_SIGNAL_IDENTIFIER(pb->Sat[pos].Biases[j].Type)
912 G_INTEGER_INDICATOR(pb->Sat[pos].Biases[j].SignalIntegerIndicator)
913 G_WIDE_LANE_INDICATOR(pb->Sat[pos].Biases[j].SignalsWideLaneIntegerIndicator)
914 G_DISCONTINUITY_COUNTER(pb->Sat[pos].Biases[j].SignalDiscontinuityCounter)
915 G_PHASE_BIAS(pb->Sat[pos].Biases[j].Bias)
916 }
917 }
[956]918 }
[5664]919 else if(type >= corbase[s])
[956]920 {
[5664]921 switch(type-corbase[s])
922 {
923 case COBOFS_ORBIT:
924 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
925 co->messageType = type;
926 switch(s)
927 {
928 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
929 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
930 case CLOCKORBIT_SATBDS:
931 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
932 break;
933 case CLOCKORBIT_SATGLONASS:
934 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
935 break;
936 }
937 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
938 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
939 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
940 G_SSR_IOD(co->SSRIOD)
941 G_SSR_PROVIDER_ID(co->SSRProviderID)
942 G_SSR_SOLUTION_ID(co->SSRSolutionID)
943 G_NO_OF_SATELLITES(nums)
944 co->Supplied[COBOFS_ORBIT] |= 1;
[1842]945#ifdef DEBUG
[5664]946 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->EpochTime[s],
947 co->UpdateInterval,mmi,co->NumberOfSat[s],nums, co->SatRefDatum);
[1842]948#endif
[5664]949 for(i = 0; i < nums; ++i)
950 {
951 switch(s)
952 {
953 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
954 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
955 G_GPS_SATELLITE_ID(id)
956 break;
957 case CLOCKORBIT_SATQZSS:
958 G_QZSS_SATELLITE_ID(id)
959 break;
960 case CLOCKORBIT_SATGLONASS:
961 G_GLONASS_SATELLITE_ID(id)
962 break;
963 }
964 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
965 ;
966 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
967 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
968 co->Sat[pos].ID = id;
[1840]969
[5664]970 switch(s)
971 {
972 case CLOCKORBIT_SATGPS:
973 case CLOCKORBIT_SATQZSS:
974 G_GPS_IODE(co->Sat[pos].IOD)
975 break;
976 case CLOCKORBIT_SATGLONASS:
977 G_GLONASS_IOD(co->Sat[pos].IOD)
978 break;
979 case CLOCKORBIT_SATGALILEO:
980 G_GALILEO_IOD(co->Sat[pos].IOD)
981 break;
982 case CLOCKORBIT_SATSBAS:
983 G_SBAS_T0MOD(co->Sat[pos].toe)
984 G_SBAS_IODCRC(co->Sat[pos].IOD)
985 break;
986 case CLOCKORBIT_SATBDS:
987 G_BDS_TOEMOD(co->Sat[pos].toe)
988 G_BDS_IODCRC(co->Sat[pos].IOD)
989 break;
990 }
991 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
992 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
993 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
994 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
995 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
996 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
[1842]997#ifdef DEBUG
[5664]998 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",
999 co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
1000 co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
1001 co->Sat[pos].Orbit.DotDeltaRadial,
1002 co->Sat[pos].Orbit.DotDeltaAlongTrack,
1003 co->Sat[pos].Orbit.DotDeltaCrossTrack);
[1842]1004#endif
[5664]1005 }
1006 break;
1007 case COBOFS_CLOCK:
1008 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
1009 co->messageType = type;
1010 switch(s)
1011 {
1012 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1013 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1014 case CLOCKORBIT_SATBDS:
1015 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1016 break;
1017 case CLOCKORBIT_SATGLONASS:
1018 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1019 break;
1020 }
1021 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1022 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1023 G_SSR_IOD(co->SSRIOD)
1024 G_SSR_PROVIDER_ID(co->SSRProviderID)
1025 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1026 G_NO_OF_SATELLITES(nums)
1027 co->Supplied[COBOFS_CLOCK] |= 1;
[1842]1028#ifdef DEBUG
[5664]1029 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->EpochTime[s],
1030 co->UpdateInterval,mmi,co->NumberOfSat[s],nums);
[1842]1031#endif
[5664]1032 for(i = 0; i < nums; ++i)
1033 {
1034 switch(s)
1035 {
1036 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1037 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1038 G_GPS_SATELLITE_ID(id)
1039 break;
1040 case CLOCKORBIT_SATQZSS:
1041 G_QZSS_SATELLITE_ID(id)
1042 break;
1043 case CLOCKORBIT_SATGLONASS:
1044 G_GLONASS_SATELLITE_ID(id)
1045 break;
1046 }
1047 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
1048 ;
1049 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1050 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
1051 co->Sat[pos].ID = id;
[1840]1052
[5664]1053 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
1054 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
1055 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
[1842]1056#ifdef DEBUG
[5664]1057 fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
1058 co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
1059 co->Sat[pos].Clock.DeltaA2);
[1842]1060#endif
[5664]1061 }
1062 break;
1063 case COBOFS_COMBINED:
1064 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
1065 co->messageType = type;
1066 switch(s)
1067 {
1068 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1069 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1070 case CLOCKORBIT_SATBDS:
1071 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1072 break;
1073 case CLOCKORBIT_SATGLONASS:
1074 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1075 break;
1076 }
1077 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1078 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1079 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
1080 G_SSR_IOD(co->SSRIOD)
1081 G_SSR_PROVIDER_ID(co->SSRProviderID)
1082 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1083 G_NO_OF_SATELLITES(nums)
1084 co->Supplied[COBOFS_ORBIT] |= 1;
1085 co->Supplied[COBOFS_CLOCK] |= 1;
1086 for(i = 0; i < nums; ++i)
1087 {
1088 switch(s)
1089 {
1090 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1091 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1092 G_GPS_SATELLITE_ID(id)
1093 break;
1094 case CLOCKORBIT_SATQZSS:
1095 G_QZSS_SATELLITE_ID(id)
1096 break;
1097 case CLOCKORBIT_SATGLONASS:
1098 G_GLONASS_SATELLITE_ID(id)
1099 break;
1100 }
1101 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
1102 ;
1103 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1104 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
1105 co->Sat[pos].ID = id;
[1840]1106
[5664]1107 switch(s)
1108 {
1109 case CLOCKORBIT_SATGPS:
1110 case CLOCKORBIT_SATQZSS:
1111 G_GPS_IODE(co->Sat[pos].IOD)
1112 break;
1113 case CLOCKORBIT_SATGLONASS:
1114 G_GLONASS_IOD(co->Sat[pos].IOD)
1115 break;
1116 case CLOCKORBIT_SATGALILEO:
1117 G_GALILEO_IOD(co->Sat[pos].IOD)
1118 break;
1119 case CLOCKORBIT_SATSBAS:
1120 G_SBAS_T0MOD(co->Sat[pos].toe)
1121 G_SBAS_IODCRC(co->Sat[pos].IOD)
1122 break;
1123 case CLOCKORBIT_SATBDS:
1124 G_BDS_TOEMOD(co->Sat[pos].toe)
1125 G_BDS_IODCRC(co->Sat[pos].IOD)
1126 break;
1127 }
1128 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
1129 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
1130 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
1131 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
1132 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
1133 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
1134 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
1135 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
1136 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
1137 }
1138 break;
1139 case COBOFS_URA:
1140 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
1141 co->messageType = type;
1142 switch(s)
1143 {
1144 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1145 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1146 case CLOCKORBIT_SATBDS:
1147 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1148 break;
1149 case CLOCKORBIT_SATGLONASS:
1150 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1151 break;
1152 }
1153 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1154 G_SSR_IOD(co->SSRIOD)
1155 G_SSR_PROVIDER_ID(co->SSRProviderID)
1156 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1157 G_NO_OF_SATELLITES(nums)
1158 co->Supplied[COBOFS_URA] |= 1;
1159 for(i = 0; i < nums; ++i)
1160 {
1161 switch(s)
1162 {
1163 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1164 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1165 G_GPS_SATELLITE_ID(id)
1166 break;
1167 case CLOCKORBIT_SATQZSS:
1168 G_QZSS_SATELLITE_ID(id)
1169 break;
1170 case CLOCKORBIT_SATGLONASS:
1171 G_GLONASS_SATELLITE_ID(id)
1172 break;
1173 }
1174 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
1175 ;
1176 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1177 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
1178 co->Sat[pos].ID = id;
[1840]1179
[5664]1180 G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
1181 }
1182 break;
1183 case COBOFS_HR:
1184 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
1185 co->messageType = type;
1186 switch(s)
1187 {
1188 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1189 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1190 case CLOCKORBIT_SATBDS:
1191 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1192 break;
1193 case CLOCKORBIT_SATGLONASS:
1194 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1195 break;
1196 }
1197 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1198 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1199 G_SSR_IOD(co->SSRIOD)
1200 G_SSR_PROVIDER_ID(co->SSRProviderID)
1201 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1202 G_NO_OF_SATELLITES(nums)
1203 co->Supplied[COBOFS_HR] |= 1;
1204 for(i = 0; i < nums; ++i)
1205 {
1206 switch(s)
1207 {
1208 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1209 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1210 G_GPS_SATELLITE_ID(id)
1211 break;
1212 case CLOCKORBIT_SATQZSS:
1213 G_QZSS_SATELLITE_ID(id)
1214 break;
1215 case CLOCKORBIT_SATGLONASS:
1216 G_GLONASS_SATELLITE_ID(id)
1217 break;
1218 }
1219 for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
1220 ;
1221 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1222 else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
1223 co->Sat[pos].ID = id;
[1840]1224
[5664]1225 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
1226 }
1227 break;
1228 case COBOFS_BIAS:
1229 if(!b) return GCOBR_NOCODEBIASPARAMETER;
1230 b->messageType = type;
1231 switch(s)
1232 {
1233 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1234 case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1235 case CLOCKORBIT_SATBDS:
1236 G_GPS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s])
1237 break;
1238 case CLOCKORBIT_SATGLONASS:
1239 G_GLONASS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s])
1240 break;
1241 }
1242 G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
1243 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1244 G_SSR_IOD(b->SSRIOD)
1245 G_SSR_PROVIDER_ID(b->SSRProviderID)
1246 G_SSR_SOLUTION_ID(b->SSRSolutionID)
1247 G_NO_OF_SATELLITES(nums)
1248 for(i = 0; i < nums; ++i)
1249 {
1250 switch(s)
1251 {
1252 case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1253 case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1254 G_GPS_SATELLITE_ID(id)
1255 break;
1256 case CLOCKORBIT_SATQZSS:
1257 G_QZSS_SATELLITE_ID(id)
1258 break;
1259 case CLOCKORBIT_SATGLONASS:
1260 G_GLONASS_SATELLITE_ID(id)
1261 break;
1262 }
1263 for(pos = satoffset[s]; pos < satoffset[s]+b->NumberOfSat[s] && b->Sat[pos].ID != id; ++pos)
1264 ;
1265 if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1266 else if(pos == b->NumberOfSat[s] + satoffset[s]) ++b->NumberOfSat[s];
1267 b->Sat[pos].ID = id;
[1840]1268
[5664]1269 G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases)
1270 for(j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j)
1271 {
1272 G_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
1273 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
1274 }
1275 }
1276 break;
1277 default:
1278 return GCOBR_UNKNOWNTYPE;
[956]1279 }
[5664]1280#ifdef COR_LATENCY
1281 if(s == CLOCKORBIT_SATGPS && type-corbase[s] != COBOFS_BIAS)
[956]1282 {
[5664]1283 co->epochGPS[co->epochSize] = co->EpochTime[s];
1284 if(co->epochSize < COR_LATENCYCOUNT)
1285 ++co->epochSize;
[956]1286 }
[5664]1287#endif
1288#ifdef DEBUG
1289 for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type)
1290 numbits += 8;
1291 fprintf(stderr, "numbits left %d\n",numbits);
1292#endif
1293 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
[956]1294 }
1295 }
[5664]1296 return GCOBR_UNKNOWNTYPE;
[956]1297}
1298#endif /* NODECODE */
Note: See TracBrowser for help on using the repository browser.