source: ntrip/branches/BNC_2.12/src/RTCM3/clock_and_orbit/clock_orbit_rtcm.c @ 8988

Last change on this file since 8988 was 8988, checked in by stuerze, 2 months ago

disable debug output

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