backup
[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 #include <time.h>
17 #include <math.h>
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 : %.2f s\n", (double)get_sirf_4u(buf+p)/100);
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 PRN : %3d\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 : %.2f s\n", (double)get_sirf_4u(buf + p)/100);
226         p += 4;
227         printf("  Channels : %d\n", buf[p]);
228         p += 1;
229         for (;p<n;) {
230                 printf("  SatID: %d Azimuth: %f Elevation: %f\n",
231                        buf[p], (double)buf[p+1]/2.0*3.0, (double)buf[p+2]/2.0);
232                 p += 3;
233                 printf("    State : %02x\n", get_sirf_2u(buf + p));
234                 p += 2;
235                 for (i=0; i<10; i++) {
236                         printf("    SNR %2d: %d dB\n", i+1, buf[p+i]);
237                 }
238                 p+= 10;
239         }
240
241         return 0;
242 }
243
244 int decode_sirf_msg_7(unsigned char *buf, int n, struct timeval *tv)
245 {
246         int p = 0;
247         time_t t;
248         struct tm * tm;
249         unsigned int week;
250         double seconds;
251         double t1, t2;
252         const int leapsecond = 17;
253
254         printf("Clock Status Data\n");
255         p += 1;
256         printf("  Extended GPS Week : %u\n", week = get_sirf_2u(buf + p));
257         p += 2;
258         printf("  GPS Time of Week : %.2f s\n",
259                seconds = (double)get_sirf_4u(buf + p)/100);
260         p += 4;
261         printf("  Satellites used : %u\n", buf[p]);
262         p += 1;
263         printf("  Clock Drift : %u Hz\n", get_sirf_4u(buf + p));
264         p += 4;
265         printf("  Clock Bias : %.9f s \n", 1e-9*(double)get_sirf_4u(buf + p));
266         p += 4;
267         printf("  Estimated GPS Time : %.3f s\n", 1e-3*(double)get_sirf_4u(buf + p));
268         p += 4;
269
270         /* Convert tv into YYYY-mm-dd HH:MM:SS UTC */
271         t1 = tv->tv_sec + 1e-6*tv->tv_usec;
272         t = floor(t1);
273         tm = gmtime(&t);
274         printf("  Local time : %4u-%02u-%02u %02u:%02u:%02u %06lu\n",
275                1900 + tm->tm_year, 1 + tm->tm_mon, tm->tm_mday,
276                tm->tm_hour, tm->tm_min, tm->tm_sec,
277                (unsigned long)(1e6*(t1-t)));
278
279         /* Convert GPS time into YYYY-mm-dd HH:MM:SS UTC
280          * (origin : 1980-01-06)
281          */
282         t2 = 315964800 + week * 604800 + seconds - leapsecond;
283         t = floor(t2);
284         tm = gmtime(&t);
285         printf("  GPS time   : %4u-%02u-%02u %02u:%02u:%02u %06lu [diff %.6f]\n",
286                1900 + tm->tm_year, 1 + tm->tm_mon, tm->tm_mday,
287                tm->tm_hour, tm->tm_min, tm->tm_sec,
288                (unsigned long)(1e6*(t2-t)), t1-t2);
289
290         return 0;
291 }
292
293 int decode_sirf_msg_8(unsigned char *buf, int n)
294 {
295         int i, p = 0;
296
297         printf("50 BPS Data\n");
298         p += 1;
299         printf("  Channel %d, SV# %d\n", buf[p], buf[p+1]);
300         p += 2;
301         for (i=0; i<10; i++) {
302                 printf("  Word %d : %08x\n", i, get_sirf_4u(buf+p));
303                 p += 4;
304         }
305
306         return 0;
307 }
308
309 int decode_sirf_msg_13(unsigned char *buf, int n)
310 {
311         int p = 0, i, n_sat;
312
313         printf("Visible List\n");
314         p += 1;
315         n_sat = buf[p];
316         p += 1;
317         for (i=0; i<n_sat; i++) {
318                 printf("  PRN %2d : Azimuth %3d Elevation %3d\n",
319                        buf[p], get_sirf_2s(buf+p+1), get_sirf_2s(buf+p+3));
320                 p += 5;
321         }
322
323         return 0;
324 }
325
326 int decode_sirf_msg_27(unsigned char *buf, int n)
327 {
328         int i, p = 0;
329
330         printf("DGPS Status Format\n");
331         p += 1;
332         printf("  DGPS source = %d\n", buf[p]);
333         p += 1;
334         for (i=0; i<12; i++) {
335                 printf("  PRN %3u: Age: %u s, DGPS correction : %d m\n",
336                        buf[p + 14 + 3 * i], buf[p + i],
337                        get_sirf_2s(buf + p + 14 + 3 * i + 1));
338         }
339
340         return 0;
341 }
342
343 int decode_sirf_msg_28(unsigned char *buf, int n)
344 {
345         int p = 0;
346
347         printf("Navigation Library Measurement Data - Channel %d\n", buf[p+1]);
348         p += 2;
349         /* It seems that Time Tag is a clock running at 16368000 Hz
350          * (16.369 MHz according to the datasheet) and starting at 0
351          * when the GPS is inserted into the USB port. There is a
352          * rollover every 262 seconds */
353         printf("  Time Tag = 0x%08x\n", get_sirf_4u(buf+p));
354         p += 4;
355         printf("  SatID : %d\n", buf[p]);
356         p += 1;
357         printf("  GPS SW Time : %f s\n", get_sirf_dbl(buf+p));
358         p += 8;
359         printf("  Pseudorange = %f m\n", get_sirf_dbl(buf+p));
360         p += 8;
361         printf("  Carrier frequency = %f m/s\n", get_sirf_sgl(buf+p));
362         p += 4;
363         printf("  Carrier phase = %f m\n", get_sirf_dbl(buf+p));
364         p += 8;
365
366         return 0;
367 }
368
369 int decode_sirf_msg_31(unsigned char *buf, int n)
370 {
371         printf("Navigation Library Initialization Data\n");
372         /* TBD */
373
374         return 0;
375 }
376
377 int decode_sirf_msg_50(unsigned char *buf, int n)
378 {
379         int p = 0;
380
381         printf("SBAS Parameters\n");
382         p += 1;
383         printf("  SBAS PRN : %3d\n", buf[p]);
384         p += 1;
385         printf("  SBAS Mode : %02x %s%s\n", buf[p],
386                buf[p] == 0 ? "Testing" : "",
387                buf[p] == 1 ? "Integrity" : "");
388         p += 1;
389         printf("  DGPS Timeout : %u s\n", buf[p]);
390         p += 1;
391         printf("  Flag bits : %02x\n", buf[p]);
392         p += 1;
393         
394         return 0;
395 }
396
397 int decode_sirf_msg_225(unsigned char *buf, int n)
398 {
399         int p = 0;
400
401         printf("Statistics Channel\n");
402         p += 1;
403         /* Message Sub ID : we got 0 or 8 ... undocumented */
404         printf("  Message Sub ID : %d\n", buf[p]);
405         p += 1;
406
407         return 0;
408 }
409
410 int decode_sirf_msg_255(unsigned char *buf, int n)
411 {
412         int p = 0, len;
413         char msg[1024];
414
415         printf("Development Data\n");
416         p += 1;
417
418         if (n < sizeof(msg))
419                 len = n;
420         else    len = sizeof(msg) -1;
421         memcpy(msg, buf + p, len);
422         msg[len] = 0;
423
424         printf("  %s\n", msg);
425
426         return 0;
427 }
428
429 /* (buf,n) is a full SiRF message, including start/stop sequence */
430
431 int decode_sirf_msg(unsigned char *buf, int n, int *ack, struct timeval *tv) {
432         int p = 4;
433
434         switch (buf[p]) {
435         case 2: /* Measured Navigation Data Out */
436                 decode_sirf_msg_2(buf + 4, n - 8);
437                 break;
438     
439         case 4: /* Measured Tracker Data Out */
440                 decode_sirf_msg_4(buf + 4, n - 8);
441                 break;
442                 
443         case 6: /* Software Version String */
444                 printf("Software Version String : %s\n", buf+p+1);
445                 /* for GPS USB : GSW3.5.0_3.5.00.00-SDK-3EP2.01 */
446                 p += 1;
447                 break;
448                 
449         case 7: /* Clock Status Data */
450             decode_sirf_msg_7(buf + 4, n - 8, tv);
451                 break;
452                 
453         case 8: /* 50 BPS Data */
454                 decode_sirf_msg_8(buf + 4, n - 8);
455                 break;
456                 
457         case 9: /* CPU Throughput */
458                 printf("CPU Throughput\n");
459                 break;
460                 
461         case 11: /* Command Acknowledgment */
462                 printf("Command ACK ID : %d\n",
463                        buf[p+1]);
464                 *ack = buf[p+1];
465                 p += 2;
466                 break;
467                 
468         case 12: /* Command Negative Acknowledgment */
469                 printf("Command NACK ID : %d\n",
470                        buf[p+1]);
471                 *ack = buf[p+1];
472                 p += 2;
473                 break;
474
475         case 13: /* Visible List */
476                 decode_sirf_msg_13(buf + 4, n - 8);
477                 break;
478
479         case 27: /* GPS Status Format */
480                 decode_sirf_msg_27(buf + 4, n - 8);
481                 break;
482                 
483         case 28: /* Navigation Library Measurement Data */
484                 decode_sirf_msg_28(buf + 4, n - 8);
485                 break;
486                 
487         case 30: /* Navigation Library SV State Data */
488                 printf("Navigation Library SV State Data - SatID : %d\n", buf[p+1]);
489                 p += 2;
490                 printf("  GPS Time : %.6f s\n", get_sirf_dbl(buf+p));
491                 p += 8;
492                 printf("  Position X : %f m\n", get_sirf_dbl(buf+p));
493                 p += 8;
494                 printf("  Position Y : %f m\n", get_sirf_dbl(buf+p));
495                 p += 8;
496                 printf("  Position Z : %f m\n", get_sirf_dbl(buf+p));
497                 p += 8;
498                 printf("  Velocity X : %f m/s\n", get_sirf_dbl(buf+p));
499                 p += 8;
500                 printf("  Velocity Y : %f m/s\n", get_sirf_dbl(buf+p));
501                 p += 8;
502                 printf("  Velocity Z : %f m/s\n", get_sirf_dbl(buf+p));
503                 p += 8;
504                 printf("  Clock bias : %f s\n", get_sirf_dbl(buf+p));
505                 p += 8;
506                 printf("  Clock drift : %f s/s\n", get_sirf_sgl(buf+p));
507                 p += 4;
508                 p += 1;
509                 p += 4;
510                 p += 4;
511                 printf("  Ionospheric delay : %f m\n", get_sirf_sgl(buf+p));
512                 p += 4;
513                 break;
514
515         case 31:
516                 decode_sirf_msg_31(buf + 4, n - 8);
517                 break;
518                 
519         case 41: /* Geodetic Navigation Data */
520                 printf("Geodetic Navigation Data\n");
521                 p += 1;
522                 p += 2;
523                 p += 2;
524                 p += 2;
525                 p += 4;
526                 
527                 printf("Y-M-D : %04d-%02d-%02d H:M:S : %02d:%02d:%02.3f\n",
528                        (buf[p]<<8)|buf[p+1], buf[p+2], buf[p+3],
529                        buf[p+4], buf[p+5], ((buf[p+6]<<8)|buf[p+7])/1000.0);
530                 break;
531
532         case 50:
533                 decode_sirf_msg_50(buf + 4, n - 8);
534                 break;
535
536         case 225:
537                 decode_sirf_msg_225(buf + 4, n - 8);
538                 break;
539                 
540         case 255: /* Development Data */
541                 decode_sirf_msg_255(buf + 4, n - 8);
542                 break;
543
544         default:
545                 printf("MID %d is not decoded yet ...\n", buf[p]);
546                 break;
547         }
548
549         fflush(stdout);
550         return 0;
551 }
552
553 void dump_msg(unsigned char *buf, int n) {
554         int i;
555         
556         printf("%d bytes message :", n);
557         for (i=0; i<n; i++) {
558                 printf(" 0x%02x", buf[i]);
559         }
560         printf("\n");
561 }
562
563 /* return 0 on success, -1 on error */
564
565 int do_read(int fd, int * ack, struct timeval *tvp) {
566         /* SiRF message size is limited to 912 bytes */
567         int i, n;
568         static struct timeval tv0;
569
570         static unsigned char sbuffer[912*2];
571         static int p = 0;
572
573         if ((sizeof(sbuffer) - p) == 0) {
574                 printf("buffer full -> empty\n");
575                 p = 0;
576                 return -1;
577         }
578
579         if (p == 0) {
580             if (tvp != NULL)
581                 tv0 = *tvp;
582             else
583                 tv0.tv_sec = tv0.tv_usec = 0;
584         }
585
586         n = read(fd, sbuffer + p, sizeof(sbuffer) - p);
587         if (n <= 0) {
588             if (n < 0)
589                 perror("read");
590             return -1;
591         }
592
593         if (n == 0) {
594                 /* nothing to do */
595                 return 0;
596         }
597
598         /* for debug only :
599         printf("%d bytes received (total = %d):", n, p+n);
600         for (i=0; i<n; i++) {
601                 printf(" 0x%02x", sbuffer[p+i]);
602         }
603         printf("\n");
604         */
605
606         /* update p with the bytes just received */
607         p += n;
608
609         /* small parsing of received data in (sbuffer, p) : 
610            - start pattern = 0xa0 0xa2
611            - stop  pattern = 0xb0 0xb3
612
613            SiRF message format is :
614            <0xa0 0xa2><length (2 bytes)><length bytes><CRC (2 byte)><0xb0 0xb3>
615         */
616
617         for (;;) {
618                 int start_found = 0, stop_found = 0;
619
620                 /* search for start pattern */
621                 for (i=0; i<p-1; i++) {
622                         if ((sbuffer[i] == 0xa0) && (sbuffer[i+1] == 0xa2)) {
623                                 start_found = 1;
624
625                                 if (i>0) {
626                                         printf("recv: %d bytes skipped\n", i);
627                                         memmove(sbuffer, sbuffer+i, p-i);
628                                         p -= i;
629                                 }
630                                 break;
631                         }
632                 }
633
634                 /* search for stop pattern */
635                 for (i=0; i<p-1; i++) {
636                         if ((sbuffer[i] == 0xb0) && (sbuffer[i+1] == 0xb3)) {
637                                 stop_found = 1;
638           
639                                 if (start_found && stop_found) {
640                                         printf("recv: MID %d (%d bytes) - tv %lu.%06lu\n",
641                                                sbuffer[4], i+2,
642                                                tv0.tv_sec, tv0.tv_usec); 
643                                         if (check_sirf_msg(sbuffer, i+2)) {
644                                                 decode_sirf_msg(sbuffer, i+2,
645                                                                 ack, &tv0);
646                                         }
647                                 }
648           
649                                 if (!start_found)
650                                         printf("recv: %d bytes skipped\n",i+2);
651                                 memmove(sbuffer, sbuffer+(i+2), p-(i+2));
652                                 p -= (i+2);
653
654                                 break;
655                         }
656                 }
657
658                 if (!stop_found)
659                         break;
660         }
661         
662         return 0;
663 }
664
665
666 /* 
667  * (buf,n) is the payload of the message. This function adds the start/stop
668  * sequences, and the length/checksum fields. Returns 0 on success, -1 on
669  * error.
670  */
671
672 int sirf_msg_send(int fd, unsigned char *buf, int n) {
673         unsigned char sbuf[1024];
674         int i;
675         unsigned int crc;
676         int total_len = 4 + n + 4; 
677         int cmd = buf[0];
678
679         if (total_len > sizeof(sbuf)) {
680                 printf("%s: message too large\n", __func__);
681                 return -1;
682         }
683
684         /* 0xa0, 0xa2 is the start sequence */
685         sbuf[0] = 0xa0;
686         sbuf[1] = 0xa2;
687         sbuf[2] = n>>8;
688         sbuf[3] = n&0xff;
689         memcpy(sbuf+4, buf, n);
690
691         /* compute checksum on the payload */
692         crc = 0;
693         for (i=0; i<n; i++)
694                 crc += buf[i];
695         crc &= 0x7fff;
696         
697         sbuf[4+n+0] = (crc & 0xff00)>>8;
698         sbuf[4+n+1] = (crc & 0x00ff)>>0;
699         /* 0xb0, 0xb3 is the end sequence */
700         sbuf[4+n+2] = 0xb0;
701         sbuf[4+n+3] = 0xb3;
702
703         if ((n=write(fd, sbuf, total_len)) != total_len) {
704                 printf("%s: written only %d bytes out of %d\n", __func__,
705                        n, total_len);
706                 return -1;
707         }
708
709         printf("send: MID %d (%d bytes)\n", buf[0], total_len);
710
711         /* wait for ACK */
712         for (;;) {
713                 int ack = 0;
714                 if (do_read(fd, &ack, NULL) < 0) {
715                         printf("do_read failed!\n");
716                         return -1;
717                 }
718                 if (ack == cmd) {
719                         printf("got ACK for cmd %d\n", ack);
720                         break;
721                 }
722         }
723
724         return 0;
725 }
726
727 int main(int argc, const char *argv[])
728 {
729         const char device[] = "/dev/ttyUSB0";
730         int fd;
731         struct termios term;
732         int i;
733
734         /* open serial device */
735         fd = open(device, O_RDWR /*| O_NONBLOCK*/);
736         if (fd < 0) {
737                 perror(device);
738                 return -1;
739         }
740
741         /* switch to 115200 bauds */
742         if (tcgetattr(fd, &term) != 0) {
743                 perror("tcgetattr");
744         }
745         cfmakeraw(&term);
746         cfsetispeed(&term, B115200);
747         cfsetospeed(&term, B115200);
748         /* 8N1 */
749         term.c_cflag &= ~(CSIZE|PARENB|CSTOPB);
750         term.c_cflag |=   CS8;
751         if (tcsetattr(fd, TCSANOW, &term) != 0) {
752                 perror("tcsetattr");
753         }
754
755         /* switch from NMEA to SiRF binary format */
756
757         const char to_sirf[] = "$PSRF100,0,9600,8,1,0*0C\n";
758         /* -1 : we do not want to send the null terminating character */
759         //write(fd, to_sirf, sizeof(to_sirf)-1);
760
761         /* switc from SiRF binary format to NMEA */
762         unsigned char msg_129[] = { 129,
763                                     0, /* Enable NMEA debug messages */
764                                     1, 1, /* GGA Message with checksum */
765                                     1, 1, /* GLL Message with checksum */
766                                     1, 1, /* GSA Message with checksum */
767                                     1, 1, /* GSV Message with checksum */
768                                     1, 1, /* RMC Message with checksum */
769                                     1, 1, /* VTG Message with checksum */
770                                     1, 1, /* MSS Message with checksum */
771                                     1, 1, /* EPE Message with checksum */
772                                     1, 1, /* ZDA Message with checksum */
773                                     0, 0, /* unused */
774                                      /* 4800 bauds */
775                                     (4800 >> 8) & 0xff, (4800 >> 0) & 0xff};
776         //sirf_msg_send(fd, msg_129, sizeof(msg_129));
777
778         /* Set Binary Serial Port.  Note : the effect of changing the baud
779            rate is not immediate at all */
780
781         /* Tested value : 
782              1200         1200 bit/s
783              2400         2400 bit/s
784              4800         4800 bit/s
785              9600         9600 bit/s
786             19200        19200 bit/s
787             38400        38400 bit/s
788             57600        57600 bit/s
789            115200       115200 bit/s
790         */
791
792         unsigned int baud = 115200;
793         unsigned char msg_134[] = { 134, 
794                                     (baud >> 24) & 0xff,
795                                     (baud >> 16) & 0xff,
796                                     (baud >>  8) & 0xff,
797                                     (baud >>  0) & 0xff,
798                                     8, 1, 0, 0 };
799         //sirf_msg_send(fd, msg_134, sizeof(msg_134));
800         /*
801         if (tcgetattr(fd, &term) != 0) {
802                 perror("tcgetattr");
803         }
804         cfsetispeed(&term, B1200);
805         cfsetospeed(&term, B1200);
806         if (tcsetattr(fd, TCSANOW, &term) != 0) {
807                 perror("tcsetattr");
808         }
809         */
810
811         /* Poll Software Version */
812         //unsigned char msg_132[] = { 132, 0x00 };
813         //sirf_msg_send(fd, msg_132, sizeof(msg_132));
814
815         /* Initialize GPS/DR Navigation */
816         /*
817         unsigned char msg_172[] = { 0xac, 0x01, 
818                                     0x00, 0x00, 0x00, 0x00,
819                                     0x00, 0x00, 0x00, 0x00,
820                                     0x00, 0x00, 0x00, 0x00,
821                                     0x00, 0x00,
822                                     0x00, 0x00, 0x00, 0x00,
823                                     0x00, 0x00, 0x00, 0x00,
824                                     0x00, 0x00,
825                                     0x00,
826                                     (1<<4)|(1<<5)};
827         sirf_msg_send(fd, msg_172, sizeof(msg_172));
828         */
829
830         /* Initialize Data Source */
831         unsigned char msg_128[] = { 128,
832                                     0x00, 0x00, 0x00, 0x00,
833                                     0x00, 0x00, 0x00, 0x00,
834                                     0x00, 0x00, 0x00, 0x00,
835                                     0x00, 0x00, 0x00, 0x00,
836                                     0x00, 0x00, 0x00, 0x00,
837                                     0x00, 0x00,
838                                     12, /* 12 channels */
839                                     /* Clear memory */
840                                     /* (1<<2) | */
841                                     //(1<<3)|/* Factory Reset = reset baud rate */
842                                     //(1<<4) /* Enable raw track */
843                                     /*(1<<5)*/  /* debug data */ };
844         sirf_msg_send(fd, msg_128, sizeof(msg_128));
845
846         /* Enable MID 7 = Clock Status Data (contains clock bias in ns) */
847         unsigned char msg_166_7[] = { 166, 0, 7, 1, 0, 0, 0, 0 };
848         //sirf_msg_send(fd, msg_166_7, sizeof(msg_166_7));
849
850         /* Enable MID 8 = 50 BPS Data (first data to be sent every 6s cycle) */
851         unsigned char msg_166_8[] = { 166, 0, 8, 0, 0, 0, 0, 0 };
852         //sirf_msg_send(fd, msg_166_8, sizeof(msg_166_8));
853
854         /* Enable MID 28 = Navigation Library Measurement Data (contains GPS SW time in us) */
855         unsigned char msg_166_28[] = { 166, 0, 28, 1, 0, 0, 0, 0 };
856         //sirf_msg_send(fd, msg_166_28, sizeof(msg_166_28));
857
858         /* Enable MID 52 : we got a NACK since this feature is not available */
859         //unsigned char msg_166_52[] = { 166,  0, 52, 1, 0, 0, 0, 0 };
860         //sirf_msg_send(fd, msg_166_52, sizeof(msg_166_52));
861
862         /* Try to enable all messages. We got a ACK for :
863              0, Undocumented (never received)
864              2, Measure Navigation Data Out
865                 => X,Y,Z,dX,dY,dZ,T, HDOP, sat used
866                 used by gpsd to report longitude, latitude
867              4, Measured Tracker Data Out
868                 => T, SV ID, Azimuth, Elevation, SNR
869              7, Clock Status Data
870              8, 50 BPS Data
871              9, CPU Throughput
872             13, Visible List
873                 => SV ID, Azimuth, Elevation
874             18, OkToSend (never received)
875             27, DGPS Status Format
876             28, Navigation Library Measurement Data
877                 => SV ID, T, Pseudorange, Carrier freq+phase
878             29, Navigation Library DGPS Data (never received)
879             30, Navigation Library SV State Data
880                 => SV ID, T, X,Y,Z,dX,dY,dZ, Clock bias+drift, IONO
881             31, Navigation Library Initialization Data
882             41, Geodetic Navigation Data
883             46, Test Mode 3/4/5/6 (never received)
884             50, SBAS Parameters
885             52, 1 PPS Time
886             55, Test Mode 4 Track Data (never received)
887            128, Undocumented (never received)
888            255, Development Data
889          */
890         /*
891         for (i=0; i<256; i++) {
892                 printf("Trying to enable message %d\n", i);
893                 msg_166[2] = i;
894                 sirf_msg_send(fd, msg_166, sizeof(msg_166));
895         }
896         */
897         for (;;) {
898                 fd_set fdr;
899                 int r;
900                 struct timeval tv;
901
902                 FD_ZERO(&fdr);
903                 FD_SET(fd, &fdr);
904
905                 r = select(FD_SETSIZE, &fdr, NULL, NULL, NULL);
906                 gettimeofday(&tv, NULL);
907                 if (r < 0) {
908                         perror("select");
909                         break;
910                 }
911
912                 if (FD_ISSET(fd, &fdr)) {
913                         int ack = 0;
914                         if (do_read(fd, &ack, &tv) < 0)
915                                 break;
916                         if (ack != 0) {
917                                 printf("ACK for cmd %d\n", ack);
918                         }
919                 }
920         }
921
922         close (fd);
923         return 0;
924 }