source: ntrip/branches/BNC_2.12/src/RTCM3/clock_and_orbit/clock_orbit_igs.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 Author Date Id Rev URL;svn:eol-style=native
  • Property svn:mime-type set to text/plain
File size: 38.8 KB
Line 
1/* Programheader
2
3 Name:           clock_orbit_igs.c
4 Project:        RTCM3
5 Version:        $Id: clock_orbit_igs.c 8988 2020-07-20 13:56:40Z stuerze $
6 Authors:        Dirk Stöcker, Andrea Stürze
7 Description:    state space approach: IGS
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_igs.h>
19
20
21static uint32_t CRC24(long size, const unsigned char *buf) {
22  uint32_t crc = 0;
23  int i;
24
25  while (size--) {
26    crc ^= (*buf++) << (16);
27    for (i = 0; i < 8; i++)
28        {
29      crc <<= 1;
30      if (crc & 0x1000000)
31        crc ^= 0x01864cfb;
32    }
33  }
34  return crc;
35}
36
37/* NOTE: These defines are interlinked with below functions and directly modify
38 the values. This may not be optimized in terms of final program code size but
39 should be optimized in terms of speed.
40
41 modified variables are:
42 - everything defined in STARTDATA (only use ressize outside of the defines,
43 others are private)
44 - buffer
45 - size
46 */
47
48#ifndef NOENCODE
49#define STOREBITS \
50  while(numbits >= 8) { \
51    if(!size) return 0; \
52    *(buffer++) = bitbuffer>>(numbits-8); \
53    numbits -= 8; \
54    ++ressize; \
55    --size; \
56  }
57
58#define ADDBITS(a, b) { \
59    bitbuffer = (bitbuffer<<(a))|((b)&((1<<a)-1)); \
60    numbits += (a); \
61    STOREBITS \
62  }
63
64#define STARTDATA \
65  size_t ressize=0; \
66  char *blockstart; \
67  int numbits; \
68  uint64_t bitbuffer=0;
69
70#define INITBLOCK \
71  numbits = 0; \
72  blockstart = buffer; \
73  ADDBITS(8, 0xD3) \
74  ADDBITS(6, 0) \
75  ADDBITS(10, 0)
76
77#define ENDBLOCK \
78  if(numbits) { ADDBITS((8-numbits), 0) } { \
79    int len = buffer-blockstart-3; \
80    blockstart[1] |= len>>8; \
81    blockstart[2] = len; \
82    if(len > 1023) \
83      return 0; \
84    len = CRC24(len+3, (const unsigned char *) blockstart); \
85    ADDBITS(24, len) \
86  }
87
88#define SCALEADDBITS(a, b, c) ADDBITS(a, (int64_t)(c > 0 ? b*c+0.5 : b*c-0.5))
89
90#define MPI         3.141592653589793
91
92/* GNSS macros - Header part */
93#define T_RTCM_MESSAGE_NUMBER(a)         ADDBITS(12, a)      /* DF002         */
94#define T_IGS_SSR_VERSION(a)             ADDBITS( 3, a)      /*        IDF001 */
95#define T_IGS_MESSAGE_NUMBER(a)          ADDBITS( 8, a)      /*        IDF002 */
96#define T_SSR_EPOCH_TIME(a)              ADDBITS(20, a)      /*        IDF003 */
97#define T_SSR_UPDATE_INTERVAL(a)         ADDBITS( 4, a)      /* DF391, IDF004 */
98#define T_MULTIPLE_MESSAGE_INDICATOR(a)  ADDBITS( 1, a)      /* DF388, IDF005 */
99#define T_SSR_IOD(a)                     ADDBITS( 4, a)      /* DF413, IDF007 */
100#define T_SSR_PROVIDER_ID(a)             ADDBITS(16, a)      /* DF414, IDF008 */
101#define T_SSR_SOLUTION_ID(a)             ADDBITS( 4, a)      /* DF415, IDF009 */
102#define T_SATELLITE_REFERENCE_DATUM(a)   ADDBITS( 1, a)      /* DF375, IDF006 */
103#define T_NO_OF_SATELLITES(a)            ADDBITS( 6, a)      /* DF387, IDF010 */
104
105/* GNSS macros - Satellite specific part */
106#define T_GNSS_SATELLITE_ID(a)           ADDBITS( 6, a)      /*        IDF011 */
107#define T_GNSS_IOD(a)                    ADDBITS( 8, a)      /*        IDF012 */
108#define T_DELTA_RADIAL(a)                SCALEADDBITS(22,    10000.0, a) /* DF365, IDF013 */
109#define T_DELTA_ALONG_TRACK(a)           SCALEADDBITS(20,     2500.0, a) /* DF366, IDF014 */
110#define T_DELTA_CROSS_TRACK(a)           SCALEADDBITS(20,     2500.0, a) /* DF367, IDF015 */
111#define T_DELTA_DOT_RADIAL(a)            SCALEADDBITS(21,  1000000.0, a) /* DF368, IDF016 */
112#define T_DELTA_DOT_ALONG_TRACK(a)       SCALEADDBITS(19,   250000.0, a) /* DF369, IDF017 */
113#define T_DELTA_DOT_CROSS_TRACK(a)       SCALEADDBITS(19,   250000.0, a) /* DF370, IDF018 */
114
115#define T_DELTA_CLOCK_C0(a)              SCALEADDBITS(22,    10000.0, a) /* DF376, IDF019 */
116#define T_DELTA_CLOCK_C1(a)              SCALEADDBITS(21,  1000000.0, a) /* DF377, IDF020 */
117#define T_DELTA_CLOCK_C2(a)              SCALEADDBITS(27, 50000000.0, a) /* DF378, IDF021 */
118#define T_HR_CLOCK_CORRECTION(a)         SCALEADDBITS(22,    10000.0, a) /* DF390, IDF022 */
119
120#define T_NO_OF_BIASES(a)                ADDBITS(5, a)                   /*        IDF023 */
121#define T_GNSS_SIGNAL_IDENTIFIER(a)      ADDBITS(5, a)                   /*        IDF024 */
122#define T_CODE_BIAS(a)                   SCALEADDBITS(14,      100.0, a) /* DF383, IDF025 */
123#define T_YAW_ANGLE(a)                   SCALEADDBITS( 9,  256.0/MPI, a) /* DF480, IDF026 */
124#define T_YAW_RATE(a)                    SCALEADDBITS( 8, 8192.0/MPI, a) /* DF481, IDF027 */
125#define T_PHASE_BIAS(a)                  SCALEADDBITS(20,    10000.0, a) /* DF482, IDF028 */
126
127/* Phase specific part of GNSS phase bias message */
128#define T_INTEGER_INDICATOR(a)           ADDBITS( 1, a)                  /* DF483, IDF029 */
129#define T_WIDE_LANE_INDICATOR(a)         ADDBITS( 2, a)                  /* DF484, IDF030 */
130#define T_DISCONTINUITY_COUNTER(a)       ADDBITS( 4, a)                  /* DF485, IDF031 */
131#define T_DISPERSIVE_BIAS_INDICATOR(a)   ADDBITS( 1, a)                  /* DF486, IDF032 */
132#define T_MW_CONSISTENCY_INDICATOR(a)    ADDBITS( 1, a)                  /* DF487, IDF033 */
133
134#define T_SSR_URA(a)                     ADDBITS( 6, a)                  /* DF389, IDF034 */
135
136/* Ionosphere */
137#define T_NO_IONO_LAYERS(a)              ADDBITS( 2, a-1)                /* DF472, IDF035 */
138#define T_IONO_HEIGHT(a)                 SCALEADDBITS( 8,  1/10000.0, a) /* DF473, IDF036 */
139#define T_IONO_DEGREE(a)                 ADDBITS( 4, a-1)                /* DF474, IDF037 */
140#define T_IONO_ORDER(a)                  ADDBITS( 4, a-1)                /* DF475, IDF038 */
141#define T_IONO_COEFF_C(a)                SCALEADDBITS(16,      200.0, a) /* DF476, IDF039 */
142#define T_IONO_COEFF_S(a)                SCALEADDBITS(16,      200.0, a) /* DF477, IDF040 */
143#define T_VTEC_QUALITY_INDICATOR(a)      SCALEADDBITS( 9,       20.0, a) /* DF478, IDF041 */
144
145static double URAToValue(int ura) {
146  int urac, urav;
147  urac = ura >> 3;
148  urav = ura & 7;
149  if (!ura)
150    return 0;
151  else if (ura == 63)
152    return SSR_MAXURA;
153  return (pow(3, urac) * (1.0 + urav / 4.0) - 1.0) / 1000.0;
154}
155
156static int ValueToURA(double val) {
157  int ura;
158  if (!val)
159    return 0;
160  else if (val > 5.4665)
161    return 63;
162  for (ura = 1; ura < 63 && val > URAToValue(ura); ++ura)
163    ;
164  return ura;
165}
166
167static const enum ClockOrbitType corbase[CLOCKORBIT_SATNUM] = {
168  (int) COBBASE_GPS,
169  (int) COBBASE_GLONASS,
170  (int) COBBASE_GALILEO,
171  (int) COBBASE_QZSS,
172  (int) COBBASE_SBAS,
173  (int) COBBASE_BDS
174};
175
176static const enum COR_OFFSETS satoffset[CLOCKORBIT_SATNUM + 1] = {
177  CLOCKORBIT_OFFSETGPS,
178  CLOCKORBIT_OFFSETGLONASS,
179  CLOCKORBIT_OFFSETGALILEO,
180  CLOCKORBIT_OFFSETQZSS,
181  CLOCKORBIT_OFFSETSBAS,
182  CLOCKORBIT_OFFSETBDS,
183  CLOCKORBIT_COUNTSAT
184};
185
186size_t MakeClockOrbit(const struct ClockOrbit *co, enum ClockOrbitType type,
187    int moremessagesfollow, char *buffer, size_t size) {
188  unsigned int status[CLOCKORBIT_SATNUM][COBOFS_NUM], i, s;
189
190  memset(status, 0, sizeof(status));
191
192  STARTDATA
193
194  for (s = 0; s < CLOCKORBIT_SATNUM; ++s) {
195    for (i = 0; i < COBOFS_NUM; ++i) {
196      if (co->NumberOfSat[s] && (type == COTYPE_AUTO || type == corbase[s] + i) &&
197           (co->Supplied[i] || (i <= COBOFS_CLOCK &&  co->Supplied[COBOFS_COMBINED]) ||
198           (i == COBOFS_COMBINED && co->Supplied[COBOFS_ORBIT] && co->Supplied[COBOFS_CLOCK]))) {
199        status[s][i] = 1;
200        if (i == COBOFS_COMBINED) {
201          status[s][COBOFS_ORBIT] = status[s][COBOFS_CLOCK] = 0;
202        } /* disable single blocks for combined type */
203      } /* check for data */
204    } /* iterate over sub-types */
205  } /* iterate over satellite systems */
206
207  for (s = 0; s < CLOCKORBIT_SATNUM; ++s) {
208    if (status[s][COBOFS_ORBIT]) {
209      INITBLOCK
210          T_RTCM_MESSAGE_NUMBER(RTCM_MESSAGE_NUMBER_IGS)
211      T_IGS_SSR_VERSION(IGS_SSR_VERSION)
212          T_IGS_MESSAGE_NUMBER(corbase[s] + COBOFS_ORBIT)
213          T_SSR_EPOCH_TIME(co->EpochTime[s])
214      T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
215          T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
216      T_SSR_IOD(co->SSRIOD)
217      T_SSR_PROVIDER_ID(co->SSRProviderID)
218      T_SSR_SOLUTION_ID(co->SSRSolutionID)
219          T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
220      T_NO_OF_SATELLITES(co->NumberOfSat[s])
221      for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) {
222        T_GNSS_SATELLITE_ID(co->Sat[i].ID)
223        T_GNSS_IOD(co->Sat[i].IOD)
224        T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
225        T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
226        T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
227        T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
228        T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
229        T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
230      }
231      ENDBLOCK
232    }
233    if (status[s][COBOFS_CLOCK]) {
234      INITBLOCK
235          T_RTCM_MESSAGE_NUMBER(RTCM_MESSAGE_NUMBER_IGS)
236      T_IGS_SSR_VERSION(IGS_SSR_VERSION)
237          T_IGS_MESSAGE_NUMBER(corbase[s] + COBOFS_CLOCK)
238          T_SSR_EPOCH_TIME(co->EpochTime[s])
239      T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
240          T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
241      T_SSR_IOD(co->SSRIOD)
242      T_SSR_PROVIDER_ID(co->SSRProviderID)
243      T_SSR_SOLUTION_ID(co->SSRSolutionID)
244      T_NO_OF_SATELLITES(co->NumberOfSat[s])
245      for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) {
246        T_GNSS_SATELLITE_ID(co->Sat[i].ID)
247        T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
248        T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
249        T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)
250      }
251      ENDBLOCK
252    }
253    if (status[s][COBOFS_COMBINED]) {
254#ifdef SPLITBLOCK
255      int nums = co->NumberOfSat[s];
256      int left, start = satoffset[s];
257      if(nums > 28) {/* split block when more than 28 sats */
258        left = nums - 28;
259        nums = 28;
260      }
261      else {
262        left = 0;
263      }
264      while(nums) {
265#endif
266      INITBLOCK
267          T_RTCM_MESSAGE_NUMBER(RTCM_MESSAGE_NUMBER_IGS)
268      T_IGS_SSR_VERSION(IGS_SSR_VERSION)
269          T_IGS_MESSAGE_NUMBER(corbase[s] + COBOFS_COMBINED)
270          T_SSR_EPOCH_TIME(co->EpochTime[s])
271      T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
272#ifdef SPLITBLOCK
273      T_MULTIPLE_MESSAGE_INDICATOR((moremessagesfollow || left) ? 1 : 0)
274#else
275      T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
276#endif
277      T_SSR_IOD(co->SSRIOD)
278      T_SSR_PROVIDER_ID(co->SSRProviderID)
279      T_SSR_SOLUTION_ID(co->SSRSolutionID)
280          T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
281#ifdef SPLITBLOCK
282      T_NO_OF_SATELLITES(nums)
283      for(i = start; i < start+nums; ++i) {
284#else
285      T_NO_OF_SATELLITES(co->NumberOfSat[s])
286      for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) {
287#endif
288        T_GNSS_SATELLITE_ID(co->Sat[i].ID)
289        T_GNSS_IOD(co->Sat[i].IOD)
290        T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
291        T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
292        T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
293        T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
294        T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
295        T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
296        T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
297        T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
298        T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)
299      }
300      ENDBLOCK
301#ifdef SPLITBLOCK
302      start += nums;
303      nums = left;
304      left = 0;
305    }
306#endif
307    }
308    if (status[s][COBOFS_HR]) {
309      INITBLOCK
310          T_RTCM_MESSAGE_NUMBER(RTCM_MESSAGE_NUMBER_IGS)
311      T_IGS_SSR_VERSION(IGS_SSR_VERSION)
312          T_IGS_MESSAGE_NUMBER(corbase[s] + COBOFS_HR)
313          T_SSR_EPOCH_TIME(co->EpochTime[s])
314      T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
315          T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
316      T_SSR_IOD(co->SSRIOD)
317      T_SSR_PROVIDER_ID(co->SSRProviderID)
318      T_SSR_SOLUTION_ID(co->SSRSolutionID)
319      T_NO_OF_SATELLITES(co->NumberOfSat[s])
320      for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) {
321        T_GNSS_SATELLITE_ID(co->Sat[i].ID)
322        T_HR_CLOCK_CORRECTION(co->Sat[i].hrclock)
323      }
324      ENDBLOCK
325    }
326    if (status[s][COBOFS_URA]) {
327      INITBLOCK
328          T_RTCM_MESSAGE_NUMBER(RTCM_MESSAGE_NUMBER_IGS)
329      T_IGS_SSR_VERSION(IGS_SSR_VERSION)
330          T_IGS_MESSAGE_NUMBER(corbase[s] + COBOFS_URA)
331          T_SSR_EPOCH_TIME(co->EpochTime[s])
332      T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
333          T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
334      T_SSR_IOD(co->SSRIOD)
335      T_SSR_PROVIDER_ID(co->SSRProviderID)
336      T_SSR_SOLUTION_ID(co->SSRSolutionID)
337      T_NO_OF_SATELLITES(co->NumberOfSat[s])
338      for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) {
339        T_GNSS_SATELLITE_ID(co->Sat[i].ID)
340        T_SSR_URA(ValueToURA(co->Sat[i].UserRangeAccuracy))
341      }
342      ENDBLOCK
343    }
344  }
345  return ressize;
346}
347
348size_t MakeCodeBias(const struct CodeBias *b, enum CodeBiasType type,
349    int moremessagesfollow, char *buffer, size_t size) {
350  unsigned int s, i, j;
351
352  STARTDATA
353
354  for (s = 0; s < CLOCKORBIT_SATNUM; ++s) {
355    if (b->NumberOfSat[s] && (type == CBTYPE_AUTO || type == corbase[s] + COBOFS_CBIAS)) {
356      INITBLOCK
357          T_RTCM_MESSAGE_NUMBER(RTCM_MESSAGE_NUMBER_IGS)
358      T_IGS_SSR_VERSION(IGS_SSR_VERSION)
359          T_IGS_MESSAGE_NUMBER(corbase[s] + COBOFS_CBIAS)
360          T_SSR_EPOCH_TIME(b->EpochTime[s])
361      T_SSR_UPDATE_INTERVAL(b->UpdateInterval)
362          T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
363      T_SSR_IOD(b->SSRIOD)
364      T_SSR_PROVIDER_ID(b->SSRProviderID)
365      T_SSR_SOLUTION_ID(b->SSRSolutionID)
366      T_NO_OF_SATELLITES(b->NumberOfSat[s])
367      for (i = satoffset[s]; i < satoffset[s] + b->NumberOfSat[s]; ++i) {
368        T_GNSS_SATELLITE_ID(b->Sat[i].ID)
369        T_NO_OF_BIASES(b->Sat[i].NumberOfCodeBiases)
370        for (j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j) {
371          T_GNSS_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type)
372          T_CODE_BIAS(b->Sat[i].Biases[j].Bias)
373        }
374      }
375      ENDBLOCK
376    }
377  }
378  return ressize;
379}
380
381size_t MakePhaseBias(const struct PhaseBias *b, enum PhaseBiasType type,
382    int moremessagesfollow, char *buffer, size_t size) {
383  unsigned int s, i, j;
384
385  STARTDATA
386
387  for (s = 0; s < CLOCKORBIT_SATNUM; ++s)       {
388    if (b->NumberOfSat[s] && (type == PBTYPE_AUTO || type == corbase[s] + COBOFS_PBIAS)) {
389      INITBLOCK
390          T_RTCM_MESSAGE_NUMBER(RTCM_MESSAGE_NUMBER_IGS)
391      T_IGS_SSR_VERSION(IGS_SSR_VERSION)
392          T_IGS_MESSAGE_NUMBER(corbase[s] + COBOFS_PBIAS)
393          T_SSR_EPOCH_TIME(b->EpochTime[s])
394      T_SSR_UPDATE_INTERVAL(b->UpdateInterval)
395          T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
396      T_SSR_IOD(b->SSRIOD)
397      T_SSR_PROVIDER_ID(b->SSRProviderID)
398      T_SSR_SOLUTION_ID(b->SSRSolutionID)
399      T_DISPERSIVE_BIAS_INDICATOR(b->DispersiveBiasConsistencyIndicator ? 1 : 0)
400      T_MW_CONSISTENCY_INDICATOR(b->MWConsistencyIndicator ? 1 : 0)
401      T_NO_OF_SATELLITES(b->NumberOfSat[s])
402      for (i = satoffset[s]; i < satoffset[s] + b->NumberOfSat[s]; ++i) {
403        T_GNSS_SATELLITE_ID(b->Sat[i].ID)
404        T_NO_OF_BIASES(b->Sat[i].NumberOfPhaseBiases)
405        T_YAW_ANGLE(b->Sat[i].YawAngle)
406        T_YAW_RATE(b->Sat[i].YawRate)
407        for (j = 0; j < b->Sat[i].NumberOfPhaseBiases; ++j) {
408          T_GNSS_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type)
409          T_INTEGER_INDICATOR(b->Sat[i].Biases[j].SignalIntegerIndicator ? 1 : 0)
410          T_WIDE_LANE_INDICATOR(b->Sat[i].Biases[j].SignalsWideLaneIntegerIndicator)
411          T_DISCONTINUITY_COUNTER(b->Sat[i].Biases[j].SignalDiscontinuityCounter)
412          T_PHASE_BIAS(b->Sat[i].Biases[j].Bias)
413        }
414      }
415      ENDBLOCK
416    }
417  }
418  return ressize;
419}
420
421size_t MakeVTEC(const struct VTEC *v, int moremessagesfollow, char *buffer, size_t size) {
422  unsigned int l, o, d;
423
424  STARTDATA
425    INITBLOCK
426
427  T_RTCM_MESSAGE_NUMBER(RTCM_MESSAGE_NUMBER_IGS)
428  T_IGS_SSR_VERSION(IGS_SSR_VERSION)
429  T_IGS_MESSAGE_NUMBER(VTEC_BASE)
430  T_SSR_EPOCH_TIME(v->EpochTime)
431  T_SSR_UPDATE_INTERVAL(v->UpdateInterval)
432  T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
433  T_SSR_IOD(v->SSRIOD)
434  T_SSR_PROVIDER_ID(v->SSRProviderID)
435  T_SSR_SOLUTION_ID(v->SSRSolutionID)
436  T_VTEC_QUALITY_INDICATOR(v->Quality)
437  T_NO_IONO_LAYERS(v->NumLayers)
438  for (l = 0; l < v->NumLayers; ++l) {
439    T_IONO_HEIGHT(v->Layers[l].Height)
440    T_IONO_DEGREE(v->Layers[l].Degree)
441    T_IONO_ORDER(v->Layers[l].Order)
442    for (o = 0; o <= v->Layers[l].Order; ++o) {
443      for (d = o; d <= v->Layers[l].Degree; ++d) {
444        T_IONO_COEFF_C(v->Layers[l].Cosinus[d][o])
445      }
446    }
447    for (o = 1; o <= v->Layers[l].Order; ++o) {
448      for (d = o; d <= v->Layers[l].Degree; ++d) {
449        T_IONO_COEFF_S(v->Layers[l].Sinus[d][o])
450      }
451    }
452  }
453  ENDBLOCK
454  return ressize;
455}
456#endif /* NOENCODE */
457
458#ifndef NODECODE
459
460#define DECODESTART \
461  int numbits=0; \
462  uint64_t bitbuffer=0;
463
464#define LOADBITS(a) { \
465  while((a) > numbits) { \
466    if(!size--) return GCOBR_SHORTMESSAGE; \
467    bitbuffer = (bitbuffer<<8)|((unsigned char)*(buffer++)); \
468    numbits += 8; \
469  } \
470}
471
472/* extract bits from data stream
473 b = variable to store result, a = number of bits */
474#define GETBITS(b, a) { \
475  LOADBITS(a) \
476  b = (bitbuffer<<(64-numbits))>>(64-(a)); \
477  numbits -= (a); \
478}
479
480/* extract bits from data stream
481 b = variable to store result, a = number of bits */
482#define GETBITSFACTOR(b, a, c) { \
483  LOADBITS(a) \
484  b = ((bitbuffer<<(64-numbits))>>(64-(a)))*(c); \
485  numbits -= (a); \
486}
487
488/* extract signed floating value from data stream
489 b = variable to store result, a = number of bits */
490#define GETFLOATSIGN(b, a, c) { \
491  LOADBITS(a) \
492  b = ((double)(((int64_t)(bitbuffer<<(64-numbits)))>>(64-(a))))*(c); \
493  numbits -= (a); \
494}
495
496/* extract floating value from data stream
497 b = variable to store result, a = number of bits, c = scale factor */
498#define GETFLOAT(b, a, c) { \
499  LOADBITS(a) \
500  b = ((double)((bitbuffer<<(sizeof(bitbuffer)*8-numbits))>>(sizeof(bitbuffer)*8-(a))))*(c); \
501  numbits -= (a); \
502}
503
504#define SKIPBITS(b) { LOADBITS(b) numbits -= (b); }
505
506/* GPS macros also used for other systems when matching! */
507#define G_HEADER(a)                      GETBITS(a,  8)
508#define G_RESERVEDH(a)                   GETBITS(a,  6)
509#define G_SIZE(a)                        GETBITS(a, 10)
510
511/* GNSS macros - Header part */
512#define G_RTCM_MESSAGE_NUMBER(a)         GETBITS(a, 12)      /* DF002         */
513#define G_IGS_SSR_VERSION(a)             GETBITS(a,  3)      /*        IDF001 */
514#define G_IGS_MESSAGE_NUMBER(a)          GETBITS(a,  8)      /*        IDF002 */
515#define G_SSR_EPOCH_TIME(a)              GETBITS(a, 20)      /*        IDF003 */
516#define G_SSR_EPOCH_TIME_CHECK(a, b)         {unsigned int temp; GETBITS(temp, 20) \
517 if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
518#define G_SSR_UPDATE_INTERVAL(a)         GETBITS(a,  4)      /* DF391, IDF004 */
519#define G_MULTIPLE_MESSAGE_INDICATOR(a)  GETBITS(a,  1)      /* DF388, IDF005 */
520#define G_SSR_IOD(a)                     GETBITS(a,  4)      /* DF413, IDF007 */
521#define G_SSR_PROVIDER_ID(a)             GETBITS(a, 16)      /* DF414, IDF008 */
522#define G_SSR_SOLUTION_ID(a)             GETBITS(a,  4)      /* DF415, IDF009 */
523#define G_SATELLITE_REFERENCE_DATUM(a)   GETBITS(a,  1)      /* DF375, IDF006 */
524#define G_NO_OF_SATELLITES(a)            GETBITS(a,  6)      /* DF387, IDF010 */
525
526/* GNSS macros - Satellite specific part */
527#define G_GNSS_SATELLITE_ID(a)           GETBITS(a,  6)                    /*        IDF011 */
528#define G_GNSS_IOD(a)                    GETBITS(a,  8)                    /*        IDF012 */
529
530#define G_DELTA_RADIAL(a)                GETFLOATSIGN(a, 22,   1/10000.0)  /* DF365, IDF013 */
531#define G_DELTA_ALONG_TRACK(a)           GETFLOATSIGN(a, 20,    1/2500.0)  /* DF366, IDF014 */
532#define G_DELTA_CROSS_TRACK(a)           GETFLOATSIGN(a, 20,    1/2500.0)  /* DF367, IDF015 */
533#define G_DELTA_DOT_RADIAL(a)            GETFLOATSIGN(a, 21, 1/1000000.0)  /* DF368, IDF016 */
534#define G_DELTA_DOT_ALONG_TRACK(a)       GETFLOATSIGN(a, 19,  1/250000.0)  /* DF369, IDF017 */
535#define G_DELTA_DOT_CROSS_TRACK(a)       GETFLOATSIGN(a, 19,  1/250000.0)  /* DF370, IDF018 */
536
537#define G_DELTA_CLOCK_C0(a)              GETFLOATSIGN(a, 22,    1/10000.0) /* DF376, IDF019 */
538#define G_DELTA_CLOCK_C1(a)              GETFLOATSIGN(a, 21,  1/1000000.0) /* DF377, IDF020 */
539#define G_DELTA_CLOCK_C2(a)              GETFLOATSIGN(a, 27, 1/50000000.0) /* DF378, IDF021 */
540#define G_HR_CLOCK_CORRECTION(a)         GETFLOATSIGN(a, 22,    1/10000.0) /* DF390, IDF022 */
541
542#define G_NO_OF_BIASES(a)                GETBITS(a,  5)                    /*        IDF023 */
543#define G_GNSS_SIGNAL_IDENTIFIER(a)      GETBITS(a,  5)                    /*        IDF024 */
544#define G_CODE_BIAS(a)                   GETFLOATSIGN(a, 14, 1/100.0)      /* DF383, IDF025 */
545#define G_YAW_ANGLE(a)                   GETFLOAT    (a,  9, MPI/256.0)    /* DF480, IDF026 */
546#define G_YAW_RATE(a)                    GETFLOATSIGN(a,  8, MPI/8192.0)   /* DF481, IDF027 */
547#define G_PHASE_BIAS(a)                  GETFLOATSIGN(a, 20, 1/10000.)     /* DF482, IDF028 */
548
549#define G_INTEGER_INDICATOR(a)           GETBITS(a,  1)                    /* DF483, IDF029 */
550#define G_WIDE_LANE_INDICATOR(a)         GETBITS(a,  2)                    /* DF484, IDF030 */
551#define G_DISCONTINUITY_COUNTER(a)       GETBITS(a,  4)                    /* DF485, IDF031 */
552#define G_DISPERSIVE_BIAS_INDICATOR(a)   GETBITS(a,  1)                    /* DF486, IDF032 */
553#define G_MW_CONSISTENCY_INDICATOR(a)    GETBITS(a,  1)                    /* DF487, IDF033 */
554
555#define G_SSR_URA(a)                     {int temp; GETBITS(temp, 6) \
556 (a) = URAToValue(temp);}                                                  /* DF389, IDF034 */
557
558/* Ionosphere */
559#define G_NO_IONO_LAYERS(a) {unsigned int temp; GETBITS(temp, 2) a = temp+1;} /* DF472, IDF035 */
560#define G_IONO_HEIGHT(a)                 GETFLOAT(a, 8 ,    10000.0)          /* DF473, IDF036 */
561#define G_IONO_DEGREE(a)    {unsigned int temp; GETBITS(temp, 4) a = temp+1;} /* DF474, IDF037 */
562#define G_IONO_ORDER(a)     {unsigned int temp; GETBITS(temp, 4) a = temp+1;} /* DF475, IDF038 */
563#define G_IONO_COEFF_C(a)                GETFLOATSIGN(a, 16,1/200.0)          /* DF476, IDF039 */
564#define G_IONO_COEFF_S(a)                GETFLOATSIGN(a, 16,1/200.0)          /* DF477, IDF040 */
565#define G_VTEC_QUALITY_INDICATOR(a)      GETFLOAT     (a, 9, 1/20.0)          /* DF478, IDF041 */
566
567enum GCOB_RETURN GetSSR(struct ClockOrbit *co, struct CodeBias *b,struct VTEC *v,
568    struct PhaseBias *pb, const char *buffer, size_t size, int *bytesused) {
569  int mmi = 0, h, rs;
570  unsigned int type, pos, i, j, s, nums, id, version;
571  size_t sizeofrtcmblock;
572  const char *blockstart = buffer;
573  DECODESTART
574
575  if (size < 7)
576    return GCOBR_SHORTBUFFER;
577
578#ifdef DEBUG
579  fprintf(stderr, "GetSSR START: size %d, numbits %d\n",(int)size, numbits);
580#endif
581
582  G_HEADER(h)
583  G_RESERVEDH(rs)
584  G_SIZE(sizeofrtcmblock);
585
586  if ((unsigned char) h != 0xD3 || rs)
587    return GCOBR_UNKNOWNDATA;
588  if (size < sizeofrtcmblock + 3) /* 3 header bytes already removed */
589    return GCOBR_MESSAGEEXCEEDSBUFFER;
590  if (CRC24(sizeofrtcmblock + 3, (const unsigned char *) blockstart) !=
591      (uint32_t) ((((unsigned char) buffer[sizeofrtcmblock]) << 16) |
592          (((unsigned char) buffer[sizeofrtcmblock + 1]) << 8) |
593          (((unsigned char) buffer[sizeofrtcmblock + 2]))))
594    return GCOBR_CRCMISMATCH;
595  size = sizeofrtcmblock; /* reduce size, so overflows are detected */
596
597  G_RTCM_MESSAGE_NUMBER(type)
598  G_IGS_SSR_VERSION(version)
599#ifdef DEBUG
600  fprintf(stderr, "rtcmType %d igsVersion %d ",type, version);
601#endif
602  G_IGS_MESSAGE_NUMBER(type)
603#ifdef DEBUG
604  fprintf(stderr, "igsType IM%d size %d\n",type,(int)sizeofrtcmblock);
605#endif
606  if (bytesused)
607    *bytesused = sizeofrtcmblock + 6;
608
609  if (type == VTEC_BASE) {
610    unsigned int l, o, d;
611    if (!v)
612      return GCOBR_NOVTECPARAMETER;
613    memset(v, 0, sizeof(*v));
614    G_SSR_EPOCH_TIME(v->EpochTime)
615    G_SSR_UPDATE_INTERVAL(v->UpdateInterval)
616    G_MULTIPLE_MESSAGE_INDICATOR(mmi)
617    G_SSR_IOD(v->SSRIOD)
618    G_SSR_PROVIDER_ID(v->SSRProviderID)
619    G_SSR_SOLUTION_ID(v->SSRSolutionID)
620    G_VTEC_QUALITY_INDICATOR(v->Quality)
621    G_NO_IONO_LAYERS(v->NumLayers)
622#ifdef DEBUG
623          fprintf(stderr, "epochTime %d ui %d mmi %d ssrIod %d providerId %d solId %d vtecQ %8.3f numLay %d \n",
624                          v->EpochTime, v->UpdateInterval, mmi,
625                                  v->SSRIOD, v->SSRProviderID, v->SSRSolutionID, v->Quality, v->NumLayers);
626#endif
627    for (l = 0; l < v->NumLayers; ++l) {
628      G_IONO_HEIGHT(v->Layers[l].Height)
629      G_IONO_DEGREE(v->Layers[l].Degree)
630      G_IONO_ORDER(v->Layers[l].Order)
631#ifdef DEBUG
632          fprintf(stderr, "h  %8.3f deg %d ord %d \n",
633                          v->Layers[l].Height, v->Layers[l].Degree, v->Layers[l].Order);
634#endif
635      for (o = 0; o <= v->Layers[l].Order; ++o) {
636        for (d = o; d <= v->Layers[l].Degree; ++d) {
637          G_IONO_COEFF_C(v->Layers[l].Cosinus[d][o])
638#ifdef DEBUG
639          fprintf(stderr, "C[%02d][%02d]  %8.3f \n",
640                          d, o, v->Layers[l].Cosinus[d][o]);
641#endif
642        }
643      }
644      for (o = 1; o <= v->Layers[l].Order; ++o) {
645        for (d = o; d <= v->Layers[l].Degree; ++d) {
646          G_IONO_COEFF_S(v->Layers[l].Sinus[d][o])
647#ifdef DEBUG
648          fprintf(stderr, "S[%02d][%02d]  %8.3f \n",
649                          d, o, v->Layers[l].Sinus[d][o]);
650#endif
651        }
652      }
653    }
654#ifdef DEBUG
655    for(type = 0; type < (unsigned int)size && (unsigned char)buffer[type] != 0xD3; ++type)
656    numbits += 8;
657    fprintf(stderr, "numbits left %d\n",numbits);
658#endif
659    return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
660  }
661  for (s = CLOCKORBIT_SATNUM; s-- > 0;) {
662    if (type >= corbase[s]) {
663      switch (type - corbase[s]) {
664        case COBOFS_ORBIT:
665          if (!co)
666            return GCOBR_NOCLOCKORBITPARAMETER;
667          co->messageType = type;
668          G_SSR_EPOCH_TIME_CHECK(co->EpochTime[s], co->NumberOfSat[s])
669          G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
670          G_MULTIPLE_MESSAGE_INDICATOR(mmi)
671          G_SSR_IOD(co->SSRIOD)
672          G_SSR_PROVIDER_ID(co->SSRProviderID)
673          G_SSR_SOLUTION_ID(co->SSRSolutionID)
674          G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
675          G_NO_OF_SATELLITES(nums)
676          co->Supplied[COBOFS_ORBIT] |= 1;
677#ifdef DEBUG
678          fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d rd %d ssrIod %d providerId %d solId %d\n",
679                          co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
680                                  co->SatRefDatum, co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
681#endif
682          for (i = 0; i < nums; ++i) {
683            G_GNSS_SATELLITE_ID(id)
684            for (pos = satoffset[s];
685                pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
686                ++pos)
687              ;
688            if (pos >= satoffset[s + 1])
689              return GCOBR_DATAMISMATCH;
690            else if (pos == co->NumberOfSat[s] + satoffset[s])
691              ++co->NumberOfSat[s];
692            co->Sat[pos].ID = id;
693            G_GNSS_IOD(co->Sat[pos].IOD)
694            G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
695            G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
696            G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
697            G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
698            G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
699            G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
700#ifdef DEBUG
701            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",
702                co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
703                co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
704                co->Sat[pos].Orbit.DotDeltaRadial,
705                co->Sat[pos].Orbit.DotDeltaAlongTrack,
706                co->Sat[pos].Orbit.DotDeltaCrossTrack);
707#endif
708          }
709          break;
710        case COBOFS_CLOCK:
711          if (!co)
712            return GCOBR_NOCLOCKORBITPARAMETER;
713          co->messageType = type;
714          G_SSR_EPOCH_TIME_CHECK(co->EpochTime[s], co->NumberOfSat[s])
715          G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
716          G_MULTIPLE_MESSAGE_INDICATOR(mmi)
717          G_SSR_IOD(co->SSRIOD)
718          G_SSR_PROVIDER_ID(co->SSRProviderID)
719          G_SSR_SOLUTION_ID(co->SSRSolutionID)
720          G_NO_OF_SATELLITES(nums)
721          co->Supplied[COBOFS_CLOCK] |= 1;
722#ifdef DEBUG
723          fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d\n",
724                          co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
725                                  co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
726#endif
727          for (i = 0; i < nums; ++i) {
728            G_GNSS_SATELLITE_ID(id)
729            for (pos = satoffset[s];
730                pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
731                ++pos)
732              ;
733            if (pos >= satoffset[s + 1])
734              return GCOBR_DATAMISMATCH;
735            else if (pos == co->NumberOfSat[s] + satoffset[s])
736              ++co->NumberOfSat[s];
737            co->Sat[pos].ID = id;
738            G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
739            G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
740            G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
741#ifdef DEBUG
742            fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
743                co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
744                co->Sat[pos].Clock.DeltaA2);
745#endif
746          }
747          break;
748        case COBOFS_COMBINED:
749          if (!co)
750            return GCOBR_NOCLOCKORBITPARAMETER;
751          co->messageType = type;
752          G_SSR_EPOCH_TIME_CHECK(co->EpochTime[s], co->NumberOfSat[s])
753          G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
754          G_MULTIPLE_MESSAGE_INDICATOR(mmi)
755          G_SSR_IOD(co->SSRIOD)
756          G_SSR_PROVIDER_ID(co->SSRProviderID)
757          G_SSR_SOLUTION_ID(co->SSRSolutionID)
758          G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
759          G_NO_OF_SATELLITES(nums)
760          co->Supplied[COBOFS_ORBIT] |= 1;
761          co->Supplied[COBOFS_CLOCK] |= 1;
762#ifdef DEBUG
763          fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d rd %d ssrIod %d providerId %d solId %d\n",
764                          co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
765                                  co->SatRefDatum, co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
766#endif
767          for (i = 0; i < nums; ++i) {
768            G_GNSS_SATELLITE_ID(id)
769            for (pos = satoffset[s];
770                pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
771                ++pos)
772              ;
773            if (pos >= satoffset[s + 1])
774              return GCOBR_DATAMISMATCH;
775            else if (pos == co->NumberOfSat[s] + satoffset[s])
776              ++co->NumberOfSat[s];
777            co->Sat[pos].ID = id;
778            G_GNSS_IOD(co->Sat[pos].IOD)
779            G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
780            G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
781            G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
782            G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
783            G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
784            G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
785            G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
786            G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
787            G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
788#ifdef DEBUG
789            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",
790                co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
791                co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
792                co->Sat[pos].Orbit.DotDeltaRadial, co->Sat[pos].Orbit.DotDeltaAlongTrack,
793                co->Sat[pos].Orbit.DotDeltaCrossTrack,
794                                co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1, co->Sat[pos].Clock.DeltaA2);
795#endif
796          }
797          break;
798        case COBOFS_URA:
799          if (!co)
800            return GCOBR_NOCLOCKORBITPARAMETER;
801          co->messageType = type;
802          G_SSR_EPOCH_TIME_CHECK(co->EpochTime[s], co->NumberOfSat[s])
803          G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
804          G_MULTIPLE_MESSAGE_INDICATOR(mmi)
805          G_SSR_IOD(co->SSRIOD)
806          G_SSR_PROVIDER_ID(co->SSRProviderID)
807          G_SSR_SOLUTION_ID(co->SSRSolutionID)
808          G_NO_OF_SATELLITES(nums)
809          co->Supplied[COBOFS_URA] |= 1;
810#ifdef DEBUG
811          fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d\n",
812                          co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
813                                  co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
814#endif
815          for (i = 0; i < nums; ++i) {
816            G_GNSS_SATELLITE_ID(id)
817            for (pos = satoffset[s];
818                pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
819                ++pos)
820              ;
821            if (pos >= satoffset[s + 1])
822              return GCOBR_DATAMISMATCH;
823            else if (pos == co->NumberOfSat[s] + satoffset[s])
824              ++co->NumberOfSat[s];
825            co->Sat[pos].ID = id;
826            G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
827#ifdef DEBUG
828            fprintf(stderr, "id %2d ura %8.3f \n",
829                co->Sat[pos].ID, co->Sat[pos].UserRangeAccuracy);
830#endif
831          }
832          break;
833        case COBOFS_HR:
834          if (!co)
835            return GCOBR_NOCLOCKORBITPARAMETER;
836          co->messageType = type;
837          G_SSR_EPOCH_TIME_CHECK(co->EpochTime[s], co->NumberOfSat[s])
838          G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
839          G_MULTIPLE_MESSAGE_INDICATOR(mmi)
840          G_SSR_IOD(co->SSRIOD)
841          G_SSR_PROVIDER_ID(co->SSRProviderID)
842          G_SSR_SOLUTION_ID(co->SSRSolutionID)
843          G_NO_OF_SATELLITES(nums)
844          co->Supplied[COBOFS_HR] |= 1;
845#ifdef DEBUG
846          fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d\n",
847                          co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
848                                  co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
849#endif
850          for (i = 0; i < nums; ++i) {
851            G_GNSS_SATELLITE_ID(id)
852            for (pos = satoffset[s];
853                pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
854                ++pos)
855              ;
856            if (pos >= satoffset[s + 1])
857              return GCOBR_DATAMISMATCH;
858            else if (pos == co->NumberOfSat[s] + satoffset[s])
859              ++co->NumberOfSat[s];
860            co->Sat[pos].ID = id;
861            G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
862#ifdef DEBUG
863            fprintf(stderr, "id %2d hrClock %8.3f \n",
864                    co->Sat[pos].ID, co->Sat[pos].hrclock);
865#endif
866          }
867          break;
868        case COBOFS_CBIAS:
869          if (!b)
870            return GCOBR_NOCODEBIASPARAMETER;
871          b->messageType = type;
872          G_SSR_EPOCH_TIME_CHECK(b->EpochTime[s], b->NumberOfSat[s])
873          G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
874          G_MULTIPLE_MESSAGE_INDICATOR(mmi)
875          G_SSR_IOD(b->SSRIOD)
876          G_SSR_PROVIDER_ID(b->SSRProviderID)
877          G_SSR_SOLUTION_ID(b->SSRSolutionID)
878          G_NO_OF_SATELLITES(nums)
879#ifdef DEBUG
880          fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d\n",
881                          b->EpochTime[s], b->UpdateInterval,mmi,b->NumberOfSat[s],nums,
882                                  b->SSRIOD, b->SSRProviderID, b->SSRSolutionID);
883#endif
884          for (i = 0; i < nums; ++i) {
885            G_GNSS_SATELLITE_ID(id)
886            for (pos = satoffset[s];
887                pos < satoffset[s] + b->NumberOfSat[s] && b->Sat[pos].ID != id;
888                ++pos)
889              ;
890            if (pos >= satoffset[s + 1])
891              return GCOBR_DATAMISMATCH;
892            else if (pos == b->NumberOfSat[s] + satoffset[s])
893              ++b->NumberOfSat[s];
894            b->Sat[pos].ID = id;
895            G_NO_OF_BIASES(b->Sat[pos].NumberOfCodeBiases)
896#ifdef DEBUG
897            fprintf(stderr, "id %2d #%d ",
898                    b->Sat[pos].ID, b->Sat[pos].NumberOfCodeBiases);
899#endif
900            for (j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j) {
901              G_GNSS_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
902              G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
903#ifdef DEBUG
904            fprintf(stderr, "t%02d b %8.2f ",
905                    b->Sat[pos].Biases[j].Type, b->Sat[pos].Biases[j].Bias);
906#endif
907            }
908#ifdef DEBUG
909            fprintf(stderr, "\n");
910#endif
911          }
912          break;
913        case COBOFS_PBIAS:
914            if (!pb)
915              return GCOBR_NOPHASEBIASPARAMETER;
916            pb->messageType = type;
917            G_SSR_EPOCH_TIME_CHECK(pb->EpochTime[s], pb->NumberOfSat[s])
918            G_SSR_UPDATE_INTERVAL(pb->UpdateInterval)
919            G_MULTIPLE_MESSAGE_INDICATOR(mmi)
920            G_SSR_IOD(pb->SSRIOD)
921            G_SSR_PROVIDER_ID(pb->SSRProviderID)
922            G_SSR_SOLUTION_ID(pb->SSRSolutionID)
923            G_DISPERSIVE_BIAS_INDICATOR(pb->DispersiveBiasConsistencyIndicator)
924            G_MW_CONSISTENCY_INDICATOR(pb->MWConsistencyIndicator)
925            G_NO_OF_SATELLITES(nums)
926#ifdef DEBUG
927          fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d dispInd %d mwInd %d\n",
928                          pb->EpochTime[s], pb->UpdateInterval,mmi,pb->NumberOfSat[s],nums,
929                                  pb->SSRIOD, pb->SSRProviderID, pb->SSRSolutionID,
930                                  pb->DispersiveBiasConsistencyIndicator, pb->MWConsistencyIndicator);
931#endif
932            for (i = 0; i < nums; ++i) {
933              G_GNSS_SATELLITE_ID(id)
934              for (pos = satoffset[s];
935                  pos < satoffset[s] + pb->NumberOfSat[s] && pb->Sat[pos].ID != id;
936                  ++pos)
937                ;
938              if (pos >= satoffset[s + 1])
939                return GCOBR_DATAMISMATCH;
940              else if (pos == pb->NumberOfSat[s] + satoffset[s])
941                ++pb->NumberOfSat[s];
942              pb->Sat[pos].ID = id;
943              G_NO_OF_BIASES(pb->Sat[pos].NumberOfPhaseBiases)
944              G_YAW_ANGLE(pb->Sat[pos].YawAngle)
945              G_YAW_RATE(pb->Sat[pos].YawRate)
946#ifdef DEBUG
947            fprintf(stderr, "id %2d #%d y %10.6f yr %10.6f ",
948                    pb->Sat[pos].ID, pb->Sat[pos].NumberOfPhaseBiases,
949                                        pb->Sat[pos].YawAngle/MPI, pb->Sat[pos].YawRate/MPI);
950#endif
951              for (j = 0; j < pb->Sat[pos].NumberOfPhaseBiases; ++j) {
952                G_GNSS_SIGNAL_IDENTIFIER(pb->Sat[pos].Biases[j].Type)
953                G_INTEGER_INDICATOR(pb->Sat[pos].Biases[j].SignalIntegerIndicator)
954                G_WIDE_LANE_INDICATOR(pb->Sat[pos].Biases[j].SignalsWideLaneIntegerIndicator)
955                G_DISCONTINUITY_COUNTER(pb->Sat[pos].Biases[j].SignalDiscontinuityCounter)
956                G_PHASE_BIAS(pb->Sat[pos].Biases[j].Bias)
957#ifdef DEBUG
958            fprintf(stderr, "t%02d int %d wl %d disc %d b %8.4f ",
959                    pb->Sat[pos].Biases[j].Type,
960                                        pb->Sat[pos].Biases[j].SignalIntegerIndicator,
961                                        pb->Sat[pos].Biases[j].SignalsWideLaneIntegerIndicator,
962                                        pb->Sat[pos].Biases[j].SignalDiscontinuityCounter,
963                                        pb->Sat[pos].Biases[j].Bias);
964#endif
965              }
966#ifdef DEBUG
967            fprintf(stderr, "\n");
968#endif
969            }
970          break;
971        default:
972          continue;
973      }
974#ifdef DEBUG
975      for(type = 0; type < (unsigned int)size && (unsigned char)buffer[type] != 0xD3; ++type)
976      numbits += 8;
977      fprintf(stderr, "numbits left %d\n",numbits);
978#endif
979      return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
980    }
981  }
982  return GCOBR_UNKNOWNTYPE;
983}
984#endif /* NODECODE */
985
986
987
Note: See TracBrowser for help on using the repository browser.