Context Navigation

source: ntrip/trunk/BNS/RTCM/clock_orbit_rtcm.c@ 864

Last change on this file since 864 was 864, checked in by mervart, 16 years ago
* empty log message *
Property svn:executable set to ``*
File size: 22.0 KB

Line
1	/* Programheader
2
3	Name: clock_orbit_rtcm.c
4	Project: RTCM3
5	Version: $Id: clock_orbit_rtcm.c,v 1.1 2008/05/04 17:38:12 mervart Exp $
6	Authors: Dirk Stöcker
7	Description: state space approach for RTCM3
8	*/
9
10	#include <stdio.h>
11	#include <string.h>
12	#include <stdint.h>
13	#include "clock_orbit_rtcm.h"
14
15	static uint32_t CRC24(long size, const unsigned char *buf)
16	{
17	uint32_t crc = 0;
18	int i;
19
20	while(size--)
21	{
22	crc ^= (*buf++) << (16);
23	for(i = 0; i < 8; i++)
24	{
25	crc <<= 1;
26	if(crc & 0x1000000)
27	crc ^= 0x01864cfb;
28	}
29	}
30	return crc;
31	}
32
33	/* NOTE: These defines are interlinked with below functions and directly modify
34	the values. This may not be optimized in terms of final program code size but
35	should be optimized in terms of speed.
36
37	modified variables are:
38	- everything defined in STARTDATA (only use ressize outside of the defines,
39	others are private)
40	- buffer
41	- size
42	*/
43
44	#ifndef NOENCODE
45	#define STOREBITS \
46	while(numbits >= 8) \
47	{ \
48	if(!size) return 0; \
49	*(buffer++) = bitbuffer>>(numbits-8); \
50	numbits -= 8; \
51	++ressize; \
52	--size; \
53	}
54
55	#define ADDBITS(a, b) \
56	{ \
57	bitbuffer = (bitbuffer<<(a))\|((b)&((1<<a)-1)); \
58	numbits += (a); \
59	STOREBITS \
60	}
61
62	#define STARTDATA \
63	size_t ressize=0; \
64	char *blockstart; \
65	int numbits; \
66	uint64_t bitbuffer=0;
67
68	#define INITBLOCK \
69	numbits = 0; \
70	blockstart = buffer; \
71	ADDBITS(8, 0xD3) \
72	ADDBITS(6, 0) \
73	ADDBITS(10, 0)
74
75	#define ENDBLOCK \
76	if(numbits) { ADDBITS((8-numbits), 0) } \
77	{ \
78	int len = buffer-blockstart-3; \
79	blockstart[1] \|= len>>8; \
80	blockstart[2] = len; \
81	len = CRC24(len+3, (const unsigned char *) blockstart); \
82	ADDBITS(24, len) \
83	}
84
85	#define SCALEADDBITS(a, b, c) ADDBITS(a, (int64_t)(b*c))
86
87	#if 0
88	#define DEBUGSCALEADDBITS(n, a, b, c) \
89	{ \
90	int64_t x = b*c, z; \
91	uint64_t y; \
92	y = (x&((1<<a)-1)); \
93	z = ((int64_t)(y<<(64-a)))>>(64-a); \
94	fprintf(stderr, "Type " # n " val %19.15f*%11.1f %16llX %16llX %16llX %s\n", \
95	c, b, x, y, z, x != z ? "OVERFLOW" : "OK"); \
96	} \
97	SCALEADDBITS(a,b,c)
98	#else
99	#define DEBUGSCALEADDBITS(n, a, b, c) SCALEADDBITS(a,b,c)
100	#endif
101
102	/* standard values */
103	#define T_MESSAGE_NUMBER(a) ADDBITS(12, a) /* DF002 */
104	#define T_RESERVED6 ADDBITS(6, 0) /* DF001 */
105	#define T_GPS_SATELLITE_ID(a) ADDBITS(6, a) /* DF068 */
106	#define T_GPS_IODE(a) ADDBITS(8, a) /* DF071 */
107	#define T_GLONASS_IOD(a) ADDBITS(8, a) /* DF237 */
108
109	/* defined values */
110	#define T_MULTIPLE_MESSAGE_INDICATOR(a) ADDBITS(1, a)
111	#define T_GPS_EPOCH_TIME(a) ADDBITS(20, a)
112	#define T_GLONASS_EPOCH_TIME(a) ADDBITS(17, a)
113	#define T_GLONASS_SATELLITE_ID(a) ADDBITS(6, a)
114	#define T_NO_OF_SATELLITES(a) ADDBITS(5, a)
115	#define T_SATELLITE_REFERENCE_POINT(a) ADDBITS(1, a)
116	#define T_SATELLITE_REFERENCE_DATUM(a) ADDBITS(1, a)
117	#define T_NO_OF_CODE_BIASES(a) ADDBITS(5, a)
118	#define T_GPS_CODE_TYPE(a) ADDBITS(5, a)
119	#define T_GLONASS_CODE_TYPE(a) ADDBITS(5, a)
120
121	/* yet undefined values */
122	#define T_DELTA_RADIAL(a) DEBUGSCALEADDBITS(dr, 20, 1000.0, a)
123	#define T_DELTA_ALONG_TRACK(a) DEBUGSCALEADDBITS(da, 20, 1000.0, a)
124	#define T_DELTA_CROSS_TRACK(a) DEBUGSCALEADDBITS(dc, 20, 1000.0, a)
125	#define T_DELTA_DOT_RADIAL(a) DEBUGSCALEADDBITS(Dr, 20, 100000.0, a)
126	#define T_DELTA_DOT_ALONG_TRACK(a) DEBUGSCALEADDBITS(Dr, 20, 100000.0, a)
127	#define T_DELTA_DOT_CROSS_TRACK(a) DEBUGSCALEADDBITS(Dr, 20, 100000.0, a)
128	#define T_DELTA_DOT_DOT_RADIAL(a) DEBUGSCALEADDBITS(DR, 20, 5000000.0, a)
129	#define T_DELTA_DOT_DOT_ALONG_TRACK(a) DEBUGSCALEADDBITS(DA, 20, 5000000.0, a)
130	#define T_DELTA_DOT_DOT_CROSS_TRACK(a) DEBUGSCALEADDBITS(DC, 20, 5000000.0, a)
131	#define T_DELTA_A0(a) DEBUGSCALEADDBITS(A0, 20, 1000.0, a)
132	#define T_DELTA_A1(a) DEBUGSCALEADDBITS(A1, 20, 100000.0, a)
133	#define T_DELTA_A2(a) DEBUGSCALEADDBITS(A2, 20, 5000000.0, a)
134	#define T_CODE_BIAS(a) DEBUGSCALEADDBITS(CB, 20, 100.0, a)
135
136	size_t MakeClockOrbit(const struct ClockOrbit *co, enum ClockOrbitType type,
137	int moremessagesfollow, char *buffer, size_t size)
138	{
139	int gpsor=0, gpscl=0, gpsco=0, gloor=0, glocl=0, gloco=0, mmi, i;
140	STARTDATA
141
142	if(co->NumberOfGPSSat && co->OrbitDataSupplied
143	&& (type == COTYPE_AUTO \|\| type == COTYPE_GPSORBIT))
144	gpsor = 1;
145	if(co->NumberOfGPSSat && co->ClockDataSupplied
146	&& (type == COTYPE_AUTO \|\| type == COTYPE_GPSCLOCK))
147	gpscl = 1;
148	if(co->NumberOfGPSSat && co->ClockDataSupplied && co->OrbitDataSupplied
149	&& (type == COTYPE_AUTO \|\| type == COTYPE_GPSCOMBINED))
150	{
151	gpsco = 1; gpsor = 0; gpscl = 0;
152	}
153	if(co->NumberOfGLONASSSat && co->OrbitDataSupplied
154	&& (type == COTYPE_AUTO \|\| type == COTYPE_GLONASSORBIT))
155	gloor = 1;
156	if(co->NumberOfGLONASSSat && co->ClockDataSupplied
157	&& (type == COTYPE_AUTO \|\| type == COTYPE_GLONASSCLOCK))
158	glocl = 1;
159	if(co->NumberOfGLONASSSat && co->ClockDataSupplied && co->OrbitDataSupplied
160	&& (type == COTYPE_AUTO \|\| type == COTYPE_GLONASSCOMBINED))
161	{
162	gloco = 1; gloor = 0; glocl = 0;
163	}
164
165	mmi = gpsor+gpscl+gpsco+gloor+glocl+gloco; /* required for multimessage */
166	if(!moremessagesfollow) --mmi;
167
168	if(gpsor)
169	{
170	INITBLOCK
171	T_MESSAGE_NUMBER(COTYPE_GPSORBIT)
172	T_GPS_EPOCH_TIME(co->GPSEpochTime)
173	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
174	--mmi;
175	T_RESERVED6
176	T_NO_OF_SATELLITES(co->NumberOfGPSSat)
177	for(i = 0; i < co->NumberOfGPSSat; ++i)
178	{
179	T_GPS_SATELLITE_ID(co->Sat[i].ID)
180	T_GPS_IODE(co->Sat[i].IOD)
181	T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
182	T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
183	T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
184	T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
185	T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
186	T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
187	T_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
188	T_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
189	T_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
190	T_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
191	T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
192	}
193	ENDBLOCK
194	}
195	if(gpscl)
196	{
197	INITBLOCK
198	T_MESSAGE_NUMBER(COTYPE_GPSCLOCK)
199	T_GPS_EPOCH_TIME(co->GPSEpochTime)
200	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
201	--mmi;
202	T_RESERVED6
203	T_NO_OF_SATELLITES(co->NumberOfGPSSat)
204	for(i = 0; i < co->NumberOfGPSSat; ++i)
205	{
206	T_GPS_SATELLITE_ID(co->Sat[i].ID)
207	T_GPS_IODE(co->Sat[i].IOD)
208	T_DELTA_A0(co->Sat[i].Clock.DeltaA0)
209	T_DELTA_A1(co->Sat[i].Clock.DeltaA1)
210	T_DELTA_A2(co->Sat[i].Clock.DeltaA2)
211	}
212	ENDBLOCK
213	}
214	if(gpsco)
215	{
216	INITBLOCK
217	T_MESSAGE_NUMBER(COTYPE_GPSCOMBINED)
218	T_GPS_EPOCH_TIME(co->GPSEpochTime)
219	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
220	--mmi;
221	T_RESERVED6
222	T_NO_OF_SATELLITES(co->NumberOfGPSSat)
223	for(i = 0; i < co->NumberOfGPSSat; ++i)
224	{
225	T_GPS_SATELLITE_ID(co->Sat[i].ID)
226	T_GPS_IODE(co->Sat[i].IOD)
227	T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
228	T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
229	T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
230	T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
231	T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
232	T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
233	T_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
234	T_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
235	T_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
236	T_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
237	T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
238	T_DELTA_A0(co->Sat[i].Clock.DeltaA0)
239	T_DELTA_A1(co->Sat[i].Clock.DeltaA1)
240	T_DELTA_A2(co->Sat[i].Clock.DeltaA2)
241	}
242	ENDBLOCK
243	}
244	if(gloor)
245	{
246	INITBLOCK
247	T_MESSAGE_NUMBER(COTYPE_GLONASSORBIT)
248	T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
249	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
250	--mmi;
251	T_RESERVED6
252	T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
253	for(i = CLOCKORBIT_NUMGPS;
254	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
255	{
256	T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
257	T_GLONASS_IOD(co->Sat[i].IOD)
258	T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
259	T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
260	T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
261	T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
262	T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
263	T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
264	T_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
265	T_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
266	T_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
267	T_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
268	T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
269	}
270	ENDBLOCK
271	}
272	if(glocl)
273	{
274	INITBLOCK
275	T_MESSAGE_NUMBER(COTYPE_GLONASSCLOCK)
276	T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
277	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
278	--mmi;
279	T_RESERVED6
280	T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
281	for(i = CLOCKORBIT_NUMGPS;
282	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
283	{
284	T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
285	T_GLONASS_IOD(co->Sat[i].IOD)
286	T_DELTA_A0(co->Sat[i].Clock.DeltaA0)
287	T_DELTA_A1(co->Sat[i].Clock.DeltaA1)
288	T_DELTA_A2(co->Sat[i].Clock.DeltaA2)
289	}
290	ENDBLOCK
291	}
292	if(gloco)
293	{
294	INITBLOCK
295	T_MESSAGE_NUMBER(COTYPE_GLONASSCOMBINED)
296	T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
297	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
298	--mmi;
299	T_RESERVED6
300	T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
301	for(i = CLOCKORBIT_NUMGPS;
302	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
303	{
304	T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
305	T_GLONASS_IOD(co->Sat[i].IOD)
306	T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
307	T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
308	T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
309	T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
310	T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
311	T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
312	T_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
313	T_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
314	T_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
315	T_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
316	T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
317	T_DELTA_A0(co->Sat[i].Clock.DeltaA0)
318	T_DELTA_A1(co->Sat[i].Clock.DeltaA1)
319	T_DELTA_A2(co->Sat[i].Clock.DeltaA2)
320	}
321	ENDBLOCK
322	}
323
324	return ressize;
325	}
326
327	size_t MakeBias(const struct Bias *b, enum BiasType type,
328	int moremessagesfollow, char *buffer, size_t size)
329	{
330	int gps, glo, mmi, i, j;
331	STARTDATA
332
333	if(b->NumberOfGPSSat && (type == BTYPE_AUTO \|\| type == BTYPE_GPS))
334	gps = 1;
335	if(b->NumberOfGLONASSSat && (type == BTYPE_AUTO \|\| type == BTYPE_GLONASS))
336	glo = 1;
337
338	mmi = gps+glo; /* required for multimessage */
339	if(!moremessagesfollow) --mmi;
340
341	if(gps)
342	{
343	INITBLOCK
344	T_MESSAGE_NUMBER(BTYPE_GPS)
345	T_GPS_EPOCH_TIME(b->GPSEpochTime)
346	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
347	--mmi;
348	T_RESERVED6
349	T_NO_OF_SATELLITES(b->NumberOfGPSSat)
350	for(i = 0; i < b->NumberOfGPSSat; ++i)
351	{
352	T_GPS_SATELLITE_ID(b->Sat[i].ID)
353	T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
354	for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
355	{
356	T_GPS_CODE_TYPE(b->Sat[i].Biases[j].Type)
357	T_CODE_BIAS(b->Sat[i].Biases[j].Bias)
358	}
359	}
360	ENDBLOCK
361	}
362	if(glo)
363	{
364	INITBLOCK
365	T_MESSAGE_NUMBER(BTYPE_GLONASS)
366	T_GPS_EPOCH_TIME(b->GLONASSEpochTime)
367	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
368	--mmi;
369	T_RESERVED6
370	T_NO_OF_SATELLITES(b->NumberOfGLONASSSat)
371	for(i = CLOCKORBIT_NUMGPS;
372	i < CLOCKORBIT_NUMGPS+b->NumberOfGLONASSSat; ++i)
373	{
374	T_GLONASS_SATELLITE_ID(b->Sat[i].ID)
375	T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
376	for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
377	{
378	T_GLONASS_CODE_TYPE(b->Sat[i].Biases[j].Type)
379	T_CODE_BIAS(b->Sat[i].Biases[j].Bias)
380	}
381	}
382	ENDBLOCK
383	}
384
385	return ressize;
386	}
387	#endif /* NOENCODE */
388
389	#ifndef NODECODE
390
391	#define DECODESTART \
392	int numbits=0; \
393	uint64_t bitbuffer=0;
394
395	#define LOADBITS(a) \
396	{ \
397	while((a) > numbits) \
398	{ \
399	if(!size--) return -2; \
400	bitbuffer = (bitbuffer<<8)\|((unsigned char)*(buffer++)); \
401	numbits += 8; \
402	} \
403	}
404
405	/* extract bits from data stream
406	b = variable to store result, a = number of bits */
407	#define GETBITS(b, a) \
408	{ \
409	LOADBITS(a) \
410	b = (bitbuffer<<(64-numbits))>>(64-(a)); \
411	numbits -= (a); \
412	}
413
414	/* extract signed floating value from data stream
415	b = variable to store result, a = number of bits */
416	#define GETFLOATSIGN(b, a, c) \
417	{ \
418	LOADBITS(a) \
419	b = ((double)(((int64_t)(bitbuffer<<(64-numbits)))>>(64-(a))))*(c); \
420	numbits -= (a); \
421	}
422
423	#define SKIPBITS(b) { LOADBITS(b) numbits -= (b); }
424
425	/* standard values */
426	#define G_HEADER SKIPBITS(8+6+10)
427	#define G_MESSAGE_NUMBER(a) GETBITS(a, 12) /* DF002 */
428	#define G_RESERVED6 SKIPBITS(6) /* DF001 */
429	#define G_GPS_SATELLITE_ID(a) GETBITS(a, 6) /* DF068 */
430	#define G_GPS_IODE(a) GETBITS(a, 8) /* DF071 */
431	#define G_GLONASS_IOD(a) GETBITS(a, 8) /* DF237 */
432
433	/* defined values */
434	#define G_MULTIPLE_MESSAGE_INDICATOR(a) GETBITS(a, 1)
435	#define G_GPS_EPOCH_TIME(a) GETBITS(a, 20)
436	#define G_GLONASS_EPOCH_TIME(a) GETBITS(a, 17)
437	#define G_GLONASS_SATELLITE_ID(a) GETBITS(a, 6)
438	#define G_NO_OF_SATELLITES(a) GETBITS(a, 5)
439	#define G_SATELLITE_REFERENCE_POINT(a) GETBITS(a, 1)
440	#define G_SATELLITE_REFERENCE_DATUM(a) GETBITS(a, 1)
441	#define G_NO_OF_CODE_BIASES(a) GETBITS(a, 5)
442	#define G_GPS_CODE_TYPE(a) GETBITS(a, 5)
443	#define G_GLONASS_CODE_TYPE(a) GETBITS(a, 5)
444
445	/* yet undefined values */
446	#define G_DELTA_RADIAL(a) GETFLOATSIGN(a, 20, 1/1000.0)
447	#define G_DELTA_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/1000.0)
448	#define G_DELTA_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/1000.0)
449	#define G_DELTA_DOT_RADIAL(a) GETFLOATSIGN(a, 20, 1/100000.0)
450	#define G_DELTA_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/100000.0)
451	#define G_DELTA_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/100000.0)
452	#define G_DELTA_DOT_DOT_RADIAL(a) GETFLOATSIGN(a, 20, 1/5000000.0)
453	#define G_DELTA_DOT_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/5000000.0)
454	#define G_DELTA_DOT_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/5000000.0)
455	#define G_DELTA_A0(a) GETFLOATSIGN(a, 20, 1/1000.0)
456	#define G_DELTA_A1(a) GETFLOATSIGN(a, 20, 1/100000.0)
457	#define G_DELTA_A2(a) GETFLOATSIGN(a, 20, 1/5000000.0)
458	#define G_CODE_BIAS(a) GETFLOATSIGN(a, 20, 1/100.0)
459
460	/* FIXME: Joining data does no care for satellite numbers, dates and so on.
461	It will only work with data, which is stored and the same order and number as
462	the previos blocks! */
463
464	int GetClockOrbitBias(struct ClockOrbit co, struct Bias b,
465	const char *buffer, size_t size)
466	{
467	int type, mmi=0, i, j;
468	DECODESTART
469
470	G_HEADER
471	G_MESSAGE_NUMBER(type)
472	switch(type)
473	{
474	case COTYPE_GPSORBIT:
475	if(!co) return -5;
476	G_GPS_EPOCH_TIME(co->GPSEpochTime)
477	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
478	G_RESERVED6
479	G_NO_OF_SATELLITES(co->NumberOfGPSSat)
480	co->OrbitDataSupplied = 1;
481	for(i = 0; i < co->NumberOfGPSSat; ++i)
482	{
483	G_GPS_SATELLITE_ID(co->Sat[i].ID)
484	G_GPS_IODE(co->Sat[i].IOD)
485	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
486	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
487	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
488	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
489	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
490	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
491	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
492	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
493	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
494	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
495	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
496	}
497	break;
498	case COTYPE_GPSCLOCK:
499	if(!co) return -5;
500	G_GPS_EPOCH_TIME(co->GPSEpochTime)
501	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
502	G_RESERVED6
503	G_NO_OF_SATELLITES(co->NumberOfGPSSat)
504	co->ClockDataSupplied = 1;
505	for(i = 0; i < co->NumberOfGPSSat; ++i)
506	{
507	G_GPS_SATELLITE_ID(co->Sat[i].ID)
508	G_GPS_IODE(co->Sat[i].IOD)
509	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
510	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
511	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
512	}
513	break;
514	case COTYPE_GPSCOMBINED:
515	if(!co) return -5;
516	G_GPS_EPOCH_TIME(co->GPSEpochTime)
517	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
518	G_RESERVED6
519	G_NO_OF_SATELLITES(co->NumberOfGPSSat)
520	co->OrbitDataSupplied = 1;
521	co->ClockDataSupplied = 1;
522	for(i = 0; i < co->NumberOfGPSSat; ++i)
523	{
524	G_GPS_SATELLITE_ID(co->Sat[i].ID)
525	G_GPS_IODE(co->Sat[i].IOD)
526	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
527	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
528	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
529	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
530	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
531	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
532	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
533	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
534	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
535	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
536	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
537	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
538	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
539	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
540	}
541	break;
542	case COTYPE_GLONASSORBIT:
543	if(!co) return -5;
544	G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
545	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
546	G_RESERVED6
547	G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
548	co->OrbitDataSupplied = 1;
549	for(i = CLOCKORBIT_NUMGPS;
550	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
551	{
552	G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
553	G_GLONASS_IOD(co->Sat[i].IOD)
554	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
555	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
556	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
557	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
558	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
559	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
560	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
561	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
562	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
563	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
564	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
565	}
566	break;
567	case COTYPE_GLONASSCLOCK:
568	if(!co) return -5;
569	G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
570	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
571	G_RESERVED6
572	G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
573	co->ClockDataSupplied = 1;
574	for(i = CLOCKORBIT_NUMGPS;
575	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
576	{
577	G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
578	G_GLONASS_IOD(co->Sat[i].IOD)
579	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
580	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
581	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
582	}
583	break;
584	case COTYPE_GLONASSCOMBINED:
585	if(!co) return -5;
586	G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
587	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
588	G_RESERVED6
589	G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
590	co->OrbitDataSupplied = 1;
591	co->ClockDataSupplied = 1;
592	for(i = CLOCKORBIT_NUMGPS;
593	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
594	{
595	G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
596	G_GLONASS_IOD(co->Sat[i].IOD)
597	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
598	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
599	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
600	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
601	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
602	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
603	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
604	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
605	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
606	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
607	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
608	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
609	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
610	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
611	}
612	break;
613	case BTYPE_GPS:
614	if(!b) return -4;
615	G_GPS_EPOCH_TIME(b->GPSEpochTime)
616	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
617	G_RESERVED6
618	G_NO_OF_SATELLITES(b->NumberOfGPSSat)
619	for(i = 0; i < b->NumberOfGPSSat; ++i)
620	{
621	G_GPS_SATELLITE_ID(b->Sat[i].ID)
622	G_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
623	for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
624	{
625	G_GPS_CODE_TYPE(b->Sat[i].Biases[j].Type)
626	G_CODE_BIAS(b->Sat[i].Biases[j].Bias)
627	}
628	}
629	break;
630	case BTYPE_GLONASS:
631	if(!b) return -4;
632	G_GPS_EPOCH_TIME(b->GLONASSEpochTime)
633	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
634	G_RESERVED6
635	G_NO_OF_SATELLITES(b->NumberOfGLONASSSat)
636	for(i = CLOCKORBIT_NUMGPS;
637	i < CLOCKORBIT_NUMGPS+b->NumberOfGLONASSSat; ++i)
638	{
639	G_GLONASS_SATELLITE_ID(b->Sat[i].ID)
640	G_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
641	for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
642	{
643	G_GLONASS_CODE_TYPE(b->Sat[i].Biases[j].Type)
644	G_CODE_BIAS(b->Sat[i].Biases[j].Bias)
645	}
646	}
647	break;
648	default:
649	return -3;
650	}
651	return mmi ? 1 : 0;
652	}
653	#endif /* NODECODE */

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

Download in other formats: