Added sirf.c to decode SiRF messages (work in progress)
authorBenoit Papillault <benoit.papillault@free.fr>
Mon, 4 Jul 2011 16:37:16 +0000 (18:37 +0200)
committerBenoit Papillault <benoit.papillault@free.fr>
Mon, 4 Jul 2011 16:37:16 +0000 (18:37 +0200)
sirf.c [new file with mode: 0644]

diff --git a/sirf.c b/sirf.c
new file mode 100644 (file)
index 0000000..20105ca
--- /dev/null
+++ b/sirf.c
@@ -0,0 +1,198 @@
+/*
+  Author : Benoit Papillault <benoit.papillault@free.fr>
+  Creation : 2011-07-03
+  Goal : Understanding SiRF protocol for GPS devices
+*/
+
+#include <stdio.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>
+
+/* 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;
+  }
+
+  printf("valid message (%d bytes)\n", length);
+  return 1;
+}
+
+int do_read(int fd) {
+  unsigned char buffer[912]; /* SiRF message size is limited to 912 bytes */
+  int i, j, n;
+
+  static unsigned char sbuffer[912];
+  static int p = 0;
+
+  sleep(1);
+  n = read(fd, buffer, sizeof(buffer));
+  if (n > 0) {
+    printf("%d bytes read :", n);
+    for (i=0; i<n; i++) {
+      printf(" 0x%02x", buffer[i]);
+    }
+    printf("\n");
+
+    if (p + n < sizeof(sbuffer)) {
+      memcpy(sbuffer + p, buffer, n);
+      p += n;
+    } 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 :
+       - 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\n", i);
+
+         if (i>0) {
+           printf("message: %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;
+         printf("stop pattern found at offset %d\n", i);
+         
+         printf("%d bytes message :", i+2);
+         for (j=0; j<i+2; j++) {
+           printf(" 0x%02x", sbuffer[j]);
+         }
+         printf("\n");
+
+         if (start_found && stop_found) {
+           check_sirf_msg(sbuffer, i+2);
+         }
+         
+         memmove(sbuffer, sbuffer+i+2, i+2);
+         p -= i+2;
+       }
+      }
+
+      if (!stop_found)
+       break;
+    }
+  }
+
+  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;
+  }
+
+  if (tcgetattr(fd, &term) != 0) {
+    perror("tcgetattr");
+  }
+  cfsetispeed(&term, B4800);
+  cfsetospeed(&term, B4800);
+  if (tcsetattr(fd, TCSANOW, &term) != 0) {
+    perror("tcsetattr");
+  }
+
+  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);
+    }
+  }
+
+  close (fd);
+  return 0;
+}