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

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

minor changes
  • 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 8997 2020-07-22 11:45:12Z 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
20static uint32_t CRC24(long size, const unsigned char *buf) {
21 uint32_t crc = 0;
22 int i;
23
24 while (size--) {
25 crc ^= (*buf++) << (16);
26 for (i = 0; i < 8; i++)
27 {
28 crc <<= 1;
29 if (crc & 0x1000000)
30 crc ^= 0x01864cfb;
31 }
32 }
33 return crc;
34}
35
36/* NOTE: These defines are interlinked with below functions and directly modify
37 the values. This may not be optimized in terms of final program code size but
38 should be optimized in terms of speed.
39
40 modified variables are:
41 - everything defined in STARTDATA (only use ressize outside of the defines,
42 others are private)
43 - buffer
44 - size
45 */
46
47#ifndef NOENCODE
48#define STOREBITS \
49 while(numbits >= 8) { \
50 if(!size) return 0; \
51 *(buffer++) = bitbuffer>>(numbits-8); \
52 numbits -= 8; \
53 ++ressize; \
54 --size; \
55 }
56
57#define ADDBITS(a, b) { \
58 bitbuffer = (bitbuffer<<(a))|((b)&((1<<a)-1)); \
59 numbits += (a); \
60 STOREBITS \
61 }
62
63#define STARTDATA \
64 size_t ressize=0; \
65 char *blockstart; \
66 int numbits; \
67 uint64_t bitbuffer=0;
68
69#define INITBLOCK \
70 numbits = 0; \
71 blockstart = buffer; \
72 ADDBITS(8, 0xD3) \
73 ADDBITS(6, 0) \
74 ADDBITS(10, 0)
75
76#define ENDBLOCK \
77 if(numbits) { ADDBITS((8-numbits), 0) } { \
78 int len = buffer-blockstart-3; \
79 blockstart[1] |= len>>8; \
80 blockstart[2] = len; \
81 if(len > 1023) \
82 return 0; \
83 len = CRC24(len+3, (const unsigned char *) blockstart); \
84 ADDBITS(24, len) \
85 }
86
87#define SCALEADDBITS(a, b, c) ADDBITS(a, (int64_t)(c > 0 ? b*c+0.5 : b*c-0.5))
88
89#define MPI 3.141592653589793
90
91/* GNSS macros - Header part */
92#define T_RTCM_MESSAGE_NUMBER(a) ADDBITS(12, a) /* DF002 */
93#define T_IGS_SSR_VERSION(a) ADDBITS( 3, a) /* IDF001 */
94#define T_IGS_MESSAGE_NUMBER(a) ADDBITS( 8, a) /* IDF002 */
95#define T_SSR_EPOCH_TIME(a) ADDBITS(20, a) /* IDF003 */
96#define T_SSR_UPDATE_INTERVAL(a) ADDBITS( 4, a) /* DF391, IDF004 */
97#define T_MULTIPLE_MESSAGE_INDICATOR(a) ADDBITS( 1, a) /* DF388, IDF005 */
98#define T_SSR_IOD(a) ADDBITS( 4, a) /* DF413, IDF007 */
99#define T_SSR_PROVIDER_ID(a) ADDBITS(16, a) /* DF414, IDF008 */
100#define T_SSR_SOLUTION_ID(a) ADDBITS( 4, a) /* DF415, IDF009 */
101#define T_SATELLITE_REFERENCE_DATUM(a) ADDBITS( 1, a) /* DF375, IDF006 */
102#define T_NO_OF_SATELLITES(a) ADDBITS( 6, a) /* DF387, IDF010 */
103
104/* GNSS macros - Satellite specific part */
105#define T_GNSS_SATELLITE_ID(a) ADDBITS( 6, a) /* IDF011 */
106#define T_GNSS_IOD(a) ADDBITS( 8, a) /* IDF012 */
107#define T_DELTA_RADIAL(a) SCALEADDBITS(22, 10000.0, a) /* DF365, IDF013 */
108#define T_DELTA_ALONG_TRACK(a) SCALEADDBITS(20, 2500.0, a) /* DF366, IDF014 */
109#define T_DELTA_CROSS_TRACK(a) SCALEADDBITS(20, 2500.0, a) /* DF367, IDF015 */
110#define T_DELTA_DOT_RADIAL(a) SCALEADDBITS(21, 1000000.0, a) /* DF368, IDF016 */
111#define T_DELTA_DOT_ALONG_TRACK(a) SCALEADDBITS(19, 250000.0, a) /* DF369, IDF017 */
112#define T_DELTA_DOT_CROSS_TRACK(a) SCALEADDBITS(19, 250000.0, a) /* DF370, IDF018 */
113
114#define T_DELTA_CLOCK_C0(a) SCALEADDBITS(22, 10000.0, a) /* DF376, IDF019 */
115#define T_DELTA_CLOCK_C1(a) SCALEADDBITS(21, 1000000.0, a) /* DF377, IDF020 */
116#define T_DELTA_CLOCK_C2(a) SCALEADDBITS(27, 50000000.0, a) /* DF378, IDF021 */
117#define T_HR_CLOCK_CORRECTION(a) SCALEADDBITS(22, 10000.0, a) /* DF390, IDF022 */
118
119#define T_NO_OF_BIASES(a) ADDBITS(5, a) /* IDF023 */
120#define T_GNSS_SIGNAL_IDENTIFIER(a) ADDBITS(5, a) /* IDF024 */
121#define T_CODE_BIAS(a) SCALEADDBITS(14, 100.0, a) /* DF383, IDF025 */
122#define T_YAW_ANGLE(a) SCALEADDBITS( 9, 256.0/MPI, a) /* DF480, IDF026 */
123#define T_YAW_RATE(a) SCALEADDBITS( 8, 8192.0/MPI, a) /* DF481, IDF027 */
124#define T_PHASE_BIAS(a) SCALEADDBITS(20, 10000.0, a) /* DF482, IDF028 */
125
126/* Phase specific part of GNSS phase bias message */
127#define T_INTEGER_INDICATOR(a) ADDBITS( 1, a) /* DF483, IDF029 */
128#define T_WIDE_LANE_INDICATOR(a) ADDBITS( 2, a) /* DF484, IDF030 */
129#define T_DISCONTINUITY_COUNTER(a) ADDBITS( 4, a) /* DF485, IDF031 */
130#define T_DISPERSIVE_BIAS_INDICATOR(a) ADDBITS( 1, a) /* DF486, IDF032 */
131#define T_MW_CONSISTENCY_INDICATOR(a) ADDBITS( 1, a) /* DF487, IDF033 */
132
133/* URA */
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/* Phase specific part of GNSS phase bias message */
550#define G_INTEGER_INDICATOR(a) GETBITS(a, 1) /* DF483, IDF029 */
551#define G_WIDE_LANE_INDICATOR(a) GETBITS(a, 2) /* DF484, IDF030 */
552#define G_DISCONTINUITY_COUNTER(a) GETBITS(a, 4) /* DF485, IDF031 */
553#define G_DISPERSIVE_BIAS_INDICATOR(a) GETBITS(a, 1) /* DF486, IDF032 */
554#define G_MW_CONSISTENCY_INDICATOR(a) GETBITS(a, 1) /* DF487, IDF033 */
555
556/* URA */
557#define G_SSR_URA(a) {int temp; GETBITS(temp, 6) \
558 (a) = URAToValue(temp);} /* DF389, IDF034 */
559
560/* Ionosphere */
561#define G_NO_IONO_LAYERS(a) {unsigned int temp; GETBITS(temp, 2) a = temp+1;} /* DF472, IDF035 */
562#define G_IONO_HEIGHT(a) GETFLOAT(a, 8 , 10000.0) /* DF473, IDF036 */
563#define G_IONO_DEGREE(a) {unsigned int temp; GETBITS(temp, 4) a = temp+1;} /* DF474, IDF037 */
564#define G_IONO_ORDER(a) {unsigned int temp; GETBITS(temp, 4) a = temp+1;} /* DF475, IDF038 */
565#define G_IONO_COEFF_C(a) GETFLOATSIGN(a, 16,1/200.0) /* DF476, IDF039 */
566#define G_IONO_COEFF_S(a) GETFLOATSIGN(a, 16,1/200.0) /* DF477, IDF040 */
567#define G_VTEC_QUALITY_INDICATOR(a) GETFLOAT (a, 9, 1/20.0) /* DF478, IDF041 */
568
569enum GCOB_RETURN GetSSR(struct ClockOrbit *co, struct CodeBias *b,struct VTEC *v,
570 struct PhaseBias *pb, const char *buffer, size_t size, int *bytesused) {
571 int mmi = 0, h, rs;
572 unsigned int type, pos, i, j, s, nums, id, version;
573 size_t sizeofrtcmblock;
574 const char *blockstart = buffer;
575 DECODESTART
576
577 if (size < 7)
578 return GCOBR_SHORTBUFFER;
579
580#ifdef DEBUG
581 fprintf(stderr, "GetSSR START: size %d, numbits %d\n",(int)size, numbits);
582#endif
583
584 G_HEADER(h)
585 G_RESERVEDH(rs)
586 G_SIZE(sizeofrtcmblock);
587
588 if ((unsigned char) h != 0xD3 || rs)
589 return GCOBR_UNKNOWNDATA;
590 if (size < sizeofrtcmblock + 3) /* 3 header bytes already removed */
591 return GCOBR_MESSAGEEXCEEDSBUFFER;
592 if (CRC24(sizeofrtcmblock + 3, (const unsigned char *) blockstart) !=
593 (uint32_t) ((((unsigned char) buffer[sizeofrtcmblock]) << 16) |
594 (((unsigned char) buffer[sizeofrtcmblock + 1]) << 8) |
595 (((unsigned char) buffer[sizeofrtcmblock + 2]))))
596 return GCOBR_CRCMISMATCH;
597 size = sizeofrtcmblock; /* reduce size, so overflows are detected */
598
599 G_RTCM_MESSAGE_NUMBER(type)
600 G_IGS_SSR_VERSION(version)
601#ifdef DEBUG
602 fprintf(stderr, "rtcmType %d igsVersion %d ",type, version);
603#endif
604 G_IGS_MESSAGE_NUMBER(type)
605#ifdef DEBUG
606 fprintf(stderr, "igsType IM%d size %d\n",type,(int)sizeofrtcmblock);
607#endif
608 if (bytesused)
609 *bytesused = sizeofrtcmblock + 6;
610
611 if (type == VTEC_BASE) {
612 unsigned int l, o, d;
613 if (!v)
614 return GCOBR_NOVTECPARAMETER;
615 memset(v, 0, sizeof(*v));
616 G_SSR_EPOCH_TIME(v->EpochTime)
617 G_SSR_UPDATE_INTERVAL(v->UpdateInterval)
618 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
619 G_SSR_IOD(v->SSRIOD)
620 G_SSR_PROVIDER_ID(v->SSRProviderID)
621 G_SSR_SOLUTION_ID(v->SSRSolutionID)
622 G_VTEC_QUALITY_INDICATOR(v->Quality)
623 G_NO_IONO_LAYERS(v->NumLayers)
624#ifdef DEBUG
625 fprintf(stderr, "epochTime %d ui %d mmi %d ssrIod %d providerId %d solId %d vtecQ %8.3f numLay %d \n",
626 v->EpochTime, v->UpdateInterval, mmi,
627 v->SSRIOD, v->SSRProviderID, v->SSRSolutionID, v->Quality, v->NumLayers);
628#endif
629 for (l = 0; l < v->NumLayers; ++l) {
630 G_IONO_HEIGHT(v->Layers[l].Height)
631 G_IONO_DEGREE(v->Layers[l].Degree)
632 G_IONO_ORDER(v->Layers[l].Order)
633#ifdef DEBUG
634 fprintf(stderr, "h %8.3f deg %d ord %d \n",
635 v->Layers[l].Height, v->Layers[l].Degree, v->Layers[l].Order);
636#endif
637 for (o = 0; o <= v->Layers[l].Order; ++o) {
638 for (d = o; d <= v->Layers[l].Degree; ++d) {
639 G_IONO_COEFF_C(v->Layers[l].Cosinus[d][o])
640#ifdef DEBUG
641 fprintf(stderr, "C[%02d][%02d] %8.3f \n",
642 d, o, v->Layers[l].Cosinus[d][o]);
643#endif
644 }
645 }
646 for (o = 1; o <= v->Layers[l].Order; ++o) {
647 for (d = o; d <= v->Layers[l].Degree; ++d) {
648 G_IONO_COEFF_S(v->Layers[l].Sinus[d][o])
649#ifdef DEBUG
650 fprintf(stderr, "S[%02d][%02d] %8.3f \n",
651 d, o, v->Layers[l].Sinus[d][o]);
652#endif
653 }
654 }
655 }
656#ifdef DEBUG
657 for(type = 0; type < (unsigned int)size && (unsigned char)buffer[type] != 0xD3; ++type)
658 numbits += 8;
659 fprintf(stderr, "numbits left %d\n",numbits);
660#endif
661 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
662 }
663 for (s = CLOCKORBIT_SATNUM; s-- > 0;) {
664 if (type >= corbase[s]) {
665 switch (type - corbase[s]) {
666 case COBOFS_ORBIT:
667 if (!co)
668 return GCOBR_NOCLOCKORBITPARAMETER;
669 co->messageType = type;
670 G_SSR_EPOCH_TIME_CHECK(co->EpochTime[s], co->NumberOfSat[s])
671 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
672 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
673 G_SSR_IOD(co->SSRIOD)
674 G_SSR_PROVIDER_ID(co->SSRProviderID)
675 G_SSR_SOLUTION_ID(co->SSRSolutionID)
676 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
677 G_NO_OF_SATELLITES(nums)
678 co->Supplied[COBOFS_ORBIT] |= 1;
679#ifdef DEBUG
680 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d rd %d ssrIod %d providerId %d solId %d\n",
681 co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
682 co->SatRefDatum, co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
683#endif
684 for (i = 0; i < nums; ++i) {
685 G_GNSS_SATELLITE_ID(id)
686 for (pos = satoffset[s];
687 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
688 ++pos)
689 ;
690 if (pos >= satoffset[s + 1])
691 return GCOBR_DATAMISMATCH;
692 else if (pos == co->NumberOfSat[s] + satoffset[s])
693 ++co->NumberOfSat[s];
694 co->Sat[pos].ID = id;
695 G_GNSS_IOD(co->Sat[pos].IOD)
696 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
697 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
698 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
699 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
700 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
701 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
702#ifdef DEBUG
703 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",
704 co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
705 co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
706 co->Sat[pos].Orbit.DotDeltaRadial,
707 co->Sat[pos].Orbit.DotDeltaAlongTrack,
708 co->Sat[pos].Orbit.DotDeltaCrossTrack);
709#endif
710 }
711 break;
712 case COBOFS_CLOCK:
713 if (!co)
714 return GCOBR_NOCLOCKORBITPARAMETER;
715 co->messageType = type;
716 G_SSR_EPOCH_TIME_CHECK(co->EpochTime[s], co->NumberOfSat[s])
717 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
718 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
719 G_SSR_IOD(co->SSRIOD)
720 G_SSR_PROVIDER_ID(co->SSRProviderID)
721 G_SSR_SOLUTION_ID(co->SSRSolutionID)
722 G_NO_OF_SATELLITES(nums)
723 co->Supplied[COBOFS_CLOCK] |= 1;
724#ifdef DEBUG
725 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d\n",
726 co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
727 co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
728#endif
729 for (i = 0; i < nums; ++i) {
730 G_GNSS_SATELLITE_ID(id)
731 for (pos = satoffset[s];
732 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
733 ++pos)
734 ;
735 if (pos >= satoffset[s + 1])
736 return GCOBR_DATAMISMATCH;
737 else if (pos == co->NumberOfSat[s] + satoffset[s])
738 ++co->NumberOfSat[s];
739 co->Sat[pos].ID = id;
740 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
741 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
742 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
743#ifdef DEBUG
744 fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
745 co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
746 co->Sat[pos].Clock.DeltaA2);
747#endif
748 }
749 break;
750 case COBOFS_COMBINED:
751 if (!co)
752 return GCOBR_NOCLOCKORBITPARAMETER;
753 co->messageType = type;
754 G_SSR_EPOCH_TIME_CHECK(co->EpochTime[s], co->NumberOfSat[s])
755 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
756 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
757 G_SSR_IOD(co->SSRIOD)
758 G_SSR_PROVIDER_ID(co->SSRProviderID)
759 G_SSR_SOLUTION_ID(co->SSRSolutionID)
760 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
761 G_NO_OF_SATELLITES(nums)
762 co->Supplied[COBOFS_ORBIT] |= 1;
763 co->Supplied[COBOFS_CLOCK] |= 1;
764#ifdef DEBUG
765 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d rd %d ssrIod %d providerId %d solId %d\n",
766 co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
767 co->SatRefDatum, co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
768#endif
769 for (i = 0; i < nums; ++i) {
770 G_GNSS_SATELLITE_ID(id)
771 for (pos = satoffset[s];
772 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
773 ++pos)
774 ;
775 if (pos >= satoffset[s + 1])
776 return GCOBR_DATAMISMATCH;
777 else if (pos == co->NumberOfSat[s] + satoffset[s])
778 ++co->NumberOfSat[s];
779 co->Sat[pos].ID = id;
780 G_GNSS_IOD(co->Sat[pos].IOD)
781 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
782 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
783 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
784 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
785 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
786 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
787 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
788 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
789 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
790#ifdef DEBUG
791 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",
792 co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
793 co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
794 co->Sat[pos].Orbit.DotDeltaRadial, co->Sat[pos].Orbit.DotDeltaAlongTrack,
795 co->Sat[pos].Orbit.DotDeltaCrossTrack,
796 co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1, co->Sat[pos].Clock.DeltaA2);
797#endif
798 }
799 break;
800 case COBOFS_URA:
801 if (!co)
802 return GCOBR_NOCLOCKORBITPARAMETER;
803 co->messageType = type;
804 G_SSR_EPOCH_TIME_CHECK(co->EpochTime[s], co->NumberOfSat[s])
805 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
806 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
807 G_SSR_IOD(co->SSRIOD)
808 G_SSR_PROVIDER_ID(co->SSRProviderID)
809 G_SSR_SOLUTION_ID(co->SSRSolutionID)
810 G_NO_OF_SATELLITES(nums)
811 co->Supplied[COBOFS_URA] |= 1;
812#ifdef DEBUG
813 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d\n",
814 co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
815 co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
816#endif
817 for (i = 0; i < nums; ++i) {
818 G_GNSS_SATELLITE_ID(id)
819 for (pos = satoffset[s];
820 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
821 ++pos)
822 ;
823 if (pos >= satoffset[s + 1])
824 return GCOBR_DATAMISMATCH;
825 else if (pos == co->NumberOfSat[s] + satoffset[s])
826 ++co->NumberOfSat[s];
827 co->Sat[pos].ID = id;
828 G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
829#ifdef DEBUG
830 fprintf(stderr, "id %2d ura %8.3f \n",
831 co->Sat[pos].ID, co->Sat[pos].UserRangeAccuracy);
832#endif
833 }
834 break;
835 case COBOFS_HR:
836 if (!co)
837 return GCOBR_NOCLOCKORBITPARAMETER;
838 co->messageType = type;
839 G_SSR_EPOCH_TIME_CHECK(co->EpochTime[s], co->NumberOfSat[s])
840 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
841 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
842 G_SSR_IOD(co->SSRIOD)
843 G_SSR_PROVIDER_ID(co->SSRProviderID)
844 G_SSR_SOLUTION_ID(co->SSRSolutionID)
845 G_NO_OF_SATELLITES(nums)
846 co->Supplied[COBOFS_HR] |= 1;
847#ifdef DEBUG
848 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d\n",
849 co->EpochTime[s], co->UpdateInterval,mmi,co->NumberOfSat[s],nums,
850 co->SSRIOD, co->SSRProviderID, co->SSRSolutionID);
851#endif
852 for (i = 0; i < nums; ++i) {
853 G_GNSS_SATELLITE_ID(id)
854 for (pos = satoffset[s];
855 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
856 ++pos)
857 ;
858 if (pos >= satoffset[s + 1])
859 return GCOBR_DATAMISMATCH;
860 else if (pos == co->NumberOfSat[s] + satoffset[s])
861 ++co->NumberOfSat[s];
862 co->Sat[pos].ID = id;
863 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
864#ifdef DEBUG
865 fprintf(stderr, "id %2d hrClock %8.3f \n",
866 co->Sat[pos].ID, co->Sat[pos].hrclock);
867#endif
868 }
869 break;
870 case COBOFS_CBIAS:
871 if (!b)
872 return GCOBR_NOCODEBIASPARAMETER;
873 b->messageType = type;
874 G_SSR_EPOCH_TIME_CHECK(b->EpochTime[s], b->NumberOfSat[s])
875 G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
876 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
877 G_SSR_IOD(b->SSRIOD)
878 G_SSR_PROVIDER_ID(b->SSRProviderID)
879 G_SSR_SOLUTION_ID(b->SSRSolutionID)
880 G_NO_OF_SATELLITES(nums)
881#ifdef DEBUG
882 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d\n",
883 b->EpochTime[s], b->UpdateInterval,mmi,b->NumberOfSat[s],nums,
884 b->SSRIOD, b->SSRProviderID, b->SSRSolutionID);
885#endif
886 for (i = 0; i < nums; ++i) {
887 G_GNSS_SATELLITE_ID(id)
888 for (pos = satoffset[s];
889 pos < satoffset[s] + b->NumberOfSat[s] && b->Sat[pos].ID != id;
890 ++pos)
891 ;
892 if (pos >= satoffset[s + 1])
893 return GCOBR_DATAMISMATCH;
894 else if (pos == b->NumberOfSat[s] + satoffset[s])
895 ++b->NumberOfSat[s];
896 b->Sat[pos].ID = id;
897 G_NO_OF_BIASES(b->Sat[pos].NumberOfCodeBiases)
898#ifdef DEBUG
899 fprintf(stderr, "id %2d #%d ",
900 b->Sat[pos].ID, b->Sat[pos].NumberOfCodeBiases);
901#endif
902 for (j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j) {
903 G_GNSS_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
904 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
905#ifdef DEBUG
906 fprintf(stderr, "t%02d b %8.2f ",
907 b->Sat[pos].Biases[j].Type, b->Sat[pos].Biases[j].Bias);
908#endif
909 }
910#ifdef DEBUG
911 fprintf(stderr, "\n");
912#endif
913 }
914 break;
915 case COBOFS_PBIAS:
916 if (!pb)
917 return GCOBR_NOPHASEBIASPARAMETER;
918 pb->messageType = type;
919 G_SSR_EPOCH_TIME_CHECK(pb->EpochTime[s], pb->NumberOfSat[s])
920 G_SSR_UPDATE_INTERVAL(pb->UpdateInterval)
921 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
922 G_SSR_IOD(pb->SSRIOD)
923 G_SSR_PROVIDER_ID(pb->SSRProviderID)
924 G_SSR_SOLUTION_ID(pb->SSRSolutionID)
925 G_DISPERSIVE_BIAS_INDICATOR(pb->DispersiveBiasConsistencyIndicator)
926 G_MW_CONSISTENCY_INDICATOR(pb->MWConsistencyIndicator)
927 G_NO_OF_SATELLITES(nums)
928#ifdef DEBUG
929 fprintf(stderr, "epochTime %d ui %d mmi %d sats %d/%d ssrIod %d providerId %d solId %d dispInd %d mwInd %d\n",
930 pb->EpochTime[s], pb->UpdateInterval,mmi,pb->NumberOfSat[s],nums,
931 pb->SSRIOD, pb->SSRProviderID, pb->SSRSolutionID,
932 pb->DispersiveBiasConsistencyIndicator, pb->MWConsistencyIndicator);
933#endif
934 for (i = 0; i < nums; ++i) {
935 G_GNSS_SATELLITE_ID(id)
936 for (pos = satoffset[s];
937 pos < satoffset[s] + pb->NumberOfSat[s] && pb->Sat[pos].ID != id;
938 ++pos)
939 ;
940 if (pos >= satoffset[s + 1])
941 return GCOBR_DATAMISMATCH;
942 else if (pos == pb->NumberOfSat[s] + satoffset[s])
943 ++pb->NumberOfSat[s];
944 pb->Sat[pos].ID = id;
945 G_NO_OF_BIASES(pb->Sat[pos].NumberOfPhaseBiases)
946 G_YAW_ANGLE(pb->Sat[pos].YawAngle)
947 G_YAW_RATE(pb->Sat[pos].YawRate)
948#ifdef DEBUG
949 fprintf(stderr, "id %2d #%d y %10.6f yr %10.6f ",
950 pb->Sat[pos].ID, pb->Sat[pos].NumberOfPhaseBiases,
951 pb->Sat[pos].YawAngle/MPI, pb->Sat[pos].YawRate/MPI);
952#endif
953 for (j = 0; j < pb->Sat[pos].NumberOfPhaseBiases; ++j) {
954 G_GNSS_SIGNAL_IDENTIFIER(pb->Sat[pos].Biases[j].Type)
955 G_INTEGER_INDICATOR(pb->Sat[pos].Biases[j].SignalIntegerIndicator)
956 G_WIDE_LANE_INDICATOR(pb->Sat[pos].Biases[j].SignalsWideLaneIntegerIndicator)
957 G_DISCONTINUITY_COUNTER(pb->Sat[pos].Biases[j].SignalDiscontinuityCounter)
958 G_PHASE_BIAS(pb->Sat[pos].Biases[j].Bias)
959#ifdef DEBUG
960 fprintf(stderr, "t%02d int %d wl %d disc %d b %8.4f ",
961 pb->Sat[pos].Biases[j].Type,
962 pb->Sat[pos].Biases[j].SignalIntegerIndicator,
963 pb->Sat[pos].Biases[j].SignalsWideLaneIntegerIndicator,
964 pb->Sat[pos].Biases[j].SignalDiscontinuityCounter,
965 pb->Sat[pos].Biases[j].Bias);
966#endif
967 }
968#ifdef DEBUG
969 fprintf(stderr, "\n");
970#endif
971 }
972 break;
973 default:
974 continue;
975 }
976#ifdef DEBUG
977 for(type = 0; type < (unsigned int)size && (unsigned char)buffer[type] != 0xD3; ++type)
978 numbits += 8;
979 fprintf(stderr, "numbits left %d\n",numbits);
980#endif
981 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
982 }
983 }
984 return GCOBR_UNKNOWNTYPE;
985}
986#endif /* NODECODE */
987
988
989
Note: See TracBrowser for help on using the repository browser.