source: ntrip/trunk/BNC/src/RTCM3/clock_and_orbit/clock_orbit_rtcm.c@ 8970

Last change on this file since 8970 was 8970, checked in by stuerze, 15 months ago

intial imprort for igs ssr encoding and decoding

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