Program to record raw pseudorange and clock
[gps] / sirf3.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 FILE * file = stdout;
18
19 /* return 1 if message is a valid SiRF message, 0 if invalid */
20
21 int check_sirf_msg(unsigned char *buf, int n) {
22         int length, crc, computed_crc, i;
23
24         /* check size (at least 8 bytes) */
25         if (n<8) {
26                 printf("msg too small (%d bytes)\n", n);
27                 return 0;
28         }
29
30         /* check start pattern */
31         if ((buf[0] != 0xa0) || (buf[1] != 0xa2)) {
32                 printf("invalid start pattern : 0x%02x 0x%02x\n",
33                        buf[0], buf[1]);
34                 return 0;
35         }
36   
37         /* check length */
38         length = ((buf[2]<<8) | (buf[3]));
39         if (length & 0x8000) {
40                 printf("invalid length : 0x%04x bytes\n", length);
41                 return 0;
42         }
43
44         if (length > 912) {
45                 printf("warning : length (%d bytes) is above 912\n", length);
46         }
47
48         if (4 + length + 4 != n) {
49                 printf("invalid length : %d bytes, buf buffer is %d bytes\n",
50                        length, n);
51                 return 0;
52         }
53   
54         /* check checksum */
55         crc = ((buf[n-4]<<8) | (buf[n-3]));
56         if (crc & 0x8000) {
57                 printf("invalid crc : 0x%04x\n", crc);
58                 return 0;
59         }
60
61         computed_crc = 0;
62         for (i=4; i<n-4; i++) {
63                 computed_crc += buf[i];
64         }
65         computed_crc &= 0x7fff;
66
67         if (computed_crc != crc) {
68                 printf("invalid crc : 0x%04x computed crc : 0x%04x\n",
69                        crc, computed_crc);
70                 return 0;
71         }
72   
73         /* check stop pattern */
74         if ((buf[n-2] != 0xb0) || (buf[n-1] != 0xb3)) {
75                 printf("invalid stop pattern : 0x%02x 0x%02x\n",
76                        buf[n-2], buf[n-1]);
77                 return 0;
78         }
79
80         return 1;
81 }
82
83 double get_sirf_dbl(unsigned char *buf) {
84   double r;
85   unsigned char * ptr = (unsigned char *)&r;
86
87   ptr[0] = buf[3];
88   ptr[1] = buf[2];
89   ptr[2] = buf[1];
90   ptr[3] = buf[0];
91   ptr[4] = buf[7];
92   ptr[5] = buf[6];
93   ptr[6] = buf[5];
94   ptr[7] = buf[4];
95
96   if (sizeof(double) == 8) {
97     return r;
98   } else {
99     printf("get_sirf_dbl implementation error\n");
100   }
101 }
102
103 int get_sirf_2s(unsigned char *buf) {
104         return (((signed char)buf[0])<<8) | buf[1];
105 }
106
107 int get_sirf_4s(unsigned char *buf) {
108         return (((signed char)buf[0])<<24)|(buf[1]<<16)|(buf[2]<<8)|buf[3];
109 }
110
111 unsigned int get_sirf_2u(unsigned char *buf) {
112         return (buf[0]<<8) | buf[1];
113 }
114
115 unsigned int get_sirf_4u(unsigned char *buf) {
116         return (buf[0]<<24) | (buf[1]<<16) | (buf[2]<<8) | buf[3];
117 }
118
119 float get_sirf_sgl(unsigned char *buf) {
120   float r;
121   unsigned char *ptr = (unsigned char *)&r;
122
123   ptr[0] = buf[3];
124   ptr[1] = buf[2];
125   ptr[2] = buf[1];
126   ptr[3] = buf[0];
127
128   if (sizeof(float) == 4) {
129     return r;
130   } else {
131     printf("get_sirf_sgl implementation error\n");
132   }
133 }
134
135 int decode_sirf_msg_2(unsigned char * buf, int n)
136 {
137         int i, p = 0, n_sat;
138         unsigned char mode;
139         const char * altmode = "";
140         const char * pmode = "";
141
142         printf("Measure Navigation Data Out\n");
143         p += 1;
144         printf("  X-position : %d m\n", get_sirf_4s(buf+p));
145         p += 4;
146         printf("  Y-position : %d m\n", get_sirf_4s(buf+p));
147         p += 4;
148         printf("  Z-position : %d m\n", get_sirf_4s(buf+p));
149         p += 4;
150         printf("  X-velocity : %f m/s\n", (double)get_sirf_2s(buf+p) / 8.0);
151         p += 2;
152         printf("  Y-velocity : %f m/s\n", (double)get_sirf_2s(buf+p) / 8.0);
153         p += 2;
154         printf("  Z-velocity : %f m/s\n", (double)get_sirf_2s(buf+p) / 8.0);
155         p += 2;
156         /* Mode 1 */
157         mode = buf[p];
158
159         switch ((mode >> 4) & 3) {
160         case 0:
161                 altmode = "No altitude hold applied"; break;
162         case 1 :
163                 altmode = "Holding of altitude from KF"; break;
164         case 2:
165                 altmode = "Holding of altitude from user input"; break;
166         case 3:
167                 altmode = "Always hold altitude"; break;
168         }
169
170         switch ( mode & 7) {
171         case 0:
172                 pmode = "No navigation solution"; break;
173         case 1:
174                 pmode = "1-SV solution (Kalman filter)"; break;
175         case 2:
176                 pmode = "2-SV solution (Kalman filter)"; break;
177         case 3:
178                 pmode = "3-SV solution (Kalman filter)"; break;
179         case 4:
180                 pmode = ">3S-SV solution (Kalman filter)"; break;
181         case 5:
182                 pmode = "2D solution (least squares)"; break;
183         case 6:
184                 pmode = "3D solution (least squares)"; break;
185         case 7:
186                 pmode = "Dead-Reckoning solution (no satellites)"; break;
187         }
188
189         printf("  Mode 1 : %02x %s %s %s %s %s\n", 
190                mode ,
191                mode & (1<<7) ? "DGPS:Yes" : "DGPS:No",
192                mode & (1<<6) ? "DOP mask exceeded " : "",
193                altmode, 
194                mode & (1<<3) ? "TicklePower position " : "",
195                pmode);
196         p += 1;
197         /* HDOP */
198         printf("  HDOP : %f\n", (double)(unsigned)buf[p] / 5.0);
199         p += 1;
200         /* Mode 2 */
201         mode = buf[p];
202         printf("  Mode 2 : %02x\n", mode);
203         p += 1;
204         printf("  GPS Week : %u\n", get_sirf_2u(buf+p));
205         p += 2;
206         printf("  GPS Time of Week : %f\n", (double)get_sirf_4u(buf+p)/100.0);
207         p += 4;
208         printf("  Satellites used : %d\n", n_sat = buf[p]);
209         p += 1;
210         for (i=0; (i<12) & (i<n_sat); i++) {
211                         printf("  Channel %2d SatID: %3u\n", i+1, buf[p+i]);
212         }
213
214         return 0;
215 }
216
217 int decode_sirf_msg_4(unsigned char *buf, int n)
218 {
219         int i, p = 0;
220
221         printf("Measured Tracker Data Out\n");
222         p += 1;
223         printf("  GPS Week : %d\n", get_sirf_2s(buf + p));
224         p += 2;
225         printf("  GPS Time of Week : %.3f\n",
226                (double)get_sirf_4u(buf + p) / 100.0);
227         p += 4;
228         printf("  Channels : %d\n", buf[p]);
229         p += 1;
230         for (;p<n;) {
231                 printf("  SatID: %3u Azimuth: %f Elevation: %f\n",
232                        buf[p], (double)buf[p+1]/2.0*3.0, (double)buf[p+2]/2.0);
233                 p += 3;
234                 printf("    State : %02x\n", get_sirf_2u(buf + p));
235                 p += 2;
236                 for (i=0; i<10; i++) {
237                         printf("    SNR %2d: %d dB\n", i+1, buf[p+i]);
238                 }
239                 p+= 10;
240         }
241
242         return 0;
243 }
244
245 int decode_sirf_msg_7(unsigned char *buf, int n)
246 {
247         int p = 0;
248         int gps_week;
249         double gps_second;
250         double bias;
251
252         printf("Clock Status Data\n");
253         p += 1;
254         printf("  Extended GPS Week : %u\n",
255                gps_week = get_sirf_2u(buf + p));
256         p += 2;
257         printf("  GPS Time of Week : %f s\n",
258                gps_second = (double)get_sirf_4u(buf + p) / 100.0);
259         p += 4;
260         printf("  Satellites used : %u\n", buf[p]);
261         p += 1;
262         printf("  Clock Drift : %u Hz\n", get_sirf_4u(buf + p));
263         p += 4;
264         bias = (double)get_sirf_4u(buf + p) * 1e-9;
265         printf("  Clock Bias : %.9f ns\n", bias);
266         p += 4;
267         printf("  Estimated GPS Time : %u ms\n", get_sirf_4u(buf + p));
268         p += 4;
269
270         /* record : 7 <GPS week> <GPS second> <bias> */
271         fprintf(file, "7 %u %.12f %.12f\n",
272                 gps_week, gps_second, bias);
273
274         return 0;
275 }
276
277 int decode_sirf_msg_8(unsigned char *buf, int n)
278 {
279         int i, p = 0;
280
281         printf("50 BPS Data\n");
282         p += 1;
283         printf("  Channel %d, SV# %d\n", buf[p], buf[p+1]);
284         p += 2;
285         for (i=0; i<10; i++) {
286                 printf("  Word %d : %08x\n", i, get_sirf_4u(buf+p));
287                 p += 4;
288         }
289
290         return 0;
291 }
292
293 int decode_sirf_msg_13(unsigned char *buf, int n)
294 {
295         int p = 0, i, n_sat;
296
297         printf("Visible List\n");
298         p += 1;
299         n_sat = buf[p];
300         p += 1;
301         for (i=0; i<n_sat; i++) {
302                 printf("  SatID %3u : Azimuth %3d Elevation %3d\n",
303                        buf[p], get_sirf_2s(buf+p+1), get_sirf_2s(buf+p+3));
304                 p += 5;
305         }
306
307         return 0;
308 }
309
310 int decode_sirf_msg_27(unsigned char *buf, int n)
311 {
312         int i, p = 0;
313
314         printf("DGPS Status Format\n");
315         p += 1;
316         printf("  DGPS source = %d\n", buf[p]);
317         p += 1;
318         for (i=0; i<12; i++) {
319                 printf("  SatID %3u: Age: %u s, DGPS correction : %d m\n",
320                        buf[p + 14 + 3 * i], buf[p + i],
321                        get_sirf_2s(buf + p + 14 + 3 * i + 1));
322         }
323
324         return 0;
325 }
326
327 int decode_sirf_msg_28(unsigned char *buf, int n)
328 {
329         int p = 0;
330         int satID;
331         double gps_second;
332         double pseudorange;
333         double snr_avg = 0.0;
334         int i;
335         int tit;
336         unsigned int tt;
337         double tt_in_seconds;
338         const int gps_clock = 16368000; // Hz
339         int snr_was_zero = 0;
340
341         printf("Navigation Library Measurement Data - Channel %d\n", buf[p+1]);
342         p += 2;
343         tt=get_sirf_4u(buf+p);
344         tt_in_seconds = (double)tt / (double)gps_clock;
345         /* divided by GPS clock freq */
346         printf("  Time Tag = 0x%08x (%.9f s)\n", tt, tt_in_seconds);
347         p += 4;
348         printf("  SatID : %3u\n", satID=buf[p]);
349         p += 1;
350         gps_second=get_sirf_dbl(buf+p);
351         printf("  GPS SW Time : %.9f s [delta with TT : %.9f s]\n",
352                gps_second, gps_second - tt_in_seconds);
353         p += 8;
354         printf("  Pseudorange = %.3f m\n", pseudorange = get_sirf_dbl(buf+p));
355         p += 8;
356         printf("  Carrier frequency = %f m/s\n", get_sirf_sgl(buf+p));
357         p += 4;
358         printf("  Carrier phase = %f m\n", get_sirf_dbl(buf+p));
359         p += 8;
360         printf("  Time in track = %u ms\n", tit = get_sirf_2u(buf+p));
361         p += 2;
362         printf("  Sync flags = 0x%x\n", buf[p]);
363         p += 1;
364         for (i=0; i<10; i++) {
365                 int snr;
366
367                 printf("  SNR %3u dB-Hz\n", snr = buf[p+i]);
368                 snr_avg += 0.1 * snr;
369                 if (snr == 0) snr_was_zero = 1;
370         }
371         /* it seems that pseudo range is incorrect (too small) if snr was 0
372            at least once */
373         if (snr_was_zero) snr_avg = 0.0;
374         p += 10;
375
376         /* write RAW record : 28 <tt> <GPS second> <pseudorange> <tit> <snr> */
377         fprintf(file,
378                 "28 %u %.12f %.6f %.3f %.3f\n",
379                 tt, gps_second, pseudorange, (double)tit * 1e-3, snr_avg);
380
381         return 0;
382 }
383
384 int decode_sirf_msg_31(unsigned char *buf, int n)
385 {
386         printf("Navigation Library Initialization Data\n");
387         /* TBD */
388
389         return 0;
390 }
391
392 int decode_sirf_msg_41(unsigned char *buf, int n)
393 {
394         int p = 0;
395
396         printf("Geodetic Navigation Data\n");
397         p += 1;
398         p += 2;
399         p += 2;
400         printf("  GPS week : %u\n", get_sirf_2u(buf + p));
401         p += 2;
402         printf("  GPS seconds : %.3f\n", (double)get_sirf_4u(buf + p) * 0.001);
403         p += 4;
404                 
405         printf("  Y-M-D : %04d-%02d-%02d H:M:S : %02d:%02d:%02.3f\n",
406                get_sirf_2u(buf+p), buf[p+2], buf[p+3],
407                buf[p+4], buf[p+5], (double)get_sirf_2u(buf+p+6) * 0.001);
408
409         return 0;
410 }
411
412 int decode_sirf_msg_50(unsigned char *buf, int n)
413 {
414         int p = 0;
415
416         printf("SBAS Parameters\n");
417         p += 1;
418         printf("  SBAS SatID : %3u\n", buf[p]);
419         p += 1;
420         printf("  SBAS Mode : %02x %s%s\n", buf[p],
421                buf[p] == 0 ? "Testing" : "",
422                buf[p] == 1 ? "Integrity" : "");
423         p += 1;
424         printf("  DGPS Timeout : %u s\n", buf[p]);
425         p += 1;
426         printf("  Flag bits : %02x\n", buf[p]);
427         p += 1;
428         
429         return 0;
430 }
431
432 int decode_sirf_msg_225(unsigned char *buf, int n)
433 {
434         int p = 0;
435
436         printf("Statistics Channel\n");
437         p += 1;
438         /* Message Sub ID : we got 0 or 8 ... undocumented */
439         printf("  Message Sub ID : %d\n", buf[p]);
440         p += 1;
441
442         return 0;
443 }
444
445 int decode_sirf_msg_255(unsigned char *buf, int n)
446 {
447         int p = 0, len;
448         char msg[1024];
449
450         printf("Development Data\n");
451         p += 1;
452
453         if (n < sizeof(msg))
454                 len = n;
455         else    len = sizeof(msg) -1;
456         memcpy(msg, buf + p, len);
457         msg[len] = 0;
458
459         printf("  %s\n", msg);
460
461         return 0;
462 }
463
464 /* (buf,n) is a full SiRF message, including start/stop sequence */
465
466 int decode_sirf_msg(unsigned char *buf, int n, int *ack) {
467         int p = 4;
468
469         switch (buf[p]) {
470         case 2: /* Measured Navigation Data Out */
471                 decode_sirf_msg_2(buf + 4, n - 8);
472                 break;
473     
474         case 4: /* Measured Tracker Data Out */
475                 decode_sirf_msg_4(buf + 4, n - 8);
476                 break;
477                 
478         case 6: /* Software Version String */
479                 printf("Software Version String : %s\n", buf+p+1);
480                 /* for GPS USB : GSW3.5.0_3.5.00.00-SDK-3EP2.01 */
481                 p += 1;
482                 break;
483                 
484         case 7: /* Clock Status Data */
485                 decode_sirf_msg_7(buf + 4, n - 8);
486                 break;
487                 
488         case 8: /* 50 BPS Data */
489                 decode_sirf_msg_8(buf + 4, n - 8);
490                 break;
491                 
492         case 9: /* CPU Throughput */
493                 printf("CPU Throughput\n");
494                 break;
495                 
496         case 11: /* Command Acknowledgment */
497                 printf("Command ACK ID : %d\n",
498                        buf[p+1]);
499                 *ack = buf[p+1];
500                 p += 2;
501                 break;
502                 
503         case 12: /* Command Negative Acknowledgment */
504                 printf("Command NACK ID : %d\n",
505                        buf[p+1]);
506                 *ack = buf[p+1];
507                 p += 2;
508                 break;
509
510         case 13: /* Visible List */
511                 decode_sirf_msg_13(buf + 4, n - 8);
512                 break;
513
514         case 27: /* GPS Status Format */
515                 decode_sirf_msg_27(buf + 4, n - 8);
516                 break;
517                 
518         case 28: /* Navigation Library Measurement Data */
519                 decode_sirf_msg_28(buf + 4, n - 8);
520                 break;
521                 
522         case 30: /* Navigation Library SV State Data */
523                 printf("Navigation Library SV State Data - SatID : %3u\n", buf[p+1]);
524                 p += 2;
525                 printf("  GPS Time : %f s\n", get_sirf_dbl(buf+p));
526                 p += 8;
527                 printf("  Position X : %f m\n", get_sirf_dbl(buf+p));
528                 p += 8;
529                 printf("  Position Y : %f m\n", get_sirf_dbl(buf+p));
530                 p += 8;
531                 printf("  Position Z : %f m\n", get_sirf_dbl(buf+p));
532                 p += 8;
533                 printf("  Velocity X : %f m/s\n", get_sirf_dbl(buf+p));
534                 p += 8;
535                 printf("  Velocity Y : %f m/s\n", get_sirf_dbl(buf+p));
536                 p += 8;
537                 printf("  Velocity Z : %f m/s\n", get_sirf_dbl(buf+p));
538                 p += 8;
539                 printf("  Clock bias : %f s\n", get_sirf_dbl(buf+p));
540                 p += 8;
541                 printf("  Clock drift : %f s/s\n", get_sirf_sgl(buf+p));
542                 p += 4;
543                 p += 1;
544                 p += 4;
545                 p += 4;
546                 printf("  Ionospheric delay : %f m\n", get_sirf_sgl(buf+p));
547                 p += 4;
548                 break;
549
550         case 31:
551                 decode_sirf_msg_31(buf + 4, n - 8);
552                 break;
553                 
554         case 41: /* Geodetic Navigation Data */
555                 decode_sirf_msg_41(buf + 4, n - 8);
556                 break;
557
558         case 50:
559                 decode_sirf_msg_50(buf + 4, n - 8);
560                 break;
561
562         case 225:
563                 decode_sirf_msg_225(buf + 4, n - 8);
564                 break;
565                 
566         case 255: /* Development Data */
567                 decode_sirf_msg_255(buf + 4, n - 8);
568                 break;
569
570         default:
571                 printf("Message ID %d is not decoded yet ...\n", buf[p]);
572                 break;
573         }
574
575         fflush(stdout);
576         return 0;
577 }
578
579 void dump_msg(unsigned char *buf, int n) {
580         int i;
581         
582         printf("%d bytes message :", n);
583         for (i=0; i<n; i++) {
584                 printf(" 0x%02x", buf[i]);
585         }
586         printf("\n");
587 }
588
589 /* return 0 on success, -1 on error */
590
591 int do_read(int fd, int * ack) {
592         /* SiRF message size is limited to 912 bytes */
593         int i, n;
594
595         static unsigned char sbuffer[912*2];
596         static int p = 0;
597
598         if ((sizeof(sbuffer) - p) == 0) {
599                 printf("buffer full -> empty\n");
600                 p = 0;
601                 return -1;
602         }
603
604         n = read(fd, sbuffer + p, sizeof(sbuffer) - p);
605         if (n <= 0) {
606                 perror("read");
607                 return -1;
608         }
609
610         if (n == 0) {
611                 /* nothing to do */
612                 return 0;
613         }
614
615         /* for debug only :
616         printf("%d bytes received (total = %d):", n, p+n);
617         for (i=0; i<n; i++) {
618                 printf(" 0x%02x", sbuffer[p+i]);
619         }
620         printf("\n");
621         */
622
623         /* update p with the bytes just received */
624         p += n;
625
626         /* small parsing of received data in (sbuffer, p) : 
627            - start pattern = 0xa0 0xa2
628            - stop  pattern = 0xb0 0xb3
629
630            SiRF message format is :
631            <0xa0 0xa2><length (2 bytes)><length bytes><CRC (2 byte)><0xb0 0xb3>
632         */
633
634         for (;;) {
635                 int start_found = 0, stop_found = 0;
636
637                 /* search for start pattern */
638                 for (i=0; i<p-1; i++) {
639                         if ((sbuffer[i] == 0xa0) && (sbuffer[i+1] == 0xa2)) {
640                                 start_found = 1;
641
642                                 if (i>0) {
643                                         printf("recv: %d bytes skipped\n", i);
644                                         memmove(sbuffer, sbuffer+i, p-i);
645                                         p -= i;
646                                 }
647                                 break;
648                         }
649                 }
650
651                 /* search for stop pattern */
652                 for (i=0; i<p-1; i++) {
653                         if ((sbuffer[i] == 0xb0) && (sbuffer[i+1] == 0xb3)) {
654                                 stop_found = 1;
655           
656                                 if (start_found && stop_found) {
657                                         printf("recv: Message ID %d (%d bytes)\n",
658                                                sbuffer[4], i+2); 
659                                         if (check_sirf_msg(sbuffer, i+2)) {
660                                                 decode_sirf_msg(sbuffer, i+2,
661                                                                 ack);
662                                         }
663                                 }
664           
665                                 if (!start_found)
666                                         printf("recv: %d bytes skipped\n",i+2);
667                                 memmove(sbuffer, sbuffer+(i+2), p-(i+2));
668                                 p -= (i+2);
669
670                                 break;
671                         }
672                 }
673
674                 if (!stop_found)
675                         break;
676         }
677         
678         return 0;
679 }
680
681
682 /* 
683  * (buf,n) is the payload of the message. This function adds the start/stop
684  * sequences, and the length/checksum fields. Returns 0 on success, -1 on
685  * error.
686  */
687
688 int sirf_msg_send(int fd, unsigned char *buf, int n) {
689         unsigned char sbuf[1024];
690         int i;
691         unsigned int crc;
692         int total_len = 4 + n + 4; 
693         int cmd = buf[0];
694
695         if (total_len > sizeof(sbuf)) {
696                 printf("%s: message too large\n", __func__);
697                 return -1;
698         }
699
700         /* 0xa0, 0xa2 is the start sequence */
701         sbuf[0] = 0xa0;
702         sbuf[1] = 0xa2;
703         sbuf[2] = n>>8;
704         sbuf[3] = n&0xff;
705         memcpy(sbuf+4, buf, n);
706
707         /* compute checksum on the payload */
708         crc = 0;
709         for (i=0; i<n; i++)
710                 crc += buf[i];
711         crc &= 0x7fff;
712         
713         sbuf[4+n+0] = (crc & 0xff00)>>8;
714         sbuf[4+n+1] = (crc & 0x00ff)>>0;
715         /* 0xb0, 0xb3 is the end sequence */
716         sbuf[4+n+2] = 0xb0;
717         sbuf[4+n+3] = 0xb3;
718
719         if ((n=write(fd, sbuf, total_len)) != total_len) {
720                 printf("%s: written only %d bytes out of %d\n", __func__,
721                        n, total_len);
722                 return -1;
723         }
724
725         printf("send: Message ID %d (%d bytes)\n", buf[0], total_len);
726
727         /* wait for ACK */
728         for (;;) {
729                 int ack = 0;
730                 if (do_read(fd, &ack) < 0) {
731                         printf("do_read failed!\n");
732                         return -1;
733                 }
734                 if (ack == cmd) {
735                         printf("got ACK for cmd %d\n", ack);
736                         break;
737                 }
738         }
739
740         return 0;
741 }
742
743 void usage(const char *argv0) {
744         printf("usage: %s [-raw observation.raw]\n"
745                "  -raw observation.raw : record a RAW observation file\n",
746                argv0);
747         exit (-1);
748 }
749
750 int main(int argc, const char *argv[])
751 {
752         const char device[] = "/dev/ttyUSB0";
753         int fd;
754         struct termios term;
755         int i;
756
757         for (i=1 ; i<argc; i++) {
758                 if (strcmp(argv[i],"-raw")==0 && i+1<argc) {
759                         const char * filename = argv[++i];
760                         file = fopen(filename,"w");
761                         if (file == NULL) {
762                                 perror(filename);
763                                 exit (-1);
764                         }
765                 } else
766                         usage(argv[0]);
767         }
768
769         /* open serial device */
770         fd = open(device, O_RDWR /*| O_NONBLOCK*/);
771         if (fd < 0) {
772                 perror(device);
773                 return -1;
774         }
775
776         /* switch to 4800 bauds */
777         if (tcgetattr(fd, &term) != 0) {
778                 perror("tcgetattr");
779         }
780         cfmakeraw(&term);
781         cfsetispeed(&term, B115200);
782         cfsetospeed(&term, B115200);
783         /* 8N1 */
784         term.c_cflag &= ~(CSIZE|PARENB|CSTOPB);
785         term.c_cflag |=   CS8;
786         if (tcsetattr(fd, TCSANOW, &term) != 0) {
787                 perror("tcsetattr");
788         }
789
790         /* switch from NMEA to SiRF binary format */
791
792         const char to_sirf[] = "$PSRF100,0,9600,8,1,0*0C\n";
793         /* -1 : we do not want to send the null terminating character */
794         //write(fd, to_sirf, sizeof(to_sirf)-1);
795
796         /* Set Binary Serial Port.  Note : the effect of changing the baud
797            rate is not immediate at all */
798
799         /* Tested value : 
800              1200         1200 bit/s
801              2400         2400 bit/s
802              4800         4800 bit/s
803              9600         9600 bit/s
804             19200        19200 bit/s
805             38400        38400 bit/s
806             57600        57600 bit/s
807            115200       115200 bit/s
808         */
809
810         unsigned int baud = 115200;
811         unsigned char msg_134[] = { 134, 
812                                     (baud >> 24) & 0xff,
813                                     (baud >> 16) & 0xff,
814                                     (baud >>  8) & 0xff,
815                                     (baud >>  0) & 0xff,
816                                     8, 1, 0, 0 };
817         sirf_msg_send(fd, msg_134, sizeof(msg_134));
818         /*
819         if (tcgetattr(fd, &term) != 0) {
820                 perror("tcgetattr");
821         }
822         cfsetispeed(&term, B1200);
823         cfsetospeed(&term, B1200);
824         if (tcsetattr(fd, TCSANOW, &term) != 0) {
825                 perror("tcsetattr");
826         }
827         */
828
829         /* Poll Software Version */
830         unsigned char msg_132[] = { 132, 0x00 };
831         sirf_msg_send(fd, msg_132, sizeof(msg_132));
832
833         /* Initialize GPS/DR Navigation */
834         /*
835         unsigned char msg_172[] = { 0xac, 0x01, 
836                                     0x00, 0x00, 0x00, 0x00,
837                                     0x00, 0x00, 0x00, 0x00,
838                                     0x00, 0x00, 0x00, 0x00,
839                                     0x00, 0x00,
840                                     0x00, 0x00, 0x00, 0x00,
841                                     0x00, 0x00, 0x00, 0x00,
842                                     0x00, 0x00,
843                                     0x00,
844                                     (1<<4)|(1<<5)};
845         sirf_msg_send(fd, msg_172, sizeof(msg_172));
846         */
847
848         /* Initialize Data Source */
849         unsigned char msg_128[] = { 128,
850                                     0x00, 0x00, 0x00, 0x00,
851                                     0x00, 0x00, 0x00, 0x00,
852                                     0x00, 0x00, 0x00, 0x00,
853                                     0x00, 0x00, 0x00, 0x00,
854                                     0x00, 0x00, 0x00, 0x00,
855                                     0x00, 0x00,
856                                     12,
857                                     /* Clear memory */
858                                     /* (1<<2) | */
859                                     /* Enable raw track & debug data */
860                                     (1<<4) | (1<<5) };
861         sirf_msg_send(fd, msg_128, sizeof(msg_128));
862
863         /* Enable MID 29 */
864         unsigned char msg_166_29[] = { 166, 0, 29, 1, 0, 0, 0, 0 };
865         sirf_msg_send(fd, msg_166_29, sizeof(msg_166_29));
866
867         /* Enable MID 52 : we got a NACK since this feature is not available */
868         unsigned char msg_166[] = { 166,  0, 52, 1, 0, 0, 0, 0 };
869         sirf_msg_send(fd, msg_166, sizeof(msg_166));
870
871         /* Try to enable all messages. We got a ACK for :
872              0, Undocumented (never received)
873              2, Measure Navigation Data Out
874                 => X,Y,Z,dX,dY,dZ,T, HDOP, sat used
875                 used by gpsd to report longitude, latitude
876              4, Measured Tracker Data Out
877                 => T, SV ID, Azimuth, Elevation, SNR
878              7, Clock Status Data
879              8, 50 BPS Data
880              9, CPU Throughput
881             13, Visible List
882                 => SV ID, Azimuth, Elevation
883             18, OkToSend (never received)
884             27, DGPS Status Format
885             28, Navigation Library Measurement Data
886                 => SV ID, T, Pseudorange, Carrier freq+phase
887             29, Navigation Library DGPS Data (never received)
888             30, Navigation Library SV State Data
889                 => SV ID, T, X,Y,Z,dX,dY,dZ, Clock bias+drift, IONO
890             31, Navigation Library Initialization Data
891             41, Geodetic Navigation Data
892             46, Test Mode 3/4/5/6 (never received)
893             50, SBAS Parameters
894             55, Test Mode 4 Track Data (never received)
895            128, Undocumented (never received)
896            255, Development Data
897          */
898         /*
899         for (i=0; i<256; i++) {
900                 printf("Trying to enable message %d\n", i);
901                 msg_166[2] = i;
902                 sirf_msg_send(fd, msg_166, sizeof(msg_166));
903         }
904         */
905         for (;;) {
906                 fd_set fdr;
907                 int r;
908
909                 FD_ZERO(&fdr);
910                 FD_SET(fd, &fdr);
911
912                 r = select(FD_SETSIZE, &fdr, NULL, NULL, NULL);
913                 if (r < 0) {
914                         perror("select");
915                         break;
916                 }
917
918                 if (FD_ISSET(fd, &fdr)) {
919                         int ack = 0;
920                         if (do_read(fd, &ack) < 0)
921                                 break;
922                         if (ack != 0) {
923                                 printf("ACK for cmd %d\n", ack);
924                         }
925                 }
926         }
927
928         fclose(file);
929         close (fd);
930         return 0;
931 }