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