Context Navigation

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

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

Line
1	/* Programheader
2
3	Name: clock_orbit_rtcm.c
4	Project: RTCM3
5	Version: $Id$
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	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
498	G_RESERVED6
499	G_NO_OF_SATELLITES(co->NumberOfGPSSat)
500	if(co->OrbitDataSupplied)
501	return GCOBR_DATAMISMATCH;
502	co->OrbitDataSupplied = 1;
503	for(i = 0; i < co->NumberOfGPSSat; ++i)
504	{
505	G_GPS_SATELLITE_ID(co->Sat[i].ID)
506	G_GPS_IODE(co->Sat[i].IOD)
507	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
508	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
509	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
510	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
511	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
512	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
513	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
514	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
515	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
516	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
517	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
518	}
519	break;
520	case COTYPE_GPSCLOCK:
521	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
522	G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
523	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
524	G_RESERVED6
525	G_NO_OF_SATELLITES(co->NumberOfGPSSat)
526	if(co->ClockDataSupplied)
527	return GCOBR_DATAMISMATCH;
528	co->ClockDataSupplied = 1;
529	for(i = 0; i < co->NumberOfGPSSat; ++i)
530	{
531	G_GPS_SATELLITE_ID(co->Sat[i].ID)
532	G_GPS_IODE(co->Sat[i].IOD)
533	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
534	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
535	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
536	}
537	break;
538	case COTYPE_GPSCOMBINED:
539	if(!co) return -5;
540	G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
541	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
542	G_RESERVED6
543	G_NO_OF_SATELLITES(co->NumberOfGPSSat)
544	if(co->ClockDataSupplied \|\| co->OrbitDataSupplied)
545	return GCOBR_DATAMISMATCH;
546	co->OrbitDataSupplied = 1;
547	co->ClockDataSupplied = 1;
548	for(i = 0; i < co->NumberOfGPSSat; ++i)
549	{
550	G_GPS_SATELLITE_ID(co->Sat[i].ID)
551	G_GPS_IODE(co->Sat[i].IOD)
552	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
553	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
554	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
555	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
556	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
557	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
558	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
559	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
560	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
561	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
562	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
563	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
564	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
565	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
566	}
567	break;
568	case COTYPE_GLONASSORBIT:
569	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
570	G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
571	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
572	G_RESERVED6
573	G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
574	if(co->OrbitDataSupplied)
575	return GCOBR_DATAMISMATCH;
576	co->OrbitDataSupplied = 1;
577	for(i = CLOCKORBIT_NUMGPS;
578	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
579	{
580	G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
581	G_GLONASS_IOD(co->Sat[i].IOD)
582	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
583	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
584	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
585	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
586	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
587	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
588	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
589	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
590	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
591	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
592	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
593	}
594	break;
595	case COTYPE_GLONASSCLOCK:
596	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
597	G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
598	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
599	G_RESERVED6
600	G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
601	if(co->ClockDataSupplied)
602	return GCOBR_DATAMISMATCH;
603	co->ClockDataSupplied = 1;
604	for(i = CLOCKORBIT_NUMGPS;
605	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
606	{
607	G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
608	G_GLONASS_IOD(co->Sat[i].IOD)
609	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
610	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
611	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
612	}
613	break;
614	case COTYPE_GLONASSCOMBINED:
615	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
616	G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
617	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
618	G_RESERVED6
619	G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
620	if(co->ClockDataSupplied \|\| co->OrbitDataSupplied)
621	return GCOBR_DATAMISMATCH;
622	co->OrbitDataSupplied = 1;
623	co->ClockDataSupplied = 1;
624	for(i = CLOCKORBIT_NUMGPS;
625	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
626	{
627	G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
628	G_GLONASS_IOD(co->Sat[i].IOD)
629	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
630	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
631	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
632	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
633	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
634	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
635	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
636	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
637	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
638	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
639	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
640	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
641	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
642	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
643	}
644	break;
645	case BTYPE_GPS:
646	if(!b) return GCOBR_NOBIASPARAMETER;
647	G_GPS_EPOCH_TIME(b->GPSEpochTime, co->NumberOfGPSSat)
648	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
649	G_RESERVED6
650	G_NO_OF_SATELLITES(b->NumberOfGPSSat)
651	for(i = 0; i < b->NumberOfGPSSat; ++i)
652	{
653	G_GPS_SATELLITE_ID(b->Sat[i].ID)
654	G_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
655	for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
656	{
657	G_GPS_CODE_TYPE(b->Sat[i].Biases[j].Type)
658	G_CODE_BIAS(b->Sat[i].Biases[j].Bias)
659	}
660	}
661	break;
662	case BTYPE_GLONASS:
663	if(!b) return GCOBR_NOBIASPARAMETER;
664	G_GPS_EPOCH_TIME(b->GLONASSEpochTime, co->NumberOfGLONASSSat)
665	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
666	G_RESERVED6
667	G_NO_OF_SATELLITES(b->NumberOfGLONASSSat)
668	for(i = CLOCKORBIT_NUMGPS;
669	i < CLOCKORBIT_NUMGPS+b->NumberOfGLONASSSat; ++i)
670	{
671	G_GLONASS_SATELLITE_ID(b->Sat[i].ID)
672	G_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
673	for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
674	{
675	G_GLONASS_CODE_TYPE(b->Sat[i].Biases[j].Type)
676	G_CODE_BIAS(b->Sat[i].Biases[j].Bias)
677	}
678	}
679	break;
680	default:
681	return GCOBR_UNKNOWNTYPE;
682	}
683	if(bytesused)
684	*bytesused = sizeofrtcmblock+6;
685	return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
686	}
687	#endif /* NODECODE */

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

Download in other formats: