Enhanced SiRF message parsing. Reformating
authorBenoit Papillault <benoit.papillault@free.fr>
Sat, 17 Dec 2011 18:08:53 +0000 (19:08 +0100)
committerBenoit Papillault <benoit.papillault@free.fr>
Sat, 17 Dec 2011 18:08:53 +0000 (19:08 +0100)
sirf.c

diff --git a/sirf.c b/sirf.c
index 03fdd35..00a047b 100644 (file)
--- a/sirf.c
+++ b/sirf.c
 #include <string.h>
 
 /* 
- * (buf,n) is the payload of the message. This function adds the
- * start/stop sequences, and the length/checksum fields
+ * (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) {
-  char sbuf[1024];
-  int i, computed_crc;
-
-  sbuf[0] = 0xa0;
-  sbuf[1] = 0xa2;
-  sbuf[2] = n>>8;
-  sbuf[3] = n&0xff;
-  memcpy(sbuf+4, buf, n);
-
-  computed_crc = 0;
-  for (i=0; i<n; i++) {
-    computed_crc += buf[i];
-  }
-  computed_crc &= 0x7fff;
+       char sbuf[1024];
+       int i, computed_crc;
+       int total_len = 4 + n + 4; 
+
+       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);
+
+       computed_crc = 0;
+       for (i=0; i<n; i++) {
+               computed_crc += buf[i];
+       }
+       computed_crc &= 0x7fff;
+       
+       sbuf[4+n+0] = computed_crc>>8;
+       sbuf[4+n+1] = computed_crc&0xff;
+       /* 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;
+       }
 
-  sbuf[4+n+0] = computed_crc>>8;
-  sbuf[4+n+1] = computed_crc&0xff;
-  sbuf[4+n+2] = 0xb0;
-  sbuf[4+n+3] = 0xb3;
+       printf("send: Message ID %d (%d bytes)\n", buf[0], total_len);
 
-  return write(fd, sbuf, 4+n+4);
+       return 0;
 }
 
 /* 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;
+       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 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 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;
-  }
+       /* 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 (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;
-  }
+       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;
-  }
+       /* 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;
+       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;
-  }
+       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;
-  }
+       /* 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;
+       }
 
-  printf("valid message (%d bytes)\n", length);
-  return 1;
+       return 1;
 }
 
 double get_sirf_dbl(unsigned char *buf) {
@@ -142,337 +162,347 @@ float get_sirf_sgl(unsigned char *buf) {
 /* (buf,n) is a full SiRF message, including start/stop sequence */
 
 int decode_sirf_msg(unsigned char *buf, int n) {
-  int i;
-  int p = 4;
-
-  printf("... decoding Message ID : %d\n", buf[p]);
-  switch (buf[p]) {
-  case 2: /* Measured Navigation Data Out */
-    printf("Measured Navigation Data\n");
-    p += 1;
-    p += 4;
-    p += 4;
-    p += 4;
-    p += 2;
-    p += 2;
-    p += 2;
-    p += 3;
-    p += 2;
-    p += 4;
-    printf("#SV : %d\n", buf[p]);
-    p += 1;
-    for (i=0; i<12; i++) {
-      printf("CH %d PRN : %d\n", i+1, buf[p+i]);
-    }
-    break;
-
-  case 4: /* Measured Tracker Data Out */
-    printf("Measured Tracker Data Out\n");
-    p += 1;
-    p += 2;
-    p += 4;
-    printf("Chans : %d\n", buf[p]);
-    p += 1;
-    for (;p<n;) {
-      printf("SV ID: %d Azimuth: %d Elevation: %d\n",
-            buf[p], buf[p+1], buf[p+2]);
-      p += 3;
-      p += 2;
-      for (i=0; i<10; i++) {
-       printf("C/No %2d: %d dB-Hz\n", i+1, buf[p+i]);
-      }
-      p+= 10;
-    }
-    
-    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 */
-    printf("Clock Status Data\n");
-    break;
-
-  case 8: /* 50 BPS Data */
-    printf("50 BPS Data\n");
-    p += 2;
-    printf("Channel %d, SV# %d\n", buf[p], buf[p+1]);
-    p += 2;
-    break;
-
-  case 9: /* CPU Throughput */
-    printf("CPU Throughput\n");
-    break;
-
-  case 11: /* Command Acknowledgment */
-    printf("Command Acknowledgment ID : %d\n",
-          buf[p+1]);
-    p += 2;
-    break;
-
-  case 28: /* Navigation Library Measurement Data */
-    printf("Navigation Library Measurement Data - Channel %d\n", buf[p+1]);
-    p += 2;
-    printf("  Time Tag = 0x%02x%02x%02x%02x\n", buf[p], buf[p+1], buf[p+2], buf[p+3]);
-    p += 4;
-    printf("  Satellite ID : %d\n", buf[p]);
-    p += 1;
-    printf("  GPS SW Time : %f s\n", get_sirf_dbl(buf+p));
-    p += 8;
-    printf("  Pseudorange = %f m\n", 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;
+       int i;
+       int p = 4;
+
+       printf("... decoding Message ID : %d\n", buf[p]);
+       switch (buf[p]) {
+       case 2: /* Measured Navigation Data Out */
+               printf("Measured Navigation Data\n");
+               p += 1;
+               p += 4;
+               p += 4;
+               p += 4;
+               p += 2;
+               p += 2;
+               p += 2;
+               p += 3;
+               p += 2;
+               p += 4;
+               printf("#SV : %d\n", buf[p]);
+               p += 1;
+               for (i=0; i<12; i++) {
+                       printf("CH %d PRN : %d\n", i+1, buf[p+i]);
+               }
+               break;
     
-    break;
-
-  case 30: /* Navigation Library SV State Data */
-    printf("Navigation Library SV State Data - Satellite ID : %d\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 41: /* Geodetic Navigation Data */
-    printf("Geodetic Navigation Data\n");
-    p += 1;
-    p += 2;
-    p += 2;
-    p += 2;
-    p += 4;
-
-    printf("Y-M-D : %04d-%02d-%02d H:M:S : %02d:%02d:%02.3f\n",
-          (buf[p]<<8)|buf[p+1], buf[p+2], buf[p+3],
-          buf[p+4], buf[p+5], ((buf[p+6]<<8)|buf[p+7])/1000.0);
-    break;
-
-  case 255: /* Development Data */
-    printf("Development Data : %s\n", buf+p+1);
-    break;
-  }
+       case 4: /* Measured Tracker Data Out */
+               printf("Measured Tracker Data Out\n");
+               p += 1;
+               p += 2;
+               p += 4;
+               printf("Chans : %d\n", buf[p]);
+               p += 1;
+               for (;p<n;) {
+                       printf("SV ID: %d Azimuth: %d Elevation: %d\n",
+                              buf[p], buf[p+1], buf[p+2]);
+                       p += 3;
+                       p += 2;
+                       for (i=0; i<10; i++) {
+                               printf("C/No %2d: %d dB-Hz\n", i+1, buf[p+i]);
+                       }
+                       p+= 10;
+               }
+               
+               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 */
+               printf("Clock Status Data\n");
+               break;
+               
+       case 8: /* 50 BPS Data */
+               printf("50 BPS Data\n");
+               p += 2;
+               printf("Channel %d, SV# %d\n", buf[p], buf[p+1]);
+               p += 2;
+               break;
+               
+       case 9: /* CPU Throughput */
+               printf("CPU Throughput\n");
+               break;
+               
+       case 11: /* Command Acknowledgment */
+               printf("Command Acknowledgment ID : %d\n",
+                      buf[p+1]);
+               p += 2;
+               break;
+               
+       case 28: /* Navigation Library Measurement Data */
+               printf("Navigation Library Measurement Data - Channel %d\n", buf[p+1]);
+               p += 2;
+               printf("  Time Tag = 0x%02x%02x%02x%02x\n", buf[p], buf[p+1], buf[p+2], buf[p+3]);
+               p += 4;
+               printf("  Satellite ID : %d\n", buf[p]);
+               p += 1;
+               printf("  GPS SW Time : %f s\n", get_sirf_dbl(buf+p));
+               p += 8;
+               printf("  Pseudorange = %f m\n", 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;
+               
+               break;
+               
+       case 30: /* Navigation Library SV State Data */
+               printf("Navigation Library SV State Data - Satellite ID : %d\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 41: /* Geodetic Navigation Data */
+               printf("Geodetic Navigation Data\n");
+               p += 1;
+               p += 2;
+               p += 2;
+               p += 2;
+               p += 4;
+               
+               printf("Y-M-D : %04d-%02d-%02d H:M:S : %02d:%02d:%02.3f\n",
+                      (buf[p]<<8)|buf[p+1], buf[p+2], buf[p+3],
+                      buf[p+4], buf[p+5], ((buf[p+6]<<8)|buf[p+7])/1000.0);
+               break;
+               
+       case 255: /* Development Data */
+               printf("Development Data : %s\n", buf+p+1);
+               break;
+       }
 
-  return 0;
+       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");
+       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) {
-  unsigned char buffer[1024]; /* SiRF message size is limited to 912 bytes */
-  int i, n;
-
-  static unsigned char sbuffer[912*2];
-  static int p = 0;
-
-  sleep(1);
-  n = read(fd, buffer, sizeof(buffer));
-  if (n > 0) {
-
-    if (p + n < sizeof(sbuffer)) {
-      memcpy(sbuffer + p, buffer, n);
-      p += n;
-      printf("sbuffer is now %d bytes\n", p);
-      //dump_msg(sbuffer, p);
-    } else {
-      printf("too much data received! (p=%d, n=%d)\n", p, n);
-    }
-
-
-    printf("%d bytes received :", p);
-    for (i=0; i<p; i++) {
-      printf(" 0x%02x", sbuffer[i]);
-    }
-    printf("\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;
-         //printf("start pattern found at offset %d (p=%d)\n", i, p);
-
-         if (i>0) {
-           printf("message: %d bytes skipped\n", i);
-           memmove(sbuffer, sbuffer+i, p-i);
-           p -= i;
-           //      printf("sbuffer is now %d bytes\n", p);
-           //      dump_msg(sbuffer, p);
-         }
-         break;
+       /* SiRF message size is limited to 912 bytes */
+       int i, n;
+
+       static unsigned char sbuffer[912*2];
+       static int p = 0;
+
+       n = read(fd, sbuffer + p, sizeof(sbuffer) - p);
+       if (n < 0) {
+               perror("read");
+               return -1;
+       }
+
+       if (n == 0) {
+               /* nothing to do */
+               return 0;
        }
-      }
 
-      /* search for stop pattern */
-      for (i=0; i<p-1; i++) {
-       if ((sbuffer[i] == 0xb0) && (sbuffer[i+1] == 0xb3)) {
-         stop_found = 1;
-         //printf("stop pattern found at offset %d\n", i);
+       /* 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) {
-           if (check_sirf_msg(sbuffer, i+2)) {
-             decode_sirf_msg(sbuffer, i+2);
-           }
-         }
+                               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);
+                                       }
+                               }
          
-         memmove(sbuffer, sbuffer+(i+2), p-(i+2));
-         p -= (i+2);
-         //      printf("sbuffer is now %d bytes\n", p);
-         //      dump_msg(sbuffer, p);
-         break;
-       }
-      }
+                               if (!start_found)
+                                       printf("recv: %d bytes skipped\n",i+2);
+                               memmove(sbuffer, sbuffer+(i+2), p-(i+2));
+                               p -= (i+2);
 
-      if (!stop_found)
-       break;
-    }
+                               break;
+                       }
+               }
 
-    fflush(stdout);
-  }
+               if (!stop_found)
+                       break;
+       }
+       
+       fflush(stdout);
 
-  return 0;
+       return 0;
 }
 
 int main(int argc, const char *argv[])
 {
-  const char device[] = "/dev/ttyUSB0";
-  int fd;
-  struct termios term;
-
-  fd = open(device, O_RDWR | O_NONBLOCK);
-  if (fd < 0) {
-    perror(device);
-    return -1;
-  }
+       const char device[] = "/dev/ttyUSB0";
+       int fd;
+       struct termios term;
+
+       /* open serial device */
+       fd = open(device, O_RDWR | O_NONBLOCK);
+       if (fd < 0) {
+               perror(device);
+               return -1;
+       }
 
-  if (tcgetattr(fd, &term) != 0) {
-    perror("tcgetattr");
-  }
-  cfsetispeed(&term, B4800);
-  cfsetospeed(&term, B4800);
-  if (tcsetattr(fd, TCSAFLUSH, &term) != 0) {
-    perror("tcsetattr");
-  }
+       /* switch to 4800 bauds */
+       if (tcgetattr(fd, &term) != 0) {
+               perror("tcgetattr");
+       }
+       cfsetispeed(&term, B4800);
+       cfsetospeed(&term, B4800);
+       if (tcsetattr(fd, TCSAFLUSH, &term) != 0) {
+               perror("tcsetattr");
+       }
 
-  /* Set Binary Serial Port */
-  /* Tested value : 
-     1200      1200 bit/s
-     2400      1200 bit/s
-     4800      2400 bit/s
-     9600      4800 bit/s
-     19200     4800 bit/s
-     57600     19200 bit/s
-     115200    no effect - 115200 
-  */
-  /*
-  unsigned int baud = 4800;
-  unsigned char msg_134[] = { 0x86, 
-                             (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, B9600);
-  cfsetospeed(&term, B9600);
-  if (tcsetattr(fd, TCSAFLUSH, &term) != 0) {
-    perror("tcsetattr");
-  }
-  */
-
-  /* Poll Software Version */
-  unsigned char msg_132[] = { 0x84, 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,
-                             (1<<4) };
-  sirf_msg_send(fd, msg_128, sizeof(msg_128));
-
-
-  for (;;) {
-    fd_set fdr;
-    int r;
-
-    FD_ZERO(&fdr);
-    FD_SET(fd, &fdr);
-
-    r = select(FD_SETSIZE, &fdr, NULL, NULL, NULL);
-    if (FD_ISSET(fd, &fdr)) {
-      do_read(fd);
-    }
-  }
+       /* Set Binary Serial Port */
+       /* Tested value : 
+            1200       1200 bit/s
+            2400       1200 bit/s
+            4800       2400 bit/s
+            9600       4800 bit/s
+           19200       4800 bit/s
+           57600       19200 bit/s
+          115200       no effect - 115200 
+       */
+
+       /*
+       unsigned int baud = 4800;
+       unsigned char msg_134[] = { 0x86, 
+                                   (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, B9600);
+       cfsetospeed(&term, B9600);
+       if (tcsetattr(fd, TCSAFLUSH, &term) != 0) {
+               perror("tcsetattr");
+       }
+       */
+
+       /* Poll Software Version */
+       unsigned char msg_132[] = { 0x84, 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 */
+                                   /* Enable raw track & debug data */
+                                   (1<<2) | (1<<4) | (1<<5) };
+       sirf_msg_send(fd, msg_128, sizeof(msg_128));
+
+       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)) {
+                       do_read(fd);
+               }
+       }
 
-  close (fd);
-  return 0;
+       close (fd);
+       return 0;
 }