source: ntrip/branches/BNC_2.12/src/RTCM3/clock_and_orbit/clock_orbit_igs.c@ 8988

Last change on this file since 8988 was 8988, checked in by stuerze, 4 years ago

disable debug output
  • Property svn:executable set to *
  • Property svn:keywords set to Author Date Id Rev URL;svn:eol-style=native
  • Property svn:mime-type set to text/plain
File size: 38.8 KB
Line 
1/* Programheader
2
3 Name: clock_orbit_igs.c
4 Project: RTCM3
5 Version: $Id: clock_orbit_igs.c 8988 2020-07-20 13:56:40Z stuerze $
6 Authors: Dirk Stöcker, Andrea Stürze
7 Description: state space approach: IGS
8 */
9
10#include <math.h>
11#include <stdio.h>
12#include <string.h>
13#ifndef sparc
14#include <stdint.h>
15#else
16#include <sys/types.h>
17#endif
18#include <clock_orbit_igs.h>
19
20
21static uint32_t CRC24(long size, const unsigned char *buf) {
22 uint32_t crc = 0;
23 int i;
24
25 while (size--) {
26 crc ^= (*buf++) << (16);
27 for (i = 0; i < 8; i++)
28 {
29 crc <<= 1;
30 if (crc & 0x1000000)
31 crc ^= 0x01864cfb;
32 }
33 }
34 return crc;
35}
36
37/* NOTE: These defines are interlinked with below functions and directly modify
38 the values. This may not be optimized in terms of final program code size but
39 should be optimized in terms of speed.
40
41 modified variables are:
42 - everything defined in STARTDATA (only use ressize outside of the defines,
43 others are private)
44 - buffer
45 - size
46 */
47
48#ifndef NOENCODE
49#define STOREBITS \
50 while(numbits >= 8) { \
51 if(!size) return 0; \
52 *(buffer++) = bitbuffer>>(numbits-8); \
53 numbits -= 8; \
54 ++ressize; \
55 --size; \
56 }
57
58#define ADDBITS(a, b) { \
59 bitbuffer = (bitbuffer<<(a))|((b)&((1<<a)-1)); \
60 numbits += (a); \
61 STOREBITS \
62 }
63
64#define STARTDATA \
65 size_t ressize=0; \
66 char *blockstart; \
67 int numbits; \
68 uint64_t bitbuffer=0;
69
70#define INITBLOCK \
71 numbits = 0; \
72 blockstart = buffer; \
73 ADDBITS(8, 0xD3) \
74 ADDBITS(6, 0) \
75 ADDBITS(10, 0)
76
77#define ENDBLOCK \
78 if(numbits) { ADDBITS((8-numbits), 0) } { \
79 int len = buffer-blockstart-3; \
80 blockstart[1] |= len>>8; \
81 blockstart[2] = len; \
82 if(len > 1023) \
83 return 0; \
84 len = CRC24(len+3, (const unsigned char *) blockstart); \
85 ADDBITS(24, len) \
86 }
87
88#define SCALEADDBITS(a, b, c) ADDBITS(a, (int64_t)(c > 0 ? b*c+0.5 : b*c-0.5))
89
90#define MPI 3.141592653589793
91
92/* GNSS macros - Header part */
93#define T_RTCM_MESSAGE_NUMBER(a) ADDBITS(12, a) /* DF002 */
94#define T_IGS_SSR_VERSION(a) ADDBITS( 3, a) /* IDF001 */
95#define T_IGS_MESSAGE_NUMBER(a) ADDBITS( 8, a) /* IDF002 */
96#define T_SSR_EPOCH_TIME(a) ADDBITS(20, a) /* IDF003 */
97#define T_SSR_UPDATE_INTERVAL(a) ADDBITS( 4, a) /* DF391, IDF004 */
98#define T_MULTIPLE_MESSAGE_INDICATOR(a) ADDBITS( 1, a) /* DF388, IDF005 */
99#define T_SSR_IOD(a) ADDBITS( 4, a) /* DF413, IDF007 */
100#define T_SSR_PROVIDER_ID(a) ADDBITS(16, a) /* DF414, IDF008 */
101#define T_SSR_SOLUTION_ID(a) ADDBITS( 4, a) /* DF415, IDF009 */
102#define T_SATELLITE_REFERENCE_DATUM(a) ADDBITS( 1, a) /* DF375, IDF006 */
103#define T_NO_OF_SATELLITES(a) ADDBITS( 6, a) /* DF387, IDF010 */
104
105/* GNSS macros - Satellite specific part */
106#define T_GNSS_SATELLITE_ID(a) ADDBITS( 6, a) /* IDF011 */
107#define T_GNSS_IOD(a) ADDBITS( 8, a) /* IDF012 */
108#define T_DELTA_RADIAL(a) SCALEADDBITS(22, 10000.0, a) /* DF365, IDF013 */
109#define T_DELTA_ALONG_TRACK(a) SCALEADDBITS(20, 2500.0, a) /* DF366, IDF014 */
110#define T_DELTA_CROSS_TRACK(a) SCALEADDBITS(20, 2500.0, a) /* DF367, IDF015 */
111#define T_DELTA_DOT_RADIAL(a) SCALEADDBITS(21, 1000000.0, a) /* DF368, IDF016 */
112#define T_DELTA_DOT_ALONG_TRACK(a) SCALEADDBITS(19, 250000.0, a) /* DF369, IDF017 */
113#define T_DELTA_DOT_CROSS_TRACK(a) SCALEADDBITS(19, 250000.0, a) /* DF370, IDF018 */
114
115#define T_DELTA_CLOCK_C0(a) SCALEADDBITS(22, 10000.0, a) /* DF376, IDF019 */
116#define T_DELTA_CLOCK_C1(a) SCALEADDBITS(21, 1000000.0, a) /* DF377, IDF020 */
117#define T_DELTA_CLOCK_C2(a) SCALEADDBITS(27, 50000000.0, a) /* DF378, IDF021 */
118#define T_HR_CLOCK_CORRECTION(a) SCALEADDBITS(22, 10000.0, a) /* DF390, IDF022 */
119
120#define T_NO_OF_BIASES(a) ADDBITS(5, a) /* IDF023 */
121#define T_GNSS_SIGNAL_IDENTIFIER(a) ADDBITS(5, a) /* IDF024 */
122#define T_CODE_BIAS(a) SCALEADDBITS(14, 100.0, a) /* DF383, IDF025 */
123#define T_YAW_ANGLE(a) SCALEADDBITS( 9, 256.0/MPI, a) /* DF480, IDF026 */
124#define T_YAW_RATE(a) SCALEADDBITS( 8, 8192.0/MPI, a) /* DF481, IDF027 */
125#define T_PHASE_BIAS(a) SCALEADDBITS(20, 10000.0, a) /* DF482, IDF028 */
126
127/* Phase specific part of GNSS phase bias message */
128#define T_INTEGER_INDICATOR(a) ADDBITS( 1, a) /* DF483, IDF029 */
129#define T_WIDE_LANE_INDICATOR(a) ADDBITS( 2, a) /* DF484, IDF030 */
130#define T_DISCONTINUITY_COUNTER(a) ADDBITS( 4, a) /* DF485, IDF031 */
131#define T_DISPERSIVE_BIAS_INDICATOR(a) ADDBITS( 1, a) /* DF486, IDF032 */
132#define T_MW_CONSISTENCY_INDICATOR(a) ADDBITS( 1, a) /* DF487, IDF033 */
133
134#define T_SSR_URA(a) ADDBITS( 6, a) /* DF389, IDF034 */
135
136/* Ionosphere */
137#define T_NO_IONO_LAYERS(a) ADDBITS( 2, a-1) /* DF472, IDF035 */
138#define T_IONO_HEIGHT(a) SCALEADDBITS( 8, 1/10000.0, a) /* DF473, IDF036 */
139#define T_IONO_DEGREE(a) ADDBITS( 4, a-1) /* DF474, IDF037 */
140#define T_IONO_ORDER(a) ADDBITS( 4, a-1) /* DF475, IDF038 */
141#define T_IONO_COEFF_C(a) SCALEADDBITS(16, 200.0, a) /* DF476, IDF039 */
142#define T_IONO_COEFF_S(a) SCALEADDBITS(16, 200.0, a) /* DF477, IDF040 */
143#define T_VTEC_QUALITY_INDICATOR(a) SCALEADDBITS( 9, 20.0, a) /* DF478, IDF041 */
144
145static double URAToValue(int ura) {
146 int urac, urav;
147 urac = ura >> 3;
148 urav = ura & 7;
149 if (!ura)
150 return 0;
151 else if (ura == 63)
152 return SSR_MAXURA;
153 return (pow(3, urac) * (1.0 + urav / 4.0) - 1.0) / 1000.0;
154}
155
156static int ValueToURA(double val) {
157 int ura;
158 if (!val)
159 return 0;
160 else if (val > 5.4665)
161 return 63;
162 for (ura = 1; ura < 63 && val > URAToValue(ura); ++ura)
163 ;
164 return ura;
165}
166
167static const enum ClockOrbitType corbase[CLOCKORBIT_SATNUM] = {
168 (int) COBBASE_GPS,
169 (int) COBBASE_GLONASS,
170 (int) COBBASE_GALILEO,
171 (int) COBBASE_QZSS,
172 (int) COBBASE_SBAS,
173 (int) COBBASE_BDS
174};
175
176static const enum COR_OFFSETS satoffset[CLOCKORBIT_SATNUM + 1] = {
177 CLOCKORBIT_OFFSETGPS,
178 CLOCKORBIT_OFFSETGLONASS,
179 CLOCKORBIT_OFFSETGALILEO,
180 CLOCKORBIT_OFFSETQZSS,
181 CLOCKORBIT_OFFSETSBAS,
182 CLOCKORBIT_OFFSETBDS,
183 CLOCKORBIT_COUNTSAT
184};
185
186size_t MakeClockOrbit(const struct ClockOrbit *co, enum ClockOrbitType type,
187 int moremessagesfollow, char *buffer, size_t size) {
188 unsigned int status[CLOCKORBIT_SATNUM][COBOFS_NUM], i, s;
189
190 memset(status, 0, sizeof(status));
191
192 STARTDATA
193
194 for (s = 0; s < CLOCKORBIT_SATNUM; ++s) {
195 for (i = 0; i < COBOFS_NUM; ++i) {
196 if (co->NumberOfSat[s] && (type == COTYPE_AUTO || type == corbase[s] + i) &&
197 (co->Supplied[i] || (i <= COBOFS_CLOCK && co->Supplied[COBOFS_COMBINED]) ||
198 (i == COBOFS_COMBINED && co->Supplied[COBOFS_ORBIT] && co->Supplied[COBOFS_CLOCK]))) {
199 status[s][i] = 1;
200 if (i == COBOFS_COMBINED) {
201 status[s][COBOFS_ORBIT] = status[s][COBOFS_CLOCK] = 0;
202 } /* disable single blocks for combined type */
203 } /* check for data */
204 } /* iterate over sub-types */
205 } /* iterate over satellite systems */
206
207 for (s = 0; s < CLOCKORBIT_SATNUM; ++s) {
208 if (status[s][COBOFS_ORBIT]) {
209 INITBLOCK
210 T_RTCM_MESSAGE_NUMBER(RTCM_MESSAGE_NUMBER_IGS)
211 T_IGS_SSR_VERSION(IGS_SSR_VERSION)
212 T_IGS_MESSAGE_NUMBER(corbase[s] + COBOFS_ORBIT)
213 T_SSR_EPOCH_TIME(co->EpochTime[s])
214 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
215 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
216 T_SSR_IOD(co->SSRIOD)
217 T_SSR_PROVIDER_ID(co->SSRProviderID)
218 T_SSR_SOLUTION_ID(co->SSRSolutionID)
219 T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
220 T_NO_OF_SATELLITES(co->NumberOfSat[s])
221 for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) {
222 T_GNSS_SATELLITE_ID(co->Sat[i].ID)
223 T_GNSS_IOD(co->Sat[i].IOD)
224 T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
225 T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
226 T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
227 T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
228 T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
229 T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
230 }
231 ENDBLOCK
232 }
233 if (status[s][COBOFS_CLOCK]) {
234 INITBLOCK
235 T_RTCM_MESSAGE_NUMBER(RTCM_MESSAGE_NUMBER_IGS)
236 T_IGS_SSR_VERSION(IGS_SSR_VERSION)
237 T_IGS_MESSAGE_NUMBER(corbase[s] + COBOFS_CLOCK)
238 T_SSR_EPOCH_TIME(co->EpochTime[s])
239 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
240 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
241 T_SSR_IOD(co->SSRIOD)
242 T_SSR_PROVIDER_ID(co->SSRProviderID)
243 T_SSR_SOLUTION_ID(co->SSRSolutionID)
244 T_NO_OF_SATELLITES(co->NumberOfSat[s])
245 for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) {
246 T_GNSS_SATELLITE_ID(co->Sat[i].ID)
247 T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
248 T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
249 T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)
250 }
251 ENDBLOCK
252 }
253 if (status[s][COBOFS_COMBINED]) {
254#ifdef SPLITBLOCK
255 int nums = co->NumberOfSat[s];
256 int left, start = satoffset[s];
257 if(nums > 28) {/* split block when more than 28 sats */
258 left = nums - 28;
259 nums = 28;
260 }
261 else {
262 left = 0;
263 }
264 while(nums) {
265#endif
266 INITBLOCK
267 T_RTCM_MESSAGE_NUMBER(RTCM_MESSAGE_NUMBER_IGS)
268 T_IGS_SSR_VERSION(IGS_SSR_VERSION)
269 T_IGS_MESSAGE_NUMBER(corbase[s] + COBOFS_COMBINED)
270 T_SSR_EPOCH_TIME(co->EpochTime[s])
271 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
272#ifdef SPLITBLOCK
273 T_MULTIPLE_MESSAGE_INDICATOR((moremessagesfollow || left) ? 1 : 0)
274#else
275 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
276#endif
277 T_SSR_IOD(co->SSRIOD)
278 T_SSR_PROVIDER_ID(co->SSRProviderID)
279 T_SSR_SOLUTION_ID(co->SSRSolutionID)
280 T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
281#ifdef SPLITBLOCK
282 T_NO_OF_SATELLITES(nums)
283 for(i = start; i < start+nums; ++i) {
284#else
285 T_NO_OF_SATELLITES(co->NumberOfSat[s])
286 for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) {
287#endif
288 T_GNSS_SATELLITE_ID(co->Sat[i].ID)
289 T_GNSS_IOD(co->Sat[i].IOD)
290 T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
291 T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
292 T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
293 T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
294 T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
295 T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
296 T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
297 T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
298 T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)
299 }
300 ENDBLOCK
301#ifdef SPLITBLOCK
302 start += nums;
303 nums = left;
304 left = 0;
305 }
306#endif
307 }
308 if (status[s][COBOFS_HR]) {
309 INITBLOCK
310 T_RTCM_MESSAGE_NUMBER(RTCM_MESSAGE_NUMBER_IGS)
311 T_IGS_SSR_VERSION(IGS_SSR_VERSION)
312 T_IGS_MESSAGE_NUMBER(corbase[s] + COBOFS_HR)
313 T_SSR_EPOCH_TIME(co->EpochTime[s])
314 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
315 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
316 T_SSR_IOD(co->SSRIOD)
317 T_SSR_PROVIDER_ID(co->SSRProviderID)
318 T_SSR_SOLUTION_ID(co->SSRSolutionID)
319 T_NO_OF_SATELLITES(co->NumberOfSat[s])
320 for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) {
321 T_GNSS_SATELLITE_ID(co->Sat[i].ID)
322 T_HR_CLOCK_CORRECTION(co->Sat[i].hrclock)
323 }
324 ENDBLOCK
325 }
326 if (status[s][COBOFS_URA]) {
327 INITBLOCK
328 T_RTCM_MESSAGE_NUMBER(RTCM_MESSAGE_NUMBER_IGS)
329 T_IGS_SSR_VERSION(IGS_SSR_VERSION)
330 T_IGS_MESSAGE_NUMBER(corbase[s] + COBOFS_URA)
331 T_SSR_EPOCH_TIME(co->EpochTime[s])
332 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
333 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
334 T_SSR_IOD(co->SSRIOD)
335 T_SSR_PROVIDER_ID(co->SSRProviderID)
336 T_SSR_SOLUTION_ID(co->SSRSolutionID)
337 T_NO_OF_SATELLITES(co->NumberOfSat[s])
338 for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) {
339 T_GNSS_SATELLITE_ID(co->Sat[i].ID)
340 T_SSR_URA(ValueToURA(co->Sat[i].UserRangeAccuracy))
341 }
342 ENDBLOCK
343 }
344 }
345 return ressize;
346}
347
348size_t MakeCodeBias(const struct CodeBias *b, enum CodeBiasType type,
349 int moremessagesfollow, char *buffer, size_t size) {
350 unsigned int s, i, j;
351
352 STARTDATA
353
354 for (s = 0; s < CLOCKORBIT_SATNUM; ++s) {
355 if (b->NumberOfSat[s] && (type == CBTYPE_AUTO || type == corbase[s] + COBOFS_CBIAS)) {
356 INITBLOCK
357 T_RTCM_MESSAGE_NUMBER(RTCM_MESSAGE_NUMBER_IGS)
358 T_IGS_SSR_VERSION(IGS_SSR_VERSION)
359 T_IGS_MESSAGE_NUMBER(corbase[s] + COBOFS_CBIAS)
360 T_SSR_EPOCH_TIME(b->EpochTime[s])
361 T_SSR_UPDATE_INTERVAL(b->UpdateInterval)
362 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
363 T_SSR_IOD(b->SSRIOD)
364 T_SSR_PROVIDER_ID(b->SSRProviderID)
365 T_SSR_SOLUTION_ID(b->SSRSolutionID)
366 T_NO_OF_SATELLITES(b->NumberOfSat[s])
367 for (i = satoffset[s]; i < satoffset[s] + b->NumberOfSat[s]; ++i) {
368 T_GNSS_SATELLITE_ID(b->Sat[i].ID)
369 T_NO_OF_BIASES(b->Sat[i].NumberOfCodeBiases)
370 for (j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j) {
371 T_GNSS_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type)
372 T_CODE_BIAS(b->Sat[i].Biases[j].Bias)
373 }
374 }
375 ENDBLOCK
376 }
377 }
378 return ressize;
379}
380
381size_t MakePhaseBias(const struct PhaseBias *b, enum PhaseBiasType type,
382 int moremessagesfollow, char *buffer, size_t size) {
383 unsigned int s, i, j;
384
385 STARTDATA
386
387 for (s = 0; s < CLOCKORBIT_SATNUM; ++s) {
388 if (b->NumberOfSat[s] && (type == PBTYPE_AUTO || type == corbase[s] + COBOFS_PBIAS)) {
389 INITBLOCK
390 T_RTCM_MESSAGE_NUMBER(RTCM_MESSAGE_NUMBER_IGS)
391 T_IGS_SSR_VERSION(IGS_SSR_VERSION)
392 T_IGS_MESSAGE_NUMBER(corbase[s] + COBOFS_PBIAS)
393 T_SSR_EPOCH_TIME(b->EpochTime[s])
394 T_SSR_UPDATE_INTERVAL(b->UpdateInterval)
395 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
396 T_SSR_IOD(b->SSRIOD)
397 T_SSR_PROVIDER_ID(b->SSRProviderID)
398 T_SSR_SOLUTION_ID(b->SSRSolutionID)
399 T_DISPERSIVE_BIAS_INDICATOR(b->DispersiveBiasConsistencyIndicator ? 1 : 0)
400 T_MW_CONSISTENCY_INDICATOR(b->MWConsistencyIndicator ? 1 : 0)
401 T_NO_OF_SATELLITES(b->NumberOfSat[s])
402 for (i = satoffset[s]; i < satoffset[s] + b->NumberOfSat[s]; ++i) {
403 T_GNSS_SATELLITE_ID(b->Sat[i].ID)
404 T_NO_OF_BIASES(b->Sat[i].NumberOfPhaseBiases)
405 T_YAW_ANGLE(b->Sat[i].YawAngle)
406 T_YAW_RATE(b->Sat[i].YawRate)
407 for (j = 0; j < b->Sat[i].NumberOfPhaseBiases; ++j) {
408 T_GNSS_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type)
409 T_INTEGER_INDICATOR(b->Sat[i].Biases[j].SignalIntegerIndicator ? 1 : 0)
410 T_WIDE_LANE_INDICATOR(b->Sat[i].Biases[j].SignalsWideLaneIntegerIndicator)
411 T_DISCONTINUITY_COUNTER(b->Sat[i].Biases[j].SignalDiscontinuityCounter)
412 T_PHASE_BIAS(b->Sat[i].Biases[j].Bias)
413 }
414 }
415 ENDBLOCK
416 }
417 }
418 return ressize;
419}
420
421size_t MakeVTEC(const struct VTEC *v, int moremessagesfollow, char *buffer, size_t size) {
422 unsigned int l, o, d;
423
424 STARTDATA
425 INITBLOCK
426
427 T_RTCM_MESSAGE_NUMBER(RTCM_MESSAGE_NUMBER_IGS)
428 T_IGS_SSR_VERSION(IGS_SSR_VERSION)
429 T_IGS_MESSAGE_NUMBER(VTEC_BASE)
430 T_SSR_EPOCH_TIME(v->EpochTime)
431 T_SSR_UPDATE_INTERVAL(v->UpdateInterval)
432 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
433 T_SSR_IOD(v->SSRIOD)
434 T_SSR_PROVIDER_ID(v->SSRProviderID)
435 T_SSR_SOLUTION_ID(v->SSRSolutionID)
436 T_VTEC_QUALITY_INDICATOR(v->Quality)
437 T_NO_IONO_LAYERS(v->NumLayers)
438 for (l = 0; l < v->NumLayers; ++l) {
439 T_IONO_HEIGHT(v->Layers[l].Height)
440 T_IONO_DEGREE(v->Layers[l].Degree)
441 T_IONO_ORDER(v->Layers[l].Order)
442 for (o = 0; o <= v->Layers[l].Order; ++o) {
443 for (d = o; d <= v->Layers[l].Degree; ++d) {
444 T_IONO_COEFF_C(v->Layers[l].Cosinus[d][o])
445 }
446 }
447 for (o = 1; o <= v->Layers[l].Order; ++o) {
448 for (d = o; d <= v->Layers[l].Degree; ++d) {
449 T_IONO_COEFF_S(v->Layers[l].Sinus[d][o])
450 }
451 }
452 }
453 ENDBLOCK
454 return ressize;
455}
456#endif /* NOENCODE */
457
458#ifndef NODECODE
459
460#define DECODESTART \
461 int numbits=0; \
462 uint64_t bitbuffer=0;
463
464#define LOADBITS(a) { \
465 while((a) > numbits) { \
466 if(!size--) return GCOBR_SHORTMESSAGE; \
467 bitbuffer = (bitbuffer<<8)|((unsigned char)*(buffer++)); \
468 numbits += 8; \
469 } \
470}
471
472/* extract bits from data stream
473 b = variable to store result, a = number of bits */
474#define GETBITS(b, a) { \
475 LOADBITS(a) \
476 b = (bitbuffer<<(64-numbits))>>(64-(a)); \
477 numbits -= (a); \
478}
479
480/* extract bits from data stream
481 b = variable to store result, a = number of bits */
482#define GETBITSFACTOR(b, a, c) { \
483 LOADBITS(a) \
484 b = ((bitbuffer<<(64-numbits))>>(64-(a)))*(c); \
485 numbits -= (a); \
486}
487
488/* extract signed floating value from data stream
489 b = variable to store result, a = number of bits */
490#define GETFLOATSIGN(b, a, c) { \
491 LOADBITS(a) \
492 b = ((double)(((int64_t)(bitbuffer<<(64-numbits)))>>(64-(a))))*(c); \
493 numbits -= (a); \
494}
495
496/* extract floating value from data stream
497 b = variable to store result, a = number of bits, c = scale factor */
498#define GETFLOAT(b, a, c) { \
499 LOADBITS(a) \
500 b = ((double)((bitbuffer<<(sizeof(bitbuffer)*8-numbits))>>(sizeof(bitbuffer)*8-(a))))*(c); \
501 numbits -= (a); \
502}
503
504#define SKIPBITS(b) { LOADBITS(b) numbits -= (b); }
505
506/* GPS macros also used for other systems when matching! */
507#define G_HEADER(a) GETBITS(a, 8)
508#define G_RESERVEDH(a) GETBITS(a, 6)
509#define G_SIZE(a) GETBITS(a, 10)
510
511/* GNSS macros - Header part */
512#define G_RTCM_MESSAGE_NUMBER(a) GETBITS(a, 12) /* DF002 */
513#define G_IGS_SSR_VERSION(a) GETBITS(a, 3) /* IDF001 */
514#define G_IGS_MESSAGE_NUMBER(a) GETBITS(a, 8) /* IDF002 */
515#define G_SSR_EPOCH_TIME(a) GETBITS(a, 20) /* IDF003 */
516#define G_SSR_EPOCH_TIME_CHECK(a, b) {unsigned int temp; GETBITS(temp, 20) \
517 if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
518#define G_SSR_UPDATE_INTERVAL(a) GETBITS(a, 4) /* DF391, IDF004 */
519#define G_MULTIPLE_MESSAGE_INDICATOR(a) GETBITS(a, 1) /* DF388, IDF005 */
520#define G_SSR_IOD(a) GETBITS(a, 4) /* DF413, IDF007 */
521#define G_SSR_PROVIDER_ID(a) GETBITS(a, 16) /* DF414, IDF008 */
522#define G_SSR_SOLUTION_ID(a) GETBITS(a, 4) /* DF415, IDF009 */
523#define G_SATELLITE_REFERENCE_DATUM(a) GETBITS(a, 1) /* DF375, IDF006 */
524#define G_NO_OF_SATELLITES(a) GETBITS(a, 6) /* DF387, IDF010 */
525
526/* GNSS macros - Satellite specific part */
527#define G_GNSS_SATELLITE_ID(a) GETBITS(a, 6) /* IDF011 */
528#define G_GNSS_IOD(a) GETBITS(a, 8) /* IDF012 */
529
530#define G_DELTA_RADIAL(a) GETFLOATSIGN(a, 22, 1/10000.0) /* DF365, IDF013 */
531#define G_DELTA_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0) /* DF366, IDF014 */
532#define G_DELTA_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0) /* DF367, IDF015 */
533#define G_DELTA_DOT_RADIAL(a) GETFLOATSIGN(a, 21, 1/1000000.0) /* DF368, IDF016 */
534#define G_DELTA_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0) /* DF369, IDF017 */
535#define G_DELTA_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0) /* DF370, IDF018 */
536
537#define G_DELTA_CLOCK_C0(a) GETFLOATSIGN(a, 22, 1/10000.0) /* DF376, IDF019 */
538#define G_DELTA_CLOCK_C1(a) GETFLOATSIGN(a, 21, 1/1000000.0) /* DF377, IDF020 */
539#define G_DELTA_CLOCK_C2(a) GETFLOATSIGN(a, 27, 1/50000000.0) /* DF378, IDF021 */
540#define G_HR_CLOCK_CORRECTION(a) GETFLOATSIGN(a, 22, 1/10000.0) /* DF390, IDF022 */
541
542#define G_NO_OF_BIASES(a) GETBITS(a, 5) /* IDF023 */
543#define G_GNSS_SIGNAL_IDENTIFIER(a) GETBITS(a, 5) /* IDF024 */
544#define G_CODE_BIAS(a) GETFLOATSIGN(a, 14, 1/100.0) /* DF383, IDF025 */
545#define G_YAW_ANGLE(a) GETFLOAT (a, 9, MPI/256.0) /* DF480, IDF026 */
546#define G_YAW_RATE(a) GETFLOATSIGN(a, 8, MPI/8192.0) /* DF481, IDF027 */
547#define G_PHASE_BIAS(a) GETFLOATSIGN(a, 20, 1/10000.) /* DF482, IDF028 */
548
549#define G_INTEGER_INDICATOR(a) GETBITS(a, 1) /* DF483, IDF029 */
550#define G_WIDE_LANE_INDICATOR(a) GETBITS(a, 2) /* DF484, IDF030 */
551#define G_DISCONTINUITY_COUNTER(a) GETBITS(a, 4) /* DF485, IDF031 */
552#define G_DISPERSIVE_BIAS_INDICATOR(a) GETBITS(a, 1) /* DF486, IDF032 */
553#define G_MW_CONSISTENCY_INDICATOR(a) GETBITS(a, 1) /* DF487, IDF033 */
554
555#define G_SSR_URA(a) {int temp; GETBITS(temp, 6) \
556 (a) = URAToValue(temp);} /* DF389, IDF034 */
557
558/* Ionosphere */
559#define G_NO_IONO_LAYERS(a) {unsigned int temp; GETBITS(temp, 2) a = temp+1;} /* DF472, IDF035 */
560#define G_IONO_HEIGHT(a) GETFLOAT(a, 8 , 10000.0) /* DF473, IDF036 */
561#define G_IONO_DEGREE(a) {unsigned int temp; GETBITS(temp, 4) a = temp+1;} /* DF474, IDF037 */
562#define G_IONO_ORDER(a) {unsigned int temp; GETBITS(temp, 4) a = temp+1;} /* DF475, IDF038 */
563#define G_IONO_COEFF_C(a) GETFLOATSIGN(a, 16,1/200.0) /* DF476, IDF039 */
564#define G_IONO_COEFF_S(a) GETFLOATSIGN(a, 16,1/200.0) /* DF477, IDF040 */
565#define G_VTEC_QUALITY_INDICATOR(a) GETFLOAT (a, 9, 1/20.0) /* DF478, IDF041 */
566
567enum GCOB_RETURN GetSSR(struct ClockOrbit *co, struct CodeBias *b,struct VTEC *v,
568 struct PhaseBias *pb, const char *buffer, size_t size, int *bytesused) {
569 int mmi = 0, h, rs;
570 unsigned int type, pos, i, j, s, nums, id, version;
571 size_t sizeofrtcmblock;
572 const char *blockstart = buffer;
573 DECODESTART
574
575 if (size < 7)
576 return GCOBR_SHORTBUFFER;
577
578#ifdef DEBUG
579 fprintf(stderr, "GetSSR START: size %d, numbits %d\n",(int)size, numbits);
580#endif
581
582 G_HEADER(h)
583 G_RESERVEDH(rs)
584 G_SIZE(sizeofrtcmblock);
585
586 if ((unsigned char) h != 0xD3 || rs)
587 return GCOBR_UNKNOWNDATA;
588 if (size < sizeofrtcmblock + 3) /* 3 header bytes already removed */
589 return GCOBR_MESSAGEEXCEEDSBUFFER;
590 if (CRC24(sizeofrtcmblock + 3, (const unsigned char *) blockstart) !=
591 (uint32_t) ((((unsigned char) buffer[sizeofrtcmblock]) << 16) |
592 (((unsigned char) buffer[sizeofrtcmblock + 1]) << 8) |
593 (((unsigned char) buffer[sizeofrtcmblock + 2]))))
594 return GCOBR_CRCMISMATCH;
595 size = sizeofrtcmblock; /* reduce size, so overflows are detected */
596
597 G_RTCM_MESSAGE_NUMBER(type)
598 G_IGS_SSR_VERSION(version)
599#ifdef DEBUG
600 fprintf(stderr, "rtcmType %d igsVersion %d ",type, version);
601#endif
602 G_IGS_MESSAGE_NUMBER(type)
603#ifdef DEBUG
604 fprintf(stderr, "igsType IM%d size %d\n",type,(int)sizeofrtcmblock);
605#endif
606 if (bytesused)
607 *bytesused = sizeofrtcmblock + 6;
608
609 if (type == VTEC_BASE) {
610 unsigned int l, o, d;
611 if (!v)
612 return GCOBR_NOVTECPARAMETER;
613 memset(v, 0, sizeof(*v));
614 G_SSR_EPOCH_TIME(v->EpochTime)
615 G_SSR_UPDATE_INTERVAL(v->UpdateInterval)
616 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
617 G_SSR_IOD(v->SSRIOD)
618 G_SSR_PROVIDER_ID(v->SSRProviderID)
619 G_SSR_SOLUTION_ID(v->SSRSolutionID)
620 G_VTEC_QUALITY_INDICATOR(v->Quality)
621 G_NO_IONO_LAYERS(v->NumLayers)
622#ifdef DEBUG
623 fprintf(stderr, "epochTime %d ui %d mmi %d ssrIod %d providerId %d solId %d vtecQ %8.3f numLay %d \n",
624 v->EpochTime, v->UpdateInterval, mmi,
625 v->SSRIOD, v->SSRProviderID, v->SSRSolutionID, v->Quality, v->NumLayers);
626#endif
627 for (l = 0; l < v->NumLayers; ++l) {
628 G_IONO_HEIGHT(v->Layers[l].Height)
629 G_IONO_DEGREE(v->Layers[l].Degree)
630 G_IONO_ORDER(v->Layers[l].Order)
631#ifdef DEBUG
632 fprintf(stderr, "h %8.3f deg %d ord %d \n",
633 v->Layers[l].Height, v->Layers[l].Degree, v->Layers[l].Order);
634#endif
635 for (o = 0; o <= v->Layers[l].Order; ++o) {
636 for (d = o; d <= v->Layers[l].Degree; ++d) {
637 G_IONO_COEFF_C(v->Layers[l].Cosinus[d][o])
638#ifdef DEBUG
639 fprintf(stderr, "C[%02d][%02d] %8.3f \n",
640 d, o, v->Layers[l].Cosinus[d][o]);
641#endif
642 }
643 }
644 for (o = 1; o <= v->Layers[l].Order; ++o) {
645 for (d = o; d <= v->Layers[l].Degree; ++d) {
646 G_IONO_COEFF_S(v->Layers[l].Sinus[d][o])
647#ifdef DEBUG
648 fprintf(stderr, "S[%02d][%02d] %8.3f \n",
649 d, o, v->Layers[l].Sinus[d][o]);
650#endif
651 }
652 }
653 }
654#ifdef DEBUG
655 for(type = 0; type < (unsigned int)size && (unsigned char)buffer[type] != 0xD3; ++type)
656 numbits += 8;
657 fprintf(stderr, "numbits left %d\n",numbits);
658#endif
659 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
660 }
661 for (s = CLOCKORBIT_SATNUM; s-- > 0;) {
662 if (type >= corbase[s]) {
663 switch (type - corbase[s]) {
664 case COBOFS_ORBIT:
665 if (!co)
666 return GCOBR_NOCLOCKORBITPARAMETER;
667 co->messageType = type;
668 G_SSR_EPOCH_TIME_CHECK(co->EpochTime[s], co->NumberOfSat[s])
669 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
670 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
671 G_SSR_IOD(co->SSRIOD)
672 G_SSR_PROVIDER_ID(co->SSRProviderID)
673 G_SSR_SOLUTION_ID(co->SSRSolutionID)
674 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
675 G_NO_OF_SATELLITES(nums)
676 co->Supplied[COBOFS_ORBIT] |= 1;
677#ifdef DEBUG
678 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d rd %d ssrIod %d providerId %d solId %d\n",
679 co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
680 co->SatRefDatum, co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
681#endif
682 for (i = 0; i < nums; ++i) {
683 G_GNSS_SATELLITE_ID(id)
684 for (pos = satoffset[s];
685 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
686 ++pos)
687 ;
688 if (pos >= satoffset[s + 1])
689 return GCOBR_DATAMISMATCH;
690 else if (pos == co->NumberOfSat[s] + satoffset[s])
691 ++co->NumberOfSat[s];
692 co->Sat[pos].ID = id;
693 G_GNSS_IOD(co->Sat[pos].IOD)
694 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
695 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
696 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
697 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
698 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
699 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
700#ifdef DEBUG
701 fprintf(stderr, "id %2d iod %3d dr %8.4f da %8.4f dc %8.4f dr %8.3f da %8.3f dc %8.3f\n",
702 co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
703 co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
704 co->Sat[pos].Orbit.DotDeltaRadial,
705 co->Sat[pos].Orbit.DotDeltaAlongTrack,
706 co->Sat[pos].Orbit.DotDeltaCrossTrack);
707#endif
708 }
709 break;
710 case COBOFS_CLOCK:
711 if (!co)
712 return GCOBR_NOCLOCKORBITPARAMETER;
713 co->messageType = type;
714 G_SSR_EPOCH_TIME_CHECK(co->EpochTime[s], co->NumberOfSat[s])
715 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
716 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
717 G_SSR_IOD(co->SSRIOD)
718 G_SSR_PROVIDER_ID(co->SSRProviderID)
719 G_SSR_SOLUTION_ID(co->SSRSolutionID)
720 G_NO_OF_SATELLITES(nums)
721 co->Supplied[COBOFS_CLOCK] |= 1;
722#ifdef DEBUG
723 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d\n",
724 co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
725 co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
726#endif
727 for (i = 0; i < nums; ++i) {
728 G_GNSS_SATELLITE_ID(id)
729 for (pos = satoffset[s];
730 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
731 ++pos)
732 ;
733 if (pos >= satoffset[s + 1])
734 return GCOBR_DATAMISMATCH;
735 else if (pos == co->NumberOfSat[s] + satoffset[s])
736 ++co->NumberOfSat[s];
737 co->Sat[pos].ID = id;
738 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
739 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
740 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
741#ifdef DEBUG
742 fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
743 co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
744 co->Sat[pos].Clock.DeltaA2);
745#endif
746 }
747 break;
748 case COBOFS_COMBINED:
749 if (!co)
750 return GCOBR_NOCLOCKORBITPARAMETER;
751 co->messageType = type;
752 G_SSR_EPOCH_TIME_CHECK(co->EpochTime[s], co->NumberOfSat[s])
753 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
754 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
755 G_SSR_IOD(co->SSRIOD)
756 G_SSR_PROVIDER_ID(co->SSRProviderID)
757 G_SSR_SOLUTION_ID(co->SSRSolutionID)
758 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
759 G_NO_OF_SATELLITES(nums)
760 co->Supplied[COBOFS_ORBIT] |= 1;
761 co->Supplied[COBOFS_CLOCK] |= 1;
762#ifdef DEBUG
763 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d rd %d ssrIod %d providerId %d solId %d\n",
764 co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
765 co->SatRefDatum, co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
766#endif
767 for (i = 0; i < nums; ++i) {
768 G_GNSS_SATELLITE_ID(id)
769 for (pos = satoffset[s];
770 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
771 ++pos)
772 ;
773 if (pos >= satoffset[s + 1])
774 return GCOBR_DATAMISMATCH;
775 else if (pos == co->NumberOfSat[s] + satoffset[s])
776 ++co->NumberOfSat[s];
777 co->Sat[pos].ID = id;
778 G_GNSS_IOD(co->Sat[pos].IOD)
779 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
780 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
781 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
782 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
783 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
784 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
785 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
786 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
787 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
788#ifdef DEBUG
789 fprintf(stderr, "id %2d iod %3d dr %10.6f da %10.6f dc %10.6f dr %10.6f da %10.6f dc %10.6f c0 %10.6f c1 %10.6f c2 %10.6f\n",
790 co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
791 co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
792 co->Sat[pos].Orbit.DotDeltaRadial, co->Sat[pos].Orbit.DotDeltaAlongTrack,
793 co->Sat[pos].Orbit.DotDeltaCrossTrack,
794 co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1, co->Sat[pos].Clock.DeltaA2);
795#endif
796 }
797 break;
798 case COBOFS_URA:
799 if (!co)
800 return GCOBR_NOCLOCKORBITPARAMETER;
801 co->messageType = type;
802 G_SSR_EPOCH_TIME_CHECK(co->EpochTime[s], co->NumberOfSat[s])
803 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
804 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
805 G_SSR_IOD(co->SSRIOD)
806 G_SSR_PROVIDER_ID(co->SSRProviderID)
807 G_SSR_SOLUTION_ID(co->SSRSolutionID)
808 G_NO_OF_SATELLITES(nums)
809 co->Supplied[COBOFS_URA] |= 1;
810#ifdef DEBUG
811 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d\n",
812 co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
813 co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
814#endif
815 for (i = 0; i < nums; ++i) {
816 G_GNSS_SATELLITE_ID(id)
817 for (pos = satoffset[s];
818 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
819 ++pos)
820 ;
821 if (pos >= satoffset[s + 1])
822 return GCOBR_DATAMISMATCH;
823 else if (pos == co->NumberOfSat[s] + satoffset[s])
824 ++co->NumberOfSat[s];
825 co->Sat[pos].ID = id;
826 G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
827#ifdef DEBUG
828 fprintf(stderr, "id %2d ura %8.3f \n",
829 co->Sat[pos].ID, co->Sat[pos].UserRangeAccuracy);
830#endif
831 }
832 break;
833 case COBOFS_HR:
834 if (!co)
835 return GCOBR_NOCLOCKORBITPARAMETER;
836 co->messageType = type;
837 G_SSR_EPOCH_TIME_CHECK(co->EpochTime[s], co->NumberOfSat[s])
838 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
839 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
840 G_SSR_IOD(co->SSRIOD)
841 G_SSR_PROVIDER_ID(co->SSRProviderID)
842 G_SSR_SOLUTION_ID(co->SSRSolutionID)
843 G_NO_OF_SATELLITES(nums)
844 co->Supplied[COBOFS_HR] |= 1;
845#ifdef DEBUG
846 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d\n",
847 co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
848 co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
849#endif
850 for (i = 0; i < nums; ++i) {
851 G_GNSS_SATELLITE_ID(id)
852 for (pos = satoffset[s];
853 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
854 ++pos)
855 ;
856 if (pos >= satoffset[s + 1])
857 return GCOBR_DATAMISMATCH;
858 else if (pos == co->NumberOfSat[s] + satoffset[s])
859 ++co->NumberOfSat[s];
860 co->Sat[pos].ID = id;
861 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
862#ifdef DEBUG
863 fprintf(stderr, "id %2d hrClock %8.3f \n",
864 co->Sat[pos].ID, co->Sat[pos].hrclock);
865#endif
866 }
867 break;
868 case COBOFS_CBIAS:
869 if (!b)
870 return GCOBR_NOCODEBIASPARAMETER;
871 b->messageType = type;
872 G_SSR_EPOCH_TIME_CHECK(b->EpochTime[s], b->NumberOfSat[s])
873 G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
874 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
875 G_SSR_IOD(b->SSRIOD)
876 G_SSR_PROVIDER_ID(b->SSRProviderID)
877 G_SSR_SOLUTION_ID(b->SSRSolutionID)
878 G_NO_OF_SATELLITES(nums)
879#ifdef DEBUG
880 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d\n",
881 b->EpochTime[s], b->UpdateInterval,mmi,b->NumberOfSat[s],nums,
882 b->SSRIOD, b->SSRProviderID, b->SSRSolutionID);
883#endif
884 for (i = 0; i < nums; ++i) {
885 G_GNSS_SATELLITE_ID(id)
886 for (pos = satoffset[s];
887 pos < satoffset[s] + b->NumberOfSat[s] && b->Sat[pos].ID != id;
888 ++pos)
889 ;
890 if (pos >= satoffset[s + 1])
891 return GCOBR_DATAMISMATCH;
892 else if (pos == b->NumberOfSat[s] + satoffset[s])
893 ++b->NumberOfSat[s];
894 b->Sat[pos].ID = id;
895 G_NO_OF_BIASES(b->Sat[pos].NumberOfCodeBiases)
896#ifdef DEBUG
897 fprintf(stderr, "id %2d #%d ",
898 b->Sat[pos].ID, b->Sat[pos].NumberOfCodeBiases);
899#endif
900 for (j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j) {
901 G_GNSS_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
902 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
903#ifdef DEBUG
904 fprintf(stderr, "t%02d b %8.2f ",
905 b->Sat[pos].Biases[j].Type, b->Sat[pos].Biases[j].Bias);
906#endif
907 }
908#ifdef DEBUG
909 fprintf(stderr, "\n");
910#endif
911 }
912 break;
913 case COBOFS_PBIAS:
914 if (!pb)
915 return GCOBR_NOPHASEBIASPARAMETER;
916 pb->messageType = type;
917 G_SSR_EPOCH_TIME_CHECK(pb->EpochTime[s], pb->NumberOfSat[s])
918 G_SSR_UPDATE_INTERVAL(pb->UpdateInterval)
919 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
920 G_SSR_IOD(pb->SSRIOD)
921 G_SSR_PROVIDER_ID(pb->SSRProviderID)
922 G_SSR_SOLUTION_ID(pb->SSRSolutionID)
923 G_DISPERSIVE_BIAS_INDICATOR(pb->DispersiveBiasConsistencyIndicator)
924 G_MW_CONSISTENCY_INDICATOR(pb->MWConsistencyIndicator)
925 G_NO_OF_SATELLITES(nums)
926#ifdef DEBUG
927 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d dispInd %d mwInd %d\n",
928 pb->EpochTime[s], pb->UpdateInterval,mmi,pb->NumberOfSat[s],nums,
929 pb->SSRIOD, pb->SSRProviderID, pb->SSRSolutionID,
930 pb->DispersiveBiasConsistencyIndicator, pb->MWConsistencyIndicator);
931#endif
932 for (i = 0; i < nums; ++i) {
933 G_GNSS_SATELLITE_ID(id)
934 for (pos = satoffset[s];
935 pos < satoffset[s] + pb->NumberOfSat[s] && pb->Sat[pos].ID != id;
936 ++pos)
937 ;
938 if (pos >= satoffset[s + 1])
939 return GCOBR_DATAMISMATCH;
940 else if (pos == pb->NumberOfSat[s] + satoffset[s])
941 ++pb->NumberOfSat[s];
942 pb->Sat[pos].ID = id;
943 G_NO_OF_BIASES(pb->Sat[pos].NumberOfPhaseBiases)
944 G_YAW_ANGLE(pb->Sat[pos].YawAngle)
945 G_YAW_RATE(pb->Sat[pos].YawRate)
946#ifdef DEBUG
947 fprintf(stderr, "id %2d #%d y %10.6f yr %10.6f ",
948 pb->Sat[pos].ID, pb->Sat[pos].NumberOfPhaseBiases,
949 pb->Sat[pos].YawAngle/MPI, pb->Sat[pos].YawRate/MPI);
950#endif
951 for (j = 0; j < pb->Sat[pos].NumberOfPhaseBiases; ++j) {
952 G_GNSS_SIGNAL_IDENTIFIER(pb->Sat[pos].Biases[j].Type)
953 G_INTEGER_INDICATOR(pb->Sat[pos].Biases[j].SignalIntegerIndicator)
954 G_WIDE_LANE_INDICATOR(pb->Sat[pos].Biases[j].SignalsWideLaneIntegerIndicator)
955 G_DISCONTINUITY_COUNTER(pb->Sat[pos].Biases[j].SignalDiscontinuityCounter)
956 G_PHASE_BIAS(pb->Sat[pos].Biases[j].Bias)
957#ifdef DEBUG
958 fprintf(stderr, "t%02d int %d wl %d disc %d b %8.4f ",
959 pb->Sat[pos].Biases[j].Type,
960 pb->Sat[pos].Biases[j].SignalIntegerIndicator,
961 pb->Sat[pos].Biases[j].SignalsWideLaneIntegerIndicator,
962 pb->Sat[pos].Biases[j].SignalDiscontinuityCounter,
963 pb->Sat[pos].Biases[j].Bias);
964#endif
965 }
966#ifdef DEBUG
967 fprintf(stderr, "\n");
968#endif
969 }
970 break;
971 default:
972 continue;
973 }
974#ifdef DEBUG
975 for(type = 0; type < (unsigned int)size && (unsigned char)buffer[type] != 0xD3; ++type)
976 numbits += 8;
977 fprintf(stderr, "numbits left %d\n",numbits);
978#endif
979 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
980 }
981 }
982 return GCOBR_UNKNOWNTYPE;
983}
984#endif /* NODECODE */
985
986
987
Note: See TracBrowser for help on using the repository browser.