Context Navigation

source: ntrip/trunk/clock_and_orbit/clock_orbit_rtcm.c@ 1574

Last change on this file since 1574 was 1238, checked in by stoecker, 16 years ago
changes by Mr. Weber
Property svn:executable set to ``*
File size: 24.2 KB

Line
1	/* Programheader
2
3	Name: clock_orbit_rtcm.c
4	Project: RTCM3
5	Version: $Id: clock_orbit_rtcm.c,v 1.4 2008/11/16 00:05:02 weber 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 GCOBR_SHORTBUFFER; \
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(a) GETBITS(a,8)
427	#define G_RESERVEDH(a) GETBITS(a,6)
428	#define G_SIZE(a) GETBITS(a, 10)
429	#define G_MESSAGE_NUMBER(a) GETBITS(a, 12) /* DF002 */
430	#define G_RESERVED6 SKIPBITS(6) /* DF001 */
431	#define G_GPS_SATELLITE_ID(a) {int temp; GETBITS(temp, 6) \
432	if(a && a != temp) return GCOBR_DATAMISMATCH; a = temp;} /* DF068 */
433	#define G_GPS_IODE(a) GETBITS(a, 8) /* DF071 */
434	#define G_GLONASS_IOD(a) GETBITS(a, 8) /* DF237 */
435
436	/* defined values */
437	#define G_MULTIPLE_MESSAGE_INDICATOR(a) GETBITS(a, 1)
438	#define G_GPS_EPOCH_TIME(a, b) {int temp; GETBITS(temp, 20) \
439	if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
440	#define G_GLONASS_EPOCH_TIME(a, b) {int temp; GETBITS(temp, 17) \
441	if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
442	#define G_GLONASS_SATELLITE_ID(a) {int temp; GETBITS(temp, 6) \
443	if(a && a != temp) return GCOBR_DATAMISMATCH; a = temp;}
444	#define G_NO_OF_SATELLITES(a) {int temp; GETBITS(temp, 5) \
445	if(a && a != temp) return GCOBR_DATAMISMATCH; a = temp;}
446	#define G_SATELLITE_REFERENCE_POINT(a) GETBITS(a, 1)
447	#define G_SATELLITE_REFERENCE_DATUM(a) GETBITS(a, 1)
448	#define G_NO_OF_CODE_BIASES(a) GETBITS(a, 5)
449	#define G_GPS_CODE_TYPE(a) GETBITS(a, 5)
450	#define G_GLONASS_CODE_TYPE(a) GETBITS(a, 5)
451
452	/* yet undefined values */
453	#define G_DELTA_RADIAL(a) GETFLOATSIGN(a, 20, 1/1000.0)
454	#define G_DELTA_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/1000.0)
455	#define G_DELTA_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/1000.0)
456	#define G_DELTA_DOT_RADIAL(a) GETFLOATSIGN(a, 20, 1/100000.0)
457	#define G_DELTA_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/100000.0)
458	#define G_DELTA_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/100000.0)
459	#define G_DELTA_DOT_DOT_RADIAL(a) GETFLOATSIGN(a, 20, 1/5000000.0)
460	#define G_DELTA_DOT_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/5000000.0)
461	#define G_DELTA_DOT_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/5000000.0)
462	#define G_DELTA_A0(a) GETFLOATSIGN(a, 20, 1/1000.0)
463	#define G_DELTA_A1(a) GETFLOATSIGN(a, 20, 1/100000.0)
464	#define G_DELTA_A2(a) GETFLOATSIGN(a, 20, 1/5000000.0)
465	#define G_CODE_BIAS(a) GETFLOATSIGN(a, 20, 1/100.0)
466
467	enum GCOB_RETURN GetClockOrbitBias(struct ClockOrbit co, struct Bias b,
468	const char buffer, size_t size, int bytesused)
469	{
470	int type, mmi=0, i, j, h, rs, sizeofrtcmblock;
471	const char *blockstart = buffer;
472	DECODESTART
473
474	if(size < 7)
475	return GCOBR_SHORTBUFFER;
476
477	G_HEADER(h)
478	G_RESERVEDH(rs)
479	G_SIZE(sizeofrtcmblock);
480
481	if((unsigned char)h != 0xD3 \|\| rs)
482	return GCOBR_UNKNOWNDATA;
483	if(size < sizeofrtcmblock + 3) /* 3 header bytes already removed */
484	return GCOBR_MESSAGEEXCEEDSBUFFER;
485	if(CRC24(sizeofrtcmblock+3, (const unsigned char *) blockstart) !=
486	((((unsigned char)buffer[sizeofrtcmblock])<<16)\|
487	(((unsigned char)buffer[sizeofrtcmblock+1])<<8)\|
488	(((unsigned char)buffer[sizeofrtcmblock+2]))))
489	return GCOBR_CRCMISMATCH;
490
491	G_MESSAGE_NUMBER(type)
492	switch(type)
493	{
494	case COTYPE_GPSORBIT:
495	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
496	G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
497	co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
498	if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
499	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
500	G_RESERVED6
501	G_NO_OF_SATELLITES(co->NumberOfGPSSat)
502	if(co->OrbitDataSupplied)
503	return GCOBR_DATAMISMATCH;
504	co->OrbitDataSupplied = 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_RADIAL(co->Sat[i].Orbit.DeltaRadial)
510	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
511	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
512	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
513	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
514	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
515	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
516	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
517	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
518	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
519	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
520	}
521	break;
522	case COTYPE_GPSCLOCK:
523	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
524	G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
525	co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
526	if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
527	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
528	G_RESERVED6
529	G_NO_OF_SATELLITES(co->NumberOfGPSSat)
530	if(co->ClockDataSupplied)
531	return GCOBR_DATAMISMATCH;
532	co->ClockDataSupplied = 1;
533	for(i = 0; i < co->NumberOfGPSSat; ++i)
534	{
535	G_GPS_SATELLITE_ID(co->Sat[i].ID)
536	G_GPS_IODE(co->Sat[i].IOD)
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_GPSCOMBINED:
543	if(!co) return -5;
544	G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
545	co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
546	if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
547	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
548	G_RESERVED6
549	G_NO_OF_SATELLITES(co->NumberOfGPSSat)
550	if(co->ClockDataSupplied \|\| co->OrbitDataSupplied)
551	return GCOBR_DATAMISMATCH;
552	co->OrbitDataSupplied = 1;
553	co->ClockDataSupplied = 1;
554	for(i = 0; i < co->NumberOfGPSSat; ++i)
555	{
556	G_GPS_SATELLITE_ID(co->Sat[i].ID)
557	G_GPS_IODE(co->Sat[i].IOD)
558	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
559	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
560	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
561	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
562	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
563	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
564	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
565	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
566	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
567	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
568	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
569	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
570	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
571	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
572	}
573	break;
574	case COTYPE_GLONASSORBIT:
575	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
576	G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
577	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
578	G_RESERVED6
579	G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
580	if(co->OrbitDataSupplied)
581	return GCOBR_DATAMISMATCH;
582	co->OrbitDataSupplied = 1;
583	for(i = CLOCKORBIT_NUMGPS;
584	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
585	{
586	G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
587	G_GLONASS_IOD(co->Sat[i].IOD)
588	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
589	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
590	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
591	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
592	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
593	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
594	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
595	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
596	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
597	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
598	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
599	}
600	break;
601	case COTYPE_GLONASSCLOCK:
602	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
603	G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
604	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
605	G_RESERVED6
606	G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
607	if(co->ClockDataSupplied)
608	return GCOBR_DATAMISMATCH;
609	co->ClockDataSupplied = 1;
610	for(i = CLOCKORBIT_NUMGPS;
611	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
612	{
613	G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
614	G_GLONASS_IOD(co->Sat[i].IOD)
615	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
616	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
617	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
618	}
619	break;
620	case COTYPE_GLONASSCOMBINED:
621	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
622	G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
623	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
624	G_RESERVED6
625	G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
626	if(co->ClockDataSupplied \|\| co->OrbitDataSupplied)
627	return GCOBR_DATAMISMATCH;
628	co->OrbitDataSupplied = 1;
629	co->ClockDataSupplied = 1;
630	for(i = CLOCKORBIT_NUMGPS;
631	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
632	{
633	G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
634	G_GLONASS_IOD(co->Sat[i].IOD)
635	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
636	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
637	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
638	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
639	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
640	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
641	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
642	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
643	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
644	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
645	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
646	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
647	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
648	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
649	}
650	break;
651	case BTYPE_GPS:
652	if(!b) return GCOBR_NOBIASPARAMETER;
653	G_GPS_EPOCH_TIME(b->GPSEpochTime, co->NumberOfGPSSat)
654	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
655	G_RESERVED6
656	G_NO_OF_SATELLITES(b->NumberOfGPSSat)
657	for(i = 0; i < b->NumberOfGPSSat; ++i)
658	{
659	G_GPS_SATELLITE_ID(b->Sat[i].ID)
660	G_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
661	for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
662	{
663	G_GPS_CODE_TYPE(b->Sat[i].Biases[j].Type)
664	G_CODE_BIAS(b->Sat[i].Biases[j].Bias)
665	}
666	}
667	break;
668	case BTYPE_GLONASS:
669	if(!b) return GCOBR_NOBIASPARAMETER;
670	G_GPS_EPOCH_TIME(b->GLONASSEpochTime, co->NumberOfGLONASSSat)
671	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
672	G_RESERVED6
673	G_NO_OF_SATELLITES(b->NumberOfGLONASSSat)
674	for(i = CLOCKORBIT_NUMGPS;
675	i < CLOCKORBIT_NUMGPS+b->NumberOfGLONASSSat; ++i)
676	{
677	G_GLONASS_SATELLITE_ID(b->Sat[i].ID)
678	G_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
679	for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
680	{
681	G_GLONASS_CODE_TYPE(b->Sat[i].Biases[j].Type)
682	G_CODE_BIAS(b->Sat[i].Biases[j].Bias)
683	}
684	}
685	break;
686	default:
687	return GCOBR_UNKNOWNTYPE;
688	}
689	if(bytesused)
690	*bytesused = sizeofrtcmblock+6;
691	return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
692	}
693	#endif /* NODECODE */

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

Download in other formats: