source: ntrip/trunk/BNC/src/RTCM3/clock_and_orbit/clock_orbit_igs.c@ 8996

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

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