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

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

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