source: ntrip/trunk/BNC/src/RTCM3/clock_and_orbit/clock_orbit_rtcm.c@ 8970

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

intial imprort for igs ssr encoding and decoding
  • Property svn:executable set to *
  • Property svn:keywords set to Id
File size: 45.6 KB
RevLine 
[956]1/* Programheader
2
[8963]3 Name: clock_orbit_rtcm.c
4 Project: RTCM3
5 Version: $Id: clock_orbit_rtcm.c 8970 2020-07-15 07:24:30Z stuerze $
6 Authors: Dirk Stöcker
[8969]7 Description: state space approach: RTCM
[8963]8 */
[956]9
[2421]10#include <math.h>
[956]11#include <stdio.h>
12#include <string.h>
[2323]13#ifndef sparc
[956]14#include <stdint.h>
[2323]15#else
16#include <sys/types.h>
17#endif
[956]18#include "clock_orbit_rtcm.h"
19
[8963]20static uint32_t CRC24(long size, const unsigned char *buf) {
[956]21 uint32_t crc = 0;
22 int i;
23
[8963]24 while (size--) {
[956]25 crc ^= (*buf++) << (16);
[8963]26 for (i = 0; i < 8; i++)
27 {
[956]28 crc <<= 1;
[8963]29 if (crc & 0x1000000)
[956]30 crc ^= 0x01864cfb;
31 }
32 }
33 return crc;
34}
35
36/* NOTE: These defines are interlinked with below functions and directly modify
[8963]37 the values. This may not be optimized in terms of final program code size but
38 should be optimized in terms of speed.
[956]39
[8963]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 */
[956]46
47#ifndef NOENCODE
48#define STOREBITS \
[8963]49 while(numbits >= 8) { \
[956]50 if(!size) return 0; \
51 *(buffer++) = bitbuffer>>(numbits-8); \
52 numbits -= 8; \
53 ++ressize; \
54 --size; \
55 }
56
[8963]57#define ADDBITS(a, b) { \
[956]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 \
[8963]77 if(numbits) { ADDBITS((8-numbits), 0) } { \
[956]78 int len = buffer-blockstart-3; \
79 blockstart[1] |= len>>8; \
80 blockstart[2] = len; \
[1826]81 if(len > 1023) \
82 return 0; \
[956]83 len = CRC24(len+3, (const unsigned char *) blockstart); \
84 ADDBITS(24, len) \
85 }
86
[5664]87#define SCALEADDBITS(a, b, c) ADDBITS(a, (int64_t)(c > 0 ? b*c+0.5 : b*c-0.5))
[956]88
[5664]89#define MPI 3.141592653589793
[956]90
[5664]91/* GPS macros also used for other systems when matching! */
[8970]92#define T_RTCM_MESSAGE_NUMBER(a) ADDBITS(12, a) /* DF002 */
93#define T_GPS_SATELLITE_ID(a) ADDBITS( 6, a) /* DF068 */
94#define T_QZSS_SATELLITE_ID(a) ADDBITS( 4, a) /* DF249 */
95#define T_GLONASS_SATELLITE_ID(a) ADDBITS( 5, a)
[5664]96
[8970]97#define T_GPS_IODE(a) ADDBITS( 8, a) /* DF071 */
98#define T_GLONASS_IOD(a) ADDBITS( 8, a) /* DF239 */
99#define T_GALILEO_IOD(a) ADDBITS(10, a) /* DF459 */
100#define T_SBAS_T0MOD(a) ADDBITS( 9, (a/16)) /* DF468 */
101#define T_SBAS_IODCRC(a) ADDBITS(24, a) /* DF469 */
102#define T_BDS_TOEMOD(a) ADDBITS(10, (a/8)) /* DF470 */
103#define T_BDS_IOD(a) ADDBITS( 8, a) /* DF471 */
[5664]104
[1808]105#define T_DELTA_RADIAL(a) SCALEADDBITS(22, 10000.0, a)
106#define T_DELTA_ALONG_TRACK(a) SCALEADDBITS(20, 2500.0, a)
107#define T_DELTA_CROSS_TRACK(a) SCALEADDBITS(20, 2500.0, a)
108#define T_DELTA_DOT_RADIAL(a) SCALEADDBITS(21, 1000000.0, a)
109#define T_DELTA_DOT_ALONG_TRACK(a) SCALEADDBITS(19, 250000.0, a)
110#define T_DELTA_DOT_CROSS_TRACK(a) SCALEADDBITS(19, 250000.0, a)
[1581]111
[956]112#define T_SATELLITE_REFERENCE_DATUM(a) ADDBITS(1, a)
[1808]113#define T_DELTA_CLOCK_C0(a) SCALEADDBITS(22, 10000.0, a)
114#define T_DELTA_CLOCK_C1(a) SCALEADDBITS(21, 1000000.0, a)
[1581]115#define T_DELTA_CLOCK_C2(a) SCALEADDBITS(27, 50000000.0, a)
[8970]116#define T_NO_OF_CODE_BIASES(a) ADDBITS( 5, a)
117#define T_NO_OF_PHASE_BIASES(a) ADDBITS( 5, a)
118#define T_SIGNAL_IDENTIFIER(a) ADDBITS( 5, a)
[1808]119#define T_CODE_BIAS(a) SCALEADDBITS(14, 100.0, a)
[8970]120#define T_YAW_ANGLE(a) SCALEADDBITS( 9, 256.0/MPI, a)
121#define T_YAW_RATE(a) SCALEADDBITS( 8, 8192.0/MPI, a)
[5664]122#define T_PHASE_BIAS(a) SCALEADDBITS(20, 10000.0, a)
[956]123
[1581]124#define T_GPS_EPOCH_TIME(a) ADDBITS(20, a)
125#define T_GLONASS_EPOCH_TIME(a) ADDBITS(17, a)
[8970]126#define T_NO_OF_SATELLITES(a) ADDBITS( 6, a)
127#define T_MULTIPLE_MESSAGE_INDICATOR(a) ADDBITS( 1, a)
128#define T_DISPERSIVE_BIAS_INDICATOR(a) ADDBITS( 1, a)
129#define T_MW_CONSISTENCY_INDICATOR(a) ADDBITS( 1, a)
130#define T_INTEGER_INDICATOR(a) ADDBITS( 1, a)
131#define T_WIDE_LANE_INDICATOR(a) ADDBITS( 2, a)
132#define T_DISCONTINUITY_COUNTER(a) ADDBITS( 4, a)
133#define T_SSR_URA(a) ADDBITS( 6, a)
[1808]134#define T_HR_CLOCK_CORRECTION(a) SCALEADDBITS(22, 10000.0, a)
[8970]135#define T_SSR_UPDATE_INTERVAL(a) ADDBITS( 4, a)
[956]136
[8970]137#define T_SSR_IOD(a) ADDBITS( 4, a)
[3511]138#define T_SSR_PROVIDER_ID(a) ADDBITS(16, a)
[8970]139#define T_SSR_SOLUTION_ID(a) ADDBITS( 4, a)
[3511]140
[8970]141#define T_NO_IONO_LAYERS(a) ADDBITS( 2, a-1)
142#define T_VTEC_QUALITY_INDICATOR(a) SCALEADDBITS( 9, 20.0, a)
[5664]143#define T_IONO_COEFF(a) SCALEADDBITS(16, 200.0, a)
[8970]144#define T_IONO_DEGREE(a) ADDBITS( 4, a-1)
145#define T_IONO_ORDER(a) ADDBITS( 4, a-1)
146#define T_IONO_HEIGHT(a) SCALEADDBITS( 8, 1/10000.0, a)
[5664]147
[8963]148static double URAToValue(int ura) {
[2421]149 int urac, urav;
150 urac = ura >> 3;
151 urav = ura & 7;
[8963]152 if (!ura)
[2421]153 return 0;
[8963]154 else if (ura == 63)
[2421]155 return SSR_MAXURA;
[8963]156 return (pow(3, urac) * (1.0 + urav / 4.0) - 1.0) / 1000.0;
[2421]157}
158
[8963]159static int ValueToURA(double val) {
[2421]160 int ura;
[8963]161 if (!val)
[2421]162 return 0;
[8963]163 else if (val > 5.4665)
[2421]164 return 63;
[8963]165 for (ura = 1; ura < 63 && val > URAToValue(ura); ++ura)
[2421]166 ;
167 return ura;
168}
169
[8963]170static const enum ClockOrbitType corbase[CLOCKORBIT_SATNUM] = {
171 (int) COBBASE_GPS,
172 (int) COBBASE_GLONASS,
173 (int) COBBASE_GALILEO,
174 (int) COBBASE_QZSS,
175 (int) COBBASE_SBAS,
176 (int) COBBASE_BDS
[5664]177};
[8963]178
179static const enum COR_OFFSETS satoffset[CLOCKORBIT_SATNUM + 1] = {
180 CLOCKORBIT_OFFSETGPS,
181 CLOCKORBIT_OFFSETGLONASS,
182 CLOCKORBIT_OFFSETGALILEO,
183 CLOCKORBIT_OFFSETQZSS,
184 CLOCKORBIT_OFFSETSBAS,
185 CLOCKORBIT_OFFSETBDS,
[5664]186 CLOCKORBIT_COUNTSAT
187};
188
[956]189size_t MakeClockOrbit(const struct ClockOrbit *co, enum ClockOrbitType type,
[8963]190 int moremessagesfollow, char *buffer, size_t size) {
[5664]191 unsigned int status[CLOCKORBIT_SATNUM][COBOFS_NUM], i, s;
[1819]192
[5664]193 memset(status, 0, sizeof(status));
[956]194
[8970]195 STARTDATA
[8963]196
197 for (s = 0; s < CLOCKORBIT_SATNUM; ++s) {
198 for (i = 0; i < COBOFS_NUM; ++i) {
199 if (co->NumberOfSat[s] && (type == COTYPE_AUTO || type == corbase[s] + i) &&
200 (co->Supplied[i] || (i <= COBOFS_CLOCK && co->Supplied[COBOFS_COMBINED]) ||
201 (i == COBOFS_COMBINED && co->Supplied[COBOFS_ORBIT] && co->Supplied[COBOFS_CLOCK]))) {
[5664]202 status[s][i] = 1;
[8963]203 if (i == COBOFS_COMBINED) {
[5664]204 status[s][COBOFS_ORBIT] = status[s][COBOFS_CLOCK] = 0;
205 } /* disable single blocks for combined type */
206 } /* check for data */
207 } /* iterate over RTCM data types */
208 } /* iterate over satellite systems */
[956]209
[8963]210 for (s = 0; s < CLOCKORBIT_SATNUM; ++s) {
211 if (status[s][COBOFS_ORBIT]) {
[1840]212 INITBLOCK
[8970]213 T_RTCM_MESSAGE_NUMBER(corbase[s] + COBOFS_ORBIT)
[8963]214 switch (s) {
215 case CLOCKORBIT_SATGPS:
216 case CLOCKORBIT_SATGALILEO:
217 case CLOCKORBIT_SATQZSS:
218 case CLOCKORBIT_SATSBAS:
219 case CLOCKORBIT_SATBDS:
220 T_GPS_EPOCH_TIME(co->EpochTime[s])
221 break;
222 case CLOCKORBIT_SATGLONASS:
223 T_GLONASS_EPOCH_TIME(co->EpochTime[s])
224 break;
[5664]225 }
[1840]226 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
[5664]227 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
[2421]228 T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
[3511]229 T_SSR_IOD(co->SSRIOD)
230 T_SSR_PROVIDER_ID(co->SSRProviderID)
231 T_SSR_SOLUTION_ID(co->SSRSolutionID)
[5664]232 T_NO_OF_SATELLITES(co->NumberOfSat[s])
[8963]233 for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) {
234 switch (s) {
235 case CLOCKORBIT_SATGPS:
236 T_GPS_SATELLITE_ID(co->Sat[i].ID)
237 T_GPS_IODE(co->Sat[i].IOD)
238 break;
239 case CLOCKORBIT_SATGLONASS:
240 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
241 T_GLONASS_IOD(co->Sat[i].IOD)
242 break;
243 case CLOCKORBIT_SATGALILEO:
244 T_GPS_SATELLITE_ID(co->Sat[i].ID)
245 T_GALILEO_IOD(co->Sat[i].IOD)
246 break;
247 case CLOCKORBIT_SATQZSS:
248 T_QZSS_SATELLITE_ID(co->Sat[i].ID)
249 T_GPS_IODE(co->Sat[i].IOD)
250 break;
251 case CLOCKORBIT_SATSBAS:
252 T_GPS_SATELLITE_ID(co->Sat[i].ID)
253 T_SBAS_T0MOD(co->Sat[i].toe)
254 T_SBAS_IODCRC(co->Sat[i].IOD)
255 break;
256 case CLOCKORBIT_SATBDS:
257 T_GPS_SATELLITE_ID(co->Sat[i].ID)
258 T_BDS_TOEMOD(co->Sat[i].toe)
259 T_BDS_IOD(co->Sat[i].IOD)
260 break;
[5664]261 }
[1840]262 T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
263 T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
264 T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
265 T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
266 T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
267 T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
[5664]268 }
269 ENDBLOCK
270 }
[8963]271 if (status[s][COBOFS_CLOCK]) {
[5664]272 INITBLOCK
[8970]273 T_RTCM_MESSAGE_NUMBER(corbase[s] + COBOFS_CLOCK)
[8963]274 switch (s) {
275 case CLOCKORBIT_SATGPS:
276 case CLOCKORBIT_SATGALILEO:
277 case CLOCKORBIT_SATQZSS:
278 case CLOCKORBIT_SATSBAS:
279 case CLOCKORBIT_SATBDS:
280 T_GPS_EPOCH_TIME(co->EpochTime[s])
281 break;
282 case CLOCKORBIT_SATGLONASS:
283 T_GLONASS_EPOCH_TIME(co->EpochTime[s])
284 break;
[5664]285 }
286 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
287 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
288 T_SSR_IOD(co->SSRIOD)
289 T_SSR_PROVIDER_ID(co->SSRProviderID)
290 T_SSR_SOLUTION_ID(co->SSRSolutionID)
291 T_NO_OF_SATELLITES(co->NumberOfSat[s])
[8963]292 for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) {
293 switch (s) {
294 case CLOCKORBIT_SATGPS:
295 case CLOCKORBIT_SATGALILEO:
296 case CLOCKORBIT_SATSBAS:
297 case CLOCKORBIT_SATBDS:
298 T_GPS_SATELLITE_ID(co->Sat[i].ID)
299 break;
300 case CLOCKORBIT_SATQZSS:
301 T_QZSS_SATELLITE_ID(co->Sat[i].ID)
302 break;
303 case CLOCKORBIT_SATGLONASS:
304 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
305 break;
[5664]306 }
[1840]307 T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
308 T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
309 T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)
310 }
311 ENDBLOCK
[5664]312 }
[8963]313 if (status[s][COBOFS_COMBINED]) {
[2432]314#ifdef SPLITBLOCK
[5664]315 int nums = co->NumberOfSat[s];
316 int left, start = satoffset[s];
[8963]317 if(nums > 28) {/* split block when more than 28 sats */
[5664]318 left = nums - 28;
319 nums = 28;
320 }
[8963]321 else {
[5664]322 left = 0;
323 }
[8963]324 while(nums) {
[2432]325#endif
[8963]326 INITBLOCK
[8970]327 T_RTCM_MESSAGE_NUMBER(corbase[s] + COBOFS_COMBINED)
[8963]328 switch (s) {
329 case CLOCKORBIT_SATGPS:
330 case CLOCKORBIT_SATGALILEO:
331 case CLOCKORBIT_SATQZSS:
332 case CLOCKORBIT_SATSBAS:
[5664]333 case CLOCKORBIT_SATBDS:
334 T_GPS_EPOCH_TIME(co->EpochTime[s])
335 break;
336 case CLOCKORBIT_SATGLONASS:
337 T_GLONASS_EPOCH_TIME(co->EpochTime[s])
338 break;
[8963]339 }
340 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
[5664]341#ifdef SPLITBLOCK
[8963]342 T_MULTIPLE_MESSAGE_INDICATOR((moremessagesfollow || left) ? 1 : 0)
[5664]343#else
[8963]344 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
[5664]345#endif
[8963]346 T_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
347 T_SSR_IOD(co->SSRIOD)
348 T_SSR_PROVIDER_ID(co->SSRProviderID)
349 T_SSR_SOLUTION_ID(co->SSRSolutionID)
[5664]350#ifdef SPLITBLOCK
[8963]351 T_NO_OF_SATELLITES(nums)
352 for(i = start; i < start+nums; ++i)
[5664]353#else
[8963]354 T_NO_OF_SATELLITES(co->NumberOfSat[s])
355 for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i)
[5664]356#endif
[8963]357 {
358 switch (s) {
[5664]359 case CLOCKORBIT_SATGPS:
360 T_GPS_SATELLITE_ID(co->Sat[i].ID)
361 T_GPS_IODE(co->Sat[i].IOD)
362 break;
363 case CLOCKORBIT_SATGLONASS:
364 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
365 T_GLONASS_IOD(co->Sat[i].IOD)
366 break;
367 case CLOCKORBIT_SATGALILEO:
368 T_GPS_SATELLITE_ID(co->Sat[i].ID)
369 T_GALILEO_IOD(co->Sat[i].IOD)
370 break;
371 case CLOCKORBIT_SATQZSS:
372 T_QZSS_SATELLITE_ID(co->Sat[i].ID)
373 T_GPS_IODE(co->Sat[i].IOD)
374 break;
375 case CLOCKORBIT_SATSBAS:
376 T_GPS_SATELLITE_ID(co->Sat[i].ID)
377 T_SBAS_T0MOD(co->Sat[i].toe)
378 T_SBAS_IODCRC(co->Sat[i].IOD)
379 break;
380 case CLOCKORBIT_SATBDS:
381 T_GPS_SATELLITE_ID(co->Sat[i].ID)
382 T_BDS_TOEMOD(co->Sat[i].toe)
[8410]383 T_BDS_IOD(co->Sat[i].IOD)
[5664]384 break;
385 }
[8963]386 T_DELTA_RADIAL(co->Sat[i].Orbit.DeltaRadial)
387 T_DELTA_ALONG_TRACK(co->Sat[i].Orbit.DeltaAlongTrack)
388 T_DELTA_CROSS_TRACK(co->Sat[i].Orbit.DeltaCrossTrack)
389 T_DELTA_DOT_RADIAL(co->Sat[i].Orbit.DotDeltaRadial)
390 T_DELTA_DOT_ALONG_TRACK(co->Sat[i].Orbit.DotDeltaAlongTrack)
391 T_DELTA_DOT_CROSS_TRACK(co->Sat[i].Orbit.DotDeltaCrossTrack)
392 T_DELTA_CLOCK_C0(co->Sat[i].Clock.DeltaA0)
393 T_DELTA_CLOCK_C1(co->Sat[i].Clock.DeltaA1)
394 T_DELTA_CLOCK_C2(co->Sat[i].Clock.DeltaA2)
395 }
396 ENDBLOCK
[5664]397#ifdef SPLITBLOCK
[8963]398 start += nums;
399 nums = left;
400 left = 0;
401 }
[5664]402#endif
[1581]403 }
[8963]404 if (status[s][COBOFS_HR]) {
[5664]405 INITBLOCK
[8970]406 T_RTCM_MESSAGE_NUMBER(corbase[s] + COBOFS_HR)
[8963]407 switch (s) {
408 case CLOCKORBIT_SATGPS:
409 case CLOCKORBIT_SATGALILEO:
410 case CLOCKORBIT_SATQZSS:
411 case CLOCKORBIT_SATSBAS:
412 case CLOCKORBIT_SATBDS:
413 T_GPS_EPOCH_TIME(co->EpochTime[s])
414 break;
415 case CLOCKORBIT_SATGLONASS:
416 T_GLONASS_EPOCH_TIME(co->EpochTime[s])
417 break;
[5664]418 }
419 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
420 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
421 T_SSR_IOD(co->SSRIOD)
422 T_SSR_PROVIDER_ID(co->SSRProviderID)
423 T_SSR_SOLUTION_ID(co->SSRSolutionID)
424 T_NO_OF_SATELLITES(co->NumberOfSat[s])
[8963]425 for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) {
426 switch (s) {
427 case CLOCKORBIT_SATGPS:
428 case CLOCKORBIT_SATGALILEO:
429 case CLOCKORBIT_SATSBAS:
430 case CLOCKORBIT_SATBDS:
431 T_GPS_SATELLITE_ID(co->Sat[i].ID)
432 break;
433 case CLOCKORBIT_SATQZSS:
434 T_QZSS_SATELLITE_ID(co->Sat[i].ID)
435 break;
436 case CLOCKORBIT_SATGLONASS:
437 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
438 break;
[5664]439 }
440 T_HR_CLOCK_CORRECTION(co->Sat[i].hrclock)
441 }
442 ENDBLOCK
[1581]443 }
[8963]444 if (status[s][COBOFS_URA]) {
[5664]445 INITBLOCK
[8970]446 T_RTCM_MESSAGE_NUMBER(corbase[s] + COBOFS_URA)
[8963]447 switch (s) {
448 case CLOCKORBIT_SATGPS:
449 case CLOCKORBIT_SATGALILEO:
450 case CLOCKORBIT_SATQZSS:
451 case CLOCKORBIT_SATSBAS:
452 case CLOCKORBIT_SATBDS:
453 T_GPS_EPOCH_TIME(co->EpochTime[s])
454 break;
455 case CLOCKORBIT_SATGLONASS:
456 T_GLONASS_EPOCH_TIME(co->EpochTime[s])
457 break;
[5664]458 }
[5670]459 T_SSR_UPDATE_INTERVAL(co->UpdateInterval)
[5664]460 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
461 T_SSR_IOD(co->SSRIOD)
462 T_SSR_PROVIDER_ID(co->SSRProviderID)
463 T_SSR_SOLUTION_ID(co->SSRSolutionID)
464 T_NO_OF_SATELLITES(co->NumberOfSat[s])
[8963]465 for (i = satoffset[s]; i < satoffset[s] + co->NumberOfSat[s]; ++i) {
466 switch (s) {
467 case CLOCKORBIT_SATGPS:
468 case CLOCKORBIT_SATGALILEO:
469 case CLOCKORBIT_SATSBAS:
470 case CLOCKORBIT_SATBDS:
471 T_GPS_SATELLITE_ID(co->Sat[i].ID)
472 break;
473 case CLOCKORBIT_SATQZSS:
474 T_QZSS_SATELLITE_ID(co->Sat[i].ID)
475 break;
476 case CLOCKORBIT_SATGLONASS:
477 T_GLONASS_SATELLITE_ID(co->Sat[i].ID)
478 break;
[5664]479 }
480 T_SSR_URA(ValueToURA(co->Sat[i].UserRangeAccuracy))
481 }
482 ENDBLOCK
[956]483 }
484 }
[5664]485 return ressize;
486}
487
488size_t MakeCodeBias(const struct CodeBias *b, enum CodeBiasType type,
[8963]489 int moremessagesfollow, char *buffer, size_t size) {
[5664]490 unsigned int s, i, j;
491
492 STARTDATA
493
[8963]494 for (s = 0; s < CLOCKORBIT_SATNUM; ++s) {
[8967]495 if (b->NumberOfSat[s] && (type == CBTYPE_AUTO || type == corbase[s] + COBOFS_CBIAS)) {
[5664]496 INITBLOCK
[8970]497 T_RTCM_MESSAGE_NUMBER(corbase[s] + COBOFS_CBIAS)
[8963]498 switch (s) {
499 case CLOCKORBIT_SATGPS:
500 case CLOCKORBIT_SATGALILEO:
501 case CLOCKORBIT_SATQZSS:
502 case CLOCKORBIT_SATSBAS:
503 case CLOCKORBIT_SATBDS:
504 T_GPS_EPOCH_TIME(b->EpochTime[s])
505 break;
506 case CLOCKORBIT_SATGLONASS:
507 T_GLONASS_EPOCH_TIME(b->EpochTime[s])
508 break;
[5664]509 }
510 T_SSR_UPDATE_INTERVAL(b->UpdateInterval)
511 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
512 T_SSR_IOD(b->SSRIOD)
513 T_SSR_PROVIDER_ID(b->SSRProviderID)
514 T_SSR_SOLUTION_ID(b->SSRSolutionID)
515 T_NO_OF_SATELLITES(b->NumberOfSat[s])
[8963]516 for (i = satoffset[s]; i < satoffset[s] + b->NumberOfSat[s]; ++i) {
517 switch (s) {
518 case CLOCKORBIT_SATGPS:
519 case CLOCKORBIT_SATGALILEO:
520 case CLOCKORBIT_SATSBAS:
521 case CLOCKORBIT_SATBDS:
522 T_GPS_SATELLITE_ID(b->Sat[i].ID)
523 break;
524 case CLOCKORBIT_SATQZSS:
525 T_QZSS_SATELLITE_ID(b->Sat[i].ID)
526 break;
527 case CLOCKORBIT_SATGLONASS:
528 T_GLONASS_SATELLITE_ID(b->Sat[i].ID)
529 break;
[5664]530 }
531 T_NO_OF_CODE_BIASES(b->Sat[i].NumberOfCodeBiases)
[8963]532 for (j = 0; j < b->Sat[i].NumberOfCodeBiases; ++j) {
[5664]533 T_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type)
534 T_CODE_BIAS(b->Sat[i].Biases[j].Bias)
535 }
536 }
537 ENDBLOCK
[956]538 }
539 }
[5664]540 return ressize;
541}
542
543size_t MakePhaseBias(const struct PhaseBias *b, enum PhaseBiasType type,
[8963]544 int moremessagesfollow, char *buffer, size_t size) {
[5664]545 unsigned int s, i, j;
546
547 STARTDATA
548
[8963]549 for (s = 0; s < CLOCKORBIT_SATNUM; ++s) {
550 if (b->NumberOfSat[s] && (type == PBTYPE_AUTO || type == s + PBTYPE_BASE)) {
[5664]551 INITBLOCK
[8970]552 T_RTCM_MESSAGE_NUMBER(s + PBTYPE_BASE)
[8963]553 switch (s) {
554 case CLOCKORBIT_SATGPS:
555 case CLOCKORBIT_SATGALILEO:
556 case CLOCKORBIT_SATQZSS:
557 case CLOCKORBIT_SATSBAS:
558 case CLOCKORBIT_SATBDS:
559 T_GPS_EPOCH_TIME(b->EpochTime[s])
560 break;
561 case CLOCKORBIT_SATGLONASS:
562 T_GLONASS_EPOCH_TIME(b->EpochTime[s])
563 break;
[5664]564 }
565 T_SSR_UPDATE_INTERVAL(b->UpdateInterval)
566 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
567 T_SSR_IOD(b->SSRIOD)
568 T_SSR_PROVIDER_ID(b->SSRProviderID)
569 T_SSR_SOLUTION_ID(b->SSRSolutionID)
570 T_DISPERSIVE_BIAS_INDICATOR(b->DispersiveBiasConsistencyIndicator ? 1 : 0)
571 T_MW_CONSISTENCY_INDICATOR(b->MWConsistencyIndicator ? 1 : 0)
[7777]572 T_NO_OF_SATELLITES(b->NumberOfSat[s])
[8963]573 for (i = satoffset[s]; i < satoffset[s] + b->NumberOfSat[s]; ++i) {
574 switch (s) {
575 case CLOCKORBIT_SATGPS:
576 case CLOCKORBIT_SATGALILEO:
577 case CLOCKORBIT_SATSBAS:
578 case CLOCKORBIT_SATBDS:
579 T_GPS_SATELLITE_ID(b->Sat[i].ID)
580 break;
581 case CLOCKORBIT_SATQZSS:
582 T_QZSS_SATELLITE_ID(b->Sat[i].ID)
583 break;
584 case CLOCKORBIT_SATGLONASS:
585 T_GLONASS_SATELLITE_ID(b->Sat[i].ID)
586 break;
[5664]587 }
[7777]588 T_NO_OF_PHASE_BIASES(b->Sat[i].NumberOfPhaseBiases)
[5664]589 T_YAW_ANGLE(b->Sat[i].YawAngle)
590 T_YAW_RATE(b->Sat[i].YawRate)
[8963]591 for (j = 0; j < b->Sat[i].NumberOfPhaseBiases; ++j) {
[5664]592 T_SIGNAL_IDENTIFIER(b->Sat[i].Biases[j].Type)
[8963]593 T_INTEGER_INDICATOR(
594 b->Sat[i].Biases[j].SignalIntegerIndicator ? 1 : 0)
595 T_WIDE_LANE_INDICATOR(
596 b->Sat[i].Biases[j].SignalsWideLaneIntegerIndicator)
597 T_DISCONTINUITY_COUNTER(
598 b->Sat[i].Biases[j].SignalDiscontinuityCounter)
[5664]599 T_PHASE_BIAS(b->Sat[i].Biases[j].Bias)
600 }
601 }
602 ENDBLOCK
[956]603 }
604 }
605 return ressize;
606}
607
[8963]608size_t MakeVTEC(const struct VTEC *v, int moremessagesfollow, char *buffer, size_t size) {
[5664]609 unsigned int l, o, d;
[1819]610
[956]611 STARTDATA
[8963]612 INITBLOCK
[8970]613 T_RTCM_MESSAGE_NUMBER(VTEC_BASE)
[956]614
[5664]615 T_GPS_EPOCH_TIME(v->EpochTime)
616 T_SSR_UPDATE_INTERVAL(v->UpdateInterval)
617 T_MULTIPLE_MESSAGE_INDICATOR(moremessagesfollow ? 1 : 0)
618 T_SSR_IOD(v->SSRIOD)
619 T_SSR_PROVIDER_ID(v->SSRProviderID)
620 T_SSR_SOLUTION_ID(v->SSRSolutionID)
621 T_VTEC_QUALITY_INDICATOR(v->Quality)
622 T_NO_IONO_LAYERS(v->NumLayers)
[8963]623 for (l = 0; l < v->NumLayers; ++l) {
[5664]624 T_IONO_HEIGHT(v->Layers[l].Height)
625 T_IONO_DEGREE(v->Layers[l].Degree)
626 T_IONO_ORDER(v->Layers[l].Order)
[8963]627 for (o = 0; o <= v->Layers[l].Order; ++o) {
628 for (d = o; d <= v->Layers[l].Degree; ++d) {
[5664]629 T_IONO_COEFF(v->Layers[l].Cosinus[d][o])
[956]630 }
631 }
[8963]632 for (o = 1; o <= v->Layers[l].Order; ++o) {
633 for (d = o; d <= v->Layers[l].Degree; ++d) {
[5664]634 T_IONO_COEFF(v->Layers[l].Sinus[d][o])
[956]635 }
636 }
637 }
[5664]638 ENDBLOCK
[956]639 return ressize;
640}
641#endif /* NOENCODE */
642
643#ifndef NODECODE
644
645#define DECODESTART \
646 int numbits=0; \
647 uint64_t bitbuffer=0;
648
[8963]649#define LOADBITS(a) { \
[956]650 while((a) > numbits) \
651 { \
[1808]652 if(!size--) return GCOBR_SHORTMESSAGE; \
[956]653 bitbuffer = (bitbuffer<<8)|((unsigned char)*(buffer++)); \
654 numbits += 8; \
655 } \
656}
657
658/* extract bits from data stream
[8963]659 b = variable to store result, a = number of bits */
660#define GETBITS(b, a) { \
[956]661 LOADBITS(a) \
662 b = (bitbuffer<<(64-numbits))>>(64-(a)); \
663 numbits -= (a); \
664}
665
[5664]666/* extract bits from data stream
[8963]667 b = variable to store result, a = number of bits */
668#define GETBITSFACTOR(b, a, c) { \
[5664]669 LOADBITS(a) \
670 b = ((bitbuffer<<(64-numbits))>>(64-(a)))*(c); \
671 numbits -= (a); \
672}
673
[956]674/* extract signed floating value from data stream
[8963]675 b = variable to store result, a = number of bits */
676#define GETFLOATSIGN(b, a, c) { \
[956]677 LOADBITS(a) \
678 b = ((double)(((int64_t)(bitbuffer<<(64-numbits)))>>(64-(a))))*(c); \
679 numbits -= (a); \
680}
681
[5664]682/* extract floating value from data stream
[8963]683 b = variable to store result, a = number of bits, c = scale factor */
684#define GETFLOAT(b, a, c) { \
[5664]685 LOADBITS(a) \
686 b = ((double)((bitbuffer<<(sizeof(bitbuffer)*8-numbits))>>(sizeof(bitbuffer)*8-(a))))*(c); \
687 numbits -= (a); \
688}
689
[956]690#define SKIPBITS(b) { LOADBITS(b) numbits -= (b); }
691
[5664]692/* GPS macros also used for other systems when matching! */
[956]693#define G_HEADER(a) GETBITS(a,8)
694#define G_RESERVEDH(a) GETBITS(a,6)
695#define G_SIZE(a) GETBITS(a, 10)
[5664]696#define G_MESSAGE_NUMBER(a) GETBITS(a, 12) /* DF002 */
697#define G_GPS_SATELLITE_ID(a) GETBITS(a, 6) /* DF068 */
698#define G_QZSS_SATELLITE_ID(a) GETBITS(a, 4) /* DF249 */
699#define G_GLONASS_SATELLITE_ID(a) GETBITS(a, 5)
[956]700
[5664]701#define G_GPS_IODE(a) GETBITS(a, 8) /* DF071 */
702#define G_GLONASS_IOD(a) GETBITS(a, 8) /* DF237 */
703#define G_GALILEO_IOD(a) GETBITS(a, 10) /* DF459 */
704#define G_SBAS_T0MOD(a) GETBITSFACTOR(a, 9, 16) /* DF468 */
705#define G_SBAS_IODCRC(a) GETBITS(a, 24) /* DF469 */
706#define G_BDS_TOEMOD(a) GETBITSFACTOR(a, 10, 8) /* DF470 */
[8410]707#define G_BDS_IOD(a) GETBITS(a, 8) /* DF471 */
[5664]708
[956]709/* defined values */
[1581]710#define G_DELTA_RADIAL(a) GETFLOATSIGN(a, 22, 1/10000.0)
[1808]711#define G_DELTA_ALONG_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0)
712#define G_DELTA_CROSS_TRACK(a) GETFLOATSIGN(a, 20, 1/2500.0)
[1581]713#define G_DELTA_DOT_RADIAL(a) GETFLOATSIGN(a, 21, 1/1000000.0)
[1808]714#define G_DELTA_DOT_ALONG_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0)
715#define G_DELTA_DOT_CROSS_TRACK(a) GETFLOATSIGN(a, 19, 1/250000.0)
[1581]716
717#define G_SATELLITE_REFERENCE_DATUM(a) GETBITS(a, 1)
718#define G_DELTA_CLOCK_C0(a) GETFLOATSIGN(a, 22, 1/10000.0)
719#define G_DELTA_CLOCK_C1(a) GETFLOATSIGN(a, 21, 1/1000000.0)
720#define G_DELTA_CLOCK_C2(a) GETFLOATSIGN(a, 27, 1/50000000.0)
721#define G_NO_OF_CODE_BIASES(a) GETBITS(a, 5)
[5664]722#define G_NO_OF_PHASE_BIASES(a) GETBITS(a, 5)
723#define G_SIGNAL_IDENTIFIER(a) GETBITS(a, 5)
[1581]724#define G_CODE_BIAS(a) GETFLOATSIGN(a, 14, 1/100.0)
[5664]725#define G_YAW_ANGLE(a) GETFLOAT(a, 9, MPI/256.0)
726#define G_YAW_RATE(a) GETFLOATSIGN(a, 8, MPI/8192.0)
727#define G_PHASE_BIAS(a) GETFLOATSIGN(a, 20, 1/10000.0)
[1581]728
[5664]729#define G_GPS_EPOCH_TIME(a, b) {unsigned int temp; GETBITS(temp, 20) \
[956]730 if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
[5664]731#define G_GLONASS_EPOCH_TIME(a, b) {unsigned int temp; GETBITS(temp, 17) \
[956]732 if(b && a != temp) return GCOBR_TIMEMISMATCH; a = temp;}
[5664]733#define G_EPOCH_TIME(a) GETBITS(a, 20)
[1840]734#define G_NO_OF_SATELLITES(a) GETBITS(a, 6)
[1581]735#define G_MULTIPLE_MESSAGE_INDICATOR(a) GETBITS(a, 1)
[5664]736#define G_DISPERSIVE_BIAS_INDICATOR(a) GETBITS(a, 1)
737#define G_MW_CONSISTENCY_INDICATOR(a) GETBITS(a, 1)
738#define G_INTEGER_INDICATOR(a) GETBITS(a, 1)
[6205]739#define G_WIDE_LANE_INDICATOR(a) GETBITS(a, 2)
[5664]740#define G_DISCONTINUITY_COUNTER(a) GETBITS(a, 4)
[2421]741#define G_SSR_URA(a) {int temp; GETBITS(temp, 6) \
742 (a) = URAToValue(temp);}
[1581]743#define G_HR_CLOCK_CORRECTION(a) GETFLOATSIGN(a, 22, 1/10000.0)
744#define G_SSR_UPDATE_INTERVAL(a) GETBITS(a, 4)
745
[3511]746#define G_SSR_IOD(a) GETBITS(a, 4)
747#define G_SSR_PROVIDER_ID(a) GETBITS(a, 16)
748#define G_SSR_SOLUTION_ID(a) GETBITS(a, 4)
749
[5664]750#define G_NO_IONO_LAYERS(a) {unsigned int temp; GETBITS(temp, 2) a = temp+1; }
751#define G_VTEC_QUALITY_INDICATOR(a) GETFLOAT(a, 9, 1/20.0)
752#define G_IONO_COEFF(a) GETFLOATSIGN(a, 16,1/200.0)
753#define G_IONO_DEGREE(a) {unsigned int temp; GETBITS(temp, 4) a = temp+1; }
754#define G_IONO_ORDER(a) {unsigned int temp; GETBITS(temp, 4) a = temp+1; }
755#define G_IONO_HEIGHT(a) GETFLOAT(a, 8 , 10000.0)
756
[8963]757enum GCOB_RETURN GetSSR(struct ClockOrbit *co, struct CodeBias *b,struct VTEC *v,
758 struct PhaseBias *pb, const char *buffer, size_t size, int *bytesused) {
759 int mmi = 0, h, rs;
[5664]760 unsigned int type, pos, i, j, s, nums, id;
[1819]761 size_t sizeofrtcmblock;
[956]762 const char *blockstart = buffer;
763 DECODESTART
764
[8963]765 if (size < 7)
[956]766 return GCOBR_SHORTBUFFER;
767
[1842]768#ifdef DEBUG
[5664]769 fprintf(stderr, "GetClockOrbitBias START: size %d, numbits %d\n",size, numbits);
[1842]770#endif
771
[956]772 G_HEADER(h)
773 G_RESERVEDH(rs)
774 G_SIZE(sizeofrtcmblock);
775
[8963]776 if ((unsigned char) h != 0xD3 || rs)
[956]777 return GCOBR_UNKNOWNDATA;
[8963]778 if (size < sizeofrtcmblock + 3) /* 3 header bytes already removed */
[956]779 return GCOBR_MESSAGEEXCEEDSBUFFER;
[8963]780 if (CRC24(sizeofrtcmblock + 3, (const unsigned char *) blockstart) !=
781 (uint32_t) ((((unsigned char) buffer[sizeofrtcmblock]) << 16) |
782 (((unsigned char) buffer[sizeofrtcmblock + 1]) << 8) |
783 (((unsigned char) buffer[sizeofrtcmblock + 2]))))
[956]784 return GCOBR_CRCMISMATCH;
[1808]785 size = sizeofrtcmblock; /* reduce size, so overflows are detected */
[956]786
787 G_MESSAGE_NUMBER(type)
[1808]788#ifdef DEBUG
[8963]789 fprintf(stderr, "type %d size %d\n",type,sizeofrtcmblock);
[1808]790#endif
[8963]791 if (bytesused)
792 *bytesused = sizeofrtcmblock + 6;
793 if (type == VTEC_BASE) {
[5664]794 unsigned int l, o, d;
[8963]795 if (!v)
796 return GCOBR_NOVTECPARAMETER;
[5664]797 memset(v, 0, sizeof(*v));
798 G_EPOCH_TIME(v->EpochTime)
799 G_SSR_UPDATE_INTERVAL(v->UpdateInterval)
[956]800 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
[5664]801 G_SSR_IOD(v->SSRIOD)
802 G_SSR_PROVIDER_ID(v->SSRProviderID)
803 G_SSR_SOLUTION_ID(v->SSRSolutionID)
804 G_VTEC_QUALITY_INDICATOR(v->Quality)
805 G_NO_IONO_LAYERS(v->NumLayers)
[8963]806 for (l = 0; l < v->NumLayers; ++l) {
[5664]807 G_IONO_HEIGHT(v->Layers[l].Height)
808 G_IONO_DEGREE(v->Layers[l].Degree)
809 G_IONO_ORDER(v->Layers[l].Order)
[8963]810 for (o = 0; o <= v->Layers[l].Order; ++o) {
811 for (d = o; d <= v->Layers[l].Degree; ++d) {
[5664]812 G_IONO_COEFF(v->Layers[l].Cosinus[d][o])
813 }
814 }
[8963]815 for (o = 1; o <= v->Layers[l].Order; ++o) {
816 for (d = o; d <= v->Layers[l].Degree; ++d) {
[5664]817 G_IONO_COEFF(v->Layers[l].Sinus[d][o])
818 }
819 }
[956]820 }
[1842]821#ifdef DEBUG
[5664]822 for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type)
[8963]823 numbits += 8;
[5664]824 fprintf(stderr, "numbits left %d\n",numbits);
[1842]825#endif
[5664]826 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
827 }
[8963]828 for (s = CLOCKORBIT_SATNUM; s-- > 0;) {
829 if (type == PBTYPE_BASE + s) {
830 if (!pb)
831 return GCOBR_NOPHASEBIASPARAMETER;
[5664]832 pb->messageType = type;
[8963]833 switch (s) {
834 case CLOCKORBIT_SATGPS:
835 case CLOCKORBIT_SATGALILEO:
836 case CLOCKORBIT_SATQZSS:
837 case CLOCKORBIT_SATSBAS:
838 case CLOCKORBIT_SATBDS:
839 G_GPS_EPOCH_TIME(pb->EpochTime[s], pb->NumberOfSat[s])
840 break;
841 case CLOCKORBIT_SATGLONASS:
842 G_GLONASS_EPOCH_TIME(pb->EpochTime[s], pb->NumberOfSat[s])
843 break;
[5664]844 }
845 G_SSR_UPDATE_INTERVAL(pb->UpdateInterval)
846 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
847 G_SSR_IOD(pb->SSRIOD)
848 G_SSR_PROVIDER_ID(pb->SSRProviderID)
849 G_SSR_SOLUTION_ID(pb->SSRSolutionID)
850 G_DISPERSIVE_BIAS_INDICATOR(pb->DispersiveBiasConsistencyIndicator)
851 G_MW_CONSISTENCY_INDICATOR(pb->MWConsistencyIndicator)
852 G_NO_OF_SATELLITES(nums)
[8963]853 for (i = 0; i < nums; ++i) {
854 switch (s) {
855 case CLOCKORBIT_SATGPS:
856 case CLOCKORBIT_SATGALILEO:
857 case CLOCKORBIT_SATSBAS:
858 case CLOCKORBIT_SATBDS:
859 G_GPS_SATELLITE_ID(id)
860 break;
861 case CLOCKORBIT_SATQZSS:
862 G_QZSS_SATELLITE_ID(id)
863 break;
864 case CLOCKORBIT_SATGLONASS:
865 G_GLONASS_SATELLITE_ID(id)
866 break;
[5664]867 }
[8963]868 for (pos = satoffset[s];
869 pos < satoffset[s] + pb->NumberOfSat[s] && pb->Sat[pos].ID != id;
870 ++pos)
[5664]871 ;
[8963]872 if (pos >= satoffset[s + 1])
873 return GCOBR_DATAMISMATCH;
874 else if (pos == pb->NumberOfSat[s] + satoffset[s])
875 ++pb->NumberOfSat[s];
[5664]876 pb->Sat[pos].ID = id;
[1840]877
[5664]878 G_NO_OF_PHASE_BIASES(pb->Sat[pos].NumberOfPhaseBiases)
879 G_YAW_ANGLE(pb->Sat[pos].YawAngle)
880 G_YAW_RATE(pb->Sat[pos].YawRate)
[8963]881 for (j = 0; j < pb->Sat[pos].NumberOfPhaseBiases; ++j) {
[6204]882 G_SIGNAL_IDENTIFIER(pb->Sat[pos].Biases[j].Type)
[5664]883 G_INTEGER_INDICATOR(pb->Sat[pos].Biases[j].SignalIntegerIndicator)
[8963]884 G_WIDE_LANE_INDICATOR(
885 pb->Sat[pos].Biases[j].SignalsWideLaneIntegerIndicator)
886 G_DISCONTINUITY_COUNTER(
887 pb->Sat[pos].Biases[j].SignalDiscontinuityCounter)
[5664]888 G_PHASE_BIAS(pb->Sat[pos].Biases[j].Bias)
889 }
890 }
[6207]891#ifdef DEBUG
892 for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type)
[8963]893 numbits += 8;
[6207]894 fprintf(stderr, "numbits left %d\n",numbits);
895#endif
896 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
[956]897 }
[8963]898 else if (type >= corbase[s]) {
899 switch (type - corbase[s]) {
900 case COBOFS_ORBIT:
901 if (!co)
902 return GCOBR_NOCLOCKORBITPARAMETER;
903 co->messageType = type;
904 switch (s) {
905 case CLOCKORBIT_SATGPS:
906 case CLOCKORBIT_SATGALILEO:
907 case CLOCKORBIT_SATQZSS:
908 case CLOCKORBIT_SATSBAS:
909 case CLOCKORBIT_SATBDS:
910 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
911 break;
912 case CLOCKORBIT_SATGLONASS:
913 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
914 break;
915 }
916 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
917 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
918 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
919 G_SSR_IOD(co->SSRIOD)
920 G_SSR_PROVIDER_ID(co->SSRProviderID)
921 G_SSR_SOLUTION_ID(co->SSRSolutionID)
922 G_NO_OF_SATELLITES(nums)
923 co->Supplied[COBOFS_ORBIT] |= 1;
[1842]924#ifdef DEBUG
[8963]925 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d rd %d\n",co->EpochTime[s],
926 co->UpdateInterval,mmi,co->NumberOfSat[s],nums, co->SatRefDatum);
[1842]927#endif
[8963]928 for (i = 0; i < nums; ++i) {
929 switch (s) {
930 case CLOCKORBIT_SATGPS:
931 case CLOCKORBIT_SATGALILEO:
932 case CLOCKORBIT_SATSBAS:
933 case CLOCKORBIT_SATBDS:
934 G_GPS_SATELLITE_ID(id)
935 break;
936 case CLOCKORBIT_SATQZSS:
937 G_QZSS_SATELLITE_ID(id)
938 break;
939 case CLOCKORBIT_SATGLONASS:
940 G_GLONASS_SATELLITE_ID(id)
941 break;
942 }
943 for (pos = satoffset[s];
944 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
945 ++pos)
946 ;
947 if (pos >= satoffset[s + 1])
948 return GCOBR_DATAMISMATCH;
949 else if (pos == co->NumberOfSat[s] + satoffset[s])
950 ++co->NumberOfSat[s];
951 co->Sat[pos].ID = id;
[1840]952
[8963]953 switch (s) {
954 case CLOCKORBIT_SATGPS:
955 case CLOCKORBIT_SATQZSS:
956 G_GPS_IODE(co->Sat[pos].IOD)
957 break;
958 case CLOCKORBIT_SATGLONASS:
959 G_GLONASS_IOD(co->Sat[pos].IOD)
960 break;
961 case CLOCKORBIT_SATGALILEO:
962 G_GALILEO_IOD(co->Sat[pos].IOD)
963 break;
964 case CLOCKORBIT_SATSBAS:
965 G_SBAS_T0MOD(co->Sat[pos].toe)
966 G_SBAS_IODCRC(co->Sat[pos].IOD)
967 break;
968 case CLOCKORBIT_SATBDS:
969 G_BDS_TOEMOD(co->Sat[pos].toe)
970 G_BDS_IOD(co->Sat[pos].IOD)
971 break;
972 }
973 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
974 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
975 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
976 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
977 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
978 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
[1842]979#ifdef DEBUG
[8963]980 fprintf(stderr, "id %2d iod %3d dr %8.3f da %8.3f dc %8.3f dr %8.3f da %8.3f dc %8.3f\n",
981 co->Sat[pos].ID,co->Sat[pos].IOD,co->Sat[pos].Orbit.DeltaRadial,
982 co->Sat[pos].Orbit.DeltaAlongTrack,co->Sat[pos].Orbit.DeltaCrossTrack,
983 co->Sat[pos].Orbit.DotDeltaRadial,
984 co->Sat[pos].Orbit.DotDeltaAlongTrack,
985 co->Sat[pos].Orbit.DotDeltaCrossTrack);
[1842]986#endif
[8963]987 }
[5664]988 break;
[8963]989 case COBOFS_CLOCK:
990 if (!co)
991 return GCOBR_NOCLOCKORBITPARAMETER;
992 co->messageType = type;
993 switch (s) {
994 case CLOCKORBIT_SATGPS:
995 case CLOCKORBIT_SATGALILEO:
996 case CLOCKORBIT_SATQZSS:
997 case CLOCKORBIT_SATSBAS:
998 case CLOCKORBIT_SATBDS:
999 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1000 break;
1001 case CLOCKORBIT_SATGLONASS:
1002 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1003 break;
1004 }
1005 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1006 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1007 G_SSR_IOD(co->SSRIOD)
1008 G_SSR_PROVIDER_ID(co->SSRProviderID)
1009 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1010 G_NO_OF_SATELLITES(nums)
1011 co->Supplied[COBOFS_CLOCK] |= 1;
[1842]1012#ifdef DEBUG
[8963]1013 fprintf(stderr, "epochtime %d ui %d mmi %d sats %d/%d\n",co->EpochTime[s],
1014 co->UpdateInterval,mmi,co->NumberOfSat[s],nums);
[1842]1015#endif
[8963]1016 for (i = 0; i < nums; ++i) {
1017 switch (s) {
1018 case CLOCKORBIT_SATGPS:
1019 case CLOCKORBIT_SATGALILEO:
1020 case CLOCKORBIT_SATSBAS:
1021 case CLOCKORBIT_SATBDS:
1022 G_GPS_SATELLITE_ID(id)
1023 break;
1024 case CLOCKORBIT_SATQZSS:
1025 G_QZSS_SATELLITE_ID(id)
1026 break;
1027 case CLOCKORBIT_SATGLONASS:
1028 G_GLONASS_SATELLITE_ID(id)
1029 break;
1030 }
1031 for (pos = satoffset[s];
1032 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
1033 ++pos)
1034 ;
1035 if (pos >= satoffset[s + 1])
1036 return GCOBR_DATAMISMATCH;
1037 else if (pos == co->NumberOfSat[s] + satoffset[s])
1038 ++co->NumberOfSat[s];
1039 co->Sat[pos].ID = id;
[1840]1040
[8963]1041 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
1042 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
1043 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
[1842]1044#ifdef DEBUG
[8963]1045 fprintf(stderr, "id %2d c0 %8.3f c1 %8.3f c2 %8.3f\n",
1046 co->Sat[pos].ID, co->Sat[pos].Clock.DeltaA0, co->Sat[pos].Clock.DeltaA1,
1047 co->Sat[pos].Clock.DeltaA2);
[1842]1048#endif
[8963]1049 }
[5664]1050 break;
[8963]1051 case COBOFS_COMBINED:
1052 if (!co)
1053 return GCOBR_NOCLOCKORBITPARAMETER;
1054 co->messageType = type;
1055 switch (s) {
1056 case CLOCKORBIT_SATGPS:
1057 case CLOCKORBIT_SATGALILEO:
1058 case CLOCKORBIT_SATQZSS:
1059 case CLOCKORBIT_SATSBAS:
1060 case CLOCKORBIT_SATBDS:
1061 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1062 break;
1063 case CLOCKORBIT_SATGLONASS:
1064 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1065 break;
[5664]1066 }
[8963]1067 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1068 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1069 G_SATELLITE_REFERENCE_DATUM(co->SatRefDatum)
1070 G_SSR_IOD(co->SSRIOD)
1071 G_SSR_PROVIDER_ID(co->SSRProviderID)
1072 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1073 G_NO_OF_SATELLITES(nums)
1074 co->Supplied[COBOFS_ORBIT] |= 1;
1075 co->Supplied[COBOFS_CLOCK] |= 1;
1076 for (i = 0; i < nums; ++i) {
1077 switch (s) {
1078 case CLOCKORBIT_SATGPS:
1079 case CLOCKORBIT_SATGALILEO:
1080 case CLOCKORBIT_SATSBAS:
1081 case CLOCKORBIT_SATBDS:
1082 G_GPS_SATELLITE_ID(id)
1083 break;
1084 case CLOCKORBIT_SATQZSS:
1085 G_QZSS_SATELLITE_ID(id)
1086 break;
1087 case CLOCKORBIT_SATGLONASS:
1088 G_GLONASS_SATELLITE_ID(id)
1089 break;
1090 }
1091 for (pos = satoffset[s];
1092 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
1093 ++pos)
1094 ;
1095 if (pos >= satoffset[s + 1])
1096 return GCOBR_DATAMISMATCH;
1097 else if (pos == co->NumberOfSat[s] + satoffset[s])
1098 ++co->NumberOfSat[s];
1099 co->Sat[pos].ID = id;
[1840]1100
[8963]1101 switch (s) {
1102 case CLOCKORBIT_SATGPS:
1103 case CLOCKORBIT_SATQZSS:
1104 G_GPS_IODE(co->Sat[pos].IOD)
1105 break;
1106 case CLOCKORBIT_SATGLONASS:
1107 G_GLONASS_IOD(co->Sat[pos].IOD)
1108 break;
1109 case CLOCKORBIT_SATGALILEO:
1110 G_GALILEO_IOD(co->Sat[pos].IOD)
1111 break;
1112 case CLOCKORBIT_SATSBAS:
1113 G_SBAS_T0MOD(co->Sat[pos].toe)
1114 G_SBAS_IODCRC(co->Sat[pos].IOD)
1115 break;
1116 case CLOCKORBIT_SATBDS:
1117 G_BDS_TOEMOD(co->Sat[pos].toe)
1118 G_BDS_IOD(co->Sat[pos].IOD)
1119 break;
1120 }
1121 G_DELTA_RADIAL(co->Sat[pos].Orbit.DeltaRadial)
1122 G_DELTA_ALONG_TRACK(co->Sat[pos].Orbit.DeltaAlongTrack)
1123 G_DELTA_CROSS_TRACK(co->Sat[pos].Orbit.DeltaCrossTrack)
1124 G_DELTA_DOT_RADIAL(co->Sat[pos].Orbit.DotDeltaRadial)
1125 G_DELTA_DOT_ALONG_TRACK(co->Sat[pos].Orbit.DotDeltaAlongTrack)
1126 G_DELTA_DOT_CROSS_TRACK(co->Sat[pos].Orbit.DotDeltaCrossTrack)
1127 G_DELTA_CLOCK_C0(co->Sat[pos].Clock.DeltaA0)
1128 G_DELTA_CLOCK_C1(co->Sat[pos].Clock.DeltaA1)
1129 G_DELTA_CLOCK_C2(co->Sat[pos].Clock.DeltaA2)
[5664]1130 }
1131 break;
[8963]1132 case COBOFS_URA:
1133 if (!co)
1134 return GCOBR_NOCLOCKORBITPARAMETER;
1135 co->messageType = type;
1136 switch (s) {
1137 case CLOCKORBIT_SATGPS:
1138 case CLOCKORBIT_SATGALILEO:
1139 case CLOCKORBIT_SATQZSS:
1140 case CLOCKORBIT_SATSBAS:
1141 case CLOCKORBIT_SATBDS:
1142 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1143 break;
1144 case CLOCKORBIT_SATGLONASS:
1145 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1146 break;
[5664]1147 }
[8963]1148 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1149 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1150 G_SSR_IOD(co->SSRIOD)
1151 G_SSR_PROVIDER_ID(co->SSRProviderID)
1152 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1153 G_NO_OF_SATELLITES(nums)
1154 co->Supplied[COBOFS_URA] |= 1;
1155 for (i = 0; i < nums; ++i) {
1156 switch (s) {
1157 case CLOCKORBIT_SATGPS:
1158 case CLOCKORBIT_SATGALILEO:
1159 case CLOCKORBIT_SATSBAS:
1160 case CLOCKORBIT_SATBDS:
1161 G_GPS_SATELLITE_ID(id)
1162 break;
1163 case CLOCKORBIT_SATQZSS:
1164 G_QZSS_SATELLITE_ID(id)
1165 break;
1166 case CLOCKORBIT_SATGLONASS:
1167 G_GLONASS_SATELLITE_ID(id)
1168 break;
1169 }
1170 for (pos = satoffset[s];
1171 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
1172 ++pos)
1173 ;
1174 if (pos >= satoffset[s + 1])
1175 return GCOBR_DATAMISMATCH;
1176 else if (pos == co->NumberOfSat[s] + satoffset[s])
1177 ++co->NumberOfSat[s];
1178 co->Sat[pos].ID = id;
[1840]1179
[8963]1180 G_SSR_URA(co->Sat[pos].UserRangeAccuracy)
1181 }
[5664]1182 break;
[8963]1183 case COBOFS_HR:
1184 if (!co)
1185 return GCOBR_NOCLOCKORBITPARAMETER;
1186 co->messageType = type;
1187 switch (s) {
1188 case CLOCKORBIT_SATGPS:
1189 case CLOCKORBIT_SATGALILEO:
1190 case CLOCKORBIT_SATQZSS:
1191 case CLOCKORBIT_SATSBAS:
1192 case CLOCKORBIT_SATBDS:
1193 G_GPS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1194 break;
1195 case CLOCKORBIT_SATGLONASS:
1196 G_GLONASS_EPOCH_TIME(co->EpochTime[s], co->NumberOfSat[s])
1197 break;
[5664]1198 }
[8963]1199 G_SSR_UPDATE_INTERVAL(co->UpdateInterval)
1200 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1201 G_SSR_IOD(co->SSRIOD)
1202 G_SSR_PROVIDER_ID(co->SSRProviderID)
1203 G_SSR_SOLUTION_ID(co->SSRSolutionID)
1204 G_NO_OF_SATELLITES(nums)
1205 co->Supplied[COBOFS_HR] |= 1;
1206 for (i = 0; i < nums; ++i) {
1207 switch (s) {
1208 case CLOCKORBIT_SATGPS:
1209 case CLOCKORBIT_SATGALILEO:
1210 case CLOCKORBIT_SATSBAS:
1211 case CLOCKORBIT_SATBDS:
1212 G_GPS_SATELLITE_ID(id)
1213 break;
1214 case CLOCKORBIT_SATQZSS:
1215 G_QZSS_SATELLITE_ID(id)
1216 break;
1217 case CLOCKORBIT_SATGLONASS:
1218 G_GLONASS_SATELLITE_ID(id)
1219 break;
1220 }
1221 for (pos = satoffset[s];
1222 pos < satoffset[s] + co->NumberOfSat[s] && co->Sat[pos].ID != id;
1223 ++pos)
1224 ;
1225 if (pos >= satoffset[s + 1])
1226 return GCOBR_DATAMISMATCH;
1227 else if (pos == co->NumberOfSat[s] + satoffset[s])
1228 ++co->NumberOfSat[s];
1229 co->Sat[pos].ID = id;
1230 G_HR_CLOCK_CORRECTION(co->Sat[pos].hrclock)
1231 }
[5664]1232 break;
[8967]1233 case COBOFS_CBIAS:
[8963]1234 if (!b)
1235 return GCOBR_NOCODEBIASPARAMETER;
1236 b->messageType = type;
1237 switch (s) {
1238 case CLOCKORBIT_SATGPS:
1239 case CLOCKORBIT_SATGALILEO:
1240 case CLOCKORBIT_SATQZSS:
1241 case CLOCKORBIT_SATSBAS:
1242 case CLOCKORBIT_SATBDS:
1243 G_GPS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s])
1244 break;
1245 case CLOCKORBIT_SATGLONASS:
1246 G_GLONASS_EPOCH_TIME(b->EpochTime[s], b->NumberOfSat[s])
1247 break;
[5664]1248 }
[8963]1249 G_SSR_UPDATE_INTERVAL(b->UpdateInterval)
1250 G_MULTIPLE_MESSAGE_INDICATOR(mmi)
1251 G_SSR_IOD(b->SSRIOD)
1252 G_SSR_PROVIDER_ID(b->SSRProviderID)
1253 G_SSR_SOLUTION_ID(b->SSRSolutionID)
1254 G_NO_OF_SATELLITES(nums)
1255 for (i = 0; i < nums; ++i) {
1256 switch (s) {
1257 case CLOCKORBIT_SATGPS:
1258 case CLOCKORBIT_SATGALILEO:
1259 case CLOCKORBIT_SATSBAS:
1260 case CLOCKORBIT_SATBDS:
1261 G_GPS_SATELLITE_ID(id)
1262 break;
1263 case CLOCKORBIT_SATQZSS:
1264 G_QZSS_SATELLITE_ID(id)
1265 break;
1266 case CLOCKORBIT_SATGLONASS:
1267 G_GLONASS_SATELLITE_ID(id)
1268 break;
1269 }
1270 for (pos = satoffset[s];
1271 pos < satoffset[s] + b->NumberOfSat[s] && b->Sat[pos].ID != id;
1272 ++pos)
1273 ;
1274 if (pos >= satoffset[s + 1])
1275 return GCOBR_DATAMISMATCH;
1276 else if (pos == b->NumberOfSat[s] + satoffset[s])
1277 ++b->NumberOfSat[s];
1278 b->Sat[pos].ID = id;
[1840]1279
[8963]1280 G_NO_OF_CODE_BIASES(b->Sat[pos].NumberOfCodeBiases)
1281 for (j = 0; j < b->Sat[pos].NumberOfCodeBiases; ++j) {
1282 G_SIGNAL_IDENTIFIER(b->Sat[pos].Biases[j].Type)
1283 G_CODE_BIAS(b->Sat[pos].Biases[j].Bias)
1284 }
[5664]1285 }
[8963]1286 break;
1287 default:
1288 continue;
[956]1289 }
[5664]1290#ifdef DEBUG
1291 for(type = 0; type < (int)size && (unsigned char)buffer[type] != 0xD3; ++type)
[8963]1292 numbits += 8;
[5664]1293 fprintf(stderr, "numbits left %d\n",numbits);
1294#endif
[6204]1295 return mmi ? GCOBR_MESSAGEFOLLOWS : GCOBR_OK;
[956]1296 }
1297 }
[5664]1298 return GCOBR_UNKNOWNTYPE;
[956]1299}
1300#endif /* NODECODE */
Note: See TracBrowser for help on using the repository browser.