backup
[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, "%3u %4u %.12f %.12f\n",
272                 7, 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 :
377            28 <satID> <tt> <GPS second> <pseudorange> <tit> <snr> */
378         fprintf(file,
379                 "%3u %3u %10u %.12f %.6f %.3f %.3f\n",
380                 28, satID, tt, gps_second, pseudorange,
381                 (double)tit * 1e-3, snr_avg);
382
383         return 0;
384 }
385
386 int decode_sirf_msg_31(unsigned char *buf, int n)
387 {
388         printf("Navigation Library Initialization Data\n");
389         /* TBD */
390
391         return 0;
392 }
393
394 int decode_sirf_msg_41(unsigned char *buf, int n)
395 {
396         int p = 0;
397
398         printf("Geodetic Navigation Data\n");
399         p += 1;
400         p += 2;
401         p += 2;
402         printf("  GPS week : %u\n", get_sirf_2u(buf + p));
403         p += 2;
404         printf("  GPS seconds : %.3f\n", (double)get_sirf_4u(buf + p) * 0.001);
405         p += 4;
406                 
407         printf("  Y-M-D : %04d-%02d-%02d H:M:S : %02d:%02d:%02.3f\n",
408                get_sirf_2u(buf+p), buf[p+2], buf[p+3],
409                buf[p+4], buf[p+5], (double)get_sirf_2u(buf+p+6) * 0.001);
410
411         return 0;
412 }
413
414 int decode_sirf_msg_50(unsigned char *buf, int n)
415 {
416         int p = 0;
417
418         printf("SBAS Parameters\n");
419         p += 1;
420         printf("  SBAS SatID : %3u\n", buf[p]);
421         p += 1;
422         printf("  SBAS Mode : %02x %s%s\n", buf[p],
423                buf[p] == 0 ? "Testing" : "",
424                buf[p] == 1 ? "Integrity" : "");
425         p += 1;
426         printf("  DGPS Timeout : %u s\n", buf[p]);
427         p += 1;
428         printf("  Flag bits : %02x\n", buf[p]);
429         p += 1;
430         
431         return 0;
432 }
433
434 int decode_sirf_msg_225(unsigned char *buf, int n)
435 {
436         int p = 0;
437
438         printf("Statistics Channel\n");
439         p += 1;
440         /* Message Sub ID : we got 0 or 8 ... undocumented */
441         printf("  Message Sub ID : %d\n", buf[p]);
442         p += 1;
443
444         return 0;
445 }
446
447 int decode_sirf_msg_255(unsigned char *buf, int n)
448 {
449         int p = 0, len;
450         char msg[1024];
451
452         printf("Development Data\n");
453         p += 1;
454
455         if (n < sizeof(msg))
456                 len = n;
457         else    len = sizeof(msg) -1;
458         memcpy(msg, buf + p, len);
459         msg[len] = 0;
460
461         printf("  %s\n", msg);
462
463         return 0;
464 }
465
466 /* (buf,n) is a full SiRF message, including start/stop sequence */
467
468 int decode_sirf_msg(unsigned char *buf, int n, int *ack) {
469         int p = 4;
470
471         switch (buf[p]) {
472         case 2: /* Measured Navigation Data Out */
473                 decode_sirf_msg_2(buf + 4, n - 8);
474                 break;
475     
476         case 4: /* Measured Tracker Data Out */
477                 decode_sirf_msg_4(buf + 4, n - 8);
478                 break;
479                 
480         case 6: /* Software Version String */
481                 printf("Software Version String : %s\n", buf+p+1);
482                 /* for GPS USB : GSW3.5.0_3.5.00.00-SDK-3EP2.01 */
483                 p += 1;
484                 break;
485                 
486         case 7: /* Clock Status Data */
487                 decode_sirf_msg_7(buf + 4, n - 8);
488                 break;
489                 
490         case 8: /* 50 BPS Data */
491                 decode_sirf_msg_8(buf + 4, n - 8);
492                 break;
493                 
494         case 9: /* CPU Throughput */
495                 printf("CPU Throughput\n");
496                 break;
497                 
498         case 11: /* Command Acknowledgment */
499                 printf("Command ACK ID : %d\n",
500                        buf[p+1]);
501                 *ack = buf[p+1];
502                 p += 2;
503                 break;
504                 
505         case 12: /* Command Negative Acknowledgment */
506                 printf("Command NACK ID : %d\n",
507                        buf[p+1]);
508                 *ack = buf[p+1];
509                 p += 2;
510                 break;
511
512         case 13: /* Visible List */
513                 decode_sirf_msg_13(buf + 4, n - 8);
514                 break;
515
516         case 27: /* GPS Status Format */
517                 decode_sirf_msg_27(buf + 4, n - 8);
518                 break;
519                 
520         case 28: /* Navigation Library Measurement Data */
521                 decode_sirf_msg_28(buf + 4, n - 8);
522                 break;
523                 
524         case 30: /* Navigation Library SV State Data */
525                 printf("Navigation Library SV State Data - SatID : %3u\n", buf[p+1]);
526                 p += 2;
527                 printf("  GPS Time : %f s\n", get_sirf_dbl(buf+p));
528                 p += 8;
529                 printf("  Position X : %f m\n", get_sirf_dbl(buf+p));
530                 p += 8;
531                 printf("  Position Y : %f m\n", get_sirf_dbl(buf+p));
532                 p += 8;
533                 printf("  Position Z : %f m\n", get_sirf_dbl(buf+p));
534                 p += 8;
535                 printf("  Velocity X : %f m/s\n", get_sirf_dbl(buf+p));
536                 p += 8;
537                 printf("  Velocity Y : %f m/s\n", get_sirf_dbl(buf+p));
538                 p += 8;
539                 printf("  Velocity Z : %f m/s\n", get_sirf_dbl(buf+p));
540                 p += 8;
541                 printf("  Clock bias : %f s\n", get_sirf_dbl(buf+p));
542                 p += 8;
543                 printf("  Clock drift : %f s/s\n", get_sirf_sgl(buf+p));
544                 p += 4;
545                 p += 1;
546                 p += 4;
547                 p += 4;
548                 printf("  Ionospheric delay : %f m\n", get_sirf_sgl(buf+p));
549                 p += 4;
550                 break;
551
552         case 31:
553                 decode_sirf_msg_31(buf + 4, n - 8);
554                 break;
555                 
556         case 41: /* Geodetic Navigation Data */
557                 decode_sirf_msg_41(buf + 4, n - 8);
558                 break;
559
560         case 50:
561                 decode_sirf_msg_50(buf + 4, n - 8);
562                 break;
563
564         case 225:
565                 decode_sirf_msg_225(buf + 4, n - 8);
566                 break;
567                 
568         case 255: /* Development Data */
569                 decode_sirf_msg_255(buf + 4, n - 8);
570                 break;
571
572         default:
573                 printf("Message ID %d is not decoded yet ...\n", buf[p]);
574                 break;
575         }
576
577         fflush(stdout);
578         return 0;
579 }
580
581 void dump_msg(unsigned char *buf, int n) {
582         int i;
583         
584         printf("%d bytes message :", n);
585         for (i=0; i<n; i++) {
586                 printf(" 0x%02x", buf[i]);
587         }
588         printf("\n");
589 }
590
591 /* return 0 on success, -1 on error */
592
593 int do_read(int fd, int * ack) {
594         /* SiRF message size is limited to 912 bytes */
595         int i, n;
596
597         static unsigned char sbuffer[912*2];
598         static int p = 0;
599
600         if ((sizeof(sbuffer) - p) == 0) {
601                 printf("buffer full -> empty\n");
602                 p = 0;
603                 return -1;
604         }
605
606         n = read(fd, sbuffer + p, sizeof(sbuffer) - p);
607         if (n <= 0) {
608                 perror("read");
609                 return -1;
610         }
611
612         if (n == 0) {
613                 /* nothing to do */
614                 return 0;
615         }
616
617         /* for debug only :
618         printf("%d bytes received (total = %d):", n, p+n);
619         for (i=0; i<n; i++) {
620                 printf(" 0x%02x", sbuffer[p+i]);
621         }
622         printf("\n");
623         */
624
625         /* update p with the bytes just received */
626         p += n;
627
628         /* small parsing of received data in (sbuffer, p) : 
629            - start pattern = 0xa0 0xa2
630            - stop  pattern = 0xb0 0xb3
631
632            SiRF message format is :
633            <0xa0 0xa2><length (2 bytes)><length bytes><CRC (2 byte)><0xb0 0xb3>
634         */
635
636         for (;;) {
637                 int start_found = 0, stop_found = 0;
638
639                 /* search for start pattern */
640                 for (i=0; i<p-1; i++) {
641                         if ((sbuffer[i] == 0xa0) && (sbuffer[i+1] == 0xa2)) {
642                                 start_found = 1;
643
644                                 if (i>0) {
645                                         printf("recv: %d bytes skipped\n", i);
646                                         memmove(sbuffer, sbuffer+i, p-i);
647                                         p -= i;
648                                 }
649                                 break;
650                         }
651                 }
652
653                 /* search for stop pattern */
654                 for (i=0; i<p-1; i++) {
655                         if ((sbuffer[i] == 0xb0) && (sbuffer[i+1] == 0xb3)) {
656                                 stop_found = 1;
657           
658                                 if (start_found && stop_found) {
659                                         printf("recv: Message ID %d (%d bytes)\n",
660                                                sbuffer[4], i+2); 
661                                         if (check_sirf_msg(sbuffer, i+2)) {
662                                                 decode_sirf_msg(sbuffer, i+2,
663                                                                 ack);
664                                         }
665                                 }
666           
667                                 if (!start_found)
668                                         printf("recv: %d bytes skipped\n",i+2);
669                                 memmove(sbuffer, sbuffer+(i+2), p-(i+2));
670                                 p -= (i+2);
671
672                                 break;
673                         }
674                 }
675
676                 if (!stop_found)
677                         break;
678         }
679         
680         return 0;
681 }
682
683
684 /* 
685  * (buf,n) is the payload of the message. This function adds the start/stop
686  * sequences, and the length/checksum fields. Returns 0 on success, -1 on
687  * error.
688  */
689
690 int sirf_msg_send(int fd, unsigned char *buf, int n) {
691         unsigned char sbuf[1024];
692         int i;
693         unsigned int crc;
694         int total_len = 4 + n + 4; 
695         int cmd = buf[0];
696
697         if (total_len > sizeof(sbuf)) {
698                 printf("%s: message too large\n", __func__);
699                 return -1;
700         }
701
702         /* 0xa0, 0xa2 is the start sequence */
703         sbuf[0] = 0xa0;
704         sbuf[1] = 0xa2;
705         sbuf[2] = n>>8;
706         sbuf[3] = n&0xff;
707         memcpy(sbuf+4, buf, n);
708
709         /* compute checksum on the payload */
710         crc = 0;
711         for (i=0; i<n; i++)
712                 crc += buf[i];
713         crc &= 0x7fff;
714         
715         sbuf[4+n+0] = (crc & 0xff00)>>8;
716         sbuf[4+n+1] = (crc & 0x00ff)>>0;
717         /* 0xb0, 0xb3 is the end sequence */
718         sbuf[4+n+2] = 0xb0;
719         sbuf[4+n+3] = 0xb3;
720
721         if ((n=write(fd, sbuf, total_len)) != total_len) {
722                 printf("%s: written only %d bytes out of %d\n", __func__,
723                        n, total_len);
724                 return -1;
725         }
726
727         printf("send: Message ID %d (%d bytes)\n", buf[0], total_len);
728
729         /* wait for ACK */
730         for (;;) {
731                 int ack = 0;
732                 if (do_read(fd, &ack) < 0) {
733                         printf("do_read failed!\n");
734                         return -1;
735                 }
736                 if (ack == cmd) {
737                         printf("got ACK for cmd %d\n", ack);
738                         break;
739                 }
740         }
741
742         return 0;
743 }
744
745 void usage(const char *argv0) {
746         printf("usage: %s [-raw observation.raw]\n"
747                "  -raw observation.raw : record a RAW observation file\n",
748                argv0);
749         exit (-1);
750 }
751
752 int main(int argc, const char *argv[])
753 {
754         const char device[] = "/dev/ttyUSB0";
755         int fd;
756         struct termios term;
757         int i;
758
759         for (i=1 ; i<argc; i++) {
760                 if (strcmp(argv[i],"-raw")==0 && i+1<argc) {
761                         const char * filename = argv[++i];
762                         file = fopen(filename,"w");
763                         if (file == NULL) {
764                                 perror(filename);
765                                 exit (-1);
766                         }
767                 } else
768                         usage(argv[0]);
769         }
770
771         /* open serial device */
772         fd = open(device, O_RDWR /*| O_NONBLOCK*/);
773         if (fd < 0) {
774                 perror(device);
775                 return -1;
776         }
777
778         /* switch to 4800 bauds */
779         if (tcgetattr(fd, &term) != 0) {
780                 perror("tcgetattr");
781         }
782         cfmakeraw(&term);
783         cfsetispeed(&term, B9600);
784         cfsetospeed(&term, B9600);
785         /* 8N1 */
786         term.c_cflag &= ~(CSIZE|PARENB|CSTOPB);
787         term.c_cflag |=   CS8;
788         if (tcsetattr(fd, TCSANOW, &term) != 0) {
789                 perror("tcsetattr");
790         }
791
792         /* switch from NMEA to SiRF binary format */
793
794         const char to_sirf[] = "$PSRF100,0,9600,8,1,0*0C\n";
795         /* -1 : we do not want to send the null terminating character */
796         write(fd, to_sirf, sizeof(to_sirf)-1);
797
798         /* Set Binary Serial Port.  Note : the effect of changing the baud
799            rate is not immediate at all */
800
801         /* Tested value : 
802              1200         1200 bit/s
803              2400         2400 bit/s
804              4800         4800 bit/s
805              9600         9600 bit/s
806             19200        19200 bit/s
807             38400        38400 bit/s
808             57600        57600 bit/s
809            115200       115200 bit/s
810         */
811
812         unsigned int baud = 9600;
813         unsigned char msg_134[] = { 134, 
814                                     (baud >> 24) & 0xff,
815                                     (baud >> 16) & 0xff,
816                                     (baud >>  8) & 0xff,
817                                     (baud >>  0) & 0xff,
818                                     8, 1, 0, 0 };
819         //sirf_msg_send(fd, msg_134, sizeof(msg_134));
820         /*
821         if (tcgetattr(fd, &term) != 0) {
822                 perror("tcgetattr");
823         }
824         cfsetispeed(&term, B1200);
825         cfsetospeed(&term, B1200);
826         if (tcsetattr(fd, TCSANOW, &term) != 0) {
827                 perror("tcsetattr");
828         }
829         */
830
831         /* Poll Software Version */
832         unsigned char msg_132[] = { 132, 0x00 };
833         sirf_msg_send(fd, msg_132, sizeof(msg_132));
834
835         /* Initialize GPS/DR Navigation */
836         /*
837         unsigned char msg_172[] = { 0xac, 0x01, 
838                                     0x00, 0x00, 0x00, 0x00,
839                                     0x00, 0x00, 0x00, 0x00,
840                                     0x00, 0x00, 0x00, 0x00,
841                                     0x00, 0x00,
842                                     0x00, 0x00, 0x00, 0x00,
843                                     0x00, 0x00, 0x00, 0x00,
844                                     0x00, 0x00,
845                                     0x00,
846                                     (1<<4)|(1<<5)};
847         sirf_msg_send(fd, msg_172, sizeof(msg_172));
848         */
849
850         /* Initialize Data Source */
851         unsigned char msg_128[] = { 128,
852                                     0x00, 0x00, 0x00, 0x00,
853                                     0x00, 0x00, 0x00, 0x00,
854                                     0x00, 0x00, 0x00, 0x00,
855                                     0x00, 0x00, 0x00, 0x00,
856                                     0x00, 0x00, 0x00, 0x00,
857                                     0x00, 0x00,
858                                     12,
859                                     /* Clear memory */
860                                     //(1<<2) |
861                                     /* Factory reset = 4800, 8N1, NMEA */
862                                     //(1<<3) |
863                                     /* Enable raw track & debug data */
864                                     (1<<4) | (1<<5) };
865         sirf_msg_send(fd, msg_128, sizeof(msg_128));
866
867         /* Enable MID 29 */
868         unsigned char msg_166_29[] = { 166, 0, 29, 1, 0, 0, 0, 0 };
869         sirf_msg_send(fd, msg_166_29, sizeof(msg_166_29));
870
871         /* Enable MID 52 : we got a NACK since this feature is not available */
872         unsigned char msg_166[] = { 166,  0, 52, 1, 0, 0, 0, 0 };
873         sirf_msg_send(fd, msg_166, sizeof(msg_166));
874
875         /* Try to enable all messages. We got a ACK for :
876              0, Undocumented (never received)
877              2, Measure Navigation Data Out
878                 => X,Y,Z,dX,dY,dZ,T, HDOP, sat used
879                 used by gpsd to report longitude, latitude
880              4, Measured Tracker Data Out
881                 => T, SV ID, Azimuth, Elevation, SNR
882              7, Clock Status Data
883              8, 50 BPS Data
884              9, CPU Throughput
885             13, Visible List
886                 => SV ID, Azimuth, Elevation
887             18, OkToSend (never received)
888             27, DGPS Status Format
889             28, Navigation Library Measurement Data
890                 => SV ID, T, Pseudorange, Carrier freq+phase
891             29, Navigation Library DGPS Data (never received)
892             30, Navigation Library SV State Data
893                 => SV ID, T, X,Y,Z,dX,dY,dZ, Clock bias+drift, IONO
894             31, Navigation Library Initialization Data
895             41, Geodetic Navigation Data
896             46, Test Mode 3/4/5/6 (never received)
897             50, SBAS Parameters
898             55, Test Mode 4 Track Data (never received)
899            128, Undocumented (never received)
900            255, Development Data
901          */
902         /*
903         for (i=0; i<256; i++) {
904                 printf("Trying to enable message %d\n", i);
905                 msg_166[2] = i;
906                 sirf_msg_send(fd, msg_166, sizeof(msg_166));
907         }
908         */
909         for (;;) {
910                 fd_set fdr;
911                 int r;
912
913                 FD_ZERO(&fdr);
914                 FD_SET(fd, &fdr);
915
916                 r = select(FD_SETSIZE, &fdr, NULL, NULL, NULL);
917                 if (r < 0) {
918                         perror("select");
919                         break;
920                 }
921
922                 if (FD_ISSET(fd, &fdr)) {
923                         int ack = 0;
924                         if (do_read(fd, &ack) < 0)
925                                 break;
926                         if (ack != 0) {
927                                 printf("ACK for cmd %d\n", ack);
928                         }
929                 }
930         }
931
932         fclose(file);
933         close (fd);
934         return 0;
935 }