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