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

Last change on this file since 1360 was 1211, checked in by mervart, 16 years ago

* empty log message *
  • Property svn:executable set to *
File size: 24.2 KB
RevLine 
[862]1/* Programheader
2
3 Name: clock_orbit_rtcm.c
4 Project: RTCM3
[1211]5 Version: $Id: clock_orbit_rtcm.c,v 1.4 2008/11/17 11:50:45 mervart Exp $
[862]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
[864]87#if 0
[862]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)
[864]98#else
99#define DEBUGSCALEADDBITS(n, a, b, c) SCALEADDBITS(a,b,c)
100#endif
[862]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{
[1211]330 int gps = 0;
331 int glo = 0;
332 int mmi, i, j;
[862]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 { \
[878]401 if(!size--) return GCOBR_SHORTBUFFER; \
[862]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 */
[878]428#define G_HEADER(a) GETBITS(a,8)
429#define G_RESERVEDH(a) GETBITS(a,6)
430#define G_SIZE(a) GETBITS(a, 10)
[862]431#define G_MESSAGE_NUMBER(a) GETBITS(a, 12) /* DF002 */
432#define G_RESERVED6 SKIPBITS(6) /* DF001 */
[878]433#define G_GPS_SATELLITE_ID(a) {int temp; GETBITS(temp, 6) \
434 if(a && a != temp) return GCOBR_DATAMISMATCH; a = temp;} /* DF068 */
[862]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)
[878]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;}
[862]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
[878]469enum GCOB_RETURN GetClockOrbitBias(struct ClockOrbit *co, struct Bias *b,
470const char *buffer, size_t size, int *bytesused)
[862]471{
[878]472 int type, mmi=0, i, j, h, rs, sizeofrtcmblock;
473 const char *blockstart = buffer;
[862]474 DECODESTART
475
[878]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
[862]493 G_MESSAGE_NUMBER(type)
494 switch(type)
495 {
496 case COTYPE_GPSORBIT:
[878]497 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
498 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
[1205]499 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
500 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
[862]501 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
502 G_RESERVED6
503 G_NO_OF_SATELLITES(co->NumberOfGPSSat)
[878]504 if(co->OrbitDataSupplied)
505 return GCOBR_DATAMISMATCH;
[862]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:
[878]525 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
526 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
[1205]527 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
528 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
[862]529 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
530 G_RESERVED6
531 G_NO_OF_SATELLITES(co->NumberOfGPSSat)
[878]532 if(co->ClockDataSupplied)
533 return GCOBR_DATAMISMATCH;
[862]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;
[878]546 G_GPS_EPOCH_TIME(co->GPSEpochTime, co->NumberOfGPSSat)
[1205]547 co->epochGPS[co->epochSize] = co->GPSEpochTime; /* Weber, for latency */
548 if(co->epochSize < 100) {co->epochSize += 1;} /* Weber, for latency */
[862]549 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
550 G_RESERVED6
551 G_NO_OF_SATELLITES(co->NumberOfGPSSat)
[878]552 if(co->ClockDataSupplied || co->OrbitDataSupplied)
553 return GCOBR_DATAMISMATCH;
[862]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:
[878]577 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
578 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
[862]579 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
580 G_RESERVED6
581 G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
[878]582 if(co->OrbitDataSupplied)
583 return GCOBR_DATAMISMATCH;
[862]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:
[878]604 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
605 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
[862]606 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
607 G_RESERVED6
608 G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
[878]609 if(co->ClockDataSupplied)
610 return GCOBR_DATAMISMATCH;
[862]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:
[878]623 if(!co) return GCOBR_NOCLOCKORBITPARAMETER;
624 G_GLONASS_EPOCH_TIME(co->GLONASSEpochTime, co->NumberOfGLONASSSat)
[862]625 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
626 G_RESERVED6
627 G_NO_OF_SATELLITES(co->NumberOfGLONASSSat)
[878]628 if(co->ClockDataSupplied || co->OrbitDataSupplied)
629 return GCOBR_DATAMISMATCH;
[862]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:
[878]654 if(!b) return GCOBR_NOBIASPARAMETER;
655 G_GPS_EPOCH_TIME(b->GPSEpochTime, co->NumberOfGPSSat)
[862]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:
[878]671 if(!b) return GCOBR_NOBIASPARAMETER;
672 G_GPS_EPOCH_TIME(b->GLONASSEpochTime, co->NumberOfGLONASSSat)
[862]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:
[878]689 return GCOBR_UNKNOWNTYPE;
[862]690 }
[878]691 if(bytesused)
692 *bytesused = sizeofrtcmblock+6;
693 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
[862]694}
695#endif /* NODECODE */
Note: See TracBrowser for help on using the repository browser.