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

Last change on this file since 1661 was 1661, checked in by weber, 15 years ago

* empty log message *
  • Property svn:executable set to *
File size: 23.8 KB
Line 
1/* Programheader
2
3 Name: clock_orbit_rtcm.c
4 Project: RTCM3
5 Version: $Id: clock_orbit_rtcm.c,v 1.6 2009/02/04 08:19:20 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
20static 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
39the values. This may not be optimized in terms of final program code size but
40should be optimized in terms of speed.
41
42modified 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
141size_t MakeClockOrbit(const struct ClockOrbit *co, enum ClockOrbitType type,
142int 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
332size_t MakeBias(const struct Bias *b, enum BiasType type,
333int 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
474enum GCOB_RETURN GetClockOrbitBias(struct ClockOrbit *co, struct Bias *b,
475const 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 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
505 G_RESERVED6
506 G_NO_OF_SATELLITES(co->NumberOfGPSSat)
507 if(co->OrbitDataSupplied)
508 return GCOBR_DATAMISMATCH;
509 co->OrbitDataSupplied = 1;
510 for(i = 0; i < co->NumberOfGPSSat; ++i)
511 {
512 G_GPS_SATELLITE_ID(co->Sat[i].ID)
513 G_GPS_IODE(co->Sat[i].IOD)
514 G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
515 G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
516 G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
517 G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
518 G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
519 G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
520 G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
521 G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
522 G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
523 G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
524 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
525 }
526 break;
527 case COTYPE_GPSCLOCK:
528 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
529 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
530 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
531 G_RESERVED6
532 G_NO_OF_SATELLITES(co->NumberOfGPSSat)
533 if(co->ClockDataSupplied)
534 return GCOBR_DATAMISMATCH;
535 co->ClockDataSupplied = 1;
536 for(i = 0; i < co->NumberOfGPSSat; ++i)
537 {
538 G_GPS_SATELLITE_ID(co->Sat[i].ID)
539 G_GPS_IODE(co->Sat[i].IOD)
540 G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
541 G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
542 G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
543 }
544 break;
545 case COTYPE_GPSCOMBINED:
546 if(!co) return -5;
547 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
548 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
549 G_RESERVED6
550 G_NO_OF_SATELLITES(co->NumberOfGPSSat)
551 if(co->ClockDataSupplied || co->OrbitDataSupplied)
552 return GCOBR_DATAMISMATCH;
553 co->OrbitDataSupplied = 1;
554 co->ClockDataSupplied = 1;
555 for(i = 0; i < co->NumberOfGPSSat; ++i)
556 {
557 G_GPS_SATELLITE_ID(co->Sat[i].ID)
558 G_GPS_IODE(co->Sat[i].IOD)
559 G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
560 G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
561 G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
562 G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
563 G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
564 G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
565 G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
566 G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
567 G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
568 G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
569 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
570 G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
571 G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
572 G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
573 }
574 break;
575 case COTYPE_GLONASSORBIT:
576 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
577 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
578 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
579 G_RESERVED6
580 G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
581 if(co->OrbitDataSupplied)
582 return GCOBR_DATAMISMATCH;
583 co->OrbitDataSupplied = 1;
584 for(i = CLOCKORBIT_NUMGPS;
585 i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
586 {
587 G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
588 G_GLONASS_IOD(co->Sat[i].IOD)
589 G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
590 G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
591 G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
592 G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
593 G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
594 G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
595 G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
596 G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
597 G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
598 G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
599 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
600 }
601 break;
602 case COTYPE_GLONASSCLOCK:
603 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
604 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
605 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
606 G_RESERVED6
607 G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
608 if(co->ClockDataSupplied)
609 return GCOBR_DATAMISMATCH;
610 co->ClockDataSupplied = 1;
611 for(i = CLOCKORBIT_NUMGPS;
612 i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
613 {
614 G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
615 G_GLONASS_IOD(co->Sat[i].IOD)
616 G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
617 G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
618 G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
619 }
620 break;
621 case COTYPE_GLONASSCOMBINED:
622 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
623 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
624 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
625 G_RESERVED6
626 G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
627 if(co->ClockDataSupplied || co->OrbitDataSupplied)
628 return GCOBR_DATAMISMATCH;
629 co->OrbitDataSupplied = 1;
630 co->ClockDataSupplied = 1;
631 for(i = CLOCKORBIT_NUMGPS;
632 i < CLOCKORBIT_NUMGPS+co->NumberOfGLONASSSat; ++i)
633 {
634 G_GLONASS_SATELLITE_ID(co->Sat[i].ID)
635 G_GLONASS_IOD(co->Sat[i].IOD)
636 G_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
637 G_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
638 G_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
639 G_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
640 G_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
641 G_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
642 G_DELTA_DOT_DOT_RADIAL(co->Sat[i].Orbit.DotDotDeltaRadial)
643 G_DELTA_DOT_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDotDeltaAlongTrack)
644 G_DELTA_DOT_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDotDeltaCrossTrack)
645 G_SATELLITE_REFERENCE_POINT(co->SatRefPoint)
646 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
647 G_DELTA_A0(co->Sat[i].Clock.DeltaA0)
648 G_DELTA_A1(co->Sat[i].Clock.DeltaA1)
649 G_DELTA_A2(co->Sat[i].Clock.DeltaA2)
650 }
651 break;
652 case BTYPE_GPS:
653 if(!b) return GCOBR_NOBIASPARAMETER;
654 G_GPS_EPOCH_TIME(b->GPSEpochTime, co->NumberOfGPSSat)
655 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
656 G_RESERVED6
657 G_NO_OF_SATELLITES(b->NumberOfGPSSat)
658 for(i = 0; i < b->NumberOfGPSSat; ++i)
659 {
660 G_GPS_SATELLITE_ID(b->Sat[i].ID)
661 G_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
662 for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
663 {
664 G_GPS_CODE_TYPE(b->Sat[i].Biases[j].Type)
665 G_CODE_BIAS(b->Sat[i].Biases[j].Bias)
666 }
667 }
668 break;
669 case BTYPE_GLONASS:
670 if(!b) return GCOBR_NOBIASPARAMETER;
671 G_GPS_EPOCH_TIME(b->GLONASSEpochTime, co->NumberOfGLONASSSat)
672 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
673 G_RESERVED6
674 G_NO_OF_SATELLITES(b->NumberOfGLONASSSat)
675 for(i = CLOCKORBIT_NUMGPS;
676 i < CLOCKORBIT_NUMGPS+b->NumberOfGLONASSSat; ++i)
677 {
678 G_GLONASS_SATELLITE_ID(b->Sat[i].ID)
679 G_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
680 for(j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j)
681 {
682 G_GLONASS_CODE_TYPE(b->Sat[i].Biases[j].Type)
683 G_CODE_BIAS(b->Sat[i].Biases[j].Bias)
684 }
685 }
686 break;
687 default:
688 return GCOBR_UNKNOWNTYPE;
689 }
690 if(bytesused)
691 *bytesused = sizeofrtcmblock+6;
692 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
693}
694#endif /* NODECODE */
Note: See TracBrowser for help on using the repository browser.