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

Last change on this file since 8997 was 8997, checked in by stuerze, 4 years ago

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