Added SiRF message documentation (preliminary)
[gps] / sirf.c
diff --git a/sirf.c b/sirf.c
index 20105ca..01dd87e 100644 (file)
--- a/sirf.c
+++ b/sirf.c
 #include <unistd.h>
 #include <string.h>
 
+/* 
+ * (buf,n) is the payload of the message. This function adds the
+ * start/stop sequences, and the length/checksum fields
+ */
+
+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;
+
+  sbuf[4+n+0] = computed_crc>>8;
+  sbuf[4+n+1] = computed_crc&0xff;
+  sbuf[4+n+2] = 0xb0;
+  sbuf[4+n+3] = 0xb3;
+
+  return write(fd, sbuf, 4+n+4);
+}
+
 /* return 1 if message is a valid SiRF message, 0 if invalid */
 
 int check_sirf_msg(unsigned char *buf, int n) {
@@ -74,37 +103,221 @@ int check_sirf_msg(unsigned char *buf, int n) {
   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");
+  }
+}
+
+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");
+  }
+}
+
+/* (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;
+    
+    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;
+}
+
+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 do_read(int fd) {
-  unsigned char buffer[912]; /* SiRF message size is limited to 912 bytes */
-  int i, j, n;
+  unsigned char buffer[1024]; /* SiRF message size is limited to 912 bytes */
+  int i, n;
 
-  static unsigned char sbuffer[912];
+  static unsigned char sbuffer[912*2];
   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;
+      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 :
+
+    /* small parsing of received data in (sbuffer, p) : 
        - start pattern = 0xa0 0xa2
        - stop  pattern = 0xb0 0xb3
 
@@ -119,12 +332,14 @@ int do_read(int fd) {
       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);
+         //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;
        }
@@ -134,26 +349,27 @@ int do_read(int fd) {
       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("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);
+           if (check_sirf_msg(sbuffer, i+2)) {
+             decode_sirf_msg(sbuffer, i+2);
+           }
          }
          
-         memmove(sbuffer, sbuffer+i+2, i+2);
-         p -= 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 (!stop_found)
        break;
     }
+
+    fflush(stdout);
   }
 
   return 0;
@@ -174,11 +390,75 @@ int main(int argc, const char *argv[])
   if (tcgetattr(fd, &term) != 0) {
     perror("tcgetattr");
   }
-  cfsetispeed(&term, B4800);
-  cfsetospeed(&term, B4800);
-  if (tcsetattr(fd, TCSANOW, &term) != 0) {
+  cfsetispeed(&term, B115200);
+  cfsetospeed(&term, B115200);
+  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;