6f07ba66941eb078462aeffedde1edb48096bc1e
[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("  SatID: %d Azimuth: %f Elevation: %f\n",
228                        buf[p], (double)buf[p+1]/2.0*3.0, (double)buf[p+2]/2.0);
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("  SatID : %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 - SatID : %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         cfmakeraw(&term);
703         cfsetispeed(&term, B115200);
704         cfsetospeed(&term, B115200);
705         /* 8N1 */
706         term.c_cflag &= ~(CSIZE|PARENB|CSTOPB);
707         term.c_cflag |=   CS8;
708         if (tcsetattr(fd, TCSANOW, &term) != 0) {
709                 perror("tcsetattr");
710         }
711
712         /* switch from NMEA to SiRF binary format */
713
714         const char to_sirf[] = "$PSRF100,0,9600,8,1,0*0C\n";
715         /* -1 : we do not want to send the null terminating character */
716         //write(fd, to_sirf, sizeof(to_sirf)-1);
717
718         /* Set Binary Serial Port.  Note : the effect of changing the baud
719            rate is not immediate at all */
720
721         /* Tested value : 
722              1200         1200 bit/s
723              2400         2400 bit/s
724              4800         4800 bit/s
725              9600         9600 bit/s
726             19200        19200 bit/s
727             38400        38400 bit/s
728             57600        57600 bit/s
729            115200       115200 bit/s
730         */
731
732         unsigned int baud = 115200;
733         unsigned char msg_134[] = { 134, 
734                                     (baud >> 24) & 0xff,
735                                     (baud >> 16) & 0xff,
736                                     (baud >>  8) & 0xff,
737                                     (baud >>  0) & 0xff,
738                                     8, 1, 0, 0 };
739         sirf_msg_send(fd, msg_134, sizeof(msg_134));
740         /*
741         if (tcgetattr(fd, &term) != 0) {
742                 perror("tcgetattr");
743         }
744         cfsetispeed(&term, B1200);
745         cfsetospeed(&term, B1200);
746         if (tcsetattr(fd, TCSANOW, &term) != 0) {
747                 perror("tcsetattr");
748         }
749         */
750
751         /* Poll Software Version */
752         unsigned char msg_132[] = { 132, 0x00 };
753         sirf_msg_send(fd, msg_132, sizeof(msg_132));
754
755         /* Initialize GPS/DR Navigation */
756         /*
757         unsigned char msg_172[] = { 0xac, 0x01, 
758                                     0x00, 0x00, 0x00, 0x00,
759                                     0x00, 0x00, 0x00, 0x00,
760                                     0x00, 0x00, 0x00, 0x00,
761                                     0x00, 0x00,
762                                     0x00, 0x00, 0x00, 0x00,
763                                     0x00, 0x00, 0x00, 0x00,
764                                     0x00, 0x00,
765                                     0x00,
766                                     (1<<4)|(1<<5)};
767         sirf_msg_send(fd, msg_172, sizeof(msg_172));
768         */
769
770         /* Initialize Data Source */
771         unsigned char msg_128[] = { 128,
772                                     0x00, 0x00, 0x00, 0x00,
773                                     0x00, 0x00, 0x00, 0x00,
774                                     0x00, 0x00, 0x00, 0x00,
775                                     0x00, 0x00, 0x00, 0x00,
776                                     0x00, 0x00, 0x00, 0x00,
777                                     0x00, 0x00,
778                                     12,
779                                     /* Clear memory */
780                                     /* (1<<2) | */
781                                     /* Enable raw track & debug data */
782                                     (1<<4) | (1<<5) };
783         sirf_msg_send(fd, msg_128, sizeof(msg_128));
784
785         /* Enable MID 29 */
786         unsigned char msg_166_29[] = { 166, 0, 29, 1, 0, 0, 0, 0 };
787         sirf_msg_send(fd, msg_166_29, sizeof(msg_166_29));
788
789         /* Enable MID 52 : we got a NACK since this feature is not available */
790         unsigned char msg_166[] = { 166,  0, 52, 1, 0, 0, 0, 0 };
791         sirf_msg_send(fd, msg_166, sizeof(msg_166));
792
793         /* Try to enable all messages. We got a ACK for :
794              0, Undocumented (never received)
795              2, Measure Navigation Data Out
796                 => X,Y,Z,dX,dY,dZ,T, HDOP, sat used
797                 used by gpsd to report longitude, latitude
798              4, Measured Tracker Data Out
799                 => T, SV ID, Azimuth, Elevation, SNR
800              7, Clock Status Data
801              8, 50 BPS Data
802              9, CPU Throughput
803             13, Visible List
804                 => SV ID, Azimuth, Elevation
805             18, OkToSend (never received)
806             27, DGPS Status Format
807             28, Navigation Library Measurement Data
808                 => SV ID, T, Pseudorange, Carrier freq+phase
809             29, Navigation Library DGPS Data (never received)
810             30, Navigation Library SV State Data
811                 => SV ID, T, X,Y,Z,dX,dY,dZ, Clock bias+drift, IONO
812             31, Navigation Library Initialization Data
813             41, Geodetic Navigation Data
814             46, Test Mode 3/4/5/6 (never received)
815             50, SBAS Parameters
816             55, Test Mode 4 Track Data (never received)
817            128, Undocumented (never received)
818            255, Development Data
819          */
820         /*
821         for (i=0; i<256; i++) {
822                 printf("Trying to enable message %d\n", i);
823                 msg_166[2] = i;
824                 sirf_msg_send(fd, msg_166, sizeof(msg_166));
825         }
826         */
827         for (;;) {
828                 fd_set fdr;
829                 int r;
830
831                 FD_ZERO(&fdr);
832                 FD_SET(fd, &fdr);
833
834                 r = select(FD_SETSIZE, &fdr, NULL, NULL, NULL);
835                 if (r < 0) {
836                         perror("select");
837                         break;
838                 }
839
840                 if (FD_ISSET(fd, &fdr)) {
841                         int ack = 0;
842                         if (do_read(fd, &ack) < 0)
843                                 break;
844                         if (ack != 0) {
845                                 printf("ACK for cmd %d\n", ack);
846                         }
847                 }
848         }
849
850         close (fd);
851         return 0;
852 }