Context Navigation

source: ntrip/trunk/clock_and_orbit/lib/clock_orbit_rtcm.c@ 7636

Last change on this file since 7636 was 6894, checked in by stuerze, 10 years ago
small fix in WL indicator encoding
Property svn:executable set to ``* Property svn:keywords set to `Id`
File size: 43.8 KB

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

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

Download in other formats: