Backup
[gps] / sirf3-post.cpp
1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <string.h>
4 #include <math.h>
5
6 #include "RinexObsStream.hpp"
7 #include "RinexObsHeader.hpp"
8 #include "RinexObsData.hpp"
9
10 #include "Position.hpp"
11 #include "DayTime.hpp"
12 #include "icd_200_constants.hpp"
13
14 using namespace std;
15 using namespace gpstk;
16
17 RinexObsStream  r_out;
18 RinexObsHeader  r_header;
19 RinexObsData    r_data;
20
21 unsigned int last_tt = 0; /* set by parse_mid_28 */
22
23 unsigned int first_tt;
24 unsigned int prev_tt = 0;
25 unsigned int prev_diff_tt = 0;
26
27 double local_second = 0.0;
28 double first_gps_second = 0.0;
29 double prev_gps_second = 0.0;
30 double prev_bias = 0.0;
31 double prev_diff_gps_second = 0.0;
32
33 int mid_7_count = 0;
34 int first_mid_7_count = 0;
35
36 int alpha_count = 0;
37 double alpha = 1.0 / 16370015.0;
38 double beta = 0.0;
39 double period = 16369007.0 * alpha;
40
41 #define TAB_SIZE 100
42 int tab_val[TAB_SIZE] = { 0, };
43 int tab_pos = 0;
44
45 int sum_val = 0;
46 int sum_count = 0;
47
48 double last_tt_sec = 0.0;
49 double prev_diff_tt_sec = 0.0;
50
51 double diff_diff_tt_sec = 0.0;
52
53 Position nominalPos(4205736.0, 166192.0, 4776266.0, Position::Cartesian);       // Clamart
54
55 #define RECORD_NR 1000
56 #define SAT_NR 100
57 #define UNUSED -1
58
59 struct satellite {
60         int satID; /* -1 if unused */
61         #define MID_28_RECEIVED 0x01
62         #define MID_30_RECEIVED 0x02
63         int flag;
64         unsigned int tt;
65         double gps_second;
66         double pseudorange;
67         double tit;
68         double snr_avg;
69 };
70
71 struct record {
72         /* from MID 2 */
73         double posx, posy, posz;
74         double velx, vely, velz;
75         /* from MID 7 */
76         int gps_week;
77         double gps_second;
78         double bias;
79         /* from MID 28 */
80         double sat_second;
81         unsigned int sat_tt;
82         struct satellite sat[SAT_NR];
83         /* from MID 30 */
84         double sat_second_t0;
85         /* computed */
86         double bias_diff;
87         double bias_diff_diff;
88
89         double sat_second_diff;
90         double sat_second_diff_diff;
91
92         int sat_tt_diff;
93         int sat_tt_diff_diff;
94
95         double sat_second_t0_diff;
96         double sat_second_t0_diff_diff;
97
98         double my_bias;
99         double my_bias_diff;
100         double my_bias_diff_diff;
101 };
102
103 struct record record_tab[RECORD_NR];
104 int record_nr = 0;
105
106 void record_init(struct record *r) {
107         int i;
108
109         memset(r, 0, sizeof(*r));
110
111         for (i=0; i<SAT_NR; i++)
112                 r->sat[i].satID = UNUSED;
113 }
114
115 void record_all_init() {
116         int i;
117
118         for (i=0; i<RECORD_NR; i++)
119                 record_init(&record_tab[i]);
120 }
121
122 void record_add_mid_2(struct record *r,
123                       double gps_second, double posx, double posy, double posz,
124                       double velx, double vely, double velz) {
125         r->posx = posx;
126         r->posy = posy;
127         r->posz = posz;
128         r->velx = velx;
129         r->vely = vely;
130         r->velz = velz;
131 }
132
133 void record_add_mid_7(struct record * r,
134                       int gps_week, double gps_second, double bias) {
135         r->gps_week = gps_week;
136         r->gps_second = gps_second;
137         r->bias = bias;
138 }
139
140 void record_add_mid_28(struct record *r,
141                        int satID, unsigned int tt,
142                        double gps_second, double pseudorange,
143                        double tit, double snr_avg) {
144         if ((0<=satID) && (satID<SAT_NR)) {
145
146                 r->sat_second = gps_second;
147                 r->sat_tt = tt;
148
149                 r->sat[satID].satID = satID;
150                 r->sat[satID].flag |= MID_28_RECEIVED;
151                 r->sat[satID].tt = tt;
152                 r->sat[satID].gps_second = gps_second;
153                 r->sat[satID].pseudorange = pseudorange;
154                 r->sat[satID].tit = tit;
155                 r->sat[satID].snr_avg = snr_avg;
156         }
157 }
158
159 int record_add_mid_30(struct record *r,
160                       int satID,
161                       double gps_second, double posx, double posy, double posz,
162                       double velx, double vely, double velz)
163 {
164         if ((0<=satID) && (satID<SAT_NR)) {
165                 r->sat[satID].flag |= MID_30_RECEIVED;
166                 /* TBD */
167         }
168
169         r->sat_second_t0 = gps_second;
170
171         return 0;
172 }
173
174
175 int rinex_open(const char *filename) {
176         r_out.open(filename, ios::out|ios::trunc);
177
178         return 0; 
179 }
180
181 int rinex_header_write() {
182         // fill in the header
183         DayTime now;
184
185         /* for 2.11 header, we need : 
186            versionValid
187            runByValid
188            markerNameValid
189            observerValid
190            receiverValid
191            antennaTypeValid
192            antennaPositionValid
193            antennaOffsetValid
194            waveFactValid
195            obsTypeValid
196            firstTimeValid
197            endValid
198         */
199         r_header.version = 2.11;
200         r_header.fileType = "Observation";
201         r_header.system.system = SatID::systemGPS;
202         r_header.valid |= RinexObsHeader::versionValid;
203
204         r_header.fileProgram = "sirf2";
205         r_header.fileAgency = "Benoit Papillault";
206         r_header.date = now.printf("%Y%m%d");
207         r_header.valid |= RinexObsHeader::runByValid;
208
209         r_header.markerName = "GPSL"; // GPS from LUCEOR
210         r_header.valid |= RinexObsHeader::markerNameValid;
211
212         r_header.observer = "Automatic";
213         r_header.agency = "LUCEOR";
214         r_header.valid |= RinexObsHeader::observerValid;
215
216         r_header.recNo = "BU-353";
217         r_header.recType = "SiRF binary";
218         r_header.recVers = "GSW3.5.0";
219         r_header.valid |= RinexObsHeader::receiverValid;
220
221         r_header.antNo = "USB";
222         r_header.antType = "Integrated";
223         r_header.valid |= RinexObsHeader::antennaTypeValid;
224
225         /* FIXME : should be a real ECEF position ? */
226         r_header.antennaPosition[0] = 0.0;
227         r_header.antennaPosition[1] = 0.0;
228         r_header.antennaPosition[2] = 0.0;
229         r_header.valid |= RinexObsHeader::antennaPositionValid;
230
231         r_header.antennaOffset[0] = 0.0;
232         r_header.antennaOffset[1] = 0.0;
233         r_header.antennaOffset[2] = 0.0;
234         r_header.valid |= RinexObsHeader::antennaOffsetValid;
235
236         r_header.wavelengthFactor[0] = 1;
237         r_header.wavelengthFactor[1] = 1;
238         r_header.valid |= RinexObsHeader::waveFactValid;
239
240         r_header.numObs = 2;
241         r_header.obsTypeList.push_back(RinexObsHeader::C1);
242         r_header.obsTypeList.push_back(RinexObsHeader::S1);
243         r_header.valid |= RinexObsHeader::obsTypeValid;
244
245         r_header.firstObs = now;
246         r_header.firstSystem.system = SatID::systemGPS;
247         r_header.valid |= RinexObsHeader::firstTimeValid;
248
249         r_header.valid |= RinexObsHeader::endValid;
250
251         // write the header
252         r_out << r_header;
253
254         return 0;
255 }
256
257 // write RINEX data record (some part are already filled)
258 int rinex_data_write() {
259
260         // write nothing if no satellite have been seen
261         if (r_data.obs.size() == 0)
262                 return 0;
263
264         // write the data epoch
265         r_out << r_data;
266
267         // reset r_data
268         r_data.obs.clear();
269
270         return 0;
271 }
272
273 /* from Message ID 28 */
274 int rinex_data_add_pseudorange_snr(int satNR,
275                                    double pseudorange,
276                                    double snr) {
277
278         // compute ssi
279         int ssi = (int)(snr / 6.0);
280         if (ssi < 0)
281                 ssi = 0;
282         if (ssi > 9)
283                 ssi = 9;
284
285         SatID sat(satNR, SatID::systemGPS);
286         r_data.obs[sat][RinexObsHeader::C1].data = pseudorange;
287         r_data.obs[sat][RinexObsHeader::C1].lli = 0;
288         r_data.obs[sat][RinexObsHeader::C1].ssi = ssi;
289
290         r_data.obs[sat][RinexObsHeader::S1].data = (double)snr;
291         r_data.obs[sat][RinexObsHeader::S1].lli = 0;
292         r_data.obs[sat][RinexObsHeader::S1].ssi = ssi;
293
294         return 0;
295 }
296
297 /* from Message ID 7 */
298 int rinex_data_add_time(int gps_week, double gps_second) {
299         
300         // fill in the missing pieces
301         r_data.epochFlag = 0;
302         r_data.time.setGPSfullweek(gps_week, gps_second);
303         r_data.numSvs = r_data.obs.size();
304         r_data.clockOffset = 0.0;
305
306         /* write data */
307         rinex_data_write();
308
309         return 0;
310 }
311
312 void tab_add_val(int val) {
313         tab_val[tab_pos] = val;
314         tab_pos = (tab_pos + 1) % TAB_SIZE;
315 }
316
317 double tab_get_avg() {
318         int i;
319         double avg = 0.0;
320
321         for (i=0; i<TAB_SIZE; i++)
322                 avg += (double)tab_val[i];
323         avg = avg / (double)TAB_SIZE;
324
325         return avg;
326 }
327
328 void parse_mid_2(const char *buf) {
329         int mid;
330         double gps_second, posx, posy, posz, velx, vely, velz;
331
332         sscanf(buf,
333                "%d %lf %lf %lf %lf %lf %lf %lf",
334                &mid, &gps_second, &posx, &posy, &posz, &velx, &vely, &velz);
335
336         if (record_nr < RECORD_NR) {
337                 record_add_mid_2(&record_tab[record_nr],
338                                  gps_second, posx, posy, posz, velx, vely, velz);
339         }
340 }
341
342 void parse_mid_7(const char *buf) {
343         int mid, gps_week;
344         double gps_second, bias;
345         int diff_tt, diff_diff_tt;
346         double diff_gps_second, diff_diff_gps_second;
347
348         mid_7_count ++;
349
350         sscanf(buf, "%d %d %lf %lf",
351                &mid, &gps_week, &gps_second, &bias);
352
353         printf("MID 7 : %u %.12f %.12f [mid_7_count %d]\n",
354                gps_week, gps_second, bias, mid_7_count);
355
356         if ((record_nr < RECORD_NR) /*&& (mid_7_count > 1000)*/) {
357                 record_add_mid_7(&record_tab[record_nr++],
358                                  gps_week, gps_second, bias);
359         }
360
361         diff_tt = last_tt - prev_tt;
362         diff_diff_tt = diff_tt - prev_diff_tt;
363
364         /* VERY IMPORTANT LINE - we supposed that bias is computed again the
365            beginning of the second. This is no longer the case. gps_second
366            is the GPS time (as opposed to local time) when this signal was
367            TRANSMITTED. If we substrat bias, we get the LOCAL (receiver)
368            clock time */
369         gps_second = gps_second - bias;
370         diff_gps_second = gps_second - prev_gps_second;
371         diff_diff_gps_second = diff_gps_second - prev_diff_gps_second;
372
373         if (mid_7_count == 1) {
374                 first_tt = last_tt;
375                 first_gps_second = gps_second;
376                 first_mid_7_count = mid_7_count;
377                 alpha_count = 0;
378         }
379
380         if (mid_7_count > 2) {
381                 double diff_tt_sec, last_diff_tt_sec;
382                 double avg;
383
384                 printf("\tsec %.12f diff_sec %.12f diff_diff_sec %.12f\n",
385                        gps_second, diff_gps_second, diff_diff_gps_second);
386                 printf("\ttt %u diff_tt %d diff_diff_tt %d\n",
387                        last_tt, diff_tt, diff_diff_tt);
388
389                 /* adjusting alpha first, detecting rollover */
390                 if ((gps_second < prev_gps_second) ||
391                     (last_tt < prev_tt) ||
392                     (bias < prev_bias)) {
393                         printf("\tResetting alpha...\n");
394                         first_tt = last_tt;
395                         first_gps_second = gps_second;
396                         first_mid_7_count = mid_7_count;
397                         alpha_count = 0;
398                 }
399
400                 diff_tt_sec = (double)diff_tt * alpha;
401                 switch (diff_diff_tt) {
402                 case -1:
403                 case 0:
404                 case 1:
405                         /* replace with the smoothed value (more
406                            accurate) */
407                         diff_tt_sec = period;
408                         break;
409                 }
410
411                 printf("\tdiff_tt_sec %.12f - diff_sec = %.12f%c [diff_diff_tt_sec %.12f]\n",
412                        diff_tt_sec, diff_tt_sec - diff_gps_second,
413                        ((diff_tt_sec - diff_gps_second) > alpha/2.0) ||
414                        ((diff_tt_sec - diff_gps_second) < -alpha/2.0) ? 'W' : ' ',
415                        diff_diff_tt_sec);
416
417                 alpha_count ++;
418                 if (alpha_count > 100) {
419                         printf("\tComputing alpha with :\n"
420                                "\ttt %u -> %u sec %.12f -> %.12f\n"
421                                "\tmid_7_count %d first_mid_7_count %d\n",
422                                first_tt, last_tt, first_gps_second, gps_second,
423                                mid_7_count, first_mid_7_count);
424                         alpha = (gps_second - first_gps_second) /
425                                 (double)(last_tt - first_tt);
426                         beta  = gps_second - (double)last_tt * alpha;
427                         printf("\talpha %.12g beta %.12g 1/alpha %.12g\n",
428                                alpha, beta, 1.0/alpha);
429                         period = (gps_second - first_gps_second) /
430                                 (double)(mid_7_count - first_mid_7_count);
431                         printf("\tperiod %.12f\n", period);
432                         alpha_count = 0;
433
434                         /* FIXME */
435                         last_tt_sec = gps_second - diff_tt_sec;
436                 }
437
438                 last_tt_sec += diff_tt_sec;
439                 last_diff_tt_sec = last_tt_sec - gps_second;
440                 printf("\tlast_tt_sec %.12f - sec = %.12f diff %.12f\n",
441                        last_tt_sec, last_diff_tt_sec,
442                        last_diff_tt_sec - prev_diff_tt_sec);
443                 prev_diff_tt_sec = last_diff_tt_sec;
444
445                 tab_add_val(diff_diff_tt);
446                 avg = tab_get_avg();
447
448                 printf("\tavg %.12f avg_sec %.12f [count %d]\n",
449                        avg, avg*alpha, mid_7_count);
450
451                 sum_val   += diff_diff_tt;
452                 sum_count += 1;
453
454                 printf("\tsum_count %d avg_sec %.12f step %.12f\n",
455                         sum_count,
456                         (double)sum_val / (double)sum_count * alpha,
457                         alpha / (double) sum_count);
458         }
459
460         prev_tt = last_tt;
461         prev_diff_tt = diff_tt;
462
463         prev_gps_second = gps_second;
464         prev_bias = bias;
465         prev_diff_gps_second = diff_gps_second;
466 }
467
468 void parse_mid_28(const char *buf) {
469         int mid, satID;
470         unsigned int tt;
471         double gps_second, pseudorange, tit, snr_avg;
472
473         sscanf(buf,"%d %d %u %lf %lf %lf %lf\n",
474                &mid, &satID, &tt, &gps_second, &pseudorange, &tit, &snr_avg);
475
476         if ((record_nr < RECORD_NR) /*&& (mid_7_count > 1000)*/) {
477                 record_add_mid_28(&record_tab[record_nr],
478                                   satID, tt, gps_second, pseudorange,
479                                   tit, snr_avg);
480         }
481
482         printf("MID 28 : %2d %10u %19.12f %18.6f %6.3f %6.3f\n",
483                satID, tt, gps_second, pseudorange, tit, snr_avg);
484
485         last_tt = tt;
486         local_second = gps_second;
487 }
488
489 void parse_mid_30(const char *buf) {
490         int mid, satID;
491         double gps_second, posx, posy, posz, velx, vely, velz;
492
493         sscanf(buf, "%d %d %lf %lf %lf %lf %lf %lf %lf\n",
494                &mid, &satID, &gps_second, &posx, &posy, &posz, &velx, &vely, &velz);
495
496         if ((record_nr < RECORD_NR)) {
497                 record_add_mid_30(&record_tab[record_nr], satID,
498                                   gps_second, posx, posy, posz, velx, vely, velz);
499         }
500 }
501
502 double distance(double x1, double y1, double z1,
503                 double x2, double y2, double z2) {
504         return (sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2)+(z1-z2)*(z1-z2)));
505 }
506
507 void print_data() {
508         int i,j;
509
510         printf("Printing data ...\n");
511
512         for (i=0; i<record_nr; i++) {
513                 struct record *r = &record_tab[i];
514
515                 printf("record %4d :\n", i);
516                 printf("  gps_week %u gps_second %.12f bias %.12f\n",
517                        r->gps_week, r->gps_second, r->bias);
518                 printf("  sat_second\t%.12f diff %.12f diff_diff %.12f\n",
519                        r->sat_second,
520                        r->sat_second_diff, r->sat_second_diff_diff);
521                 printf("  sat_tt\t%10u diff %10d diff_diff %10d\n",
522                        r->sat_tt, r->sat_tt_diff, r->sat_tt_diff_diff);
523                 printf("  sat_second_t0\t%.12f diff %.12f diff_diff %.12f\n",
524                        r->sat_second_t0, r->sat_second_t0_diff, r->sat_second_t0_diff_diff);
525                 printf("  bias\t%.12f diff %.12f diff_diff %.12f\n",
526                        r->bias, r->bias_diff, r->bias_diff_diff);
527                 printf("  my_bias\t%.12f diff %.12f diff_diff %.12f\n",
528                        r->my_bias, r->my_bias_diff, r->my_bias_diff_diff);
529                 printf("  distance %.3f\n",
530                        distance(nominalPos.X(), nominalPos.Y(), nominalPos.Z(),
531                                 r->posx, r->posy, r->posz));
532                 for (j=0; j<SAT_NR; j++) {
533                         if (r->sat[j].satID != UNUSED) {
534                                 printf("  satID %2u tt=%u gps_second %.12f "
535                                        "pseudorange %f tit %4.1f snr_avg %4.1f\n",
536                                        r->sat[j].satID,
537                                        r->sat[j].tt,
538                                        r->sat[j].gps_second,
539                                        r->sat[j].pseudorange,
540                                        r->sat[j].tit,
541                                        r->sat[j].snr_avg);
542                         }
543                 }
544         }
545 }
546
547 void save_data() {
548         int i,j;
549
550         printf("Saving data ...\n");
551
552         for (i=0; i<record_nr; i++) {
553                 struct record *r = &record_tab[i];
554
555                 /* condition on record :
556                  4 satellites with tit >= 30.0 */
557
558                 int usable_sat = 0;
559                 for (j=0; j<SAT_NR; j++) {
560                         if (r->sat[j].satID == UNUSED)
561                                 continue;
562                         if (r->sat[j].tit < 30.0)
563                                 continue;
564
565                         usable_sat++;
566                 }
567
568                 //if (usable_sat < 4)
569                 //      continue;
570
571                 for (j=0; j<SAT_NR; j++) {
572                         if (r->sat[j].satID == UNUSED)
573                                 continue;
574                         //if (r->sat[j].tit < 30.0)
575                         //      continue;
576                         if ((r->sat[j].flag &
577                              (MID_28_RECEIVED | MID_30_RECEIVED)) != 
578                             (MID_28_RECEIVED | MID_30_RECEIVED))
579                                 continue;
580
581                         //double bias = r->bias;
582                         //double bias = r->sat_second - r->gps_second;
583                         //double bias = r->sat_second - r->sat_second_t0;
584                         //r->sat[j].pseudorange -= bias * C_GPS_M,
585                                 
586                         rinex_data_add_pseudorange_snr(r->sat[j].satID,
587                                                        r->sat[j].pseudorange,
588                                                        r->sat[j].snr_avg);
589                 }
590
591                 rinex_data_add_time(r->gps_week, r->sat_second);
592                 printf("  wrote record %4d : GPS second : %.12f\n", i, r->gps_second);
593         }
594
595         //rinex_close();
596 }
597
598
599 void process_data() {
600         int i;
601
602         printf("Processing data ...\n");
603
604         for (i=0; i<record_nr; i++) {
605                 struct record *r = &record_tab[i];
606
607                 r->my_bias = r->sat_second - r->sat_second_t0;
608
609                 if (i>=1) {
610                         struct record *r_p = &record_tab[i-1];
611                         r->bias_diff = r->bias - r_p->bias;
612                         r->sat_second_diff = r->sat_second - r_p->sat_second;
613                         r->sat_tt_diff = r->sat_tt - r_p->sat_tt;
614                         r->sat_second_t0_diff = r->sat_second_t0 - r_p->sat_second_t0;
615                         r->my_bias_diff = r->my_bias - r_p->my_bias;
616                         if (i>=2) {
617                                 r->bias_diff_diff = r->bias_diff -
618                                         r_p->bias_diff;
619                                 r->sat_second_diff_diff = r->sat_second_diff -
620                                         r_p->sat_second_diff;
621                                 r->sat_tt_diff_diff = r->sat_tt_diff -
622                                         r_p->sat_tt_diff;
623                                 r->sat_second_t0_diff_diff = r->sat_second_t0_diff -
624                                         r_p->sat_second_t0_diff;
625                                 r->my_bias_diff_diff = r->my_bias_diff - r_p->my_bias_diff;
626                         } else {
627                                 r->bias_diff_diff = 0.0;
628                                 r->sat_second_diff_diff = 0.0;
629                                 r->sat_tt_diff_diff = 0.0;
630                                 r->sat_second_t0_diff_diff = 0.0;
631                                 r->my_bias_diff_diff = 0.0;
632                         }
633                 } else {
634                         r->bias_diff = 0.0;
635                         r->sat_second_diff = 0.0;
636                         r->sat_tt_diff = 0.0;
637                         r->sat_second_t0_diff = 0.0;
638                         r->my_bias_diff = 0.0;
639                 }
640         }
641 }
642
643 void usage() {
644         printf("sirf3-post file.raw [-rinex file.obs]\n");
645         exit (-1);
646 }
647
648 int main(int argc, const char *argv[]) {
649         FILE * file;
650         const char * filename = NULL;
651         char buf[1024];
652         int i;
653
654         record_all_init();
655
656         for (i=1; i<argc; i++) {
657                 if (strcmp(argv[i],"-rinex")==0 && i+1<argc) {
658                         const char * rinex = argv[++i];
659                         rinex_open(rinex);
660                         rinex_header_write();
661                 } else if (filename == NULL) {
662                         filename = argv[i];
663                 } else 
664                         usage();
665         }
666
667         if (filename == NULL)
668                 usage();
669
670         file = fopen(filename, "r");
671
672         while (fgets(buf, sizeof(buf), file) != NULL) {
673                 int mid;
674
675                 if (sscanf(buf, "%d", &mid) != 1) {
676                         printf("error: %s", buf);
677                         continue;
678                 }
679
680                 switch (mid) {
681                 case 2:
682                         parse_mid_2(buf);
683                         break;
684                 case 7:
685                         parse_mid_7(buf);
686                         break;
687                 case 28:
688                         parse_mid_28(buf);
689                         break;
690                 case 30:
691                         parse_mid_30(buf);
692                         break;
693                 default:
694                         printf("unknown MID %d\n", mid);
695                         break;
696                 }
697
698                 /* stop after a while ;-) */
699                 if (record_nr >= RECORD_NR)
700                         break;
701         }
702
703         printf("Record of data : \n");
704
705         process_data();
706         save_data();
707         print_data();
708
709         fclose (file);
710         return 0;
711 }