backup
[gps] / sirf2.cpp
1 /*
2   Author : Benoit Papillault <benoit.papillault@free.fr>
3   Creation : 2011-07-03
4   Goal : Understanding SiRF protocol for GPS devices
5 */
6
7 #include <stdio.h>
8 #include <stdlib.h>
9 #include <sys/select.h>
10 #include <sys/types.h>
11 #include <sys/stat.h>
12 #include <fcntl.h>
13 #include <termios.h>
14 #include <unistd.h>
15 #include <string.h>
16
17 #include "RinexObsStream.hpp"
18 #include "RinexObsHeader.hpp"
19 #include "RinexObsData.hpp"
20
21 #include "DayTime.hpp"
22 #include "icd_200_constants.hpp"
23
24 using namespace std;
25 using namespace gpstk;
26
27 const char *g_rinex = NULL;
28
29 int     last_ext_gps_week = 0;
30 double  last_gps_seconds = 0.0;
31
32 RinexObsStream  r_out;
33 RinexObsHeader  r_header;
34 RinexObsData    r_data;
35
36 double prev_clock_bias = 0.0;
37 double prev_gps_seconds = 0.0;
38 double prev_gps_drift = 0.0;
39 int this_tt = 0, prev_tt = 0;
40 int prev_tt_drift = 0;
41
42 int rinex_open(const char *filename) {
43         r_out.open(filename, ios::out|ios::trunc);
44
45         return 0; 
46 }
47
48 int rinex_header_write() {
49         // fill in the header
50         DayTime now;
51
52         /* for 2.11 header, we need : 
53            versionValid
54            runByValid
55            markerNameValid
56            observerValid
57            receiverValid
58            antennaTypeValid
59            antennaPositionValid
60            antennaOffsetValid
61            waveFactValid
62            obsTypeValid
63            firstTimeValid
64            endValid
65         */
66         r_header.version = 2.11;
67         r_header.fileType = "Observation";
68         r_header.system.system = SatID::systemGPS;
69         r_header.valid |= RinexObsHeader::versionValid;
70
71         r_header.fileProgram = "sirf2";
72         r_header.fileAgency = "Benoit Papillault";
73         r_header.date = now.printf("%Y%m%d");
74         r_header.valid |= RinexObsHeader::runByValid;
75
76         r_header.markerName = "GPSL"; // GPS from LUCEOR
77         r_header.valid |= RinexObsHeader::markerNameValid;
78
79         r_header.observer = "Automatic";
80         r_header.agency = "LUCEOR";
81         r_header.valid |= RinexObsHeader::observerValid;
82
83         r_header.recNo = "BU-353";
84         r_header.recType = "SiRF binary";
85         r_header.recVers = "GSW3.5.0";
86         r_header.valid |= RinexObsHeader::receiverValid;
87
88         r_header.antNo = "USB";
89         r_header.antType = "Integrated";
90         r_header.valid |= RinexObsHeader::antennaTypeValid;
91
92         /* FIXME : should be a real ECEF position ? */
93         r_header.antennaPosition[0] = 0.0;
94         r_header.antennaPosition[1] = 0.0;
95         r_header.antennaPosition[2] = 0.0;
96         r_header.valid |= RinexObsHeader::antennaPositionValid;
97
98         r_header.antennaOffset[0] = 0.0;
99         r_header.antennaOffset[1] = 0.0;
100         r_header.antennaOffset[2] = 0.0;
101         r_header.valid |= RinexObsHeader::antennaOffsetValid;
102
103         r_header.wavelengthFactor[0] = 1;
104         r_header.wavelengthFactor[1] = 1;
105         r_header.valid |= RinexObsHeader::waveFactValid;
106
107         r_header.numObs = 2;
108         r_header.obsTypeList.push_back(RinexObsHeader::C1);
109         r_header.obsTypeList.push_back(RinexObsHeader::S1);
110         r_header.valid |= RinexObsHeader::obsTypeValid;
111
112         r_header.firstObs = now;
113         r_header.firstSystem.system = SatID::systemGPS;
114         r_header.valid |= RinexObsHeader::firstTimeValid;
115
116         r_header.valid |= RinexObsHeader::endValid;
117
118         // write the header
119         r_out << r_header;
120
121         return 0;
122 }
123
124 // write RINEX data record (some part are already filled)
125 int rinex_data_write() {
126
127         // write nothing if no satellite have been seen
128         if (r_data.obs.size() == 0)
129                 return 0;
130
131         // fill in the missing pieces
132         r_data.epochFlag = 0;
133         r_data.time.setGPSfullweek(last_ext_gps_week, last_gps_seconds);
134         r_data.numSvs = r_data.obs.size();
135         r_data.clockOffset = 0.0;
136         /* r_data_.obs = set by rinex_data_add_pseudorange */
137         
138         // write the data epoch
139         r_out << r_data;
140
141         // reset r_data
142         r_data.obs.clear();
143
144         return 0;
145 }
146
147 /* from Message ID 28 */
148 int rinex_data_add_pseudorange_snr(int satNR,
149                                    double gps_seconds,
150                                    double pseudorange,
151                                    double snr) {
152         printf("RINEX range\t| GPS seconds %6.9f SatID %3u C1 %f (\n",
153                gps_seconds, satNR, pseudorange);
154
155         // compute ssi
156         int ssi = (int)(snr / 6.0);
157         if (ssi < 0)
158                 ssi = 0;
159         if (ssi > 9)
160                 ssi = 9;
161
162         // we only set GPS seconds
163         last_gps_seconds = gps_seconds;
164
165         SatID sat(satNR, SatID::systemGPS);
166         r_data.obs[sat][RinexObsHeader::C1].data = pseudorange;
167         r_data.obs[sat][RinexObsHeader::C1].lli = 0;
168         r_data.obs[sat][RinexObsHeader::C1].ssi = ssi;
169
170         r_data.obs[sat][RinexObsHeader::S1].data = (double)snr;
171         r_data.obs[sat][RinexObsHeader::S1].lli = 0;
172         r_data.obs[sat][RinexObsHeader::S1].ssi = ssi;
173
174         return 0;
175 }
176
177 /* from Message ID 7 */
178 int rinex_data_add_time_clock_bias(int ext_gps_week,
179                                    double gps_seconds, double clock_bias) {
180         printf("RINEX time\t| GPS week %5u GPS seconds %6.2f Clock bias %.9f\n",
181                ext_gps_week, gps_seconds, clock_bias);
182         
183         // set extended GPS week only
184         last_ext_gps_week = ext_gps_week;
185         
186         // adjust using clock bias
187         printf("RINEX : clock adjust %.9f => %.9f\n",
188                last_gps_seconds, last_gps_seconds - clock_bias);
189
190
191         double gps_drift = last_gps_seconds - prev_gps_seconds;
192         double freq = 0.0;
193         if ((prev_gps_drift != 0.0)
194             && (prev_gps_drift != gps_drift))
195                 freq = 1.0 / (gps_drift - prev_gps_drift);
196
197         printf("RINEX : clock_bias += %.9f Meas. drift %.12f ClockD %.12f ClockF %f\n", 
198                clock_bias - prev_clock_bias,
199                last_gps_seconds - prev_gps_seconds,
200                gps_drift - prev_gps_drift, freq);
201
202         prev_clock_bias = clock_bias;
203         prev_gps_seconds = last_gps_seconds;
204         prev_gps_drift = gps_drift;
205
206         int tt_drift = this_tt - prev_tt;
207         printf("RINEX : time tag %u => %u drift %d (+%.9f s) +%d\n",
208                prev_tt, this_tt,
209                tt_drift, (double)tt_drift/16368000.0, 
210                tt_drift - prev_tt_drift);
211         prev_tt = this_tt;
212         prev_tt_drift = tt_drift;
213
214         // adjust pseudorange
215         last_gps_seconds = floor(gps_seconds) + clock_bias;
216
217         /*
218         last_gps_seconds -= clock_bias;
219         printf("RINEX : using clock_bias %.9f\n", clock_bias);
220         RinexObsData::RinexSatMap::iterator it;
221         for (it = r_data.obs.begin(); it != r_data.obs.end(); ++it) {
222                 double old_val = (*it).second[RinexObsHeader::C1].data;
223                 double new_val = old_val - clock_bias * C_GPS_M;
224                 (*it).second[RinexObsHeader::C1].data = new_val;
225
226                 printf("RINEX satID %3u : pseudorange adjust %f (%.9f s) => %f (%.9f s)\n",
227                        (*it).first.id,
228                        old_val, old_val / C_GPS_M,
229                        new_val, new_val / C_GPS_M);
230         }
231         */
232
233         /* write data */
234         rinex_data_write();
235
236         return 0;
237 }
238
239 /* GPS clock management */
240
241 double alpha = 1.0 / 16368000, beta = 0.0;
242 int gps_clock_count = 0;
243
244 unsigned long long prev_gps_clock = 0;
245 unsigned long long last_gps_clock = 0;
246
247 double prev_bias;
248 double last_bias;
249
250 int prev_gps_second = 0;
251 int last_gps_second = 0;
252
253 /* called on every MID 28 */
254 void gps_clock_record_tt(unsigned int clock) {
255 #define GPS_CLOCK_MASK ((unsigned long long)0xffffffff)
256         unsigned long long new_gps_clock = (last_gps_clock & ~GPS_CLOCK_MASK) 
257                 | (clock & GPS_CLOCK_MASK);
258         while (new_gps_clock < last_gps_clock) {
259                 new_gps_clock += (unsigned long long)0x100000000;
260                 printf("rollover adjustment in GPS clock : 0x%llx\n", new_gps_clock);
261         }
262
263         last_gps_clock = new_gps_clock;
264 }
265
266 /* called on every MID 7 */
267 void gps_clock_record_bias(double gps_second, double bias) {
268         last_bias = bias;
269         last_gps_second = (int) gps_second;
270 }
271
272 void gps_clock_adjust() {
273
274         static int first = 1;
275
276         if (first) {
277                 first = 0;
278
279                 prev_gps_clock = last_gps_clock;
280                 prev_bias = last_bias;
281                 prev_gps_second = last_gps_second;
282
283                 alpha = 1.0 / 16368000.0;
284                 beta = (double)last_gps_second + last_bias - (double)last_gps_clock * alpha;
285
286                 printf("  alpha %.9g beta %.9f\n", alpha, beta);
287
288         }
289         
290         printf("gps_clock_count = %d\n", gps_clock_count);
291         
292         if (gps_clock_count >= 100) {
293
294                 printf("  Computing alpha & beta with count=%d\n"
295                        "  last_gps_second=%d prev_gps_second=%d\n"
296                        "  last_bias=%.9f prev_bias=%.9f\n"
297                        "  last_gps_clock=0x%llx prev_gps_clock=0x%llx\n",
298                        gps_clock_count, last_gps_second, prev_gps_second,
299                        last_bias, prev_bias,
300                        last_gps_clock, prev_gps_clock);
301
302                 alpha = ((double)(last_gps_second - prev_gps_second) + (last_bias - prev_bias )) /
303                          (double)(last_gps_clock - prev_gps_clock);
304                 beta  = ((double)last_gps_second + last_bias - (double)last_gps_clock * alpha);
305
306                 printf("  alpha %.9g beta %.9f\n", alpha, beta);
307                 
308                 prev_gps_clock = last_gps_clock;
309                 prev_bias = last_bias;
310                 prev_gps_second = last_gps_second;
311
312                 gps_clock_count = 0;
313         }
314
315         gps_clock_count ++ ;
316 }
317
318 double gps_clock_convert(unsigned int clock) {
319
320         unsigned long long new_gps_clock = (last_gps_clock & ~GPS_CLOCK_MASK) 
321                 | ((unsigned long long)clock & GPS_CLOCK_MASK);
322         while (new_gps_clock < last_gps_clock) {
323                 new_gps_clock += 0x100000000;
324                 printf("rollover adjustment in GPS clock : 0x%llx\n", new_gps_clock);
325         }
326
327         printf("%s: clock=0x%llx 1/alpha=%f beta=%.9f\n",
328                __func__, new_gps_clock, 1.0/alpha, beta);
329         return (double) new_gps_clock * alpha + beta;
330 }
331
332 /* return 1 if message is a valid SiRF message, 0 if invalid */
333
334 int check_sirf_msg(unsigned char *buf, int n) {
335         int length, crc, computed_crc, i;
336
337         /* check size (at least 8 bytes) */
338         if (n<8) {
339                 printf("msg too small (%d bytes)\n", n);
340                 return 0;
341         }
342
343         /* check start pattern */
344         if ((buf[0] != 0xa0) || (buf[1] != 0xa2)) {
345                 printf("invalid start pattern : 0x%02x 0x%02x\n",
346                        buf[0], buf[1]);
347                 return 0;
348         }
349   
350         /* check length */
351         length = ((buf[2]<<8) | (buf[3]));
352         if (length & 0x8000) {
353                 printf("invalid length : 0x%04x bytes\n", length);
354                 return 0;
355         }
356
357         if (length > 912) {
358                 printf("warning : length (%d bytes) is above 912\n", length);
359         }
360
361         if (4 + length + 4 != n) {
362                 printf("invalid length : %d bytes, buf buffer is %d bytes\n",
363                        length, n);
364                 return 0;
365         }
366   
367         /* check checksum */
368         crc = ((buf[n-4]<<8) | (buf[n-3]));
369         if (crc & 0x8000) {
370                 printf("invalid crc : 0x%04x\n", crc);
371                 return 0;
372         }
373
374         computed_crc = 0;
375         for (i=4; i<n-4; i++) {
376                 computed_crc += buf[i];
377         }
378         computed_crc &= 0x7fff;
379
380         if (computed_crc != crc) {
381                 printf("invalid crc : 0x%04x computed crc : 0x%04x\n",
382                        crc, computed_crc);
383                 return 0;
384         }
385   
386         /* check stop pattern */
387         if ((buf[n-2] != 0xb0) || (buf[n-1] != 0xb3)) {
388                 printf("invalid stop pattern : 0x%02x 0x%02x\n",
389                        buf[n-2], buf[n-1]);
390                 return 0;
391         }
392
393         return 1;
394 }
395
396 double get_sirf_dbl(unsigned char *buf) {
397   double r;
398   unsigned char * ptr = (unsigned char *)&r;
399
400   ptr[0] = buf[3];
401   ptr[1] = buf[2];
402   ptr[2] = buf[1];
403   ptr[3] = buf[0];
404   ptr[4] = buf[7];
405   ptr[5] = buf[6];
406   ptr[6] = buf[5];
407   ptr[7] = buf[4];
408
409   if (sizeof(double) == 8) {
410     return r;
411   } else {
412     printf("get_sirf_dbl implementation error\n");
413   }
414 }
415
416 int get_sirf_2s(unsigned char *buf) {
417         return (((signed char)buf[0])<<8) | buf[1];
418 }
419
420 int get_sirf_4s(unsigned char *buf) {
421         return (((signed char)buf[0])<<24)|(buf[1]<<16)|(buf[2]<<8)|buf[3];
422 }
423
424 unsigned int get_sirf_2u(unsigned char *buf) {
425         return (buf[0]<<8) | buf[1];
426 }
427
428 unsigned int get_sirf_4u(unsigned char *buf) {
429         return (buf[0]<<24) | (buf[1]<<16) | (buf[2]<<8) | buf[3];
430 }
431
432 float get_sirf_sgl(unsigned char *buf) {
433   float r;
434   unsigned char *ptr = (unsigned char *)&r;
435
436   ptr[0] = buf[3];
437   ptr[1] = buf[2];
438   ptr[2] = buf[1];
439   ptr[3] = buf[0];
440
441   if (sizeof(float) == 4) {
442     return r;
443   } else {
444     printf("get_sirf_sgl implementation error\n");
445   }
446 }
447
448 int decode_sirf_msg_2(unsigned char * buf, int n)
449 {
450         int i, p = 0, n_sat;
451         unsigned char mode;
452         const char * altmode = "";
453         const char * pmode = "";
454
455         printf("Measure Navigation Data Out\n");
456         p += 1;
457         printf("  X-position : %d m\n", get_sirf_4s(buf+p));
458         p += 4;
459         printf("  Y-position : %d m\n", get_sirf_4s(buf+p));
460         p += 4;
461         printf("  Z-position : %d m\n", get_sirf_4s(buf+p));
462         p += 4;
463         printf("  X-velocity : %f m/s\n", (double)get_sirf_2s(buf+p) / 8.0);
464         p += 2;
465         printf("  Y-velocity : %f m/s\n", (double)get_sirf_2s(buf+p) / 8.0);
466         p += 2;
467         printf("  Z-velocity : %f m/s\n", (double)get_sirf_2s(buf+p) / 8.0);
468         p += 2;
469         /* Mode 1 */
470         mode = buf[p];
471
472         switch ((mode >> 4) & 3) {
473         case 0:
474                 altmode = "No altitude hold applied"; break;
475         case 1 :
476                 altmode = "Holding of altitude from KF"; break;
477         case 2:
478                 altmode = "Holding of altitude from user input"; break;
479         case 3:
480                 altmode = "Always hold altitude"; break;
481         }
482
483         switch ( mode & 7) {
484         case 0:
485                 pmode = "No navigation solution"; break;
486         case 1:
487                 pmode = "1-SV solution (Kalman filter)"; break;
488         case 2:
489                 pmode = "2-SV solution (Kalman filter)"; break;
490         case 3:
491                 pmode = "3-SV solution (Kalman filter)"; break;
492         case 4:
493                 pmode = ">3S-SV solution (Kalman filter)"; break;
494         case 5:
495                 pmode = "2D solution (least squares)"; break;
496         case 6:
497                 pmode = "3D solution (least squares)"; break;
498         case 7:
499                 pmode = "Dead-Reckoning solution (no satellites)"; break;
500         }
501
502         printf("  Mode 1 : %02x %s %s %s %s %s\n", 
503                mode ,
504                mode & (1<<7) ? "DGPS:Yes" : "DGPS:No",
505                mode & (1<<6) ? "DOP mask exceeded " : "",
506                altmode, 
507                mode & (1<<3) ? "TicklePower position " : "",
508                pmode);
509         p += 1;
510         /* HDOP */
511         printf("  HDOP : %f\n", (double)(unsigned)buf[p] / 5.0);
512         p += 1;
513         /* Mode 2 */
514         mode = buf[p];
515         printf("  Mode 2 : %02x\n", mode);
516         p += 1;
517         printf("  GPS Week : %u\n", get_sirf_2u(buf+p));
518         p += 2;
519         printf("  GPS Time of Week : %f\n", (double)get_sirf_4u(buf+p)/100.0);
520         p += 4;
521         printf("  Satellites used : %d\n", n_sat = buf[p]);
522         p += 1;
523         for (i=0; (i<12) & (i<n_sat); i++) {
524                         printf("  Channel %2d SatID: %3u\n", i+1, buf[p+i]);
525         }
526
527         return 0;
528 }
529
530 int decode_sirf_msg_4(unsigned char *buf, int n)
531 {
532         int i, p = 0;
533
534         printf("Measured Tracker Data Out\n");
535         p += 1;
536         printf("  GPS Week : %d\n", get_sirf_2s(buf + p));
537         p += 2;
538         printf("  GPS Time of Week : %.3f\n",
539                (double)get_sirf_4u(buf + p) / 100.0);
540         p += 4;
541         printf("  Channels : %d\n", buf[p]);
542         p += 1;
543         for (;p<n;) {
544                 printf("  SatID: %3u Azimuth: %f Elevation: %f\n",
545                        buf[p], (double)buf[p+1]/2.0*3.0, (double)buf[p+2]/2.0);
546                 p += 3;
547                 printf("    State : %02x\n", get_sirf_2u(buf + p));
548                 p += 2;
549                 for (i=0; i<10; i++) {
550                         printf("    SNR %2d: %d dB\n", i+1, buf[p+i]);
551                 }
552                 p+= 10;
553         }
554
555         return 0;
556 }
557
558 int decode_sirf_msg_7(unsigned char *buf, int n)
559 {
560         int p = 0;
561         int ext_gps_week;
562         double gps_seconds;
563         double clock_bias;
564
565         printf("Clock Status Data\n");
566         p += 1;
567         printf("  Extended GPS Week : %u\n",
568                ext_gps_week = get_sirf_2u(buf + p));
569         p += 2;
570         printf("  GPS Time of Week : %f s\n",
571                gps_seconds = (double)get_sirf_4u(buf + p) / 100.0);
572         p += 4;
573         printf("  Satellites used : %u\n", buf[p]);
574         p += 1;
575         printf("  Clock Drift : %u Hz\n", get_sirf_4u(buf + p));
576         p += 4;
577         printf("  Clock Bias : %u ns\n", get_sirf_4u(buf + p));
578         clock_bias = 1e-9 * (double) get_sirf_4u(buf + p);
579         p += 4;
580         printf("  Estimated GPS Time : %u ms\n", get_sirf_4u(buf + p));
581         p += 4;
582
583         gps_clock_record_bias(gps_seconds, clock_bias);
584         gps_clock_adjust();
585
586         rinex_data_add_time_clock_bias(ext_gps_week, gps_seconds, clock_bias);
587
588         return 0;
589 }
590
591 int decode_sirf_msg_8(unsigned char *buf, int n)
592 {
593         int i, p = 0;
594
595         printf("50 BPS Data\n");
596         p += 1;
597         printf("  Channel %d, SV# %d\n", buf[p], buf[p+1]);
598         p += 2;
599         for (i=0; i<10; i++) {
600                 printf("  Word %d : %08x\n", i, get_sirf_4u(buf+p));
601                 p += 4;
602         }
603
604         return 0;
605 }
606
607 int decode_sirf_msg_13(unsigned char *buf, int n)
608 {
609         int p = 0, i, n_sat;
610
611         printf("Visible List\n");
612         p += 1;
613         n_sat = buf[p];
614         p += 1;
615         for (i=0; i<n_sat; i++) {
616                 printf("  SatID %3u : Azimuth %3d Elevation %3d\n",
617                        buf[p], get_sirf_2s(buf+p+1), get_sirf_2s(buf+p+3));
618                 p += 5;
619         }
620
621         return 0;
622 }
623
624 int decode_sirf_msg_27(unsigned char *buf, int n)
625 {
626         int i, p = 0;
627
628         printf("DGPS Status Format\n");
629         p += 1;
630         printf("  DGPS source = %d\n", buf[p]);
631         p += 1;
632         for (i=0; i<12; i++) {
633                 printf("  SatID %3u: Age: %u s, DGPS correction : %d m\n",
634                        buf[p + 14 + 3 * i], buf[p + i],
635                        get_sirf_2s(buf + p + 14 + 3 * i + 1));
636         }
637
638         return 0;
639 }
640
641 int decode_sirf_msg_28(unsigned char *buf, int n)
642 {
643         int p = 0;
644         int satID;
645         double gps_seconds;
646         double pseudorange;
647         double snr_avg = 0.0;
648         int i;
649         int tit;
650         unsigned int tt;
651         double tt_in_seconds;
652         //const int gps_clock = 16369000; /* Hz */
653           const int gps_clock = 16368000; // Hz
654         int snr_was_zero = 0;
655
656         printf("Navigation Library Measurement Data - Channel %d\n", buf[p+1]);
657         p += 2;
658         tt=get_sirf_4u(buf+p);
659         gps_clock_record_tt(tt);
660         tt_in_seconds = gps_clock_convert(tt);
661         /* divided by GPS clock freq */
662         printf("  Time Tag = 0x%08x (%.9f s)\n", tt, tt_in_seconds);
663         if (this_tt != tt)
664                 this_tt = tt;
665         p += 4;
666         printf("  SatID : %3u\n", satID=buf[p]);
667         p += 1;
668         gps_seconds=get_sirf_dbl(buf+p);
669         printf("  GPS SW Time : %.9f s [delta with TT : %.9f s]\n",
670                gps_seconds, gps_seconds - tt_in_seconds);
671         p += 8;
672         printf("  Pseudorange = %.3f m\n", pseudorange = get_sirf_dbl(buf+p));
673         p += 8;
674         printf("  Carrier frequency = %f m/s\n", get_sirf_sgl(buf+p));
675         p += 4;
676         printf("  Carrier phase = %f m\n", get_sirf_dbl(buf+p));
677         p += 8;
678         printf("  Time in track = %u ms\n", tit = get_sirf_2u(buf+p));
679         p += 2;
680         printf("  Sync flags = 0x%x\n", buf[p]);
681         p += 1;
682         for (i=0; i<10; i++) {
683                 int snr;
684
685                 printf("  SNR %3u dB-Hz\n", snr = buf[p+i]);
686                 snr_avg += 0.1 * snr;
687                 if (snr == 0) snr_was_zero = 1;
688         }
689         /* it seems that pseudo range is incorrect (too small) if snr was 0
690            at least once */
691         if (snr_was_zero) snr_avg = 0.0;
692         p += 10;
693         /* ... */
694
695         /* pseudorange is only valid if time in track is > 1s */
696         if (tit > 1000)
697                 rinex_data_add_pseudorange_snr(satID, gps_seconds,
698                                                pseudorange, snr_avg);
699
700         return 0;
701 }
702
703 int decode_sirf_msg_31(unsigned char *buf, int n)
704 {
705         printf("Navigation Library Initialization Data\n");
706         /* TBD */
707
708         return 0;
709 }
710
711 int decode_sirf_msg_41(unsigned char *buf, int n)
712 {
713         int p = 0;
714
715         printf("Geodetic Navigation Data\n");
716         p += 1;
717         p += 2;
718         p += 2;
719         printf("  GPS week : %u\n", get_sirf_2u(buf + p));
720         p += 2;
721         printf("  GPS seconds : %.3f\n", (double)get_sirf_4u(buf + p) * 0.001);
722         p += 4;
723                 
724         printf("  Y-M-D : %04d-%02d-%02d H:M:S : %02d:%02d:%02.3f\n",
725                get_sirf_2u(buf+p), buf[p+2], buf[p+3],
726                buf[p+4], buf[p+5], (double)get_sirf_2u(buf+p+6) * 0.001);
727
728         return 0;
729 }
730
731 int decode_sirf_msg_50(unsigned char *buf, int n)
732 {
733         int p = 0;
734
735         printf("SBAS Parameters\n");
736         p += 1;
737         printf("  SBAS SatID : %3u\n", buf[p]);
738         p += 1;
739         printf("  SBAS Mode : %02x %s%s\n", buf[p],
740                buf[p] == 0 ? "Testing" : "",
741                buf[p] == 1 ? "Integrity" : "");
742         p += 1;
743         printf("  DGPS Timeout : %u s\n", buf[p]);
744         p += 1;
745         printf("  Flag bits : %02x\n", buf[p]);
746         p += 1;
747         
748         return 0;
749 }
750
751 int decode_sirf_msg_225(unsigned char *buf, int n)
752 {
753         int p = 0;
754
755         printf("Statistics Channel\n");
756         p += 1;
757         /* Message Sub ID : we got 0 or 8 ... undocumented */
758         printf("  Message Sub ID : %d\n", buf[p]);
759         p += 1;
760
761         return 0;
762 }
763
764 int decode_sirf_msg_255(unsigned char *buf, int n)
765 {
766         int p = 0, len;
767         char msg[1024];
768
769         printf("Development Data\n");
770         p += 1;
771
772         if (n < sizeof(msg))
773                 len = n;
774         else    len = sizeof(msg) -1;
775         memcpy(msg, buf + p, len);
776         msg[len] = 0;
777
778         printf("  %s\n", msg);
779
780         return 0;
781 }
782
783 /* (buf,n) is a full SiRF message, including start/stop sequence */
784
785 int decode_sirf_msg(unsigned char *buf, int n, int *ack) {
786         int p = 4;
787
788         switch (buf[p]) {
789         case 2: /* Measured Navigation Data Out */
790                 decode_sirf_msg_2(buf + 4, n - 8);
791                 break;
792     
793         case 4: /* Measured Tracker Data Out */
794                 decode_sirf_msg_4(buf + 4, n - 8);
795                 break;
796                 
797         case 6: /* Software Version String */
798                 printf("Software Version String : %s\n", buf+p+1);
799                 /* for GPS USB : GSW3.5.0_3.5.00.00-SDK-3EP2.01 */
800                 p += 1;
801                 break;
802                 
803         case 7: /* Clock Status Data */
804                 decode_sirf_msg_7(buf + 4, n - 8);
805                 break;
806                 
807         case 8: /* 50 BPS Data */
808                 decode_sirf_msg_8(buf + 4, n - 8);
809                 break;
810                 
811         case 9: /* CPU Throughput */
812                 printf("CPU Throughput\n");
813                 break;
814                 
815         case 11: /* Command Acknowledgment */
816                 printf("Command ACK ID : %d\n",
817                        buf[p+1]);
818                 *ack = buf[p+1];
819                 p += 2;
820                 break;
821                 
822         case 12: /* Command Negative Acknowledgment */
823                 printf("Command NACK ID : %d\n",
824                        buf[p+1]);
825                 *ack = buf[p+1];
826                 p += 2;
827                 break;
828
829         case 13: /* Visible List */
830                 decode_sirf_msg_13(buf + 4, n - 8);
831                 break;
832
833         case 27: /* GPS Status Format */
834                 decode_sirf_msg_27(buf + 4, n - 8);
835                 break;
836                 
837         case 28: /* Navigation Library Measurement Data */
838                 decode_sirf_msg_28(buf + 4, n - 8);
839                 break;
840                 
841         case 30: /* Navigation Library SV State Data */
842                 printf("Navigation Library SV State Data - SatID : %3u\n", buf[p+1]);
843                 p += 2;
844                 printf("  GPS Time : %f s\n", get_sirf_dbl(buf+p));
845                 p += 8;
846                 printf("  Position X : %f m\n", get_sirf_dbl(buf+p));
847                 p += 8;
848                 printf("  Position Y : %f m\n", get_sirf_dbl(buf+p));
849                 p += 8;
850                 printf("  Position Z : %f m\n", get_sirf_dbl(buf+p));
851                 p += 8;
852                 printf("  Velocity X : %f m/s\n", get_sirf_dbl(buf+p));
853                 p += 8;
854                 printf("  Velocity Y : %f m/s\n", get_sirf_dbl(buf+p));
855                 p += 8;
856                 printf("  Velocity Z : %f m/s\n", get_sirf_dbl(buf+p));
857                 p += 8;
858                 printf("  Clock bias : %f s\n", get_sirf_dbl(buf+p));
859                 p += 8;
860                 printf("  Clock drift : %f s/s\n", get_sirf_sgl(buf+p));
861                 p += 4;
862                 p += 1;
863                 p += 4;
864                 p += 4;
865                 printf("  Ionospheric delay : %f m\n", get_sirf_sgl(buf+p));
866                 p += 4;
867                 break;
868
869         case 31:
870                 decode_sirf_msg_31(buf + 4, n - 8);
871                 break;
872                 
873         case 41: /* Geodetic Navigation Data */
874                 decode_sirf_msg_41(buf + 4, n - 8);
875                 break;
876
877         case 50:
878                 decode_sirf_msg_50(buf + 4, n - 8);
879                 break;
880
881         case 225:
882                 decode_sirf_msg_225(buf + 4, n - 8);
883                 break;
884                 
885         case 255: /* Development Data */
886                 decode_sirf_msg_255(buf + 4, n - 8);
887                 break;
888
889         default:
890                 printf("Message ID %d is not decoded yet ...\n", buf[p]);
891                 break;
892         }
893
894         fflush(stdout);
895         return 0;
896 }
897
898 void dump_msg(unsigned char *buf, int n) {
899         int i;
900         
901         printf("%d bytes message :", n);
902         for (i=0; i<n; i++) {
903                 printf(" 0x%02x", buf[i]);
904         }
905         printf("\n");
906 }
907
908 /* return 0 on success, -1 on error */
909
910 int do_read(int fd, int * ack) {
911         /* SiRF message size is limited to 912 bytes */
912         int i, n;
913
914         static unsigned char sbuffer[912*2];
915         static int p = 0;
916
917         if ((sizeof(sbuffer) - p) == 0) {
918                 printf("buffer full -> empty\n");
919                 p = 0;
920                 return -1;
921         }
922
923         n = read(fd, sbuffer + p, sizeof(sbuffer) - p);
924         if (n <= 0) {
925                 perror("read");
926                 return -1;
927         }
928
929         if (n == 0) {
930                 /* nothing to do */
931                 return 0;
932         }
933
934         /* for debug only :
935         printf("%d bytes received (total = %d):", n, p+n);
936         for (i=0; i<n; i++) {
937                 printf(" 0x%02x", sbuffer[p+i]);
938         }
939         printf("\n");
940         */
941
942         /* update p with the bytes just received */
943         p += n;
944
945         /* small parsing of received data in (sbuffer, p) : 
946            - start pattern = 0xa0 0xa2
947            - stop  pattern = 0xb0 0xb3
948
949            SiRF message format is :
950            <0xa0 0xa2><length (2 bytes)><length bytes><CRC (2 byte)><0xb0 0xb3>
951         */
952
953         for (;;) {
954                 int start_found = 0, stop_found = 0;
955
956                 /* search for start pattern */
957                 for (i=0; i<p-1; i++) {
958                         if ((sbuffer[i] == 0xa0) && (sbuffer[i+1] == 0xa2)) {
959                                 start_found = 1;
960
961                                 if (i>0) {
962                                         printf("recv: %d bytes skipped\n", i);
963                                         memmove(sbuffer, sbuffer+i, p-i);
964                                         p -= i;
965                                 }
966                                 break;
967                         }
968                 }
969
970                 /* search for stop pattern */
971                 for (i=0; i<p-1; i++) {
972                         if ((sbuffer[i] == 0xb0) && (sbuffer[i+1] == 0xb3)) {
973                                 stop_found = 1;
974           
975                                 if (start_found && stop_found) {
976                                         printf("recv: Message ID %d (%d bytes)\n",
977                                                sbuffer[4], i+2); 
978                                         if (check_sirf_msg(sbuffer, i+2)) {
979                                                 decode_sirf_msg(sbuffer, i+2,
980                                                                 ack);
981                                         }
982                                 }
983           
984                                 if (!start_found)
985                                         printf("recv: %d bytes skipped\n",i+2);
986                                 memmove(sbuffer, sbuffer+(i+2), p-(i+2));
987                                 p -= (i+2);
988
989                                 break;
990                         }
991                 }
992
993                 if (!stop_found)
994                         break;
995         }
996         
997         return 0;
998 }
999
1000
1001 /* 
1002  * (buf,n) is the payload of the message. This function adds the start/stop
1003  * sequences, and the length/checksum fields. Returns 0 on success, -1 on
1004  * error.
1005  */
1006
1007 int sirf_msg_send(int fd, unsigned char *buf, int n) {
1008         unsigned char sbuf[1024];
1009         int i;
1010         unsigned int crc;
1011         int total_len = 4 + n + 4; 
1012         int cmd = buf[0];
1013
1014         if (total_len > sizeof(sbuf)) {
1015                 printf("%s: message too large\n", __func__);
1016                 return -1;
1017         }
1018
1019         /* 0xa0, 0xa2 is the start sequence */
1020         sbuf[0] = 0xa0;
1021         sbuf[1] = 0xa2;
1022         sbuf[2] = n>>8;
1023         sbuf[3] = n&0xff;
1024         memcpy(sbuf+4, buf, n);
1025
1026         /* compute checksum on the payload */
1027         crc = 0;
1028         for (i=0; i<n; i++)
1029                 crc += buf[i];
1030         crc &= 0x7fff;
1031         
1032         sbuf[4+n+0] = (crc & 0xff00)>>8;
1033         sbuf[4+n+1] = (crc & 0x00ff)>>0;
1034         /* 0xb0, 0xb3 is the end sequence */
1035         sbuf[4+n+2] = 0xb0;
1036         sbuf[4+n+3] = 0xb3;
1037
1038         if ((n=write(fd, sbuf, total_len)) != total_len) {
1039                 printf("%s: written only %d bytes out of %d\n", __func__,
1040                        n, total_len);
1041                 return -1;
1042         }
1043
1044         printf("send: Message ID %d (%d bytes)\n", buf[0], total_len);
1045
1046         /* wait for ACK */
1047         for (;;) {
1048                 int ack = 0;
1049                 if (do_read(fd, &ack) < 0) {
1050                         printf("do_read failed!\n");
1051                         return -1;
1052                 }
1053                 if (ack == cmd) {
1054                         printf("got ACK for cmd %d\n", ack);
1055                         break;
1056                 }
1057         }
1058
1059         return 0;
1060 }
1061
1062 void usage(const char *argv0) {
1063         printf("usage: %s [-rinex rinex.obs]\n"
1064                "  -rinex rinex.obs : record a RINEX observation file\n",
1065                argv0);
1066         exit (-1);
1067 }
1068
1069 int main(int argc, const char *argv[])
1070 {
1071         const char device[] = "/dev/ttyUSB0";
1072         int fd;
1073         struct termios term;
1074         int i;
1075
1076         for (i=1 ; i<argc; i++) {
1077                 if (strcmp(argv[i],"-rinex")==0 && i+1<argc) {
1078                         g_rinex = argv[++i];
1079                         rinex_open(g_rinex);
1080                         rinex_header_write();
1081                 } else
1082                         usage(argv[0]);
1083         }
1084
1085         /* open serial device */
1086         fd = open(device, O_RDWR /*| O_NONBLOCK*/);
1087         if (fd < 0) {
1088                 perror(device);
1089                 return -1;
1090         }
1091
1092         /* switch to 4800 bauds */
1093         if (tcgetattr(fd, &term) != 0) {
1094                 perror("tcgetattr");
1095         }
1096         cfmakeraw(&term);
1097         cfsetispeed(&term, B115200);
1098         cfsetospeed(&term, B115200);
1099         /* 8N1 */
1100         term.c_cflag &= ~(CSIZE|PARENB|CSTOPB);
1101         term.c_cflag |=   CS8;
1102         if (tcsetattr(fd, TCSANOW, &term) != 0) {
1103                 perror("tcsetattr");
1104         }
1105
1106         /* switch from NMEA to SiRF binary format */
1107
1108         const char to_sirf[] = "$PSRF100,0,9600,8,1,0*0C\n";
1109         /* -1 : we do not want to send the null terminating character */
1110         //write(fd, to_sirf, sizeof(to_sirf)-1);
1111
1112         /* Set Binary Serial Port.  Note : the effect of changing the baud
1113            rate is not immediate at all */
1114
1115         /* Tested value : 
1116              1200         1200 bit/s
1117              2400         2400 bit/s
1118              4800         4800 bit/s
1119              9600         9600 bit/s
1120             19200        19200 bit/s
1121             38400        38400 bit/s
1122             57600        57600 bit/s
1123            115200       115200 bit/s
1124         */
1125
1126         unsigned int baud = 115200;
1127         unsigned char msg_134[] = { 134, 
1128                                     (baud >> 24) & 0xff,
1129                                     (baud >> 16) & 0xff,
1130                                     (baud >>  8) & 0xff,
1131                                     (baud >>  0) & 0xff,
1132                                     8, 1, 0, 0 };
1133         sirf_msg_send(fd, msg_134, sizeof(msg_134));
1134         /*
1135         if (tcgetattr(fd, &term) != 0) {
1136                 perror("tcgetattr");
1137         }
1138         cfsetispeed(&term, B1200);
1139         cfsetospeed(&term, B1200);
1140         if (tcsetattr(fd, TCSANOW, &term) != 0) {
1141                 perror("tcsetattr");
1142         }
1143         */
1144
1145         /* Poll Software Version */
1146         unsigned char msg_132[] = { 132, 0x00 };
1147         sirf_msg_send(fd, msg_132, sizeof(msg_132));
1148
1149         /* Initialize GPS/DR Navigation */
1150         /*
1151         unsigned char msg_172[] = { 0xac, 0x01, 
1152                                     0x00, 0x00, 0x00, 0x00,
1153                                     0x00, 0x00, 0x00, 0x00,
1154                                     0x00, 0x00, 0x00, 0x00,
1155                                     0x00, 0x00,
1156                                     0x00, 0x00, 0x00, 0x00,
1157                                     0x00, 0x00, 0x00, 0x00,
1158                                     0x00, 0x00,
1159                                     0x00,
1160                                     (1<<4)|(1<<5)};
1161         sirf_msg_send(fd, msg_172, sizeof(msg_172));
1162         */
1163
1164         /* Initialize Data Source */
1165         unsigned char msg_128[] = { 128,
1166                                     0x00, 0x00, 0x00, 0x00,
1167                                     0x00, 0x00, 0x00, 0x00,
1168                                     0x00, 0x00, 0x00, 0x00,
1169                                     0x00, 0x00, 0x00, 0x00,
1170                                     0x00, 0x00, 0x00, 0x00,
1171                                     0x00, 0x00,
1172                                     12,
1173                                     /* Clear memory */
1174                                     /* (1<<2) | */
1175                                     /* Enable raw track & debug data */
1176                                     (1<<4) | (1<<5) };
1177         sirf_msg_send(fd, msg_128, sizeof(msg_128));
1178
1179         /* Enable MID 29 */
1180         unsigned char msg_166_29[] = { 166, 0, 29, 1, 0, 0, 0, 0 };
1181         sirf_msg_send(fd, msg_166_29, sizeof(msg_166_29));
1182
1183         /* Enable MID 52 : we got a NACK since this feature is not available */
1184         unsigned char msg_166[] = { 166,  0, 52, 1, 0, 0, 0, 0 };
1185         sirf_msg_send(fd, msg_166, sizeof(msg_166));
1186
1187         /* Try to enable all messages. We got a ACK for :
1188              0, Undocumented (never received)
1189              2, Measure Navigation Data Out
1190                 => X,Y,Z,dX,dY,dZ,T, HDOP, sat used
1191                 used by gpsd to report longitude, latitude
1192              4, Measured Tracker Data Out
1193                 => T, SV ID, Azimuth, Elevation, SNR
1194              7, Clock Status Data
1195              8, 50 BPS Data
1196              9, CPU Throughput
1197             13, Visible List
1198                 => SV ID, Azimuth, Elevation
1199             18, OkToSend (never received)
1200             27, DGPS Status Format
1201             28, Navigation Library Measurement Data
1202                 => SV ID, T, Pseudorange, Carrier freq+phase
1203             29, Navigation Library DGPS Data (never received)
1204             30, Navigation Library SV State Data
1205                 => SV ID, T, X,Y,Z,dX,dY,dZ, Clock bias+drift, IONO
1206             31, Navigation Library Initialization Data
1207             41, Geodetic Navigation Data
1208             46, Test Mode 3/4/5/6 (never received)
1209             50, SBAS Parameters
1210             55, Test Mode 4 Track Data (never received)
1211            128, Undocumented (never received)
1212            255, Development Data
1213          */
1214         /*
1215         for (i=0; i<256; i++) {
1216                 printf("Trying to enable message %d\n", i);
1217                 msg_166[2] = i;
1218                 sirf_msg_send(fd, msg_166, sizeof(msg_166));
1219         }
1220         */
1221         for (;;) {
1222                 fd_set fdr;
1223                 int r;
1224
1225                 FD_ZERO(&fdr);
1226                 FD_SET(fd, &fdr);
1227
1228                 r = select(FD_SETSIZE, &fdr, NULL, NULL, NULL);
1229                 if (r < 0) {
1230                         perror("select");
1231                         break;
1232                 }
1233
1234                 if (FD_ISSET(fd, &fdr)) {
1235                         int ack = 0;
1236                         if (do_read(fd, &ack) < 0)
1237                                 break;
1238                         if (ack != 0) {
1239                                 printf("ACK for cmd %d\n", ack);
1240                         }
1241                 }
1242         }
1243
1244         close (fd);
1245         return 0;
1246 }