Program to record raw pseudorange and clock
authorBenoit Papillault <benoit.papillault@free.fr>
Thu, 29 Dec 2011 08:34:31 +0000 (09:34 +0100)
committerBenoit Papillault <benoit.papillault@free.fr>
Thu, 29 Dec 2011 08:34:31 +0000 (09:34 +0100)
sirf3.cpp [new file with mode: 0644]

diff --git a/sirf3.cpp b/sirf3.cpp
new file mode 100644 (file)
index 0000000..6e30252
--- /dev/null
+++ b/sirf3.cpp
@@ -0,0 +1,931 @@
+/*
+  Author : Benoit Papillault <benoit.papillault@free.fr>
+  Creation : 2011-07-03
+  Goal : Understanding SiRF protocol for GPS devices
+*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/select.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <string.h>
+
+FILE * file = stdout;
+
+/* return 1 if message is a valid SiRF message, 0 if invalid */
+
+int check_sirf_msg(unsigned char *buf, int n) {
+       int length, crc, computed_crc, i;
+
+       /* check size (at least 8 bytes) */
+       if (n<8) {
+               printf("msg too small (%d bytes)\n", n);
+               return 0;
+       }
+
+       /* check start pattern */
+       if ((buf[0] != 0xa0) || (buf[1] != 0xa2)) {
+               printf("invalid start pattern : 0x%02x 0x%02x\n",
+                      buf[0], buf[1]);
+               return 0;
+       }
+  
+       /* check length */
+       length = ((buf[2]<<8) | (buf[3]));
+       if (length & 0x8000) {
+               printf("invalid length : 0x%04x bytes\n", length);
+               return 0;
+       }
+
+       if (length > 912) {
+               printf("warning : length (%d bytes) is above 912\n", length);
+       }
+
+       if (4 + length + 4 != n) {
+               printf("invalid length : %d bytes, buf buffer is %d bytes\n",
+                      length, n);
+               return 0;
+       }
+  
+       /* check checksum */
+       crc = ((buf[n-4]<<8) | (buf[n-3]));
+       if (crc & 0x8000) {
+               printf("invalid crc : 0x%04x\n", crc);
+               return 0;
+       }
+
+       computed_crc = 0;
+       for (i=4; i<n-4; i++) {
+               computed_crc += buf[i];
+       }
+       computed_crc &= 0x7fff;
+
+       if (computed_crc != crc) {
+               printf("invalid crc : 0x%04x computed crc : 0x%04x\n",
+                      crc, computed_crc);
+               return 0;
+       }
+  
+       /* check stop pattern */
+       if ((buf[n-2] != 0xb0) || (buf[n-1] != 0xb3)) {
+               printf("invalid stop pattern : 0x%02x 0x%02x\n",
+                      buf[n-2], buf[n-1]);
+               return 0;
+       }
+
+       return 1;
+}
+
+double get_sirf_dbl(unsigned char *buf) {
+  double r;
+  unsigned char * ptr = (unsigned char *)&r;
+
+  ptr[0] = buf[3];
+  ptr[1] = buf[2];
+  ptr[2] = buf[1];
+  ptr[3] = buf[0];
+  ptr[4] = buf[7];
+  ptr[5] = buf[6];
+  ptr[6] = buf[5];
+  ptr[7] = buf[4];
+
+  if (sizeof(double) == 8) {
+    return r;
+  } else {
+    printf("get_sirf_dbl implementation error\n");
+  }
+}
+
+int get_sirf_2s(unsigned char *buf) {
+       return (((signed char)buf[0])<<8) | buf[1];
+}
+
+int get_sirf_4s(unsigned char *buf) {
+       return (((signed char)buf[0])<<24)|(buf[1]<<16)|(buf[2]<<8)|buf[3];
+}
+
+unsigned int get_sirf_2u(unsigned char *buf) {
+       return (buf[0]<<8) | buf[1];
+}
+
+unsigned int get_sirf_4u(unsigned char *buf) {
+       return (buf[0]<<24) | (buf[1]<<16) | (buf[2]<<8) | buf[3];
+}
+
+float get_sirf_sgl(unsigned char *buf) {
+  float r;
+  unsigned char *ptr = (unsigned char *)&r;
+
+  ptr[0] = buf[3];
+  ptr[1] = buf[2];
+  ptr[2] = buf[1];
+  ptr[3] = buf[0];
+
+  if (sizeof(float) == 4) {
+    return r;
+  } else {
+    printf("get_sirf_sgl implementation error\n");
+  }
+}
+
+int decode_sirf_msg_2(unsigned char * buf, int n)
+{
+       int i, p = 0, n_sat;
+       unsigned char mode;
+       const char * altmode = "";
+       const char * pmode = "";
+
+       printf("Measure Navigation Data Out\n");
+       p += 1;
+       printf("  X-position : %d m\n", get_sirf_4s(buf+p));
+       p += 4;
+       printf("  Y-position : %d m\n", get_sirf_4s(buf+p));
+       p += 4;
+       printf("  Z-position : %d m\n", get_sirf_4s(buf+p));
+       p += 4;
+       printf("  X-velocity : %f m/s\n", (double)get_sirf_2s(buf+p) / 8.0);
+       p += 2;
+       printf("  Y-velocity : %f m/s\n", (double)get_sirf_2s(buf+p) / 8.0);
+       p += 2;
+       printf("  Z-velocity : %f m/s\n", (double)get_sirf_2s(buf+p) / 8.0);
+       p += 2;
+       /* Mode 1 */
+       mode = buf[p];
+
+       switch ((mode >> 4) & 3) {
+       case 0:
+               altmode = "No altitude hold applied"; break;
+       case 1 :
+               altmode = "Holding of altitude from KF"; break;
+       case 2:
+               altmode = "Holding of altitude from user input"; break;
+       case 3:
+               altmode = "Always hold altitude"; break;
+       }
+
+       switch ( mode & 7) {
+       case 0:
+               pmode = "No navigation solution"; break;
+       case 1:
+               pmode = "1-SV solution (Kalman filter)"; break;
+       case 2:
+               pmode = "2-SV solution (Kalman filter)"; break;
+       case 3:
+               pmode = "3-SV solution (Kalman filter)"; break;
+       case 4:
+               pmode = ">3S-SV solution (Kalman filter)"; break;
+       case 5:
+               pmode = "2D solution (least squares)"; break;
+       case 6:
+               pmode = "3D solution (least squares)"; break;
+       case 7:
+               pmode = "Dead-Reckoning solution (no satellites)"; break;
+       }
+
+       printf("  Mode 1 : %02x %s %s %s %s %s\n", 
+              mode ,
+              mode & (1<<7) ? "DGPS:Yes" : "DGPS:No",
+              mode & (1<<6) ? "DOP mask exceeded " : "",
+              altmode, 
+              mode & (1<<3) ? "TicklePower position " : "",
+              pmode);
+       p += 1;
+       /* HDOP */
+       printf("  HDOP : %f\n", (double)(unsigned)buf[p] / 5.0);
+       p += 1;
+       /* Mode 2 */
+       mode = buf[p];
+       printf("  Mode 2 : %02x\n", mode);
+       p += 1;
+       printf("  GPS Week : %u\n", get_sirf_2u(buf+p));
+       p += 2;
+       printf("  GPS Time of Week : %f\n", (double)get_sirf_4u(buf+p)/100.0);
+       p += 4;
+       printf("  Satellites used : %d\n", n_sat = buf[p]);
+       p += 1;
+       for (i=0; (i<12) & (i<n_sat); i++) {
+                       printf("  Channel %2d SatID: %3u\n", i+1, buf[p+i]);
+       }
+
+       return 0;
+}
+
+int decode_sirf_msg_4(unsigned char *buf, int n)
+{
+       int i, p = 0;
+
+       printf("Measured Tracker Data Out\n");
+       p += 1;
+       printf("  GPS Week : %d\n", get_sirf_2s(buf + p));
+       p += 2;
+       printf("  GPS Time of Week : %.3f\n",
+              (double)get_sirf_4u(buf + p) / 100.0);
+       p += 4;
+       printf("  Channels : %d\n", buf[p]);
+       p += 1;
+       for (;p<n;) {
+               printf("  SatID: %3u Azimuth: %f Elevation: %f\n",
+                      buf[p], (double)buf[p+1]/2.0*3.0, (double)buf[p+2]/2.0);
+               p += 3;
+               printf("    State : %02x\n", get_sirf_2u(buf + p));
+               p += 2;
+               for (i=0; i<10; i++) {
+                       printf("    SNR %2d: %d dB\n", i+1, buf[p+i]);
+               }
+               p+= 10;
+       }
+
+       return 0;
+}
+
+int decode_sirf_msg_7(unsigned char *buf, int n)
+{
+       int p = 0;
+       int gps_week;
+       double gps_second;
+       double bias;
+
+       printf("Clock Status Data\n");
+       p += 1;
+       printf("  Extended GPS Week : %u\n",
+              gps_week = get_sirf_2u(buf + p));
+       p += 2;
+       printf("  GPS Time of Week : %f s\n",
+              gps_second = (double)get_sirf_4u(buf + p) / 100.0);
+       p += 4;
+       printf("  Satellites used : %u\n", buf[p]);
+       p += 1;
+       printf("  Clock Drift : %u Hz\n", get_sirf_4u(buf + p));
+       p += 4;
+       bias = (double)get_sirf_4u(buf + p) * 1e-9;
+       printf("  Clock Bias : %.9f ns\n", bias);
+       p += 4;
+       printf("  Estimated GPS Time : %u ms\n", get_sirf_4u(buf + p));
+       p += 4;
+
+       /* record : 7 <GPS week> <GPS second> <bias> */
+       fprintf(file, "7 %u %.12f %.12f\n",
+               gps_week, gps_second, bias);
+
+       return 0;
+}
+
+int decode_sirf_msg_8(unsigned char *buf, int n)
+{
+       int i, p = 0;
+
+       printf("50 BPS Data\n");
+       p += 1;
+       printf("  Channel %d, SV# %d\n", buf[p], buf[p+1]);
+       p += 2;
+       for (i=0; i<10; i++) {
+               printf("  Word %d : %08x\n", i, get_sirf_4u(buf+p));
+               p += 4;
+       }
+
+       return 0;
+}
+
+int decode_sirf_msg_13(unsigned char *buf, int n)
+{
+       int p = 0, i, n_sat;
+
+       printf("Visible List\n");
+       p += 1;
+       n_sat = buf[p];
+       p += 1;
+       for (i=0; i<n_sat; i++) {
+               printf("  SatID %3u : Azimuth %3d Elevation %3d\n",
+                      buf[p], get_sirf_2s(buf+p+1), get_sirf_2s(buf+p+3));
+               p += 5;
+       }
+
+       return 0;
+}
+
+int decode_sirf_msg_27(unsigned char *buf, int n)
+{
+       int i, p = 0;
+
+       printf("DGPS Status Format\n");
+       p += 1;
+       printf("  DGPS source = %d\n", buf[p]);
+       p += 1;
+       for (i=0; i<12; i++) {
+               printf("  SatID %3u: Age: %u s, DGPS correction : %d m\n",
+                      buf[p + 14 + 3 * i], buf[p + i],
+                      get_sirf_2s(buf + p + 14 + 3 * i + 1));
+       }
+
+       return 0;
+}
+
+int decode_sirf_msg_28(unsigned char *buf, int n)
+{
+       int p = 0;
+       int satID;
+       double gps_second;
+       double pseudorange;
+       double snr_avg = 0.0;
+       int i;
+       int tit;
+       unsigned int tt;
+       double tt_in_seconds;
+       const int gps_clock = 16368000; // Hz
+       int snr_was_zero = 0;
+
+       printf("Navigation Library Measurement Data - Channel %d\n", buf[p+1]);
+       p += 2;
+       tt=get_sirf_4u(buf+p);
+       tt_in_seconds = (double)tt / (double)gps_clock;
+       /* divided by GPS clock freq */
+       printf("  Time Tag = 0x%08x (%.9f s)\n", tt, tt_in_seconds);
+       p += 4;
+       printf("  SatID : %3u\n", satID=buf[p]);
+       p += 1;
+       gps_second=get_sirf_dbl(buf+p);
+       printf("  GPS SW Time : %.9f s [delta with TT : %.9f s]\n",
+              gps_second, gps_second - tt_in_seconds);
+       p += 8;
+       printf("  Pseudorange = %.3f m\n", pseudorange = get_sirf_dbl(buf+p));
+       p += 8;
+       printf("  Carrier frequency = %f m/s\n", get_sirf_sgl(buf+p));
+       p += 4;
+       printf("  Carrier phase = %f m\n", get_sirf_dbl(buf+p));
+       p += 8;
+       printf("  Time in track = %u ms\n", tit = get_sirf_2u(buf+p));
+       p += 2;
+       printf("  Sync flags = 0x%x\n", buf[p]);
+       p += 1;
+       for (i=0; i<10; i++) {
+               int snr;
+
+               printf("  SNR %3u dB-Hz\n", snr = buf[p+i]);
+               snr_avg += 0.1 * snr;
+               if (snr == 0) snr_was_zero = 1;
+       }
+       /* it seems that pseudo range is incorrect (too small) if snr was 0
+          at least once */
+       if (snr_was_zero) snr_avg = 0.0;
+       p += 10;
+
+       /* write RAW record : 28 <tt> <GPS second> <pseudorange> <tit> <snr> */
+       fprintf(file,
+               "28 %u %.12f %.6f %.3f %.3f\n",
+               tt, gps_second, pseudorange, (double)tit * 1e-3, snr_avg);
+
+       return 0;
+}
+
+int decode_sirf_msg_31(unsigned char *buf, int n)
+{
+       printf("Navigation Library Initialization Data\n");
+       /* TBD */
+
+       return 0;
+}
+
+int decode_sirf_msg_41(unsigned char *buf, int n)
+{
+       int p = 0;
+
+       printf("Geodetic Navigation Data\n");
+       p += 1;
+       p += 2;
+       p += 2;
+       printf("  GPS week : %u\n", get_sirf_2u(buf + p));
+       p += 2;
+       printf("  GPS seconds : %.3f\n", (double)get_sirf_4u(buf + p) * 0.001);
+       p += 4;
+               
+       printf("  Y-M-D : %04d-%02d-%02d H:M:S : %02d:%02d:%02.3f\n",
+              get_sirf_2u(buf+p), buf[p+2], buf[p+3],
+              buf[p+4], buf[p+5], (double)get_sirf_2u(buf+p+6) * 0.001);
+
+       return 0;
+}
+
+int decode_sirf_msg_50(unsigned char *buf, int n)
+{
+       int p = 0;
+
+       printf("SBAS Parameters\n");
+       p += 1;
+       printf("  SBAS SatID : %3u\n", buf[p]);
+       p += 1;
+       printf("  SBAS Mode : %02x %s%s\n", buf[p],
+              buf[p] == 0 ? "Testing" : "",
+              buf[p] == 1 ? "Integrity" : "");
+       p += 1;
+       printf("  DGPS Timeout : %u s\n", buf[p]);
+       p += 1;
+       printf("  Flag bits : %02x\n", buf[p]);
+       p += 1;
+       
+       return 0;
+}
+
+int decode_sirf_msg_225(unsigned char *buf, int n)
+{
+       int p = 0;
+
+       printf("Statistics Channel\n");
+       p += 1;
+       /* Message Sub ID : we got 0 or 8 ... undocumented */
+       printf("  Message Sub ID : %d\n", buf[p]);
+       p += 1;
+
+       return 0;
+}
+
+int decode_sirf_msg_255(unsigned char *buf, int n)
+{
+       int p = 0, len;
+       char msg[1024];
+
+       printf("Development Data\n");
+       p += 1;
+
+       if (n < sizeof(msg))
+               len = n;
+       else    len = sizeof(msg) -1;
+       memcpy(msg, buf + p, len);
+       msg[len] = 0;
+
+       printf("  %s\n", msg);
+
+       return 0;
+}
+
+/* (buf,n) is a full SiRF message, including start/stop sequence */
+
+int decode_sirf_msg(unsigned char *buf, int n, int *ack) {
+       int p = 4;
+
+       switch (buf[p]) {
+       case 2: /* Measured Navigation Data Out */
+               decode_sirf_msg_2(buf + 4, n - 8);
+               break;
+    
+       case 4: /* Measured Tracker Data Out */
+               decode_sirf_msg_4(buf + 4, n - 8);
+               break;
+               
+       case 6: /* Software Version String */
+               printf("Software Version String : %s\n", buf+p+1);
+               /* for GPS USB : GSW3.5.0_3.5.00.00-SDK-3EP2.01 */
+               p += 1;
+               break;
+               
+       case 7: /* Clock Status Data */
+               decode_sirf_msg_7(buf + 4, n - 8);
+               break;
+               
+       case 8: /* 50 BPS Data */
+               decode_sirf_msg_8(buf + 4, n - 8);
+               break;
+               
+       case 9: /* CPU Throughput */
+               printf("CPU Throughput\n");
+               break;
+               
+       case 11: /* Command Acknowledgment */
+               printf("Command ACK ID : %d\n",
+                      buf[p+1]);
+               *ack = buf[p+1];
+               p += 2;
+               break;
+               
+       case 12: /* Command Negative Acknowledgment */
+               printf("Command NACK ID : %d\n",
+                      buf[p+1]);
+               *ack = buf[p+1];
+               p += 2;
+               break;
+
+       case 13: /* Visible List */
+               decode_sirf_msg_13(buf + 4, n - 8);
+               break;
+
+       case 27: /* GPS Status Format */
+               decode_sirf_msg_27(buf + 4, n - 8);
+               break;
+               
+       case 28: /* Navigation Library Measurement Data */
+               decode_sirf_msg_28(buf + 4, n - 8);
+               break;
+               
+       case 30: /* Navigation Library SV State Data */
+               printf("Navigation Library SV State Data - SatID : %3u\n", buf[p+1]);
+               p += 2;
+               printf("  GPS Time : %f s\n", get_sirf_dbl(buf+p));
+               p += 8;
+               printf("  Position X : %f m\n", get_sirf_dbl(buf+p));
+               p += 8;
+               printf("  Position Y : %f m\n", get_sirf_dbl(buf+p));
+               p += 8;
+               printf("  Position Z : %f m\n", get_sirf_dbl(buf+p));
+               p += 8;
+               printf("  Velocity X : %f m/s\n", get_sirf_dbl(buf+p));
+               p += 8;
+               printf("  Velocity Y : %f m/s\n", get_sirf_dbl(buf+p));
+               p += 8;
+               printf("  Velocity Z : %f m/s\n", get_sirf_dbl(buf+p));
+               p += 8;
+               printf("  Clock bias : %f s\n", get_sirf_dbl(buf+p));
+               p += 8;
+               printf("  Clock drift : %f s/s\n", get_sirf_sgl(buf+p));
+               p += 4;
+               p += 1;
+               p += 4;
+               p += 4;
+               printf("  Ionospheric delay : %f m\n", get_sirf_sgl(buf+p));
+               p += 4;
+               break;
+
+       case 31:
+               decode_sirf_msg_31(buf + 4, n - 8);
+               break;
+               
+       case 41: /* Geodetic Navigation Data */
+               decode_sirf_msg_41(buf + 4, n - 8);
+               break;
+
+       case 50:
+               decode_sirf_msg_50(buf + 4, n - 8);
+               break;
+
+       case 225:
+               decode_sirf_msg_225(buf + 4, n - 8);
+               break;
+               
+       case 255: /* Development Data */
+               decode_sirf_msg_255(buf + 4, n - 8);
+               break;
+
+       default:
+               printf("Message ID %d is not decoded yet ...\n", buf[p]);
+               break;
+       }
+
+       fflush(stdout);
+       return 0;
+}
+
+void dump_msg(unsigned char *buf, int n) {
+       int i;
+       
+       printf("%d bytes message :", n);
+       for (i=0; i<n; i++) {
+               printf(" 0x%02x", buf[i]);
+       }
+       printf("\n");
+}
+
+/* return 0 on success, -1 on error */
+
+int do_read(int fd, int * ack) {
+       /* SiRF message size is limited to 912 bytes */
+       int i, n;
+
+       static unsigned char sbuffer[912*2];
+       static int p = 0;
+
+       if ((sizeof(sbuffer) - p) == 0) {
+               printf("buffer full -> empty\n");
+               p = 0;
+               return -1;
+       }
+
+       n = read(fd, sbuffer + p, sizeof(sbuffer) - p);
+       if (n <= 0) {
+               perror("read");
+               return -1;
+       }
+
+       if (n == 0) {
+               /* nothing to do */
+               return 0;
+       }
+
+       /* for debug only :
+       printf("%d bytes received (total = %d):", n, p+n);
+       for (i=0; i<n; i++) {
+               printf(" 0x%02x", sbuffer[p+i]);
+       }
+       printf("\n");
+       */
+
+       /* update p with the bytes just received */
+       p += n;
+
+       /* small parsing of received data in (sbuffer, p) : 
+          - start pattern = 0xa0 0xa2
+          - stop  pattern = 0xb0 0xb3
+
+          SiRF message format is :
+          <0xa0 0xa2><length (2 bytes)><length bytes><CRC (2 byte)><0xb0 0xb3>
+       */
+
+       for (;;) {
+               int start_found = 0, stop_found = 0;
+
+               /* search for start pattern */
+               for (i=0; i<p-1; i++) {
+                       if ((sbuffer[i] == 0xa0) && (sbuffer[i+1] == 0xa2)) {
+                               start_found = 1;
+
+                               if (i>0) {
+                                       printf("recv: %d bytes skipped\n", i);
+                                       memmove(sbuffer, sbuffer+i, p-i);
+                                       p -= i;
+                               }
+                               break;
+                       }
+               }
+
+               /* search for stop pattern */
+               for (i=0; i<p-1; i++) {
+                       if ((sbuffer[i] == 0xb0) && (sbuffer[i+1] == 0xb3)) {
+                               stop_found = 1;
+         
+                               if (start_found && stop_found) {
+                                       printf("recv: Message ID %d (%d bytes)\n",
+                                              sbuffer[4], i+2); 
+                                       if (check_sirf_msg(sbuffer, i+2)) {
+                                               decode_sirf_msg(sbuffer, i+2,
+                                                               ack);
+                                       }
+                               }
+         
+                               if (!start_found)
+                                       printf("recv: %d bytes skipped\n",i+2);
+                               memmove(sbuffer, sbuffer+(i+2), p-(i+2));
+                               p -= (i+2);
+
+                               break;
+                       }
+               }
+
+               if (!stop_found)
+                       break;
+       }
+       
+       return 0;
+}
+
+
+/* 
+ * (buf,n) is the payload of the message. This function adds the start/stop
+ * sequences, and the length/checksum fields. Returns 0 on success, -1 on
+ * error.
+ */
+
+int sirf_msg_send(int fd, unsigned char *buf, int n) {
+       unsigned char sbuf[1024];
+       int i;
+       unsigned int crc;
+       int total_len = 4 + n + 4; 
+       int cmd = buf[0];
+
+       if (total_len > sizeof(sbuf)) {
+               printf("%s: message too large\n", __func__);
+               return -1;
+       }
+
+       /* 0xa0, 0xa2 is the start sequence */
+       sbuf[0] = 0xa0;
+       sbuf[1] = 0xa2;
+       sbuf[2] = n>>8;
+       sbuf[3] = n&0xff;
+       memcpy(sbuf+4, buf, n);
+
+       /* compute checksum on the payload */
+       crc = 0;
+       for (i=0; i<n; i++)
+               crc += buf[i];
+       crc &= 0x7fff;
+       
+       sbuf[4+n+0] = (crc & 0xff00)>>8;
+       sbuf[4+n+1] = (crc & 0x00ff)>>0;
+       /* 0xb0, 0xb3 is the end sequence */
+       sbuf[4+n+2] = 0xb0;
+       sbuf[4+n+3] = 0xb3;
+
+       if ((n=write(fd, sbuf, total_len)) != total_len) {
+               printf("%s: written only %d bytes out of %d\n", __func__,
+                      n, total_len);
+               return -1;
+       }
+
+       printf("send: Message ID %d (%d bytes)\n", buf[0], total_len);
+
+       /* wait for ACK */
+       for (;;) {
+               int ack = 0;
+               if (do_read(fd, &ack) < 0) {
+                       printf("do_read failed!\n");
+                       return -1;
+               }
+               if (ack == cmd) {
+                       printf("got ACK for cmd %d\n", ack);
+                       break;
+               }
+       }
+
+       return 0;
+}
+
+void usage(const char *argv0) {
+       printf("usage: %s [-raw observation.raw]\n"
+              "  -raw observation.raw : record a RAW observation file\n",
+              argv0);
+       exit (-1);
+}
+
+int main(int argc, const char *argv[])
+{
+       const char device[] = "/dev/ttyUSB0";
+       int fd;
+       struct termios term;
+       int i;
+
+       for (i=1 ; i<argc; i++) {
+               if (strcmp(argv[i],"-raw")==0 && i+1<argc) {
+                       const char * filename = argv[++i];
+                       file = fopen(filename,"w");
+                       if (file == NULL) {
+                               perror(filename);
+                               exit (-1);
+                       }
+               } else
+                       usage(argv[0]);
+       }
+
+       /* open serial device */
+       fd = open(device, O_RDWR /*| O_NONBLOCK*/);
+       if (fd < 0) {
+               perror(device);
+               return -1;
+       }
+
+       /* switch to 4800 bauds */
+       if (tcgetattr(fd, &term) != 0) {
+               perror("tcgetattr");
+       }
+       cfmakeraw(&term);
+       cfsetispeed(&term, B115200);
+       cfsetospeed(&term, B115200);
+       /* 8N1 */
+       term.c_cflag &= ~(CSIZE|PARENB|CSTOPB);
+       term.c_cflag |=   CS8;
+       if (tcsetattr(fd, TCSANOW, &term) != 0) {
+               perror("tcsetattr");
+       }
+
+       /* switch from NMEA to SiRF binary format */
+
+       const char to_sirf[] = "$PSRF100,0,9600,8,1,0*0C\n";
+       /* -1 : we do not want to send the null terminating character */
+       //write(fd, to_sirf, sizeof(to_sirf)-1);
+
+       /* Set Binary Serial Port.  Note : the effect of changing the baud
+          rate is not immediate at all */
+
+       /* Tested value : 
+            1200         1200 bit/s
+            2400         2400 bit/s
+            4800         4800 bit/s
+            9600         9600 bit/s
+           19200        19200 bit/s
+           38400        38400 bit/s
+           57600        57600 bit/s
+          115200       115200 bit/s
+       */
+
+       unsigned int baud = 115200;
+       unsigned char msg_134[] = { 134, 
+                                   (baud >> 24) & 0xff,
+                                   (baud >> 16) & 0xff,
+                                   (baud >>  8) & 0xff,
+                                   (baud >>  0) & 0xff,
+                                   8, 1, 0, 0 };
+       sirf_msg_send(fd, msg_134, sizeof(msg_134));
+       /*
+       if (tcgetattr(fd, &term) != 0) {
+               perror("tcgetattr");
+       }
+       cfsetispeed(&term, B1200);
+       cfsetospeed(&term, B1200);
+       if (tcsetattr(fd, TCSANOW, &term) != 0) {
+               perror("tcsetattr");
+       }
+       */
+
+       /* Poll Software Version */
+       unsigned char msg_132[] = { 132, 0x00 };
+       sirf_msg_send(fd, msg_132, sizeof(msg_132));
+
+       /* Initialize GPS/DR Navigation */
+       /*
+       unsigned char msg_172[] = { 0xac, 0x01, 
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00,
+                                   0x00,
+                                   (1<<4)|(1<<5)};
+       sirf_msg_send(fd, msg_172, sizeof(msg_172));
+       */
+
+       /* Initialize Data Source */
+       unsigned char msg_128[] = { 128,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00,
+                                   12,
+                                   /* Clear memory */
+                                   /* (1<<2) | */
+                                   /* Enable raw track & debug data */
+                                   (1<<4) | (1<<5) };
+       sirf_msg_send(fd, msg_128, sizeof(msg_128));
+
+       /* Enable MID 29 */
+       unsigned char msg_166_29[] = { 166, 0, 29, 1, 0, 0, 0, 0 };
+       sirf_msg_send(fd, msg_166_29, sizeof(msg_166_29));
+
+       /* Enable MID 52 : we got a NACK since this feature is not available */
+       unsigned char msg_166[] = { 166,  0, 52, 1, 0, 0, 0, 0 };
+       sirf_msg_send(fd, msg_166, sizeof(msg_166));
+
+       /* Try to enable all messages. We got a ACK for :
+            0, Undocumented (never received)
+            2, Measure Navigation Data Out
+               => X,Y,Z,dX,dY,dZ,T, HDOP, sat used
+               used by gpsd to report longitude, latitude
+            4, Measured Tracker Data Out
+               => T, SV ID, Azimuth, Elevation, SNR
+            7, Clock Status Data
+            8, 50 BPS Data
+            9, CPU Throughput
+           13, Visible List
+               => SV ID, Azimuth, Elevation
+           18, OkToSend (never received)
+           27, DGPS Status Format
+           28, Navigation Library Measurement Data
+               => SV ID, T, Pseudorange, Carrier freq+phase
+           29, Navigation Library DGPS Data (never received)
+           30, Navigation Library SV State Data
+               => SV ID, T, X,Y,Z,dX,dY,dZ, Clock bias+drift, IONO
+           31, Navigation Library Initialization Data
+           41, Geodetic Navigation Data
+           46, Test Mode 3/4/5/6 (never received)
+           50, SBAS Parameters
+           55, Test Mode 4 Track Data (never received)
+          128, Undocumented (never received)
+          255, Development Data
+        */
+       /*
+       for (i=0; i<256; i++) {
+               printf("Trying to enable message %d\n", i);
+               msg_166[2] = i;
+               sirf_msg_send(fd, msg_166, sizeof(msg_166));
+       }
+       */
+       for (;;) {
+               fd_set fdr;
+               int r;
+
+               FD_ZERO(&fdr);
+               FD_SET(fd, &fdr);
+
+               r = select(FD_SETSIZE, &fdr, NULL, NULL, NULL);
+               if (r < 0) {
+                       perror("select");
+                       break;
+               }
+
+               if (FD_ISSET(fd, &fdr)) {
+                       int ack = 0;
+                       if (do_read(fd, &ack) < 0)
+                               break;
+                       if (ack != 0) {
+                               printf("ACK for cmd %d\n", ack);
+                       }
+               }
+       }
+
+       fclose(file);
+       close (fd);
+       return 0;
+}