Context Navigation

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

Last change on this file since 8970 was 8970, checked in by stuerze, 4 years ago
intial imprort for igs ssr encoding and decoding
Property svn:executable set to ``* Property svn:keywords set to `Id`
File size: 45.6 KB

Line
1	/* Programheader
2
3	Name: clock_orbit_rtcm.c
4	Project: RTCM3
5	Version: $Id: clock_orbit_rtcm.c 8970 2020-07-15 07:24:30Z stuerze $
6	Authors: Dirk Stöcker
7	Description: state space approach: RTCM
8	*/
9
10	#include <math.h>
11	#include <stdio.h>
12	#include <string.h>
13	#ifndef sparc
14	#include <stdint.h>
15	#else
16	#include <sys/types.h>
17	#endif
18	#include "clock_orbit_rtcm.h"
19
20	static 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 ? bc+0.5 : bc-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
148	static 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
159	static 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
170	static 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
179	static 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	size_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
488	size_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
543	size_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
608	size_t MakeVTEC(const struct VTEC v, int moremessagesfollow, char buffer, size_t size) {
609	unsigned int l, o, d;
610
611	STARTDATA
612	INITBLOCK
613	T_RTCM_MESSAGE_NUMBER(VTEC_BASE)
614
615	T_GPS_EPOCH_TIME(v->EpochTime)
616	T_SSR_UPDATE_INTERVAL(v->UpdateInterval)
617	T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
618	T_SSR_IOD(v->SSRIOD)
619	T_SSR_PROVIDER_ID(v->SSRProviderID)
620	T_SSR_SOLUTION_ID(v->SSRSolutionID)
621	T_VTEC_QUALITY_INDICATOR(v->Quality)
622	T_NO_IONO_LAYERS(v->NumLayers)
623	for (l = 0; l < v->NumLayers; ++l) {
624	T_IONO_HEIGHT(v->Layers[l].Height)
625	T_IONO_DEGREE(v->Layers[l].Degree)
626	T_IONO_ORDER(v->Layers[l].Order)
627	for (o = 0; o <= v->Layers[l].Order; ++o) {
628	for (d = o; d <= v->Layers[l].Degree; ++d) {
629	T_IONO_COEFF(v->Layers[l].Cosinus[d][o])
630	}
631	}
632	for (o = 1; o <= v->Layers[l].Order; ++o) {
633	for (d = o; d <= v->Layers[l].Degree; ++d) {
634	T_IONO_COEFF(v->Layers[l].Sinus[d][o])
635	}
636	}
637	}
638	ENDBLOCK
639	return ressize;
640	}
641	#endif /* NOENCODE */
642
643	#ifndef NODECODE
644
645	#define DECODESTART \
646	int numbits=0; \
647	uint64_t bitbuffer=0;
648
649	#define LOADBITS(a) { \
650	while((a) > numbits) \
651	{ \
652	if(!size--) return GCOBR_SHORTMESSAGE; \
653	bitbuffer = (bitbuffer<<8)\|((unsigned char)*(buffer++)); \
654	numbits += 8; \
655	} \
656	}
657
658	/* extract bits from data stream
659	b = variable to store result, a = number of bits */
660	#define GETBITS(b, a) { \
661	LOADBITS(a) \
662	b = (bitbuffer<<(64-numbits))>>(64-(a)); \
663	numbits -= (a); \
664	}
665
666	/* extract bits from data stream
667	b = variable to store result, a = number of bits */
668	#define GETBITSFACTOR(b, a, c) { \
669	LOADBITS(a) \
670	b = ((bitbuffer<<(64-numbits))>>(64-(a)))*(c); \
671	numbits -= (a); \
672	}
673
674	/* extract signed floating value from data stream
675	b = variable to store result, a = number of bits */
676	#define GETFLOATSIGN(b, a, c) { \
677	LOADBITS(a) \
678	b = ((double)(((int64_t)(bitbuffer<<(64-numbits)))>>(64-(a))))*(c); \
679	numbits -= (a); \
680	}
681
682	/* extract floating value from data stream
683	b = variable to store result, a = number of bits, c = scale factor */
684	#define GETFLOAT(b, a, c) { \
685	LOADBITS(a) \
686	b = ((double)((bitbuffer<<(sizeof(bitbuffer)8-numbits))>>(sizeof(bitbuffer)8-(a))))*(c); \
687	numbits -= (a); \
688	}
689
690	#define SKIPBITS(b) { LOADBITS(b) numbits -= (b); }
691
692	/* GPS macros also used for other systems when matching! */
693	#define G_HEADER(a) GETBITS(a,8)
694	#define G_RESERVEDH(a) GETBITS(a,6)
695	#define G_SIZE(a) GETBITS(a, 10)
696	#define G_MESSAGE_NUMBER(a) GETBITS(a, 12) /* DF002 */
697	#define G_GPS_SATELLITE_ID(a) GETBITS(a, 6) /* DF068 */
698	#define G_QZSS_SATELLITE_ID(a) GETBITS(a, 4) /* DF249 */
699	#define G_GLONASS_SATELLITE_ID(a) GETBITS(a, 5)
700
701	#define G_GPS_IODE(a) GETBITS(a, 8) /* DF071 */
702	#define G_GLONASS_IOD(a) GETBITS(a, 8) /* DF237 */
703	#define G_GALILEO_IOD(a) GETBITS(a, 10) /* DF459 */
704	#define G_SBAS_T0MOD(a) GETBITSFACTOR(a, 9, 16) /* DF468 */
705	#define G_SBAS_IODCRC(a) GETBITS(a, 24) /* DF469 */
706	#define G_BDS_TOEMOD(a) GETBITSFACTOR(a, 10, 8) /* DF470 */
707	#define G_BDS_IOD(a) GETBITS(a, 8) /* DF471 */
708
709	/* defined values */
710	#define G_DELTA_RADIAL(a) GETFLOATSIGN(a, 22, 1/10000.0)
711	#define G_DELTA_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0)
712	#define G_DELTA_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0)
713	#define G_DELTA_DOT_RADIAL(a) GETFLOATSIGN(a, 21, 1/1000000.0)
714	#define G_DELTA_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0)
715	#define G_DELTA_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0)
716
717	#define G_SATELLITE_REFERENCE_DATUM(a) GETBITS(a, 1)
718	#define G_DELTA_CLOCK_C0(a) GETFLOATSIGN(a, 22, 1/10000.0)
719	#define G_DELTA_CLOCK_C1(a) GETFLOATSIGN(a, 21, 1/1000000.0)
720	#define G_DELTA_CLOCK_C2(a) GETFLOATSIGN(a, 27, 1/50000000.0)
721	#define G_NO_OF_CODE_BIASES(a) GETBITS(a, 5)
722	#define G_NO_OF_PHASE_BIASES(a) GETBITS(a, 5)
723	#define G_SIGNAL_IDENTIFIER(a) GETBITS(a, 5)
724	#define G_CODE_BIAS(a) GETFLOATSIGN(a, 14, 1/100.0)
725	#define G_YAW_ANGLE(a) GETFLOAT(a, 9, MPI/256.0)
726	#define G_YAW_RATE(a) GETFLOATSIGN(a, 8, MPI/8192.0)
727	#define G_PHASE_BIAS(a) GETFLOATSIGN(a, 20, 1/10000.0)
728
729	#define G_GPS_EPOCH_TIME(a, b) {unsigned int temp; GETBITS(temp, 20) \
730	if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
731	#define G_GLONASS_EPOCH_TIME(a, b) {unsigned int temp; GETBITS(temp, 17) \
732	if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
733	#define G_EPOCH_TIME(a) GETBITS(a, 20)
734	#define G_NO_OF_SATELLITES(a) GETBITS(a, 6)
735	#define G_MULTIPLE_MESSAGE_INDICATOR(a) GETBITS(a, 1)
736	#define G_DISPERSIVE_BIAS_INDICATOR(a) GETBITS(a, 1)
737	#define G_MW_CONSISTENCY_INDICATOR(a) GETBITS(a, 1)
738	#define G_INTEGER_INDICATOR(a) GETBITS(a, 1)
739	#define G_WIDE_LANE_INDICATOR(a) GETBITS(a, 2)
740	#define G_DISCONTINUITY_COUNTER(a) GETBITS(a, 4)
741	#define G_SSR_URA(a) {int temp; GETBITS(temp, 6) \
742	(a) = URAToValue(temp);}
743	#define G_HR_CLOCK_CORRECTION(a) GETFLOATSIGN(a, 22, 1/10000.0)
744	#define G_SSR_UPDATE_INTERVAL(a) GETBITS(a, 4)
745
746	#define G_SSR_IOD(a) GETBITS(a, 4)
747	#define G_SSR_PROVIDER_ID(a) GETBITS(a, 16)
748	#define G_SSR_SOLUTION_ID(a) GETBITS(a, 4)
749
750	#define G_NO_IONO_LAYERS(a) {unsigned int temp; GETBITS(temp, 2) a = temp+1; }
751	#define G_VTEC_QUALITY_INDICATOR(a) GETFLOAT(a, 9, 1/20.0)
752	#define G_IONO_COEFF(a) GETFLOATSIGN(a, 16,1/200.0)
753	#define G_IONO_DEGREE(a) {unsigned int temp; GETBITS(temp, 4) a = temp+1; }
754	#define G_IONO_ORDER(a) {unsigned int temp; GETBITS(temp, 4) a = temp+1; }
755	#define G_IONO_HEIGHT(a) GETFLOAT(a, 8 , 10000.0)
756
757	enum GCOB_RETURN GetSSR(struct ClockOrbit co, struct CodeBias b,struct VTEC *v,
758	struct PhaseBias pb, const char buffer, size_t size, int *bytesused) {
759	int mmi = 0, h, rs;
760	unsigned int type, pos, i, j, s, nums, id;
761	size_t sizeofrtcmblock;
762	const char *blockstart = buffer;
763	DECODESTART
764
765	if (size < 7)
766	return GCOBR_SHORTBUFFER;
767
768	#ifdef DEBUG
769	fprintf(stderr, "GetClockOrbitBias START: size %d, numbits %d\n",size, numbits);
770	#endif
771
772	G_HEADER(h)
773	G_RESERVEDH(rs)
774	G_SIZE(sizeofrtcmblock);
775
776	if ((unsigned char) h != 0xD3 \|\| rs)
777	return GCOBR_UNKNOWNDATA;
778	if (size < sizeofrtcmblock + 3) /* 3 header bytes already removed */
779	return GCOBR_MESSAGEEXCEEDSBUFFER;
780	if (CRC24(sizeofrtcmblock + 3, (const unsigned char *) blockstart) !=
781	(uint32_t) ((((unsigned char) buffer[sizeofrtcmblock]) << 16) \|
782	(((unsigned char) buffer[sizeofrtcmblock + 1]) << 8) \|
783	(((unsigned char) buffer[sizeofrtcmblock + 2]))))
784	return GCOBR_CRCMISMATCH;
785	size = sizeofrtcmblock; /* reduce size, so overflows are detected */
786
787	G_MESSAGE_NUMBER(type)
788	#ifdef DEBUG
789	fprintf(stderr, "type %d size %d\n",type,sizeofrtcmblock);
790	#endif
791	if (bytesused)
792	*bytesused = sizeofrtcmblock + 6;
793	if (type == VTEC_BASE) {
794	unsigned int l, o, d;
795	if (!v)
796	return GCOBR_NOVTECPARAMETER;
797	memset(v, 0, sizeof(*v));
798	G_EPOCH_TIME(v->EpochTime)
799	G_SSR_UPDATE_INTERVAL(v->UpdateInterval)
800	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
801	G_SSR_IOD(v->SSRIOD)
802	G_SSR_PROVIDER_ID(v->SSRProviderID)
803	G_SSR_SOLUTION_ID(v->SSRSolutionID)
804	G_VTEC_QUALITY_INDICATOR(v->Quality)
805	G_NO_IONO_LAYERS(v->NumLayers)
806	for (l = 0; l < v->NumLayers; ++l) {
807	G_IONO_HEIGHT(v->Layers[l].Height)
808	G_IONO_DEGREE(v->Layers[l].Degree)
809	G_IONO_ORDER(v->Layers[l].Order)
810	for (o = 0; o <= v->Layers[l].Order; ++o) {
811	for (d = o; d <= v->Layers[l].Degree; ++d) {
812	G_IONO_COEFF(v->Layers[l].Cosinus[d][o])
813	}
814	}
815	for (o = 1; o <= v->Layers[l].Order; ++o) {
816	for (d = o; d <= v->Layers[l].Degree; ++d) {
817	G_IONO_COEFF(v->Layers[l].Sinus[d][o])
818	}
819	}
820	}
821	#ifdef DEBUG
822	for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type)
823	numbits += 8;
824	fprintf(stderr, "numbits left %d\n",numbits);
825	#endif
826	return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
827	}
828	for (s = CLOCKORBIT_SATNUM; s-- > 0;) {
829	if (type == PBTYPE_BASE + s) {
830	if (!pb)
831	return GCOBR_NOPHASEBIASPARAMETER;
832	pb->messageType = type;
833	switch (s) {
834	case CLOCKORBIT_SATGPS:
835	case CLOCKORBIT_SATGALILEO:
836	case CLOCKORBIT_SATQZSS:
837	case CLOCKORBIT_SATSBAS:
838	case CLOCKORBIT_SATBDS:
839	G_GPS_EPOCH_TIME(pb->EpochTime[s], pb->NumberOfSat[s])
840	break;
841	case CLOCKORBIT_SATGLONASS:
842	G_GLONASS_EPOCH_TIME(pb->EpochTime[s], pb->NumberOfSat[s])
843	break;
844	}
845	G_SSR_UPDATE_INTERVAL(pb->UpdateInterval)
846	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
847	G_SSR_IOD(pb->SSRIOD)
848	G_SSR_PROVIDER_ID(pb->SSRProviderID)
849	G_SSR_SOLUTION_ID(pb->SSRSolutionID)
850	G_DISPERSIVE_BIAS_INDICATOR(pb->DispersiveBiasConsistencyIndicator)
851	G_MW_CONSISTENCY_INDICATOR(pb->MWConsistencyIndicator)
852	G_NO_OF_SATELLITES(nums)
853	for (i = 0; i < nums; ++i) {
854	switch (s) {
855	case CLOCKORBIT_SATGPS:
856	case CLOCKORBIT_SATGALILEO:
857	case CLOCKORBIT_SATSBAS:
858	case CLOCKORBIT_SATBDS:
859	G_GPS_SATELLITE_ID(id)
860	break;
861	case CLOCKORBIT_SATQZSS:
862	G_QZSS_SATELLITE_ID(id)
863	break;
864	case CLOCKORBIT_SATGLONASS:
865	G_GLONASS_SATELLITE_ID(id)
866	break;
867	}
868	for (pos = satoffset[s];
869	pos < satoffset[s] + pb->NumberOfSat[s] && pb->Sat[pos].ID != id;
870	++pos)
871	;
872	if (pos >= satoffset[s + 1])
873	return GCOBR_DATAMISMATCH;
874	else if (pos == pb->NumberOfSat[s] + satoffset[s])
875	++pb->NumberOfSat[s];
876	pb->Sat[pos].ID = id;
877
878	G_NO_OF_PHASE_BIASES(pb->Sat[pos].NumberOfPhaseBiases)
879	G_YAW_ANGLE(pb->Sat[pos].YawAngle)
880	G_YAW_RATE(pb->Sat[pos].YawRate)
881	for (j = 0; j < pb->Sat[pos].NumberOfPhaseBiases; ++j) {
882	G_SIGNAL_IDENTIFIER(pb->Sat[pos].Biases[j].Type)
883	G_INTEGER_INDICATOR(pb->Sat[pos].Biases[j].SignalIntegerIndicator)
884	G_WIDE_LANE_INDICATOR(
885	pb->Sat[pos].Biases[j].SignalsWideLaneIntegerIndicator)
886	G_DISCONTINUITY_COUNTER(
887	pb->Sat[pos].Biases[j].SignalDiscontinuityCounter)
888	G_PHASE_BIAS(pb->Sat[pos].Biases[j].Bias)
889	}
890	}
891	#ifdef DEBUG
892	for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type)
893	numbits += 8;
894	fprintf(stderr, "numbits left %d\n",numbits);
895	#endif
896	return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
897	}
898	else if (type >= corbase[s]) {
899	switch (type - corbase[s]) {
900	case COBOFS_ORBIT:
901	if (!co)
902	return GCOBR_NOCLOCKORBITPARAMETER;
903	co->messageType = type;
904	switch (s) {
905	case CLOCKORBIT_SATGPS:
906	case CLOCKORBIT_SATGALILEO:
907	case CLOCKORBIT_SATQZSS:
908	case CLOCKORBIT_SATSBAS:
909	case CLOCKORBIT_SATBDS:
910	G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
911	break;
912	case CLOCKORBIT_SATGLONASS:
913	G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
914	break;
915	}
916	G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
917	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
918	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
919	G_SSR_IOD(co->SSRIOD)
920	G_SSR_PROVIDER_ID(co->SSRProviderID)
921	G_SSR_SOLUTION_ID(co->SSRSolutionID)
922	G_NO_OF_SATELLITES(nums)
923	co->Supplied[COBOFS_ORBIT] \|= 1;
924	#ifdef DEBUG
925	fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->EpochTime[s],
926	co->UpdateInterval,mmi,co->NumberOfSat[s],nums, co->SatRefDatum);
927	#endif
928	for (i = 0; i < nums; ++i) {
929	switch (s) {
930	case CLOCKORBIT_SATGPS:
931	case CLOCKORBIT_SATGALILEO:
932	case CLOCKORBIT_SATSBAS:
933	case CLOCKORBIT_SATBDS:
934	G_GPS_SATELLITE_ID(id)
935	break;
936	case CLOCKORBIT_SATQZSS:
937	G_QZSS_SATELLITE_ID(id)
938	break;
939	case CLOCKORBIT_SATGLONASS:
940	G_GLONASS_SATELLITE_ID(id)
941	break;
942	}
943	for (pos = satoffset[s];
944	pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
945	++pos)
946	;
947	if (pos >= satoffset[s + 1])
948	return GCOBR_DATAMISMATCH;
949	else if (pos == co->NumberOfSat[s] + satoffset[s])
950	++co->NumberOfSat[s];
951	co->Sat[pos].ID = id;
952
953	switch (s) {
954	case CLOCKORBIT_SATGPS:
955	case CLOCKORBIT_SATQZSS:
956	G_GPS_IODE(co->Sat[pos].IOD)
957	break;
958	case CLOCKORBIT_SATGLONASS:
959	G_GLONASS_IOD(co->Sat[pos].IOD)
960	break;
961	case CLOCKORBIT_SATGALILEO:
962	G_GALILEO_IOD(co->Sat[pos].IOD)
963	break;
964	case CLOCKORBIT_SATSBAS:
965	G_SBAS_T0MOD(co->Sat[pos].toe)
966	G_SBAS_IODCRC(co->Sat[pos].IOD)
967	break;
968	case CLOCKORBIT_SATBDS:
969	G_BDS_TOEMOD(co->Sat[pos].toe)
970	G_BDS_IOD(co->Sat[pos].IOD)
971	break;
972	}
973	G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
974	G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
975	G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
976	G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
977	G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
978	G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
979	#ifdef DEBUG
980	fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f\n",
981	co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
982	co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
983	co->Sat[pos].Orbit.DotDeltaRadial,
984	co->Sat[pos].Orbit.DotDeltaAlongTrack,
985	co->Sat[pos].Orbit.DotDeltaCrossTrack);
986	#endif
987	}
988	break;
989	case COBOFS_CLOCK:
990	if (!co)
991	return GCOBR_NOCLOCKORBITPARAMETER;
992	co->messageType = type;
993	switch (s) {
994	case CLOCKORBIT_SATGPS:
995	case CLOCKORBIT_SATGALILEO:
996	case CLOCKORBIT_SATQZSS:
997	case CLOCKORBIT_SATSBAS:
998	case CLOCKORBIT_SATBDS:
999	G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1000	break;
1001	case CLOCKORBIT_SATGLONASS:
1002	G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1003	break;
1004	}
1005	G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1006	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1007	G_SSR_IOD(co->SSRIOD)
1008	G_SSR_PROVIDER_ID(co->SSRProviderID)
1009	G_SSR_SOLUTION_ID(co->SSRSolutionID)
1010	G_NO_OF_SATELLITES(nums)
1011	co->Supplied[COBOFS_CLOCK] \|= 1;
1012	#ifdef DEBUG
1013	fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->EpochTime[s],
1014	co->UpdateInterval,mmi,co->NumberOfSat[s],nums);
1015	#endif
1016	for (i = 0; i < nums; ++i) {
1017	switch (s) {
1018	case CLOCKORBIT_SATGPS:
1019	case CLOCKORBIT_SATGALILEO:
1020	case CLOCKORBIT_SATSBAS:
1021	case CLOCKORBIT_SATBDS:
1022	G_GPS_SATELLITE_ID(id)
1023	break;
1024	case CLOCKORBIT_SATQZSS:
1025	G_QZSS_SATELLITE_ID(id)
1026	break;
1027	case CLOCKORBIT_SATGLONASS:
1028	G_GLONASS_SATELLITE_ID(id)
1029	break;
1030	}
1031	for (pos = satoffset[s];
1032	pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
1033	++pos)
1034	;
1035	if (pos >= satoffset[s + 1])
1036	return GCOBR_DATAMISMATCH;
1037	else if (pos == co->NumberOfSat[s] + satoffset[s])
1038	++co->NumberOfSat[s];
1039	co->Sat[pos].ID = id;
1040
1041	G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
1042	G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
1043	G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
1044	#ifdef DEBUG
1045	fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
1046	co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
1047	co->Sat[pos].Clock.DeltaA2);
1048	#endif
1049	}
1050	break;
1051	case COBOFS_COMBINED:
1052	if (!co)
1053	return GCOBR_NOCLOCKORBITPARAMETER;
1054	co->messageType = type;
1055	switch (s) {
1056	case CLOCKORBIT_SATGPS:
1057	case CLOCKORBIT_SATGALILEO:
1058	case CLOCKORBIT_SATQZSS:
1059	case CLOCKORBIT_SATSBAS:
1060	case CLOCKORBIT_SATBDS:
1061	G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1062	break;
1063	case CLOCKORBIT_SATGLONASS:
1064	G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1065	break;
1066	}
1067	G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1068	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1069	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
1070	G_SSR_IOD(co->SSRIOD)
1071	G_SSR_PROVIDER_ID(co->SSRProviderID)
1072	G_SSR_SOLUTION_ID(co->SSRSolutionID)
1073	G_NO_OF_SATELLITES(nums)
1074	co->Supplied[COBOFS_ORBIT] \|= 1;
1075	co->Supplied[COBOFS_CLOCK] \|= 1;
1076	for (i = 0; i < nums; ++i) {
1077	switch (s) {
1078	case CLOCKORBIT_SATGPS:
1079	case CLOCKORBIT_SATGALILEO:
1080	case CLOCKORBIT_SATSBAS:
1081	case CLOCKORBIT_SATBDS:
1082	G_GPS_SATELLITE_ID(id)
1083	break;
1084	case CLOCKORBIT_SATQZSS:
1085	G_QZSS_SATELLITE_ID(id)
1086	break;
1087	case CLOCKORBIT_SATGLONASS:
1088	G_GLONASS_SATELLITE_ID(id)
1089	break;
1090	}
1091	for (pos = satoffset[s];
1092	pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
1093	++pos)
1094	;
1095	if (pos >= satoffset[s + 1])
1096	return GCOBR_DATAMISMATCH;
1097	else if (pos == co->NumberOfSat[s] + satoffset[s])
1098	++co->NumberOfSat[s];
1099	co->Sat[pos].ID = id;
1100
1101	switch (s) {
1102	case CLOCKORBIT_SATGPS:
1103	case CLOCKORBIT_SATQZSS:
1104	G_GPS_IODE(co->Sat[pos].IOD)
1105	break;
1106	case CLOCKORBIT_SATGLONASS:
1107	G_GLONASS_IOD(co->Sat[pos].IOD)
1108	break;
1109	case CLOCKORBIT_SATGALILEO:
1110	G_GALILEO_IOD(co->Sat[pos].IOD)
1111	break;
1112	case CLOCKORBIT_SATSBAS:
1113	G_SBAS_T0MOD(co->Sat[pos].toe)
1114	G_SBAS_IODCRC(co->Sat[pos].IOD)
1115	break;
1116	case CLOCKORBIT_SATBDS:
1117	G_BDS_TOEMOD(co->Sat[pos].toe)
1118	G_BDS_IOD(co->Sat[pos].IOD)
1119	break;
1120	}
1121	G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
1122	G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
1123	G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
1124	G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
1125	G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
1126	G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
1127	G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
1128	G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
1129	G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
1130	}
1131	break;
1132	case COBOFS_URA:
1133	if (!co)
1134	return GCOBR_NOCLOCKORBITPARAMETER;
1135	co->messageType = type;
1136	switch (s) {
1137	case CLOCKORBIT_SATGPS:
1138	case CLOCKORBIT_SATGALILEO:
1139	case CLOCKORBIT_SATQZSS:
1140	case CLOCKORBIT_SATSBAS:
1141	case CLOCKORBIT_SATBDS:
1142	G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1143	break;
1144	case CLOCKORBIT_SATGLONASS:
1145	G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1146	break;
1147	}
1148	G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1149	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1150	G_SSR_IOD(co->SSRIOD)
1151	G_SSR_PROVIDER_ID(co->SSRProviderID)
1152	G_SSR_SOLUTION_ID(co->SSRSolutionID)
1153	G_NO_OF_SATELLITES(nums)
1154	co->Supplied[COBOFS_URA] \|= 1;
1155	for (i = 0; i < nums; ++i) {
1156	switch (s) {
1157	case CLOCKORBIT_SATGPS:
1158	case CLOCKORBIT_SATGALILEO:
1159	case CLOCKORBIT_SATSBAS:
1160	case CLOCKORBIT_SATBDS:
1161	G_GPS_SATELLITE_ID(id)
1162	break;
1163	case CLOCKORBIT_SATQZSS:
1164	G_QZSS_SATELLITE_ID(id)
1165	break;
1166	case CLOCKORBIT_SATGLONASS:
1167	G_GLONASS_SATELLITE_ID(id)
1168	break;
1169	}
1170	for (pos = satoffset[s];
1171	pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
1172	++pos)
1173	;
1174	if (pos >= satoffset[s + 1])
1175	return GCOBR_DATAMISMATCH;
1176	else if (pos == co->NumberOfSat[s] + satoffset[s])
1177	++co->NumberOfSat[s];
1178	co->Sat[pos].ID = id;
1179
1180	G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
1181	}
1182	break;
1183	case COBOFS_HR:
1184	if (!co)
1185	return GCOBR_NOCLOCKORBITPARAMETER;
1186	co->messageType = type;
1187	switch (s) {
1188	case CLOCKORBIT_SATGPS:
1189	case CLOCKORBIT_SATGALILEO:
1190	case CLOCKORBIT_SATQZSS:
1191	case CLOCKORBIT_SATSBAS:
1192	case CLOCKORBIT_SATBDS:
1193	G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1194	break;
1195	case CLOCKORBIT_SATGLONASS:
1196	G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1197	break;
1198	}
1199	G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1200	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1201	G_SSR_IOD(co->SSRIOD)
1202	G_SSR_PROVIDER_ID(co->SSRProviderID)
1203	G_SSR_SOLUTION_ID(co->SSRSolutionID)
1204	G_NO_OF_SATELLITES(nums)
1205	co->Supplied[COBOFS_HR] \|= 1;
1206	for (i = 0; i < nums; ++i) {
1207	switch (s) {
1208	case CLOCKORBIT_SATGPS:
1209	case CLOCKORBIT_SATGALILEO:
1210	case CLOCKORBIT_SATSBAS:
1211	case CLOCKORBIT_SATBDS:
1212	G_GPS_SATELLITE_ID(id)
1213	break;
1214	case CLOCKORBIT_SATQZSS:
1215	G_QZSS_SATELLITE_ID(id)
1216	break;
1217	case CLOCKORBIT_SATGLONASS:
1218	G_GLONASS_SATELLITE_ID(id)
1219	break;
1220	}
1221	for (pos = satoffset[s];
1222	pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
1223	++pos)
1224	;
1225	if (pos >= satoffset[s + 1])
1226	return GCOBR_DATAMISMATCH;
1227	else if (pos == co->NumberOfSat[s] + satoffset[s])
1228	++co->NumberOfSat[s];
1229	co->Sat[pos].ID = id;
1230	G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
1231	}
1232	break;
1233	case COBOFS_CBIAS:
1234	if (!b)
1235	return GCOBR_NOCODEBIASPARAMETER;
1236	b->messageType = type;
1237	switch (s) {
1238	case CLOCKORBIT_SATGPS:
1239	case CLOCKORBIT_SATGALILEO:
1240	case CLOCKORBIT_SATQZSS:
1241	case CLOCKORBIT_SATSBAS:
1242	case CLOCKORBIT_SATBDS:
1243	G_GPS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s])
1244	break;
1245	case CLOCKORBIT_SATGLONASS:
1246	G_GLONASS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s])
1247	break;
1248	}
1249	G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
1250	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1251	G_SSR_IOD(b->SSRIOD)
1252	G_SSR_PROVIDER_ID(b->SSRProviderID)
1253	G_SSR_SOLUTION_ID(b->SSRSolutionID)
1254	G_NO_OF_SATELLITES(nums)
1255	for (i = 0; i < nums; ++i) {
1256	switch (s) {
1257	case CLOCKORBIT_SATGPS:
1258	case CLOCKORBIT_SATGALILEO:
1259	case CLOCKORBIT_SATSBAS:
1260	case CLOCKORBIT_SATBDS:
1261	G_GPS_SATELLITE_ID(id)
1262	break;
1263	case CLOCKORBIT_SATQZSS:
1264	G_QZSS_SATELLITE_ID(id)
1265	break;
1266	case CLOCKORBIT_SATGLONASS:
1267	G_GLONASS_SATELLITE_ID(id)
1268	break;
1269	}
1270	for (pos = satoffset[s];
1271	pos < satoffset[s] + b->NumberOfSat[s] && b->Sat[pos].ID != id;
1272	++pos)
1273	;
1274	if (pos >= satoffset[s + 1])
1275	return GCOBR_DATAMISMATCH;
1276	else if (pos == b->NumberOfSat[s] + satoffset[s])
1277	++b->NumberOfSat[s];
1278	b->Sat[pos].ID = id;
1279
1280	G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases)
1281	for (j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j) {
1282	G_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
1283	G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
1284	}
1285	}
1286	break;
1287	default:
1288	continue;
1289	}
1290	#ifdef DEBUG
1291	for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type)
1292	numbits += 8;
1293	fprintf(stderr, "numbits left %d\n",numbits);
1294	#endif
1295	return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
1296	}
1297	}
1298	return GCOBR_UNKNOWNTYPE;
1299	}
1300	#endif /* NODECODE */

Note: See TracBrowser for help on using the repository browser.

Download in other formats: