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