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

Last change on this file since 1238 was 1238, checked in by stoecker, 15 years ago

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