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

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

minor changes
  • Property svn:executable set to *
  • Property svn:keywords set to Id
File size: 49.6 KB
Line 
1/* Programheader
2
3 Name: clock_orbit_rtcm.c
4 Project: RTCM3
5 Version: $Id: clock_orbit_rtcm.c 8979 2020-07-18 21:04:38Z 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_RTCM_MESSAGE_NUMBER(a) GETBITS(a, 12) /* DF002 */
697#define G_GPS_SATELLITE_ID(a) GETBITS(a, 6) /* DF068 */
698#define G_QZSS_SATELLITE_ID(a) GETBITS(a, 4) /* DF249 */
699#define G_GLONASS_SATELLITE_ID(a) GETBITS(a, 5)
700
701#define G_GPS_IODE(a) GETBITS(a, 8) /* DF071 */
702#define G_GLONASS_IOD(a) GETBITS(a, 8) /* DF237 */
703#define G_GALILEO_IOD(a) GETBITS(a, 10) /* DF459 */
704#define G_SBAS_T0MOD(a) GETBITSFACTOR(a, 9, 16) /* DF468 */
705#define G_SBAS_IODCRC(a) GETBITS(a, 24) /* DF469 */
706#define G_BDS_TOEMOD(a) GETBITSFACTOR(a, 10, 8) /* DF470 */
707#define G_BDS_IOD(a) GETBITS(a, 8) /* DF471 */
708
709/* defined values */
710#define G_DELTA_RADIAL(a) GETFLOATSIGN(a, 22, 1/10000.0)
711#define G_DELTA_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0)
712#define G_DELTA_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0)
713#define G_DELTA_DOT_RADIAL(a) GETFLOATSIGN(a, 21, 1/1000000.0)
714#define G_DELTA_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0)
715#define G_DELTA_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0)
716
717#define G_SATELLITE_REFERENCE_DATUM(a) GETBITS(a, 1)
718#define G_DELTA_CLOCK_C0(a) GETFLOATSIGN(a, 22, 1/10000.0)
719#define G_DELTA_CLOCK_C1(a) GETFLOATSIGN(a, 21, 1/1000000.0)
720#define G_DELTA_CLOCK_C2(a) GETFLOATSIGN(a, 27, 1/50000000.0)
721#define G_NO_OF_CODE_BIASES(a) GETBITS(a, 5)
722#define G_NO_OF_PHASE_BIASES(a) GETBITS(a, 5)
723#define G_SIGNAL_IDENTIFIER(a) GETBITS(a, 5)
724#define G_CODE_BIAS(a) GETFLOATSIGN(a, 14, 1/100.0)
725#define G_YAW_ANGLE(a) GETFLOAT(a, 9, MPI/256.0)
726#define G_YAW_RATE(a) GETFLOATSIGN(a, 8, MPI/8192.0)
727#define G_PHASE_BIAS(a) GETFLOATSIGN(a, 20, 1/10000.0)
728
729#define G_GPS_EPOCH_TIME(a, b) {unsigned int temp; GETBITS(temp, 20) \
730 if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
731#define G_GLONASS_EPOCH_TIME(a, b) {unsigned int temp; GETBITS(temp, 17) \
732 if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
733#define G_EPOCH_TIME(a) GETBITS(a, 20)
734#define G_NO_OF_SATELLITES(a) GETBITS(a, 6)
735#define G_MULTIPLE_MESSAGE_INDICATOR(a) GETBITS(a, 1)
736#define G_DISPERSIVE_BIAS_INDICATOR(a) GETBITS(a, 1)
737#define G_MW_CONSISTENCY_INDICATOR(a) GETBITS(a, 1)
738#define G_INTEGER_INDICATOR(a) GETBITS(a, 1)
739#define G_WIDE_LANE_INDICATOR(a) GETBITS(a, 2)
740#define G_DISCONTINUITY_COUNTER(a) GETBITS(a, 4)
741#define G_SSR_URA(a) {int temp; GETBITS(temp, 6) \
742 (a) = URAToValue(temp);}
743#define G_HR_CLOCK_CORRECTION(a) GETFLOATSIGN(a, 22, 1/10000.0)
744#define G_SSR_UPDATE_INTERVAL(a) GETBITS(a, 4)
745
746#define G_SSR_IOD(a) GETBITS(a, 4)
747#define G_SSR_PROVIDER_ID(a) GETBITS(a, 16)
748#define G_SSR_SOLUTION_ID(a) GETBITS(a, 4)
749
750#define G_NO_IONO_LAYERS(a) {unsigned int temp; GETBITS(temp, 2) a = temp+1; }
751#define G_VTEC_QUALITY_INDICATOR(a) GETFLOAT(a, 9, 1/20.0)
752#define G_IONO_COEFF(a) GETFLOATSIGN(a, 16,1/200.0)
753#define G_IONO_DEGREE(a) {unsigned int temp; GETBITS(temp, 4) a = temp+1; }
754#define G_IONO_ORDER(a) {unsigned int temp; GETBITS(temp, 4) a = temp+1; }
755#define G_IONO_HEIGHT(a) GETFLOAT(a, 8 , 10000.0)
756
757enum GCOB_RETURN GetSSR(struct ClockOrbit *co, struct CodeBias *b,struct VTEC *v,
758 struct PhaseBias *pb, const char *buffer, size_t size, int *bytesused) {
759 int mmi = 0, h, rs;
760 unsigned int type, pos, i, j, s, nums, id;
761 size_t sizeofrtcmblock;
762 const char *blockstart = buffer;
763 DECODESTART
764
765 if (size < 7)
766 return GCOBR_SHORTBUFFER;
767
768#ifdef BNC_DEBUG
769 fprintf(stderr, "GetSSR START: size %d, numbits %d\n",(int)size, numbits);
770#endif
771
772 G_HEADER(h)
773 G_RESERVEDH(rs)
774 G_SIZE(sizeofrtcmblock);
775
776 if ((unsigned char) h != 0xD3 || rs)
777 return GCOBR_UNKNOWNDATA;
778 if (size < sizeofrtcmblock + 3) /* 3 header bytes already removed */
779 return GCOBR_MESSAGEEXCEEDSBUFFER;
780 if (CRC24(sizeofrtcmblock + 3, (const unsigned char *) blockstart) !=
781 (uint32_t) ((((unsigned char) buffer[sizeofrtcmblock]) << 16) |
782 (((unsigned char) buffer[sizeofrtcmblock + 1]) << 8) |
783 (((unsigned char) buffer[sizeofrtcmblock + 2]))))
784 return GCOBR_CRCMISMATCH;
785 size = sizeofrtcmblock; /* reduce size, so overflows are detected */
786
787 G_RTCM_MESSAGE_NUMBER(type)
788#ifdef BNC_DEBUG
789 fprintf(stderr, "type %d size %d\n",type,(int)sizeofrtcmblock);
790#endif
791 if (bytesused)
792 *bytesused = sizeofrtcmblock + 6;
793 if (type == VTEC_BASE) {
794 unsigned int l, o, d;
795 if (!v)
796 return GCOBR_NOVTECPARAMETER;
797 memset(v, 0, sizeof(*v));
798 G_EPOCH_TIME(v->EpochTime)
799 G_SSR_UPDATE_INTERVAL(v->UpdateInterval)
800 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
801 G_SSR_IOD(v->SSRIOD)
802 G_SSR_PROVIDER_ID(v->SSRProviderID)
803 G_SSR_SOLUTION_ID(v->SSRSolutionID)
804 G_VTEC_QUALITY_INDICATOR(v->Quality)
805 G_NO_IONO_LAYERS(v->NumLayers)
806#ifdef BNC_DEBUG
807 fprintf(stderr, "epochTime %d ui %d mmi %d ssrIod %d providerId %d solId %d vtecQ %8.3f numLay %d \n",
808 v->EpochTime, v->UpdateInterval, mmi,
809 v->SSRIOD, v->SSRProviderID, v->SSRSolutionID, v->Quality, v->NumLayers);
810#endif
811 for (l = 0; l < v->NumLayers; ++l) {
812 G_IONO_HEIGHT(v->Layers[l].Height)
813 G_IONO_DEGREE(v->Layers[l].Degree)
814 G_IONO_ORDER(v->Layers[l].Order)
815#ifdef BNC_DEBUG
816 fprintf(stderr, "h %8.3f deg %d ord %d \n",
817 v->Layers[l].Height, v->Layers[l].Degree, v->Layers[l].Order);
818#endif
819 for (o = 0; o <= v->Layers[l].Order; ++o) {
820 for (d = o; d <= v->Layers[l].Degree; ++d) {
821 G_IONO_COEFF(v->Layers[l].Cosinus[d][o])
822#ifdef BNC_DEBUG
823 fprintf(stderr, "C[%02d][%02d] %8.3f \n",
824 d, o, v->Layers[l].Cosinus[d][o]);
825#endif
826 }
827 }
828 for (o = 1; o <= v->Layers[l].Order; ++o) {
829 for (d = o; d <= v->Layers[l].Degree; ++d) {
830 G_IONO_COEFF(v->Layers[l].Sinus[d][o])
831#ifdef BNC_DEBUG
832 fprintf(stderr, "S[%02d][%02d] %8.3f \n",
833 d, o, v->Layers[l].Sinus[d][o]);
834#endif
835 }
836 }
837 }
838#ifdef BNC_DEBUG
839 for(type = 0; type < (unsigned int)size && (unsigned char)buffer[type] != 0xD3; ++type)
840 numbits += 8;
841 fprintf(stderr, "numbits left %d\n",numbits);
842#endif
843 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
844 }
845 for (s = CLOCKORBIT_SATNUM; s-- > 0;) {
846 if (type == PBTYPE_BASE + s) {
847 if (!pb)
848 return GCOBR_NOPHASEBIASPARAMETER;
849 pb->messageType = type;
850 switch (s) {
851 case CLOCKORBIT_SATGPS:
852 case CLOCKORBIT_SATGALILEO:
853 case CLOCKORBIT_SATQZSS:
854 case CLOCKORBIT_SATSBAS:
855 case CLOCKORBIT_SATBDS:
856 G_GPS_EPOCH_TIME(pb->EpochTime[s], pb->NumberOfSat[s])
857 break;
858 case CLOCKORBIT_SATGLONASS:
859 G_GLONASS_EPOCH_TIME(pb->EpochTime[s], pb->NumberOfSat[s])
860 break;
861 }
862 G_SSR_UPDATE_INTERVAL(pb->UpdateInterval)
863 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
864 G_SSR_IOD(pb->SSRIOD)
865 G_SSR_PROVIDER_ID(pb->SSRProviderID)
866 G_SSR_SOLUTION_ID(pb->SSRSolutionID)
867 G_DISPERSIVE_BIAS_INDICATOR(pb->DispersiveBiasConsistencyIndicator)
868 G_MW_CONSISTENCY_INDICATOR(pb->MWConsistencyIndicator)
869 G_NO_OF_SATELLITES(nums)
870#ifdef BNC_DEBUG
871 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d dispInd %d mwInd %d\n",
872 pb->EpochTime[s], pb->UpdateInterval,mmi,pb->NumberOfSat[s],nums,
873 pb->SSRIOD, pb->SSRProviderID, pb->SSRSolutionID,
874 pb->DispersiveBiasConsistencyIndicator, pb->MWConsistencyIndicator);
875#endif
876 for (i = 0; i < nums; ++i) {
877 switch (s) {
878 case CLOCKORBIT_SATGPS:
879 case CLOCKORBIT_SATGALILEO:
880 case CLOCKORBIT_SATSBAS:
881 case CLOCKORBIT_SATBDS:
882 G_GPS_SATELLITE_ID(id)
883 break;
884 case CLOCKORBIT_SATQZSS:
885 G_QZSS_SATELLITE_ID(id)
886 break;
887 case CLOCKORBIT_SATGLONASS:
888 G_GLONASS_SATELLITE_ID(id)
889 break;
890 }
891 for (pos = satoffset[s];
892 pos < satoffset[s] + pb->NumberOfSat[s] && pb->Sat[pos].ID != id;
893 ++pos)
894 ;
895 if (pos >= satoffset[s + 1])
896 return GCOBR_DATAMISMATCH;
897 else if (pos == pb->NumberOfSat[s] + satoffset[s])
898 ++pb->NumberOfSat[s];
899 pb->Sat[pos].ID = id;
900 G_NO_OF_PHASE_BIASES(pb->Sat[pos].NumberOfPhaseBiases)
901 G_YAW_ANGLE(pb->Sat[pos].YawAngle)
902 G_YAW_RATE(pb->Sat[pos].YawRate)
903#ifdef BNC_DEBUG
904 fprintf(stderr, "id %2d #%d y %10.6f yr %10.6f ",
905 pb->Sat[pos].ID, pb->Sat[pos].NumberOfPhaseBiases,
906 pb->Sat[pos].YawAngle/MPI, pb->Sat[pos].YawRate/MPI);
907#endif
908 for (j = 0; j < pb->Sat[pos].NumberOfPhaseBiases; ++j) {
909 G_SIGNAL_IDENTIFIER(pb->Sat[pos].Biases[j].Type)
910 G_INTEGER_INDICATOR(pb->Sat[pos].Biases[j].SignalIntegerIndicator)
911 G_WIDE_LANE_INDICATOR(
912 pb->Sat[pos].Biases[j].SignalsWideLaneIntegerIndicator)
913 G_DISCONTINUITY_COUNTER(
914 pb->Sat[pos].Biases[j].SignalDiscontinuityCounter)
915 G_PHASE_BIAS(pb->Sat[pos].Biases[j].Bias)
916#ifdef BNC_DEBUG
917 fprintf(stderr, "t%02d int %d wl %d disc %d b %8.4f ",
918 pb->Sat[pos].Biases[j].Type,
919 pb->Sat[pos].Biases[j].SignalIntegerIndicator,
920 pb->Sat[pos].Biases[j].SignalsWideLaneIntegerIndicator,
921 pb->Sat[pos].Biases[j].SignalDiscontinuityCounter,
922 pb->Sat[pos].Biases[j].Bias);
923#endif
924 }
925#ifdef BNC_DEBUG
926 fprintf(stderr, "\n");
927#endif
928 }
929#ifdef BNC_DEBUG
930 for(type = 0; type < (unsigned int)size && (unsigned char)buffer[type] != 0xD3; ++type)
931 numbits += 8;
932 fprintf(stderr, "numbits left %d\n",numbits);
933#endif
934 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
935 }
936 else if (type >= corbase[s]) {
937 switch (type - corbase[s]) {
938 case COBOFS_ORBIT:
939 if (!co)
940 return GCOBR_NOCLOCKORBITPARAMETER;
941 co->messageType = type;
942 switch (s) {
943 case CLOCKORBIT_SATGPS:
944 case CLOCKORBIT_SATGALILEO:
945 case CLOCKORBIT_SATQZSS:
946 case CLOCKORBIT_SATSBAS:
947 case CLOCKORBIT_SATBDS:
948 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
949 break;
950 case CLOCKORBIT_SATGLONASS:
951 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
952 break;
953 }
954 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
955 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
956 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
957 G_SSR_IOD(co->SSRIOD)
958 G_SSR_PROVIDER_ID(co->SSRProviderID)
959 G_SSR_SOLUTION_ID(co->SSRSolutionID)
960 G_NO_OF_SATELLITES(nums)
961 co->Supplied[COBOFS_ORBIT] |= 1;
962#ifdef BNC_DEBUG
963 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d rd %d ssrIod %d providerId %d solId %d\n",
964 co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
965 co->SatRefDatum, co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
966#endif
967 for (i = 0; i < nums; ++i) {
968 switch (s) {
969 case CLOCKORBIT_SATGPS:
970 case CLOCKORBIT_SATGALILEO:
971 case CLOCKORBIT_SATSBAS:
972 case CLOCKORBIT_SATBDS:
973 G_GPS_SATELLITE_ID(id)
974 break;
975 case CLOCKORBIT_SATQZSS:
976 G_QZSS_SATELLITE_ID(id)
977 break;
978 case CLOCKORBIT_SATGLONASS:
979 G_GLONASS_SATELLITE_ID(id)
980 break;
981 }
982 for (pos = satoffset[s];
983 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
984 ++pos)
985 ;
986 if (pos >= satoffset[s + 1])
987 return GCOBR_DATAMISMATCH;
988 else if (pos == co->NumberOfSat[s] + satoffset[s])
989 ++co->NumberOfSat[s];
990 co->Sat[pos].ID = id;
991
992 switch (s) {
993 case CLOCKORBIT_SATGPS:
994 case CLOCKORBIT_SATQZSS:
995 G_GPS_IODE(co->Sat[pos].IOD)
996 break;
997 case CLOCKORBIT_SATGLONASS:
998 G_GLONASS_IOD(co->Sat[pos].IOD)
999 break;
1000 case CLOCKORBIT_SATGALILEO:
1001 G_GALILEO_IOD(co->Sat[pos].IOD)
1002 break;
1003 case CLOCKORBIT_SATSBAS:
1004 G_SBAS_T0MOD(co->Sat[pos].toe)
1005 G_SBAS_IODCRC(co->Sat[pos].IOD)
1006 break;
1007 case CLOCKORBIT_SATBDS:
1008 G_BDS_TOEMOD(co->Sat[pos].toe)
1009 G_BDS_IOD(co->Sat[pos].IOD)
1010 break;
1011 }
1012 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
1013 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
1014 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
1015 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
1016 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
1017 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
1018#ifdef BNC_DEBUG
1019 fprintf(stderr, "id %2d iod %3d dr %8.4f da %8.4f dc %8.4f dr %8.3f da %8.3f dc %8.3f\n",
1020 co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
1021 co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
1022 co->Sat[pos].Orbit.DotDeltaRadial,
1023 co->Sat[pos].Orbit.DotDeltaAlongTrack,
1024 co->Sat[pos].Orbit.DotDeltaCrossTrack);
1025#endif
1026 }
1027 break;
1028 case COBOFS_CLOCK:
1029 if (!co)
1030 return GCOBR_NOCLOCKORBITPARAMETER;
1031 co->messageType = type;
1032 switch (s) {
1033 case CLOCKORBIT_SATGPS:
1034 case CLOCKORBIT_SATGALILEO:
1035 case CLOCKORBIT_SATQZSS:
1036 case CLOCKORBIT_SATSBAS:
1037 case CLOCKORBIT_SATBDS:
1038 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1039 break;
1040 case CLOCKORBIT_SATGLONASS:
1041 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1042 break;
1043 }
1044 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1045 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1046 G_SSR_IOD(co->SSRIOD)
1047 G_SSR_PROVIDER_ID(co->SSRProviderID)
1048 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1049 G_NO_OF_SATELLITES(nums)
1050 co->Supplied[COBOFS_CLOCK] |= 1;
1051#ifdef BNC_DEBUG
1052 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d\n",
1053 co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
1054 co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
1055#endif
1056 for (i = 0; i < nums; ++i) {
1057 switch (s) {
1058 case CLOCKORBIT_SATGPS:
1059 case CLOCKORBIT_SATGALILEO:
1060 case CLOCKORBIT_SATSBAS:
1061 case CLOCKORBIT_SATBDS:
1062 G_GPS_SATELLITE_ID(id)
1063 break;
1064 case CLOCKORBIT_SATQZSS:
1065 G_QZSS_SATELLITE_ID(id)
1066 break;
1067 case CLOCKORBIT_SATGLONASS:
1068 G_GLONASS_SATELLITE_ID(id)
1069 break;
1070 }
1071 for (pos = satoffset[s];
1072 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
1073 ++pos)
1074 ;
1075 if (pos >= satoffset[s + 1])
1076 return GCOBR_DATAMISMATCH;
1077 else if (pos == co->NumberOfSat[s] + satoffset[s])
1078 ++co->NumberOfSat[s];
1079 co->Sat[pos].ID = id;
1080
1081 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
1082 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
1083 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
1084#ifdef BNC_DEBUG
1085 fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
1086 co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
1087 co->Sat[pos].Clock.DeltaA2);
1088#endif
1089 }
1090 break;
1091 case COBOFS_COMBINED:
1092 if (!co)
1093 return GCOBR_NOCLOCKORBITPARAMETER;
1094 co->messageType = type;
1095 switch (s) {
1096 case CLOCKORBIT_SATGPS:
1097 case CLOCKORBIT_SATGALILEO:
1098 case CLOCKORBIT_SATQZSS:
1099 case CLOCKORBIT_SATSBAS:
1100 case CLOCKORBIT_SATBDS:
1101 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1102 break;
1103 case CLOCKORBIT_SATGLONASS:
1104 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1105 break;
1106 }
1107 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1108 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1109 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
1110 G_SSR_IOD(co->SSRIOD)
1111 G_SSR_PROVIDER_ID(co->SSRProviderID)
1112 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1113 G_NO_OF_SATELLITES(nums)
1114 co->Supplied[COBOFS_ORBIT] |= 1;
1115 co->Supplied[COBOFS_CLOCK] |= 1;
1116#ifdef BNC_DEBUG
1117 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d rd %d ssrIod %d providerId %d solId %d\n",
1118 co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
1119 co->SatRefDatum, co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
1120#endif
1121 for (i = 0; i < nums; ++i) {
1122 switch (s) {
1123 case CLOCKORBIT_SATGPS:
1124 case CLOCKORBIT_SATGALILEO:
1125 case CLOCKORBIT_SATSBAS:
1126 case CLOCKORBIT_SATBDS:
1127 G_GPS_SATELLITE_ID(id)
1128 break;
1129 case CLOCKORBIT_SATQZSS:
1130 G_QZSS_SATELLITE_ID(id)
1131 break;
1132 case CLOCKORBIT_SATGLONASS:
1133 G_GLONASS_SATELLITE_ID(id)
1134 break;
1135 }
1136 for (pos = satoffset[s];
1137 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
1138 ++pos)
1139 ;
1140 if (pos >= satoffset[s + 1])
1141 return GCOBR_DATAMISMATCH;
1142 else if (pos == co->NumberOfSat[s] + satoffset[s])
1143 ++co->NumberOfSat[s];
1144 co->Sat[pos].ID = id;
1145
1146 switch (s) {
1147 case CLOCKORBIT_SATGPS:
1148 case CLOCKORBIT_SATQZSS:
1149 G_GPS_IODE(co->Sat[pos].IOD)
1150 break;
1151 case CLOCKORBIT_SATGLONASS:
1152 G_GLONASS_IOD(co->Sat[pos].IOD)
1153 break;
1154 case CLOCKORBIT_SATGALILEO:
1155 G_GALILEO_IOD(co->Sat[pos].IOD)
1156 break;
1157 case CLOCKORBIT_SATSBAS:
1158 G_SBAS_T0MOD(co->Sat[pos].toe)
1159 G_SBAS_IODCRC(co->Sat[pos].IOD)
1160 break;
1161 case CLOCKORBIT_SATBDS:
1162 G_BDS_TOEMOD(co->Sat[pos].toe)
1163 G_BDS_IOD(co->Sat[pos].IOD)
1164 break;
1165 }
1166 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
1167 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
1168 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
1169 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
1170 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
1171 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
1172 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
1173 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
1174 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
1175#ifdef BNC_DEBUG
1176 fprintf(stderr, "id %2d iod %3d dr %10.6f da %10.6f dc %10.6f dr %10.6f da %10.6f dc %10.6f c0 %10.6f c1 %10.6f c2 %10.6f\n",
1177 co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
1178 co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
1179 co->Sat[pos].Orbit.DotDeltaRadial, co->Sat[pos].Orbit.DotDeltaAlongTrack,
1180 co->Sat[pos].Orbit.DotDeltaCrossTrack,
1181 co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1, co->Sat[pos].Clock.DeltaA2);
1182#endif
1183 }
1184 break;
1185 case COBOFS_URA:
1186 if (!co)
1187 return GCOBR_NOCLOCKORBITPARAMETER;
1188 co->messageType = type;
1189 switch (s) {
1190 case CLOCKORBIT_SATGPS:
1191 case CLOCKORBIT_SATGALILEO:
1192 case CLOCKORBIT_SATQZSS:
1193 case CLOCKORBIT_SATSBAS:
1194 case CLOCKORBIT_SATBDS:
1195 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1196 break;
1197 case CLOCKORBIT_SATGLONASS:
1198 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1199 break;
1200 }
1201 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1202 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1203 G_SSR_IOD(co->SSRIOD)
1204 G_SSR_PROVIDER_ID(co->SSRProviderID)
1205 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1206 G_NO_OF_SATELLITES(nums)
1207 co->Supplied[COBOFS_URA] |= 1;
1208#ifdef BNC_DEBUG
1209 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d\n",
1210 co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
1211 co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
1212#endif
1213 for (i = 0; i < nums; ++i) {
1214 switch (s) {
1215 case CLOCKORBIT_SATGPS:
1216 case CLOCKORBIT_SATGALILEO:
1217 case CLOCKORBIT_SATSBAS:
1218 case CLOCKORBIT_SATBDS:
1219 G_GPS_SATELLITE_ID(id)
1220 break;
1221 case CLOCKORBIT_SATQZSS:
1222 G_QZSS_SATELLITE_ID(id)
1223 break;
1224 case CLOCKORBIT_SATGLONASS:
1225 G_GLONASS_SATELLITE_ID(id)
1226 break;
1227 }
1228 for (pos = satoffset[s];
1229 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
1230 ++pos)
1231 ;
1232 if (pos >= satoffset[s + 1])
1233 return GCOBR_DATAMISMATCH;
1234 else if (pos == co->NumberOfSat[s] + satoffset[s])
1235 ++co->NumberOfSat[s];
1236 co->Sat[pos].ID = id;
1237 G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
1238#ifdef BNC_DEBUG
1239 fprintf(stderr, "id %2d ura %8.3f \n",
1240 co->Sat[pos].ID, co->Sat[pos].UserRangeAccuracy);
1241#endif
1242 }
1243 break;
1244 case COBOFS_HR:
1245 if (!co)
1246 return GCOBR_NOCLOCKORBITPARAMETER;
1247 co->messageType = type;
1248 switch (s) {
1249 case CLOCKORBIT_SATGPS:
1250 case CLOCKORBIT_SATGALILEO:
1251 case CLOCKORBIT_SATQZSS:
1252 case CLOCKORBIT_SATSBAS:
1253 case CLOCKORBIT_SATBDS:
1254 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1255 break;
1256 case CLOCKORBIT_SATGLONASS:
1257 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1258 break;
1259 }
1260 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1261 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1262 G_SSR_IOD(co->SSRIOD)
1263 G_SSR_PROVIDER_ID(co->SSRProviderID)
1264 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1265 G_NO_OF_SATELLITES(nums)
1266 co->Supplied[COBOFS_HR] |= 1;
1267#ifdef BNC_DEBUG
1268 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d\n",
1269 co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
1270 co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
1271#endif
1272 for (i = 0; i < nums; ++i) {
1273 switch (s) {
1274 case CLOCKORBIT_SATGPS:
1275 case CLOCKORBIT_SATGALILEO:
1276 case CLOCKORBIT_SATSBAS:
1277 case CLOCKORBIT_SATBDS:
1278 G_GPS_SATELLITE_ID(id)
1279 break;
1280 case CLOCKORBIT_SATQZSS:
1281 G_QZSS_SATELLITE_ID(id)
1282 break;
1283 case CLOCKORBIT_SATGLONASS:
1284 G_GLONASS_SATELLITE_ID(id)
1285 break;
1286 }
1287 for (pos = satoffset[s];
1288 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
1289 ++pos)
1290 ;
1291 if (pos >= satoffset[s + 1])
1292 return GCOBR_DATAMISMATCH;
1293 else if (pos == co->NumberOfSat[s] + satoffset[s])
1294 ++co->NumberOfSat[s];
1295 co->Sat[pos].ID = id;
1296 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
1297#ifdef BNC_DEBUG
1298 fprintf(stderr, "id %2d hrClock %8.3f \n",
1299 co->Sat[pos].ID, co->Sat[pos].hrclock);
1300#endif
1301 }
1302 break;
1303 case COBOFS_CBIAS:
1304 if (!b)
1305 return GCOBR_NOCODEBIASPARAMETER;
1306 b->messageType = type;
1307 switch (s) {
1308 case CLOCKORBIT_SATGPS:
1309 case CLOCKORBIT_SATGALILEO:
1310 case CLOCKORBIT_SATQZSS:
1311 case CLOCKORBIT_SATSBAS:
1312 case CLOCKORBIT_SATBDS:
1313 G_GPS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s])
1314 break;
1315 case CLOCKORBIT_SATGLONASS:
1316 G_GLONASS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s])
1317 break;
1318 }
1319 G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
1320 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1321 G_SSR_IOD(b->SSRIOD)
1322 G_SSR_PROVIDER_ID(b->SSRProviderID)
1323 G_SSR_SOLUTION_ID(b->SSRSolutionID)
1324 G_NO_OF_SATELLITES(nums)
1325#ifdef BNC_DEBUG
1326 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d\n",
1327 b->EpochTime[s], b->UpdateInterval,mmi,b->NumberOfSat[s],nums,
1328 b->SSRIOD, b->SSRProviderID, b->SSRSolutionID);
1329#endif
1330 for (i = 0; i < nums; ++i) {
1331 switch (s) {
1332 case CLOCKORBIT_SATGPS:
1333 case CLOCKORBIT_SATGALILEO:
1334 case CLOCKORBIT_SATSBAS:
1335 case CLOCKORBIT_SATBDS:
1336 G_GPS_SATELLITE_ID(id)
1337 break;
1338 case CLOCKORBIT_SATQZSS:
1339 G_QZSS_SATELLITE_ID(id)
1340 break;
1341 case CLOCKORBIT_SATGLONASS:
1342 G_GLONASS_SATELLITE_ID(id)
1343 break;
1344 }
1345 for (pos = satoffset[s];
1346 pos < satoffset[s] + b->NumberOfSat[s] && b->Sat[pos].ID != id;
1347 ++pos)
1348 ;
1349 if (pos >= satoffset[s + 1])
1350 return GCOBR_DATAMISMATCH;
1351 else if (pos == b->NumberOfSat[s] + satoffset[s])
1352 ++b->NumberOfSat[s];
1353 b->Sat[pos].ID = id;
1354 G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases)
1355#ifdef BNC_DEBUG
1356 fprintf(stderr, "id %2d #%d ",
1357 b->Sat[pos].ID, b->Sat[pos].NumberOfCodeBiases);
1358#endif
1359 for (j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j) {
1360 G_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
1361 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
1362#ifdef BNC_DEBUG
1363 fprintf(stderr, "t%02d b %8.2f ",
1364 b->Sat[pos].Biases[j].Type, b->Sat[pos].Biases[j].Bias);
1365#endif
1366 }
1367#ifdef BNC_DEBUG
1368 fprintf(stderr, "\n");
1369#endif
1370 }
1371 break;
1372 default:
1373 continue;
1374 }
1375#ifdef BNC_DEBUG
1376 for(type = 0; type < (unsigned int)size && (unsigned char)buffer[type] != 0xD3; ++type)
1377 numbits += 8;
1378 fprintf(stderr, "numbits left %d\n",numbits);
1379#endif
1380 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
1381 }
1382 }
1383 return GCOBR_UNKNOWNTYPE;
1384}
1385#endif /* NODECODE */
Note: See TracBrowser for help on using the repository browser.