source: ntrip/trunk/clock_and_orbit/readco.c@ 9500

Last change on this file since 9500 was 6554, checked in by stoecker, 9 years ago

fix some compiler warnings
File size: 18.1 KB
RevLine 
[5664]1#include "clock_orbit_rtcm.h"
2
3#include <stdio.h>
4#include <string.h>
5#include <stdlib.h>
[6554]6#include <unistd.h>
[5664]7#include <math.h>
8
9struct ClockOrbit coIn;
10struct CodeBias cbIn;
11double lasttow_co = -1.0;
12double lasttow_cb = -1.0;
13
14typedef enum {GPS, GLO, GAL, QZSS, SBAS, BDS} SATSYS;
15int GPSUTCdiff = 16;
16char satSYS[6] = { 'G', 'R', 'E', 'J', 'S', 'C' };
17int messageTypeCo[6] = { 1060, 1066, 1243, 1249, 1255, 1261 };
18int messageTypeCb[6] = { 1059, 1065, 1242, 1248, 1254, 1260 };
19
20void printClockOrbit(const char* filename, struct ClockOrbit* clockOrb,
[6554]21 const char* flag, int ind, char satSys, int offsetGnss);
[5664]22void printClockOrbitDiff(const char* filename, struct ClockOrbit* clockOrb1,
[6554]23 struct ClockOrbit* clockOrb2, const char* flag, int ind, char satSys, int offsetGnss);
[5664]24void printCodeBias(const char* filename, struct CodeBias* codeBias,
[6554]25 const char* flag,int ind, char satSys, int offsetGnss);
[5664]26void printCodeBiasDiff(const char* filename, struct CodeBias* codeBias1,
[6554]27 struct CodeBias* codeBias2, const char* flag, int ind, char satSys,
[5669]28 int offsetGnss);
[5664]29
30int main(void) {
[6554]31 SATSYS sys;
32 const char * inputFile = "ssr1_cocb_data/CLK801330.14C";
[5669]33 for (sys = GPS; sys <= BDS; ++sys) {
[6554]34 const char *outFilenameRaw, *outFilenameDbg;
[5669]35 enum COR_SATSYSTEM CLOCKORBIT_SATGNSS = sys;
36 enum COR_OFFSETS CLOCKORBIT_OFFSETGNSS;
37 lasttow_co = -1.0;
38 lasttow_cb = -1.0;
39 switch (sys) {
40 case GPS:
41 outFilenameRaw = "ssr1_cocb_data/outfile_G.raw";
42 outFilenameDbg = "ssr1_cocb_data/outfile_G.dbg";
43 CLOCKORBIT_OFFSETGNSS = CLOCKORBIT_OFFSETGPS;
44 break;
45 case GLO:
46 outFilenameRaw = "ssr1_cocb_data/outfile_R.raw";
47 outFilenameDbg = "ssr1_cocb_data/outfile_R.dbg";
48 CLOCKORBIT_OFFSETGNSS = CLOCKORBIT_OFFSETGLONASS;
49 break;
50 case GAL:
51 outFilenameRaw = "ssr1_cocb_data/outfile_E.raw";
52 outFilenameDbg = "ssr1_cocb_data/outfile_E.dbg";
53 CLOCKORBIT_OFFSETGNSS = CLOCKORBIT_OFFSETGALILEO;
54 break;
55 case QZSS:
56 outFilenameRaw = "ssr1_cocb_data/outfile_J.raw";
57 outFilenameDbg = "ssr1_cocb_data/outfile_J.dbg";
58 CLOCKORBIT_OFFSETGNSS = CLOCKORBIT_OFFSETQZSS;
59 break;
60 case SBAS:
61 outFilenameRaw = "ssr1_cocb_data/outfile_S.raw";
62 outFilenameDbg = "ssr1_cocb_data/outfile_S.dbg";
63 CLOCKORBIT_OFFSETGNSS = CLOCKORBIT_OFFSETSBAS;
64 break;
65 case BDS:
66 outFilenameRaw = "ssr1_cocb_data/outfile_C.raw";
67 outFilenameDbg = "ssr1_cocb_data/outfile_C.dbg";
68 CLOCKORBIT_OFFSETGNSS = CLOCKORBIT_OFFSETBDS;
69 break;
70 }
71 unlink(outFilenameRaw);
72 unlink(outFilenameDbg);
73 FILE *asciiSsr, *f;
74 size_t len = 0;
75 char *buffer = 0, type = satSYS[sys];
76 asciiSsr = fopen(inputFile, "r");
77 if (asciiSsr == NULL) {
78 fprintf(stderr, "ERROR: open file %s\n", inputFile);
79 return 0;
80 }
81 while (getline(&buffer, &len, asciiSsr) > 0) { //fprintf(stderr, "line: %s", buffer);
82 char coBuffer[CLOCKORBIT_BUFFERSIZE];
83 char cbBuffer[CLOCKORBIT_BUFFERSIZE];
84 int MT = 0, messageType = 0, ui = 0, week = 0, prn = 0, iode = 0, ncb = 0;
85 double cbValue[3] = { 0.0 }, clock_a0 = 0.0, clock_a1 = 0.0, clock_a2 = 0.0, d_radial = 0.0;
86 double d_along = 0.0, d_outofplane = 0.0, dd_radial = 0.0, dd_along = 0.0, dd_outofplane = 0.0;
[6554]87 int cbType[3] = { 0 };
[5669]88 static double tow_co, tow_cb;
[6554]89 sscanf(buffer, "%d ", &messageType);
[5669]90 if (messageType == messageTypeCo[GPS]) {
91 sscanf(buffer,
92 "%d %d %d %lf %c%d %d %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
93 &messageType, &ui, &week, &tow_co, &type, &prn, &iode,
94 &clock_a0, &d_radial, &d_along, &d_outofplane,
95 &clock_a1, &dd_radial, &dd_along, &dd_outofplane,
96 &clock_a2);
97 type = satSYS[sys];
98 MT = messageTypeCo[sys];
99 switch (sys) {
100 case GPS:case GAL:case SBAS:case BDS:
101 break;
102 case GLO:
103 if (prn > 24)
104 continue;
105 break;
106 case QZSS:
107 if (prn > 10)
108 continue;
109 break;
110 }
111 if ((lasttow_co != tow_co)) { // create block
112 int nl = 0, ns = 0, l = 0;
113 struct ClockOrbit coOut;
114 if (lasttow_co >= 0) {
115 if (sys == GLO) {
116 coIn.EpochTime[CLOCKORBIT_SATGLONASS] =
117 (fmod((double)coIn.EpochTime[CLOCKORBIT_SATGLONASS]
118 + (3 * 3600 - GPSUTCdiff), 86400.0));
119 }
120 printClockOrbit(outFilenameDbg, &coIn, "INPUT",
121 CLOCKORBIT_SATGNSS, type, CLOCKORBIT_OFFSETGNSS);
122 l = MakeClockOrbit(&coIn, COTYPE_AUTO, 0, coBuffer,
123 sizeof(coBuffer));
124 if (!l)
125 fprintf(stderr, "BUILD ERROR\n");
126 else {
127 if ((f = fopen(outFilenameRaw, "ab+"))) {
128 fwrite(coBuffer, l, 1, f);
129 fclose(f);
130 } else
131 fprintf(stderr, "SAVE ERROR %s\n",
132 outFilenameRaw);
133 }
134 memset(&coOut, 0, sizeof(coOut));
135 nl = GetSSR(&coOut, 0, 0, 0, coBuffer, l, &ns);
136 if (nl < 0)
137 fprintf(stderr, "CLKORB EXTRACT ERROR %d\n", nl);
138 else if (nl > 0)
139 fprintf(stderr, "CLKORB MULTIBLOCK UNSUPPORTED IN TEST\n");
140 else if (ns != l)
141 fprintf(stderr, "CLKORB SIZE MISMATCH (%d/%d)\n", ns, l);
142 else {
143 printClockOrbit(outFilenameDbg, &coOut, "OUTPUT",
144 CLOCKORBIT_SATGNSS, type, CLOCKORBIT_OFFSETGNSS);
145 printClockOrbitDiff(outFilenameDbg, &coIn, &coOut,
146 "DIFF", CLOCKORBIT_SATGNSS, type, CLOCKORBIT_OFFSETGNSS);
147 }
148 }
149 memset(&coIn, 0, sizeof(coIn));
150 lasttow_co = tow_co;
151 coIn.messageType = MT;
152 coIn.EpochTime[CLOCKORBIT_SATGNSS] = (int) tow_co;
153 coIn.UpdateInterval = (int) ui;
154 coIn.Supplied[COBOFS_COMBINED] = 1;
155 coIn.SatRefDatum = DATUM_ITRF;
156 }
157 struct SatData *sd;
158 sd = coIn.Sat
159 + CLOCKORBIT_OFFSETGNSS
160 + coIn.NumberOfSat[CLOCKORBIT_SATGNSS];
161 sd->ID = prn;
162 sd->IOD = iode;
163 sd->Clock.DeltaA0 = clock_a0;
164 sd->Clock.DeltaA1 = clock_a1;
165 sd->Clock.DeltaA2 = clock_a2;
166 sd->Orbit.DeltaRadial = d_radial;
167 sd->Orbit.DeltaAlongTrack = d_along;
168 sd->Orbit.DeltaCrossTrack = d_outofplane;
169 sd->Orbit.DotDeltaRadial = dd_radial;
170 sd->Orbit.DotDeltaAlongTrack = dd_along;
171 sd->Orbit.DotDeltaCrossTrack = dd_outofplane;
172 ++coIn.NumberOfSat[CLOCKORBIT_SATGNSS];
173 }
174 if (messageType == messageTypeCb[GPS]) {
175 sscanf(buffer, "%d %d %d %lf %c%d %d %d %lf %d %lf %d %lf\n",
176 &messageType, &ui, &week, &tow_cb, &type, &prn, &ncb,
177 &cbType[0], &cbValue[0], &cbType[1], &cbValue[1],
178 &cbType[2], &cbValue[2]);
179 type = satSYS[sys];
180 MT = messageTypeCb[sys];
181 switch (sys) {
182 case GPS:
183 break;
184 case GLO:
185 if (prn > 24)
186 continue;
187 cbType[0] = CODETYPEGLONASS_L1_CA;
188 cbType[1] = CODETYPEGLONASS_L1_P;
189 cbType[2] = CODETYPEGLONASS_L2_CA;
190 break;
191 case GAL:
192 cbType[0] = CODETYPEGALILEO_E1_A;
193 cbType[1] = CODETYPEGALILEO_E5_I;
194 cbType[2] = CODETYPEGALILEO_E6_A;
195 break;
196 case QZSS:
197 if (prn > 10)
198 continue;
199 cbType[0] = CODETYPEQZSS_L1_CA;
200 cbType[1] = CODETYPEQZSS_L2_CM;
201 cbType[2] = CODETYPEQZSS_L5_I;
202 break;
203 case SBAS:
204 cbType[0] = CODETYPE_SBAS_L1_CA;
205 cbType[1] = CODETYPE_SBAS_L5_I;
206 cbType[2] = CODETYPE_SBAS_L5_Q;
207 break;
208 case BDS:
209 cbType[0] = CODETYPE_BDS_B1_I;
210 cbType[1] = CODETYPE_BDS_B3_I;
211 cbType[2] = CODETYPE_BDS_B2_I;
212 break;
213 }
214 if (lasttow_cb != tow_cb) { // create block
215 int nl, ns, l;
216 struct CodeBias cbOut;
217 if (lasttow_cb >= 0) {
218 if (sys == GLO) {
219 cbIn.EpochTime[CLOCKORBIT_SATGLONASS] =
220 (fmod((double)cbIn.EpochTime[CLOCKORBIT_SATGLONASS]
221 + (3 * 3600 - GPSUTCdiff), 86400.0));
222 }
223 printCodeBias(outFilenameDbg, &cbIn, "INPUT",
224 CLOCKORBIT_SATGNSS, type, CLOCKORBIT_OFFSETGNSS);
225 l = MakeCodeBias(&cbIn, BTYPE_AUTO, 0, cbBuffer,
226 sizeof(cbBuffer));
227 if (!l)
228 fprintf(stderr, "BUILD ERROR\n");
229 else {
230 if ((f = fopen(outFilenameRaw, "ab+"))) {
231 fwrite(cbBuffer, l, 1, f);
232 fclose(f);
233 } else
234 fprintf(stderr, "SAVE ERROR %s\n", outFilenameRaw);
235 }
236 memset(&cbOut, 0, sizeof(cbOut));
237 nl = GetSSR(NULL, &cbOut, NULL, 0, cbBuffer, l, &ns);
238 if (nl < 0)
239 fprintf(stderr, "CBIAS EXTRACT ERROR %d\n", nl);
240 else if (nl > 0)
241 fprintf(stderr, "CBIAS MULTIBLOCK UNSUPPORTED IN TEST\n");
242 else if (ns != l)
243 fprintf(stderr, "CBIAS SIZE MISMATCH (%d/%d)\n", ns, l);
244 else {
245 printCodeBias(outFilenameDbg, &cbOut, "OUTPUT",
246 CLOCKORBIT_SATGNSS, type, CLOCKORBIT_OFFSETGNSS);
247 printCodeBiasDiff(outFilenameDbg, &cbIn, &cbOut,
248 "DIFF", CLOCKORBIT_SATGNSS, type, CLOCKORBIT_OFFSETGNSS);
249 }
250 }
251 memset(&cbIn, 0, sizeof(cbIn));
252 lasttow_cb = tow_cb;
253 cbIn.messageType = MT;
254 cbIn.EpochTime[CLOCKORBIT_SATGNSS] = (int) tow_cb;
255 cbIn.UpdateInterval = ui;
256 coIn.Supplied[COBOFS_BIAS] = 1;
257 }
258 struct BiasSat *bs;
259 bs = cbIn.Sat
260 + CLOCKORBIT_OFFSETGNSS
261 + cbIn.NumberOfSat[CLOCKORBIT_SATGNSS];
262 bs->ID = prn;
263 bs->NumberOfCodeBiases = ncb;
264 int k = 0;
265 for (k = 0; k < bs->NumberOfCodeBiases; k++) {
266 bs->Biases[k].Type = cbType[k];
267 bs->Biases[k].Bias = cbValue[k];
268 }
269 ++cbIn.NumberOfSat[CLOCKORBIT_SATGNSS];
270 }
271 }
272 free(buffer);
[6554]273 fclose(asciiSsr);
[5669]274 }
275 return 0;
[5664]276}
277
278void printClockOrbit(const char* filename, struct ClockOrbit* clockOrb,
[6554]279 const char* flag, int ind, char satSys, int offsetGnss) {
[5669]280 int i = 0;
281 FILE *filestream = fopen(filename, "ab+");
282 if (!clockOrb->NumberOfSat[ind])
283 return;
284 if (filestream == NULL) {
285 fprintf(stderr, "ERROR: open file %s\n", filename);
286 return;
287 }
288 fprintf(filestream, "CLKORB_%s\n", flag);
289 for (i = offsetGnss; i < offsetGnss + clockOrb->NumberOfSat[ind]; ++i) {
290 fprintf(filestream,
291 "%10d %d %c%02d %5d %12.3f %12.3f %12.3f %12.3f %12.3f %12f %12f %12f %12.3f\n",
292 clockOrb->EpochTime[ind], clockOrb->UpdateInterval, satSys,
293 clockOrb->Sat[i].ID,
294 clockOrb->Sat[i].IOD,
295 clockOrb->Sat[i].Clock.DeltaA0,
296 clockOrb->Sat[i].Orbit.DeltaRadial,
297 clockOrb->Sat[i].Orbit.DeltaAlongTrack,
298 clockOrb->Sat[i].Orbit.DeltaCrossTrack,
299 clockOrb->Sat[i].Clock.DeltaA1,
300 clockOrb->Sat[i].Orbit.DotDeltaRadial,
301 clockOrb->Sat[i].Orbit.DotDeltaAlongTrack,
302 clockOrb->Sat[i].Orbit.DotDeltaCrossTrack,
303 clockOrb->Sat[i].Clock.DeltaA2);
304 }
305 fclose(filestream);
[5664]306}
307
308void printClockOrbitDiff(const char* filename, struct ClockOrbit* clockOrb1,
[6554]309 struct ClockOrbit* clockOrb2, const char* flag, int ind, char satSys,
[5669]310 int offsetGnss) {
311 int i = 0;
312 FILE *filestream = fopen(filename, "ab+");
313 if (!clockOrb1->NumberOfSat[ind])
314 return;
315 if (filestream == NULL) {
316 fprintf(stderr, "ERROR: open file %s\n", filename);
317 return;
318 }
319 fprintf(filestream, "CLKORB_%s\n", flag);
320 for (i = offsetGnss; i < offsetGnss + clockOrb1->NumberOfSat[ind]; ++i) {
321 fprintf(filestream,
322 "%10d %d %c%02d %5d %12.3f %12.3f %12.3f %12.3f %12.3f %12f %12f %12f %12.3f\n",
323 clockOrb1->EpochTime[ind]
324 - clockOrb2->EpochTime[ind],
325 clockOrb1->UpdateInterval
326 - clockOrb2->UpdateInterval, satSys,
327 clockOrb1->Sat[i].ID
328 - clockOrb2->Sat[i].ID,
329 clockOrb1->Sat[i].IOD - clockOrb2->Sat[i].IOD,
330 clockOrb1->Sat[i].Clock.DeltaA0
331 - clockOrb2->Sat[i].Clock.DeltaA0,
332 clockOrb1->Sat[i].Orbit.DeltaRadial
333 - clockOrb2->Sat[i].Orbit.DeltaRadial,
334 clockOrb1->Sat[i].Orbit.DeltaAlongTrack
335 - clockOrb2->Sat[i].Orbit.DeltaAlongTrack,
336 clockOrb1->Sat[i].Orbit.DeltaCrossTrack
337 - clockOrb2->Sat[i].Orbit.DeltaCrossTrack,
338 clockOrb1->Sat[i].Clock.DeltaA1
339 - clockOrb2->Sat[i].Clock.DeltaA1,
340 clockOrb1->Sat[i].Orbit.DotDeltaRadial
341 - clockOrb2->Sat[i].Orbit.DotDeltaRadial,
342 clockOrb1->Sat[i].Orbit.DotDeltaAlongTrack
343 - clockOrb2->Sat[i].Orbit.DotDeltaAlongTrack,
344 clockOrb1->Sat[i].Orbit.DotDeltaCrossTrack
345 - clockOrb2->Sat[i].Orbit.DotDeltaCrossTrack,
346 clockOrb1->Sat[i].Clock.DeltaA2
347 - clockOrb2->Sat[i].Clock.DeltaA2);
348 }
349 fclose(filestream);
[5664]350}
351
[6554]352void printCodeBias(const char* filename, struct CodeBias* codeBias, const char* flag,
[5669]353 int ind, char satSys, int offsetGnss) {
354 int i = 0;
355 FILE *filestream = fopen(filename, "ab+");
356 if (!codeBias->NumberOfSat[ind])
357 return;
358 if (filestream == NULL) {
359 fprintf(stderr, "ERROR: open file %s\n", filename);
360 return;
361 }
362 fprintf(filestream, "CBIAS_%s\n", flag);
363 for (i = offsetGnss; i < offsetGnss + codeBias->NumberOfSat[ind]; ++i) {
364 fprintf(filestream, "%10d %d %c%02d %2d ",
365 codeBias->EpochTime[ind], codeBias->UpdateInterval, satSys,
366 codeBias->Sat[i].ID, codeBias->Sat[i].NumberOfCodeBiases);
367 int j;
368 for (j = 0; j < codeBias->Sat[i].NumberOfCodeBiases; j++) {
369 fprintf(filestream, "%4d %12.3f", codeBias->Sat[i].Biases[j].Type,
370 codeBias->Sat[i].Biases[j].Bias);
371 }
372 fprintf(filestream, "\n");
373 }
374 fclose(filestream);
[5664]375}
376
377void printCodeBiasDiff(const char* filename, struct CodeBias* codeBias1,
[6554]378 struct CodeBias* codeBias2, const char* flag, int ind, char satSys,
[5669]379 int offsetGnss) {
380 int i = 0;
381 FILE *filestream = fopen(filename, "ab+");
382 if (!codeBias1->NumberOfSat[ind])
383 return;
384 if (filestream == NULL) {
385 fprintf(stderr, "ERROR: open file %s\n", filename);
386 return;
387 }
388 fprintf(filestream, "CBIAS_%s\n", flag);
389 for (i = offsetGnss; i < offsetGnss + codeBias1->NumberOfSat[ind]; ++i) {
390 fprintf(filestream, "%10d %d %c%02d %2d ",
391 codeBias1->EpochTime[ind]
392 - codeBias2->EpochTime[ind],
393 codeBias1->UpdateInterval
394 - codeBias2->UpdateInterval, satSys,
395 codeBias1->Sat[i].ID
396 - codeBias2->Sat[i].ID,
397 codeBias1->Sat[i].NumberOfCodeBiases
398 - codeBias2->Sat[i].NumberOfCodeBiases);
399 int j;
400 for (j = 0; j < codeBias1->Sat[i].NumberOfCodeBiases; j++) {
401 fprintf(filestream, "%4d %12.3f",
402 codeBias1->Sat[i].Biases[j].Type
403 - codeBias2->Sat[i].Biases[j].Type,
404 codeBias1->Sat[i].Biases[j].Bias
405 - codeBias2->Sat[i].Biases[j].Bias);
406 }
407 fprintf(filestream, "\n");
408 }
409 fclose(filestream);
[5664]410}
Note: See TracBrowser for help on using the repository browser.