Context Navigation

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

Last change on this file since 1282 was 1211, checked in by mervart, 16 years ago
* empty log message *
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/17 11:50:45 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 = 0;
331	int glo = 0;
332	int mmi, i, j;
333	STARTDATA
334
335	if(b->NumberOfGPSSat && (type == BTYPE_AUTO \|\| type == BTYPE_GPS))
336	gps = 1;
337	if(b->NumberOfGLONASSSat && (type == BTYPE_AUTO \|\| type == BTYPE_GLONASS))
338	glo = 1;
339
340	mmi = gps+glo; /* required for multimessage */
341	if(!moremessagesfollow) --mmi;
342
343	if(gps)
344	{
345	INITBLOCK
346	T_MESSAGE_NUMBER(BTYPE_GPS)
347	T_GPS_EPOCH_TIME(b->GPSEpochTime)
348	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
349	--mmi;
350	T_RESERVED6
351	T_NO_OF_SATELLITES(b->NumberOfGPSSat)
352	for(i = 0; i < b->NumberOfGPSSat; ++i)
353	{
354	T_GPS_SATELLITE_ID(b->Sat[i].ID)
355	T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
356	for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
357	{
358	T_GPS_CODE_TYPE(b->Sat[i].Biases[j].Type)
359	T_CODE_BIAS(b->Sat[i].Biases[j].Bias)
360	}
361	}
362	ENDBLOCK
363	}
364	if(glo)
365	{
366	INITBLOCK
367	T_MESSAGE_NUMBER(BTYPE_GLONASS)
368	T_GPS_EPOCH_TIME(b->GLONASSEpochTime)
369	T_MULTIPLE_MESSAGE_INDICATOR(mmi ? 1 :0)
370	--mmi;
371	T_RESERVED6
372	T_NO_OF_SATELLITES(b->NumberOfGLONASSSat)
373	for(i = CLOCKORBIT_NUMGPS;
374	i < CLOCKORBIT_NUMGPS+b->NumberOfGLONASSSat; ++i)
375	{
376	T_GLONASS_SATELLITE_ID(b->Sat[i].ID)
377	T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
378	for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
379	{
380	T_GLONASS_CODE_TYPE(b->Sat[i].Biases[j].Type)
381	T_CODE_BIAS(b->Sat[i].Biases[j].Bias)
382	}
383	}
384	ENDBLOCK
385	}
386
387	return ressize;
388	}
389	#endif /* NOENCODE */
390
391	#ifndef NODECODE
392
393	#define DECODESTART \
394	int numbits=0; \
395	uint64_t bitbuffer=0;
396
397	#define LOADBITS(a) \
398	{ \
399	while((a) > numbits) \
400	{ \
401	if(!size--) return GCOBR_SHORTBUFFER; \
402	bitbuffer = (bitbuffer<<8)\|((unsigned char)*(buffer++)); \
403	numbits += 8; \
404	} \
405	}
406
407	/* extract bits from data stream
408	b = variable to store result, a = number of bits */
409	#define GETBITS(b, a) \
410	{ \
411	LOADBITS(a) \
412	b = (bitbuffer<<(64-numbits))>>(64-(a)); \
413	numbits -= (a); \
414	}
415
416	/* extract signed floating value from data stream
417	b = variable to store result, a = number of bits */
418	#define GETFLOATSIGN(b, a, c) \
419	{ \
420	LOADBITS(a) \
421	b = ((double)(((int64_t)(bitbuffer<<(64-numbits)))>>(64-(a))))*(c); \
422	numbits -= (a); \
423	}
424
425	#define SKIPBITS(b) { LOADBITS(b) numbits -= (b); }
426
427	/* standard values */
428	#define G_HEADER(a) GETBITS(a,8)
429	#define G_RESERVEDH(a) GETBITS(a,6)
430	#define G_SIZE(a) GETBITS(a, 10)
431	#define G_MESSAGE_NUMBER(a) GETBITS(a, 12) /* DF002 */
432	#define G_RESERVED6 SKIPBITS(6) /* DF001 */
433	#define G_GPS_SATELLITE_ID(a) {int temp; GETBITS(temp, 6) \
434	if(a && a != temp) return GCOBR_DATAMISMATCH; a = temp;} /* DF068 */
435	#define G_GPS_IODE(a) GETBITS(a, 8) /* DF071 */
436	#define G_GLONASS_IOD(a) GETBITS(a, 8) /* DF237 */
437
438	/* defined values */
439	#define G_MULTIPLE_MESSAGE_INDICATOR(a) GETBITS(a, 1)
440	#define G_GPS_EPOCH_TIME(a, b) {int temp; GETBITS(temp, 20) \
441	if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
442	#define G_GLONASS_EPOCH_TIME(a, b) {int temp; GETBITS(temp, 17) \
443	if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
444	#define G_GLONASS_SATELLITE_ID(a) {int temp; GETBITS(temp, 6) \
445	if(a && a != temp) return GCOBR_DATAMISMATCH; a = temp;}
446	#define G_NO_OF_SATELLITES(a) {int temp; GETBITS(temp, 5) \
447	if(a && a != temp) return GCOBR_DATAMISMATCH; a = temp;}
448	#define G_SATELLITE_REFERENCE_POINT(a) GETBITS(a, 1)
449	#define G_SATELLITE_REFERENCE_DATUM(a) GETBITS(a, 1)
450	#define G_NO_OF_CODE_BIASES(a) GETBITS(a, 5)
451	#define G_GPS_CODE_TYPE(a) GETBITS(a, 5)
452	#define G_GLONASS_CODE_TYPE(a) GETBITS(a, 5)
453
454	/* yet undefined values */
455	#define G_DELTA_RADIAL(a) GETFLOATSIGN(a, 20, 1/1000.0)
456	#define G_DELTA_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/1000.0)
457	#define G_DELTA_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/1000.0)
458	#define G_DELTA_DOT_RADIAL(a) GETFLOATSIGN(a, 20, 1/100000.0)
459	#define G_DELTA_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/100000.0)
460	#define G_DELTA_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/100000.0)
461	#define G_DELTA_DOT_DOT_RADIAL(a) GETFLOATSIGN(a, 20, 1/5000000.0)
462	#define G_DELTA_DOT_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/5000000.0)
463	#define G_DELTA_DOT_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/5000000.0)
464	#define G_DELTA_A0(a) GETFLOATSIGN(a, 20, 1/1000.0)
465	#define G_DELTA_A1(a) GETFLOATSIGN(a, 20, 1/100000.0)
466	#define G_DELTA_A2(a) GETFLOATSIGN(a, 20, 1/5000000.0)
467	#define G_CODE_BIAS(a) GETFLOATSIGN(a, 20, 1/100.0)
468
469	enum GCOB_RETURN GetClockOrbitBias(struct ClockOrbit co, struct Bias b,
470	const char buffer, size_t size, int bytesused)
471	{
472	int type, mmi=0, i, j, h, rs, sizeofrtcmblock;
473	const char *blockstart = buffer;
474	DECODESTART
475
476	if(size < 7)
477	return GCOBR_SHORTBUFFER;
478
479	G_HEADER(h)
480	G_RESERVEDH(rs)
481	G_SIZE(sizeofrtcmblock);
482
483	if((unsigned char)h != 0xD3 \|\| rs)
484	return GCOBR_UNKNOWNDATA;
485	if(size < sizeofrtcmblock + 3) /* 3 header bytes already removed */
486	return GCOBR_MESSAGEEXCEEDSBUFFER;
487	if(CRC24(sizeofrtcmblock+3, (const unsigned char *) blockstart) !=
488	((((unsigned char)buffer[sizeofrtcmblock])<<16)\|
489	(((unsigned char)buffer[sizeofrtcmblock+1])<<8)\|
490	(((unsigned char)buffer[sizeofrtcmblock+2]))))
491	return GCOBR_CRCMISMATCH;
492
493	G_MESSAGE_NUMBER(type)
494	switch(type)
495	{
496	case COTYPE_GPSORBIT:
497	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
498	G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
499	co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
500	if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
501	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
502	G_RESERVED6
503	G_NO_OF_SATELLITES(co->NumberOfGPSSat)
504	if(co->OrbitDataSupplied)
505	return GCOBR_DATAMISMATCH;
506	co->OrbitDataSupplied = 1;
507	for(i = 0; i < co->NumberOfGPSSat; ++i)
508	{
509	G_GPS_SATELLITE_ID(co->Sat[i].ID)
510	G_GPS_IODE(co->Sat[i].IOD)
511	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
512	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
513	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
514	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
515	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
516	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
517	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
518	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
519	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
520	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
521	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
522	}
523	break;
524	case COTYPE_GPSCLOCK:
525	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
526	G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
527	co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
528	if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
529	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
530	G_RESERVED6
531	G_NO_OF_SATELLITES(co->NumberOfGPSSat)
532	if(co->ClockDataSupplied)
533	return GCOBR_DATAMISMATCH;
534	co->ClockDataSupplied = 1;
535	for(i = 0; i < co->NumberOfGPSSat; ++i)
536	{
537	G_GPS_SATELLITE_ID(co->Sat[i].ID)
538	G_GPS_IODE(co->Sat[i].IOD)
539	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
540	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
541	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
542	}
543	break;
544	case COTYPE_GPSCOMBINED:
545	if(!co) return -5;
546	G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
547	co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
548	if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
549	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
550	G_RESERVED6
551	G_NO_OF_SATELLITES(co->NumberOfGPSSat)
552	if(co->ClockDataSupplied \|\| co->OrbitDataSupplied)
553	return GCOBR_DATAMISMATCH;
554	co->OrbitDataSupplied = 1;
555	co->ClockDataSupplied = 1;
556	for(i = 0; i < co->NumberOfGPSSat; ++i)
557	{
558	G_GPS_SATELLITE_ID(co->Sat[i].ID)
559	G_GPS_IODE(co->Sat[i].IOD)
560	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
561	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
562	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
563	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
564	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
565	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
566	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
567	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
568	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
569	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
570	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
571	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
572	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
573	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
574	}
575	break;
576	case COTYPE_GLONASSORBIT:
577	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
578	G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
579	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
580	G_RESERVED6
581	G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
582	if(co->OrbitDataSupplied)
583	return GCOBR_DATAMISMATCH;
584	co->OrbitDataSupplied = 1;
585	for(i = CLOCKORBIT_NUMGPS;
586	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
587	{
588	G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
589	G_GLONASS_IOD(co->Sat[i].IOD)
590	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
591	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
592	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
593	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
594	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
595	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
596	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
597	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
598	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
599	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
600	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
601	}
602	break;
603	case COTYPE_GLONASSCLOCK:
604	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
605	G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
606	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
607	G_RESERVED6
608	G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
609	if(co->ClockDataSupplied)
610	return GCOBR_DATAMISMATCH;
611	co->ClockDataSupplied = 1;
612	for(i = CLOCKORBIT_NUMGPS;
613	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
614	{
615	G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
616	G_GLONASS_IOD(co->Sat[i].IOD)
617	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
618	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
619	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
620	}
621	break;
622	case COTYPE_GLONASSCOMBINED:
623	if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
624	G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
625	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
626	G_RESERVED6
627	G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
628	if(co->ClockDataSupplied \|\| co->OrbitDataSupplied)
629	return GCOBR_DATAMISMATCH;
630	co->OrbitDataSupplied = 1;
631	co->ClockDataSupplied = 1;
632	for(i = CLOCKORBIT_NUMGPS;
633	i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
634	{
635	G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
636	G_GLONASS_IOD(co->Sat[i].IOD)
637	G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
638	G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
639	G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
640	G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
641	G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
642	G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
643	G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
644	G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
645	G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
646	G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
647	G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
648	G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
649	G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
650	G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
651	}
652	break;
653	case BTYPE_GPS:
654	if(!b) return GCOBR_NOBIASPARAMETER;
655	G_GPS_EPOCH_TIME(b->GPSEpochTime, co->NumberOfGPSSat)
656	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
657	G_RESERVED6
658	G_NO_OF_SATELLITES(b->NumberOfGPSSat)
659	for(i = 0; i < b->NumberOfGPSSat; ++i)
660	{
661	G_GPS_SATELLITE_ID(b->Sat[i].ID)
662	G_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
663	for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
664	{
665	G_GPS_CODE_TYPE(b->Sat[i].Biases[j].Type)
666	G_CODE_BIAS(b->Sat[i].Biases[j].Bias)
667	}
668	}
669	break;
670	case BTYPE_GLONASS:
671	if(!b) return GCOBR_NOBIASPARAMETER;
672	G_GPS_EPOCH_TIME(b->GLONASSEpochTime, co->NumberOfGLONASSSat)
673	G_MULTIPLE_MESSAGE_INDICATOR(mmi)
674	G_RESERVED6
675	G_NO_OF_SATELLITES(b->NumberOfGLONASSSat)
676	for(i = CLOCKORBIT_NUMGPS;
677	i < CLOCKORBIT_NUMGPS+b->NumberOfGLONASSSat; ++i)
678	{
679	G_GLONASS_SATELLITE_ID(b->Sat[i].ID)
680	G_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
681	for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
682	{
683	G_GLONASS_CODE_TYPE(b->Sat[i].Biases[j].Type)
684	G_CODE_BIAS(b->Sat[i].Biases[j].Bias)
685	}
686	}
687	break;
688	default:
689	return GCOBR_UNKNOWNTYPE;
690	}
691	if(bytesused)
692	*bytesused = sizeofrtcmblock+6;
693	return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
694	}
695	#endif /* NODECODE */

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

Download in other formats: