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

Last change on this file since 956 was 956, checked in by stoecker, 16 years ago

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