Context Navigation

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

Last change on this file since 6206 was 6206, checked in by stoecker, 10 years ago
fix wl count in generate code
Property svn:executable set to ``* Property svn:keywords set to `Id`
File size: 43.6 KB

Line
1	/* Programheader
2
3	Name: clock_orbit_rtcm.c
4	Project: RTCM3
5	Version: $Id: clock_orbit_rtcm.c 6206 2014-09-25 08:46:06Z stoecker $
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	COBBASE_GPS, COBBASE_GLONASS, COBBASE_GALILEO, COBBASE_QZSS, COBBASE_SBAS,
180	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 == corbase[s]+COBOFS_BIAS))
568	{
569	INITBLOCK
570	T_MESSAGE_NUMBER(corbase[s]+COBOFS_BIAS)
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 ? 1 : 0)
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	}
920	else if(type >= corbase[s])
921	{
922	switch(type-corbase[s])
923	{
924	case COBOFS_ORBIT:
925	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
926	co->messageType = type;
927	switch(s)
928	{
929	case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
930	case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
931	case CLOCKORBIT_SATBDS:
932	G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
933	break;
934	case CLOCKORBIT_SATGLONASS:
935	G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
936	break;
937	}
938	G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
939	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
940	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
941	G_SSR_IOD(co->SSRIOD)
942	G_SSR_PROVIDER_ID(co->SSRProviderID)
943	G_SSR_SOLUTION_ID(co->SSRSolutionID)
944	G_NO_OF_SATELLITES(nums)
945	co->Supplied[COBOFS_ORBIT] \|= 1;
946	#ifdef DEBUG
947	fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->EpochTime[s],
948	co->UpdateInterval,mmi,co->NumberOfSat[s],nums, co->SatRefDatum);
949	#endif
950	for(i = 0; i < nums; ++i)
951	{
952	switch(s)
953	{
954	case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
955	case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
956	G_GPS_SATELLITE_ID(id)
957	break;
958	case CLOCKORBIT_SATQZSS:
959	G_QZSS_SATELLITE_ID(id)
960	break;
961	case CLOCKORBIT_SATGLONASS:
962	G_GLONASS_SATELLITE_ID(id)
963	break;
964	}
965	for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
966	;
967	if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
968	else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
969	co->Sat[pos].ID = id;
970
971	switch(s)
972	{
973	case CLOCKORBIT_SATGPS:
974	case CLOCKORBIT_SATQZSS:
975	G_GPS_IODE(co->Sat[pos].IOD)
976	break;
977	case CLOCKORBIT_SATGLONASS:
978	G_GLONASS_IOD(co->Sat[pos].IOD)
979	break;
980	case CLOCKORBIT_SATGALILEO:
981	G_GALILEO_IOD(co->Sat[pos].IOD)
982	break;
983	case CLOCKORBIT_SATSBAS:
984	G_SBAS_T0MOD(co->Sat[pos].toe)
985	G_SBAS_IODCRC(co->Sat[pos].IOD)
986	break;
987	case CLOCKORBIT_SATBDS:
988	G_BDS_TOEMOD(co->Sat[pos].toe)
989	G_BDS_IODCRC(co->Sat[pos].IOD)
990	break;
991	}
992	G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
993	G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
994	G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
995	G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
996	G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
997	G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
998	#ifdef DEBUG
999	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",
1000	co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
1001	co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
1002	co->Sat[pos].Orbit.DotDeltaRadial,
1003	co->Sat[pos].Orbit.DotDeltaAlongTrack,
1004	co->Sat[pos].Orbit.DotDeltaCrossTrack);
1005	#endif
1006	}
1007	break;
1008	case COBOFS_CLOCK:
1009	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
1010	co->messageType = type;
1011	switch(s)
1012	{
1013	case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1014	case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1015	case CLOCKORBIT_SATBDS:
1016	G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1017	break;
1018	case CLOCKORBIT_SATGLONASS:
1019	G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1020	break;
1021	}
1022	G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1023	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1024	G_SSR_IOD(co->SSRIOD)
1025	G_SSR_PROVIDER_ID(co->SSRProviderID)
1026	G_SSR_SOLUTION_ID(co->SSRSolutionID)
1027	G_NO_OF_SATELLITES(nums)
1028	co->Supplied[COBOFS_CLOCK] \|= 1;
1029	#ifdef DEBUG
1030	fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->EpochTime[s],
1031	co->UpdateInterval,mmi,co->NumberOfSat[s],nums);
1032	#endif
1033	for(i = 0; i < nums; ++i)
1034	{
1035	switch(s)
1036	{
1037	case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1038	case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1039	G_GPS_SATELLITE_ID(id)
1040	break;
1041	case CLOCKORBIT_SATQZSS:
1042	G_QZSS_SATELLITE_ID(id)
1043	break;
1044	case CLOCKORBIT_SATGLONASS:
1045	G_GLONASS_SATELLITE_ID(id)
1046	break;
1047	}
1048	for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
1049	;
1050	if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1051	else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
1052	co->Sat[pos].ID = id;
1053
1054	G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
1055	G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
1056	G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
1057	#ifdef DEBUG
1058	fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
1059	co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
1060	co->Sat[pos].Clock.DeltaA2);
1061	#endif
1062	}
1063	break;
1064	case COBOFS_COMBINED:
1065	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
1066	co->messageType = type;
1067	switch(s)
1068	{
1069	case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1070	case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1071	case CLOCKORBIT_SATBDS:
1072	G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1073	break;
1074	case CLOCKORBIT_SATGLONASS:
1075	G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1076	break;
1077	}
1078	G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1079	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1080	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
1081	G_SSR_IOD(co->SSRIOD)
1082	G_SSR_PROVIDER_ID(co->SSRProviderID)
1083	G_SSR_SOLUTION_ID(co->SSRSolutionID)
1084	G_NO_OF_SATELLITES(nums)
1085	co->Supplied[COBOFS_ORBIT] \|= 1;
1086	co->Supplied[COBOFS_CLOCK] \|= 1;
1087	for(i = 0; i < nums; ++i)
1088	{
1089	switch(s)
1090	{
1091	case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1092	case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1093	G_GPS_SATELLITE_ID(id)
1094	break;
1095	case CLOCKORBIT_SATQZSS:
1096	G_QZSS_SATELLITE_ID(id)
1097	break;
1098	case CLOCKORBIT_SATGLONASS:
1099	G_GLONASS_SATELLITE_ID(id)
1100	break;
1101	}
1102	for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
1103	;
1104	if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1105	else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
1106	co->Sat[pos].ID = id;
1107
1108	switch(s)
1109	{
1110	case CLOCKORBIT_SATGPS:
1111	case CLOCKORBIT_SATQZSS:
1112	G_GPS_IODE(co->Sat[pos].IOD)
1113	break;
1114	case CLOCKORBIT_SATGLONASS:
1115	G_GLONASS_IOD(co->Sat[pos].IOD)
1116	break;
1117	case CLOCKORBIT_SATGALILEO:
1118	G_GALILEO_IOD(co->Sat[pos].IOD)
1119	break;
1120	case CLOCKORBIT_SATSBAS:
1121	G_SBAS_T0MOD(co->Sat[pos].toe)
1122	G_SBAS_IODCRC(co->Sat[pos].IOD)
1123	break;
1124	case CLOCKORBIT_SATBDS:
1125	G_BDS_TOEMOD(co->Sat[pos].toe)
1126	G_BDS_IODCRC(co->Sat[pos].IOD)
1127	break;
1128	}
1129	G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
1130	G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
1131	G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
1132	G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
1133	G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
1134	G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
1135	G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
1136	G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
1137	G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
1138	}
1139	break;
1140	case COBOFS_URA:
1141	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
1142	co->messageType = type;
1143	switch(s)
1144	{
1145	case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1146	case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1147	case CLOCKORBIT_SATBDS:
1148	G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1149	break;
1150	case CLOCKORBIT_SATGLONASS:
1151	G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1152	break;
1153	}
1154	G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1155	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1156	G_SSR_IOD(co->SSRIOD)
1157	G_SSR_PROVIDER_ID(co->SSRProviderID)
1158	G_SSR_SOLUTION_ID(co->SSRSolutionID)
1159	G_NO_OF_SATELLITES(nums)
1160	co->Supplied[COBOFS_URA] \|= 1;
1161	for(i = 0; i < nums; ++i)
1162	{
1163	switch(s)
1164	{
1165	case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1166	case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1167	G_GPS_SATELLITE_ID(id)
1168	break;
1169	case CLOCKORBIT_SATQZSS:
1170	G_QZSS_SATELLITE_ID(id)
1171	break;
1172	case CLOCKORBIT_SATGLONASS:
1173	G_GLONASS_SATELLITE_ID(id)
1174	break;
1175	}
1176	for(pos = satoffset[s]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
1177	;
1178	if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1179	else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
1180	co->Sat[pos].ID = id;
1181
1182	G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
1183	}
1184	break;
1185	case COBOFS_HR:
1186	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
1187	co->messageType = type;
1188	switch(s)
1189	{
1190	case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1191	case CLOCKORBIT_SATQZSS: 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	{
1208	switch(s)
1209	{
1210	case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1211	case CLOCKORBIT_SATSBAS: 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]; pos < satoffset[s]+co->NumberOfSat[s] && co->Sat[pos].ID != id; ++pos)
1222	;
1223	if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1224	else if(pos == co->NumberOfSat[s] + satoffset[s]) ++co->NumberOfSat[s];
1225	co->Sat[pos].ID = id;
1226
1227	G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
1228	}
1229	break;
1230	case COBOFS_BIAS:
1231	if(!b) return GCOBR_NOCODEBIASPARAMETER;
1232	b->messageType = type;
1233	switch(s)
1234	{
1235	case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1236	case CLOCKORBIT_SATQZSS: case CLOCKORBIT_SATSBAS:
1237	case CLOCKORBIT_SATBDS:
1238	G_GPS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s])
1239	break;
1240	case CLOCKORBIT_SATGLONASS:
1241	G_GLONASS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s])
1242	break;
1243	}
1244	G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
1245	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1246	G_SSR_IOD(b->SSRIOD)
1247	G_SSR_PROVIDER_ID(b->SSRProviderID)
1248	G_SSR_SOLUTION_ID(b->SSRSolutionID)
1249	G_NO_OF_SATELLITES(nums)
1250	for(i = 0; i < nums; ++i)
1251	{
1252	switch(s)
1253	{
1254	case CLOCKORBIT_SATGPS: case CLOCKORBIT_SATGALILEO:
1255	case CLOCKORBIT_SATSBAS: case CLOCKORBIT_SATBDS:
1256	G_GPS_SATELLITE_ID(id)
1257	break;
1258	case CLOCKORBIT_SATQZSS:
1259	G_QZSS_SATELLITE_ID(id)
1260	break;
1261	case CLOCKORBIT_SATGLONASS:
1262	G_GLONASS_SATELLITE_ID(id)
1263	break;
1264	}
1265	for(pos = satoffset[s]; pos < satoffset[s]+b->NumberOfSat[s] && b->Sat[pos].ID != id; ++pos)
1266	;
1267	if(pos >= satoffset[s+1]) return GCOBR_DATAMISMATCH;
1268	else if(pos == b->NumberOfSat[s] + satoffset[s]) ++b->NumberOfSat[s];
1269	b->Sat[pos].ID = id;
1270
1271	G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases)
1272	for(j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j)
1273	{
1274	G_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
1275	G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
1276	}
1277	}
1278	break;
1279	default:
1280	return GCOBR_UNKNOWNTYPE;
1281	}
1282	#ifdef COR_LATENCY
1283	if(s == CLOCKORBIT_SATGPS && type-corbase[s] != COBOFS_BIAS)
1284	{
1285	co->epochGPS[co->epochSize] = co->EpochTime[s];
1286	if(co->epochSize < COR_LATENCYCOUNT)
1287	++co->epochSize;
1288	}
1289	#endif
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: