Added lots of message decoding
authorBenoit Papillault <benoit.papillault@free.fr>
Wed, 21 Dec 2011 21:59:16 +0000 (22:59 +0100)
committerBenoit Papillault <benoit.papillault@free.fr>
Wed, 21 Dec 2011 21:59:16 +0000 (22:59 +0100)
Proper message sending (waiting for ACK or NACK)
Retested baud settings (works)

sirf.c

diff --git a/sirf.c b/sirf.c
index 00a047b..219a1b8 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. 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;
-       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;
-       }
-
-       printf("send: Message ID %d (%d bytes)\n", buf[0], total_len);
-
-       return 0;
-}
-
 /* return 1 if message is a valid SiRF message, 0 if invalid */
 
 int check_sirf_msg(unsigned char *buf, int n) {
@@ -143,6 +97,22 @@ double get_sirf_dbl(unsigned char *buf) {
   }
 }
 
+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;
@@ -159,51 +129,282 @@ float get_sirf_sgl(unsigned char *buf) {
   }
 }
 
+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 PRN : %3d\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 : %u\n", get_sirf_4u(buf + p));
+       p += 4;
+       printf("  Channels : %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;
+               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;
+
+       printf("Clock Status Data\n");
+       p += 1;
+       printf("  Extended GPS Week : %u\n", get_sirf_2u(buf + p));
+       p += 2;
+       printf("  GPS Time of Week : %f s\n",
+              (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;
+       printf("  Clock Bias : %u ns\n", get_sirf_4u(buf + p));
+       p += 4;
+       printf("  Estimated GPS Time : %u ms\n", get_sirf_4u(buf + p));
+       p += 4;
+
+       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("  PRN %2d : 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("  PRN %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;
+
+       printf("Navigation Library Measurement Data - Channel %d\n", buf[p+1]);
+       p += 2;
+       printf("  Time Tag = 0x%08x\n", get_sirf_4u(buf+p));
+       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;
+
+       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_50(unsigned char *buf, int n)
+{
+       int p = 0;
+
+       printf("SBAS Parameters\n");
+       p += 1;
+       printf("  SBAS PRN : %3d\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 i;
+int decode_sirf_msg(unsigned char *buf, int n, int *ack) {
        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]);
-               }
+               decode_sirf_msg_2(buf + 4, n - 8);
                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;
-               }
-               
+               decode_sirf_msg_4(buf + 4, n - 8);
                break;
                
        case 6: /* Software Version String */
@@ -213,14 +414,11 @@ int decode_sirf_msg(unsigned char *buf, int n) {
                break;
                
        case 7: /* Clock Status Data */
-               printf("Clock Status Data\n");
+               decode_sirf_msg_7(buf + 4, n - 8);
                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;
+               decode_sirf_msg_8(buf + 4, n - 8);
                break;
                
        case 9: /* CPU Throughput */
@@ -228,27 +426,29 @@ int decode_sirf_msg(unsigned char *buf, int n) {
                break;
                
        case 11: /* Command Acknowledgment */
-               printf("Command Acknowledgment ID : %d\n",
+               printf("Command ACK ID : %d\n",
                       buf[p+1]);
+               *ack = buf[p+1];
                p += 2;
                break;
                
-       case 28: /* Navigation Library Measurement Data */
-               printf("Navigation Library Measurement Data - Channel %d\n", buf[p+1]);
+       case 12: /* Command Negative Acknowledgment */
+               printf("Command NACK ID : %d\n",
+                      buf[p+1]);
+               *ack = 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 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 */
@@ -278,6 +478,10 @@ int decode_sirf_msg(unsigned char *buf, int n) {
                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 */
                printf("Geodetic Navigation Data\n");
@@ -291,12 +495,25 @@ int decode_sirf_msg(unsigned char *buf, int 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 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 */
-               printf("Development Data : %s\n", buf+p+1);
+               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;
 }
 
@@ -312,15 +529,21 @@ void dump_msg(unsigned char *buf, int n) {
 
 /* return 0 on success, -1 on error */
 
-int do_read(int fd) {
+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) {
+       if (n <= 0) {
                perror("read");
                return -1;
        }
@@ -375,7 +598,8 @@ int do_read(int fd) {
                                        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);
+                                               decode_sirf_msg(sbuffer, i+2,
+                                                               ack);
                                        }
                                }
          
@@ -392,7 +616,67 @@ int do_read(int fd) {
                        break;
        }
        
-       fflush(stdout);
+       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;
 }
@@ -402,9 +686,10 @@ int main(int argc, const char *argv[])
        const char device[] = "/dev/ttyUSB0";
        int fd;
        struct termios term;
+       int i;
 
        /* open serial device */
-       fd = open(device, O_RDWR | O_NONBLOCK);
+       fd = open(device, O_RDWR /*| O_NONBLOCK*/);
        if (fd < 0) {
                perror(device);
                return -1;
@@ -414,40 +699,41 @@ int main(int argc, const char *argv[])
        if (tcgetattr(fd, &term) != 0) {
                perror("tcgetattr");
        }
-       cfsetispeed(&term, B4800);
-       cfsetospeed(&term, B4800);
-       if (tcsetattr(fd, TCSAFLUSH, &term) != 0) {
+       cfsetispeed(&term, B115200);
+       cfsetospeed(&term, B115200);
+       if (tcsetattr(fd, TCSANOW, &term) != 0) {
                perror("tcsetattr");
        }
 
-       /* Set Binary Serial Port */
+       /* Set Binary Serial Port.  Note : the effect of changing the baud
+          rate is not immediate at all */
+
        /* 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
+            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       no effect - 115200 
        */
 
-       /*
-       unsigned int baud = 4800;
-       unsigned char msg_134[] = { 0x86, 
+       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));
-       */
+       //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) {
+       cfsetispeed(&term, B1200);
+       cfsetospeed(&term, B1200);
+       if (tcsetattr(fd, TCSANOW, &term) != 0) {
                perror("tcsetattr");
        }
        */
@@ -480,11 +766,54 @@ int main(int argc, const char *argv[])
                                    0x00, 0x00, 0x00, 0x00,
                                    0x00, 0x00,
                                    12,
+                                   0
                                    /* Clear memory */
+                                   /* (1<<2) | */
                                    /* Enable raw track & debug data */
-                                   (1<<2) | (1<<4) | (1<<5) };
+                                   /* (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;
@@ -499,7 +828,12 @@ int main(int argc, const char *argv[])
                }
 
                if (FD_ISSET(fd, &fdr)) {
-                       do_read(fd);
+                       int ack = 0;
+                       if (do_read(fd, &ack) < 0)
+                               break;
+                       if (ack != 0) {
+                               printf("ACK for cmd %d\n", ack);
+                       }
                }
        }