Context Navigation

source: ntrip/trunk/BNC/RTCM3/clock_orbit_rtcm.c@ 1305

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

Line
1	/* Programheader
2
3	Name: clock_orbit_rtcm.c
4	Project: RTCM3
5	Version: $Id: clock_orbit_rtcm.c,v 1.4.2.3 2008/12/19 09:29:13 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
13	#ifndef sparc
14	#include <stdint.h>
15	#endif
16
17	#include <sys/types.h>
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	len = CRC24(len+3, (const unsigned char *) blockstart); \
87	ADDBITS(24, len) \
88	}
89
90	#define SCALEADDBITS(a, b, c) ADDBITS(a, (int64_t)(b*c))
91
92	#if 0
93	#define DEBUGSCALEADDBITS(n, a, b, c) \
94	{ \
95	int64_t x = b*c, z; \
96	uint64_t y; \
97	y = (x&((1<<a)-1)); \
98	z = ((int64_t)(y<<(64-a)))>>(64-a); \
99	fprintf(stderr, "Type " # n " val %19.15f*%11.1f %16llX %16llX %16llX %s\n", \
100	c, b, x, y, z, x != z ? "OVERFLOW" : "OK"); \
101	} \
102	SCALEADDBITS(a,b,c)
103	#else
104	#define DEBUGSCALEADDBITS(n, a, b, c) SCALEADDBITS(a,b,c)
105	#endif
106
107	/* standard values */
108	#define T_MESSAGE_NUMBER(a) ADDBITS(12, a) /* DF002 */
109	#define T_RESERVED6 ADDBITS(6, 0) /* DF001 */
110	#define T_GPS_SATELLITE_ID(a) ADDBITS(6, a) /* DF068 */
111	#define T_GPS_IODE(a) ADDBITS(8, a) /* DF071 */
112	#define T_GLONASS_IOD(a) ADDBITS(8, a) /* DF237 */
113
114	/* defined values */
115	#define T_MULTIPLE_MESSAGE_INDICATOR(a) ADDBITS(1, a)
116	#define T_GPS_EPOCH_TIME(a) ADDBITS(20, a)
117	#define T_GLONASS_EPOCH_TIME(a) ADDBITS(17, a)
118	#define T_GLONASS_SATELLITE_ID(a) ADDBITS(6, a)
119	#define T_NO_OF_SATELLITES(a) ADDBITS(5, a)
120	#define T_SATELLITE_REFERENCE_POINT(a) ADDBITS(1, a)
121	#define T_SATELLITE_REFERENCE_DATUM(a) ADDBITS(1, a)
122	#define T_NO_OF_CODE_BIASES(a) ADDBITS(5, a)
123	#define T_GPS_CODE_TYPE(a) ADDBITS(5, a)
124	#define T_GLONASS_CODE_TYPE(a) ADDBITS(5, a)
125
126	/* yet undefined values */
127	#define T_DELTA_RADIAL(a) DEBUGSCALEADDBITS(dr, 20, 1000.0, a)
128	#define T_DELTA_ALONG_TRACK(a) DEBUGSCALEADDBITS(da, 20, 1000.0, a)
129	#define T_DELTA_CROSS_TRACK(a) DEBUGSCALEADDBITS(dc, 20, 1000.0, a)
130	#define T_DELTA_DOT_RADIAL(a) DEBUGSCALEADDBITS(Dr, 20, 100000.0, a)
131	#define T_DELTA_DOT_ALONG_TRACK(a) DEBUGSCALEADDBITS(Dr, 20, 100000.0, a)
132	#define T_DELTA_DOT_CROSS_TRACK(a) DEBUGSCALEADDBITS(Dr, 20, 100000.0, a)
133	#define T_DELTA_DOT_DOT_RADIAL(a) DEBUGSCALEADDBITS(DR, 20, 5000000.0, a)
134	#define T_DELTA_DOT_DOT_ALONG_TRACK(a) DEBUGSCALEADDBITS(DA, 20, 5000000.0, a)
135	#define T_DELTA_DOT_DOT_CROSS_TRACK(a) DEBUGSCALEADDBITS(DC, 20, 5000000.0, a)
136	#define T_DELTA_A0(a) DEBUGSCALEADDBITS(A0, 20, 1000.0, a)
137	#define T_DELTA_A1(a) DEBUGSCALEADDBITS(A1, 20, 100000.0, a)
138	#define T_DELTA_A2(a) DEBUGSCALEADDBITS(A2, 20, 5000000.0, a)
139	#define T_CODE_BIAS(a) DEBUGSCALEADDBITS(CB, 20, 100.0, a)
140
141	size_t MakeClockOrbit(const struct ClockOrbit *co, enum ClockOrbitType type,
142	int moremessagesfollow, char *buffer, size_t size)
143	{
144	int gpsor=0, gpscl=0, gpsco=0, gloor=0, glocl=0, gloco=0, mmi, i;
145	STARTDATA
146
147	if(co->NumberOfGPSSat && co->OrbitDataSupplied
148	&& (type == COTYPE_AUTO \|\| type == COTYPE_GPSORBIT))
149	gpsor = 1;
150	if(co->NumberOfGPSSat && co->ClockDataSupplied
151	&& (type == COTYPE_AUTO \|\| type == COTYPE_GPSCLOCK))
152	gpscl = 1;
153	if(co->NumberOfGPSSat && co->ClockDataSupplied && co->OrbitDataSupplied
154	&& (type == COTYPE_AUTO \|\| type == COTYPE_GPSCOMBINED))
155	{
156	gpsco = 1; gpsor = 0; gpscl = 0;
157	}
158	if(co->NumberOfGLONASSSat && co->OrbitDataSupplied
159	&& (type == COTYPE_AUTO \|\| type == COTYPE_GLONASSORBIT))
160	gloor = 1;
161	if(co->NumberOfGLONASSSat && co->ClockDataSupplied
162	&& (type == COTYPE_AUTO \|\| type == COTYPE_GLONASSCLOCK))
163	glocl = 1;
164	if(co->NumberOfGLONASSSat && co->ClockDataSupplied && co->OrbitDataSupplied
165	&& (type == COTYPE_AUTO \|\| type == COTYPE_GLONASSCOMBINED))
166	{
167	gloco = 1; gloor = 0; glocl = 0;
168	}
169
170	mmi = gpsor+gpscl+gpsco+gloor+glocl+gloco; /* required for multimessage */
171	if(!moremessagesfollow) --mmi;
172
173	if(gpsor)
174	{
175	INITBLOCK
176	T_MESSAGE_NUMBER(COTYPE_GPSORBIT)
177	T_GPS_EPOCH_TIME(co->GPSEpochTime)
178	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
179	--mmi;
180	T_RESERVED6
181	T_NO_OF_SATELLITES(co->NumberOfGPSSat)
182	for(i = 0; i < co->NumberOfGPSSat; ++i)
183	{
184	T_GPS_SATELLITE_ID(co->Sat[i].ID)
185	T_GPS_IODE(co->Sat[i].IOD)
186	T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
187	T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
188	T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
189	T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
190	T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
191	T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
192	T_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
193	T_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
194	T_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
195	T_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
196	T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
197	}
198	ENDBLOCK
199	}
200	if(gpscl)
201	{
202	INITBLOCK
203	T_MESSAGE_NUMBER(COTYPE_GPSCLOCK)
204	T_GPS_EPOCH_TIME(co->GPSEpochTime)
205	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
206	--mmi;
207	T_RESERVED6
208	T_NO_OF_SATELLITES(co->NumberOfGPSSat)
209	for(i = 0; i < co->NumberOfGPSSat; ++i)
210	{
211	T_GPS_SATELLITE_ID(co->Sat[i].ID)
212	T_GPS_IODE(co->Sat[i].IOD)
213	T_DELTA_A0(co->Sat[i].Clock.DeltaA0)
214	T_DELTA_A1(co->Sat[i].Clock.DeltaA1)
215	T_DELTA_A2(co->Sat[i].Clock.DeltaA2)
216	}
217	ENDBLOCK
218	}
219	if(gpsco)
220	{
221	INITBLOCK
222	T_MESSAGE_NUMBER(COTYPE_GPSCOMBINED)
223	T_GPS_EPOCH_TIME(co->GPSEpochTime)
224	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
225	--mmi;
226	T_RESERVED6
227	T_NO_OF_SATELLITES(co->NumberOfGPSSat)
228	for(i = 0; i < co->NumberOfGPSSat; ++i)
229	{
230	T_GPS_SATELLITE_ID(co->Sat[i].ID)
231	T_GPS_IODE(co->Sat[i].IOD)
232	T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
233	T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
234	T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
235	T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
236	T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
237	T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
238	T_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
239	T_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
240	T_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
241	T_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
242	T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
243	T_DELTA_A0(co->Sat[i].Clock.DeltaA0)
244	T_DELTA_A1(co->Sat[i].Clock.DeltaA1)
245	T_DELTA_A2(co->Sat[i].Clock.DeltaA2)
246	}
247	ENDBLOCK
248	}
249	if(gloor)
250	{
251	INITBLOCK
252	T_MESSAGE_NUMBER(COTYPE_GLONASSORBIT)
253	T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
254	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
255	--mmi;
256	T_RESERVED6
257	T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
258	for(i = CLOCKORBIT_NUMGPS;
259	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
260	{
261	T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
262	T_GLONASS_IOD(co->Sat[i].IOD)
263	T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
264	T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
265	T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
266	T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
267	T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
268	T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
269	T_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
270	T_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
271	T_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
272	T_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
273	T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
274	}
275	ENDBLOCK
276	}
277	if(glocl)
278	{
279	INITBLOCK
280	T_MESSAGE_NUMBER(COTYPE_GLONASSCLOCK)
281	T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
282	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
283	--mmi;
284	T_RESERVED6
285	T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
286	for(i = CLOCKORBIT_NUMGPS;
287	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
288	{
289	T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
290	T_GLONASS_IOD(co->Sat[i].IOD)
291	T_DELTA_A0(co->Sat[i].Clock.DeltaA0)
292	T_DELTA_A1(co->Sat[i].Clock.DeltaA1)
293	T_DELTA_A2(co->Sat[i].Clock.DeltaA2)
294	}
295	ENDBLOCK
296	}
297	if(gloco)
298	{
299	INITBLOCK
300	T_MESSAGE_NUMBER(COTYPE_GLONASSCOMBINED)
301	T_GLONASS_EPOCH_TIME(co->GLONASSEpochTime)
302	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
303	--mmi;
304	T_RESERVED6
305	T_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
306	for(i = CLOCKORBIT_NUMGPS;
307	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
308	{
309	T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
310	T_GLONASS_IOD(co->Sat[i].IOD)
311	T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
312	T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
313	T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
314	T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
315	T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
316	T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
317	T_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
318	T_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
319	T_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
320	T_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
321	T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
322	T_DELTA_A0(co->Sat[i].Clock.DeltaA0)
323	T_DELTA_A1(co->Sat[i].Clock.DeltaA1)
324	T_DELTA_A2(co->Sat[i].Clock.DeltaA2)
325	}
326	ENDBLOCK
327	}
328
329	return ressize;
330	}
331
332	size_t MakeBias(const struct Bias *b, enum BiasType type,
333	int moremessagesfollow, char *buffer, size_t size)
334	{
335	int gps = 0;
336	int glo = 0;
337	int mmi, i, j;
338	STARTDATA
339
340	if(b->NumberOfGPSSat && (type == BTYPE_AUTO \|\| type == BTYPE_GPS))
341	gps = 1;
342	if(b->NumberOfGLONASSSat && (type == BTYPE_AUTO \|\| type == BTYPE_GLONASS))
343	glo = 1;
344
345	mmi = gps+glo; /* required for multimessage */
346	if(!moremessagesfollow) --mmi;
347
348	if(gps)
349	{
350	INITBLOCK
351	T_MESSAGE_NUMBER(BTYPE_GPS)
352	T_GPS_EPOCH_TIME(b->GPSEpochTime)
353	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
354	--mmi;
355	T_RESERVED6
356	T_NO_OF_SATELLITES(b->NumberOfGPSSat)
357	for(i = 0; i < b->NumberOfGPSSat; ++i)
358	{
359	T_GPS_SATELLITE_ID(b->Sat[i].ID)
360	T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
361	for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
362	{
363	T_GPS_CODE_TYPE(b->Sat[i].Biases[j].Type)
364	T_CODE_BIAS(b->Sat[i].Biases[j].Bias)
365	}
366	}
367	ENDBLOCK
368	}
369	if(glo)
370	{
371	INITBLOCK
372	T_MESSAGE_NUMBER(BTYPE_GLONASS)
373	T_GPS_EPOCH_TIME(b->GLONASSEpochTime)
374	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
375	--mmi;
376	T_RESERVED6
377	T_NO_OF_SATELLITES(b->NumberOfGLONASSSat)
378	for(i = CLOCKORBIT_NUMGPS;
379	i < CLOCKORBIT_NUMGPS+b->NumberOfGLONASSSat; ++i)
380	{
381	T_GLONASS_SATELLITE_ID(b->Sat[i].ID)
382	T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
383	for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
384	{
385	T_GLONASS_CODE_TYPE(b->Sat[i].Biases[j].Type)
386	T_CODE_BIAS(b->Sat[i].Biases[j].Bias)
387	}
388	}
389	ENDBLOCK
390	}
391
392	return ressize;
393	}
394	#endif /* NOENCODE */
395
396	#ifndef NODECODE
397
398	#define DECODESTART \
399	int numbits=0; \
400	uint64_t bitbuffer=0;
401
402	#define LOADBITS(a) \
403	{ \
404	while((a) > numbits) \
405	{ \
406	if(!size--) return GCOBR_SHORTBUFFER; \
407	bitbuffer = (bitbuffer<<8)\|((unsigned char)*(buffer++)); \
408	numbits += 8; \
409	} \
410	}
411
412	/* extract bits from data stream
413	b = variable to store result, a = number of bits */
414	#define GETBITS(b, a) \
415	{ \
416	LOADBITS(a) \
417	b = (bitbuffer<<(64-numbits))>>(64-(a)); \
418	numbits -= (a); \
419	}
420
421	/* extract signed floating value from data stream
422	b = variable to store result, a = number of bits */
423	#define GETFLOATSIGN(b, a, c) \
424	{ \
425	LOADBITS(a) \
426	b = ((double)(((int64_t)(bitbuffer<<(64-numbits)))>>(64-(a))))*(c); \
427	numbits -= (a); \
428	}
429
430	#define SKIPBITS(b) { LOADBITS(b) numbits -= (b); }
431
432	/* standard values */
433	#define G_HEADER(a) GETBITS(a,8)
434	#define G_RESERVEDH(a) GETBITS(a,6)
435	#define G_SIZE(a) GETBITS(a, 10)
436	#define G_MESSAGE_NUMBER(a) GETBITS(a, 12) /* DF002 */
437	#define G_RESERVED6 SKIPBITS(6) /* DF001 */
438	#define G_GPS_SATELLITE_ID(a) {int temp; GETBITS(temp, 6) \
439	if(a && a != temp) return GCOBR_DATAMISMATCH; a = temp;} /* DF068 */
440	#define G_GPS_IODE(a) GETBITS(a, 8) /* DF071 */
441	#define G_GLONASS_IOD(a) GETBITS(a, 8) /* DF237 */
442
443	/* defined values */
444	#define G_MULTIPLE_MESSAGE_INDICATOR(a) GETBITS(a, 1)
445	#define G_GPS_EPOCH_TIME(a, b) {int temp; GETBITS(temp, 20) \
446	if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
447	#define G_GLONASS_EPOCH_TIME(a, b) {int temp; GETBITS(temp, 17) \
448	if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
449	#define G_GLONASS_SATELLITE_ID(a) {int temp; GETBITS(temp, 6) \
450	if(a && a != temp) return GCOBR_DATAMISMATCH; a = temp;}
451	#define G_NO_OF_SATELLITES(a) {int temp; GETBITS(temp, 5) \
452	if(a && a != temp) return GCOBR_DATAMISMATCH; a = temp;}
453	#define G_SATELLITE_REFERENCE_POINT(a) GETBITS(a, 1)
454	#define G_SATELLITE_REFERENCE_DATUM(a) GETBITS(a, 1)
455	#define G_NO_OF_CODE_BIASES(a) GETBITS(a, 5)
456	#define G_GPS_CODE_TYPE(a) GETBITS(a, 5)
457	#define G_GLONASS_CODE_TYPE(a) GETBITS(a, 5)
458
459	/* yet undefined values */
460	#define G_DELTA_RADIAL(a) GETFLOATSIGN(a, 20, 1/1000.0)
461	#define G_DELTA_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/1000.0)
462	#define G_DELTA_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/1000.0)
463	#define G_DELTA_DOT_RADIAL(a) GETFLOATSIGN(a, 20, 1/100000.0)
464	#define G_DELTA_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/100000.0)
465	#define G_DELTA_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/100000.0)
466	#define G_DELTA_DOT_DOT_RADIAL(a) GETFLOATSIGN(a, 20, 1/5000000.0)
467	#define G_DELTA_DOT_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/5000000.0)
468	#define G_DELTA_DOT_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/5000000.0)
469	#define G_DELTA_A0(a) GETFLOATSIGN(a, 20, 1/1000.0)
470	#define G_DELTA_A1(a) GETFLOATSIGN(a, 20, 1/100000.0)
471	#define G_DELTA_A2(a) GETFLOATSIGN(a, 20, 1/5000000.0)
472	#define G_CODE_BIAS(a) GETFLOATSIGN(a, 20, 1/100.0)
473
474	enum GCOB_RETURN GetClockOrbitBias(struct ClockOrbit co, struct Bias b,
475	const char buffer, size_t size, int bytesused)
476	{
477	int type, mmi=0, i, j, h, rs, sizeofrtcmblock;
478	const char *blockstart = buffer;
479	DECODESTART
480
481	if(size < 7)
482	return GCOBR_SHORTBUFFER;
483
484	G_HEADER(h)
485	G_RESERVEDH(rs)
486	G_SIZE(sizeofrtcmblock);
487
488	if((unsigned char)h != 0xD3 \|\| rs)
489	return GCOBR_UNKNOWNDATA;
490	if(size < sizeofrtcmblock + 3) /* 3 header bytes already removed */
491	return GCOBR_MESSAGEEXCEEDSBUFFER;
492	if(CRC24(sizeofrtcmblock+3, (const unsigned char *) blockstart) !=
493	((((unsigned char)buffer[sizeofrtcmblock])<<16)\|
494	(((unsigned char)buffer[sizeofrtcmblock+1])<<8)\|
495	(((unsigned char)buffer[sizeofrtcmblock+2]))))
496	return GCOBR_CRCMISMATCH;
497
498	G_MESSAGE_NUMBER(type)
499	switch(type)
500	{
501	case COTYPE_GPSORBIT:
502	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
503	G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
504	co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
505	if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
506	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
507	G_RESERVED6
508	G_NO_OF_SATELLITES(co->NumberOfGPSSat)
509	if(co->OrbitDataSupplied)
510	return GCOBR_DATAMISMATCH;
511	co->OrbitDataSupplied = 1;
512	for(i = 0; i < co->NumberOfGPSSat; ++i)
513	{
514	G_GPS_SATELLITE_ID(co->Sat[i].ID)
515	G_GPS_IODE(co->Sat[i].IOD)
516	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
517	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
518	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
519	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
520	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
521	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
522	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
523	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
524	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
525	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
526	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
527	}
528	break;
529	case COTYPE_GPSCLOCK:
530	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
531	G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
532	co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
533	if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
534	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
535	G_RESERVED6
536	G_NO_OF_SATELLITES(co->NumberOfGPSSat)
537	if(co->ClockDataSupplied)
538	return GCOBR_DATAMISMATCH;
539	co->ClockDataSupplied = 1;
540	for(i = 0; i < co->NumberOfGPSSat; ++i)
541	{
542	G_GPS_SATELLITE_ID(co->Sat[i].ID)
543	G_GPS_IODE(co->Sat[i].IOD)
544	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
545	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
546	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
547	}
548	break;
549	case COTYPE_GPSCOMBINED:
550	if(!co) return -5;
551	G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
552	co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
553	if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
554	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
555	G_RESERVED6
556	G_NO_OF_SATELLITES(co->NumberOfGPSSat)
557	if(co->ClockDataSupplied \|\| co->OrbitDataSupplied)
558	return GCOBR_DATAMISMATCH;
559	co->OrbitDataSupplied = 1;
560	co->ClockDataSupplied = 1;
561	for(i = 0; i < co->NumberOfGPSSat; ++i)
562	{
563	G_GPS_SATELLITE_ID(co->Sat[i].ID)
564	G_GPS_IODE(co->Sat[i].IOD)
565	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
566	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
567	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
568	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
569	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
570	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
571	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
572	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
573	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
574	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
575	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
576	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
577	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
578	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
579	}
580	break;
581	case COTYPE_GLONASSORBIT:
582	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
583	G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
584	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
585	G_RESERVED6
586	G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
587	if(co->OrbitDataSupplied)
588	return GCOBR_DATAMISMATCH;
589	co->OrbitDataSupplied = 1;
590	for(i = CLOCKORBIT_NUMGPS;
591	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
592	{
593	G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
594	G_GLONASS_IOD(co->Sat[i].IOD)
595	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
596	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
597	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
598	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
599	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
600	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
601	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
602	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
603	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
604	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
605	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
606	}
607	break;
608	case COTYPE_GLONASSCLOCK:
609	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
610	G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
611	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
612	G_RESERVED6
613	G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
614	if(co->ClockDataSupplied)
615	return GCOBR_DATAMISMATCH;
616	co->ClockDataSupplied = 1;
617	for(i = CLOCKORBIT_NUMGPS;
618	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
619	{
620	G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
621	G_GLONASS_IOD(co->Sat[i].IOD)
622	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
623	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
624	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
625	}
626	break;
627	case COTYPE_GLONASSCOMBINED:
628	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
629	G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
630	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
631	G_RESERVED6
632	G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
633	if(co->ClockDataSupplied \|\| co->OrbitDataSupplied)
634	return GCOBR_DATAMISMATCH;
635	co->OrbitDataSupplied = 1;
636	co->ClockDataSupplied = 1;
637	for(i = CLOCKORBIT_NUMGPS;
638	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
639	{
640	G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
641	G_GLONASS_IOD(co->Sat[i].IOD)
642	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
643	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
644	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
645	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
646	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
647	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
648	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
649	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
650	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
651	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
652	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
653	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
654	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
655	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
656	}
657	break;
658	case BTYPE_GPS:
659	if(!b) return GCOBR_NOBIASPARAMETER;
660	G_GPS_EPOCH_TIME(b->GPSEpochTime, co->NumberOfGPSSat)
661	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
662	G_RESERVED6
663	G_NO_OF_SATELLITES(b->NumberOfGPSSat)
664	for(i = 0; i < b->NumberOfGPSSat; ++i)
665	{
666	G_GPS_SATELLITE_ID(b->Sat[i].ID)
667	G_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
668	for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
669	{
670	G_GPS_CODE_TYPE(b->Sat[i].Biases[j].Type)
671	G_CODE_BIAS(b->Sat[i].Biases[j].Bias)
672	}
673	}
674	break;
675	case BTYPE_GLONASS:
676	if(!b) return GCOBR_NOBIASPARAMETER;
677	G_GPS_EPOCH_TIME(b->GLONASSEpochTime, co->NumberOfGLONASSSat)
678	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
679	G_RESERVED6
680	G_NO_OF_SATELLITES(b->NumberOfGLONASSSat)
681	for(i = CLOCKORBIT_NUMGPS;
682	i < CLOCKORBIT_NUMGPS+b->NumberOfGLONASSSat; ++i)
683	{
684	G_GLONASS_SATELLITE_ID(b->Sat[i].ID)
685	G_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
686	for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
687	{
688	G_GLONASS_CODE_TYPE(b->Sat[i].Biases[j].Type)
689	G_CODE_BIAS(b->Sat[i].Biases[j].Bias)
690	}
691	}
692	break;
693	default:
694	return GCOBR_UNKNOWNTYPE;
695	}
696	if(bytesused)
697	*bytesused = sizeofrtcmblock+6;
698	return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
699	}
700	#endif /* NODECODE */

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

Download in other formats: