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

Last change on this file since 7777 was 7777, checked in by stuerze, 5 years ago

small bugs regarding phase bias encoding fixed

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