Added SiRF message documentation (preliminary)
[gps] / sirf.c
1 /*
2   Author : Benoit Papillault <benoit.papillault@free.fr>
3   Creation : 2011-07-03
4   Goal : Understanding SiRF protocol for GPS devices
5 */
6
7 #include <stdio.h>
8 #include <sys/select.h>
9 #include <sys/types.h>
10 #include <sys/stat.h>
11 #include <fcntl.h>
12 #include <termios.h>
13 #include <unistd.h>
14 #include <string.h>
15
16 /* 
17  * (buf,n) is the payload of the message. This function adds the
18  * start/stop sequences, and the length/checksum fields
19  */
20
21 int sirf_msg_send(int fd, unsigned char *buf, int n) {
22   char sbuf[1024];
23   int i, computed_crc;
24
25   sbuf[0] = 0xa0;
26   sbuf[1] = 0xa2;
27   sbuf[2] = n>>8;
28   sbuf[3] = n&0xff;
29   memcpy(sbuf+4, buf, n);
30
31   computed_crc = 0;
32   for (i=0; i<n; i++) {
33     computed_crc += buf[i];
34   }
35   computed_crc &= 0x7fff;
36
37   sbuf[4+n+0] = computed_crc>>8;
38   sbuf[4+n+1] = computed_crc&0xff;
39   sbuf[4+n+2] = 0xb0;
40   sbuf[4+n+3] = 0xb3;
41
42   return write(fd, sbuf, 4+n+4);
43 }
44
45 /* return 1 if message is a valid SiRF message, 0 if invalid */
46
47 int check_sirf_msg(unsigned char *buf, int n) {
48   int length, crc, computed_crc, i;
49
50   /* check size (at least 8 bytes) */
51   if (n<8) {
52     printf("msg too small (%d bytes)\n", n);
53     return 0;
54   }
55
56   /* check start pattern */
57   if ((buf[0] != 0xa0) || (buf[1] != 0xa2)) {
58     printf("invalid start pattern : 0x%02x 0x%02x\n", buf[0], buf[1]);
59     return 0;
60   }
61   
62   /* check length */
63   length = ((buf[2]<<8) | (buf[3]));
64   if (length & 0x8000) {
65     printf("invalid length : 0x%04x bytes\n", length);
66     return 0;
67   }
68
69   if (length > 912) {
70     printf("warning : length (%d bytes) is above 912\n", length);
71   }
72
73   if (4 + length + 4 != n) {
74     printf("invalid length : %d bytes, buf buffer is %d bytes\n", length, n);
75     return 0;
76   }
77   
78   /* check checksum */
79   crc = ((buf[n-4]<<8) | (buf[n-3]));
80   if (crc & 0x8000) {
81     printf("invalid crc : 0x%04x\n", crc);
82     return 0;
83   }
84
85   computed_crc = 0;
86   for (i=4; i<n-4; i++) {
87     computed_crc += buf[i];
88   }
89   computed_crc &= 0x7fff;
90
91   if (computed_crc != crc) {
92     printf("invalid crc : 0x%04x computed crc : 0x%04x\n", crc, computed_crc);
93     return 0;
94   }
95   
96   /* check stop pattern */
97   if ((buf[n-2] != 0xb0) || (buf[n-1] != 0xb3)) {
98     printf("invalid stop pattern : 0x%02x 0x%02x\n", buf[n-2], buf[n-1]);
99     return 0;
100   }
101
102   printf("valid message (%d bytes)\n", length);
103   return 1;
104 }
105
106 double get_sirf_dbl(unsigned char *buf) {
107   double r;
108   unsigned char * ptr = (unsigned char *)&r;
109
110   ptr[0] = buf[3];
111   ptr[1] = buf[2];
112   ptr[2] = buf[1];
113   ptr[3] = buf[0];
114   ptr[4] = buf[7];
115   ptr[5] = buf[6];
116   ptr[6] = buf[5];
117   ptr[7] = buf[4];
118
119   if (sizeof(double) == 8) {
120     return r;
121   } else {
122     printf("get_sirf_dbl implementation error\n");
123   }
124 }
125
126 float get_sirf_sgl(unsigned char *buf) {
127   float r;
128   unsigned char *ptr = (unsigned char *)&r;
129
130   ptr[0] = buf[3];
131   ptr[1] = buf[2];
132   ptr[2] = buf[1];
133   ptr[3] = buf[0];
134
135   if (sizeof(float) == 4) {
136     return r;
137   } else {
138     printf("get_sirf_sgl implementation error\n");
139   }
140 }
141
142 /* (buf,n) is a full SiRF message, including start/stop sequence */
143
144 int decode_sirf_msg(unsigned char *buf, int n) {
145   int i;
146   int p = 4;
147
148   printf("... decoding Message ID : %d\n", buf[p]);
149   switch (buf[p]) {
150   case 2: /* Measured Navigation Data Out */
151     printf("Measured Navigation Data\n");
152     p += 1;
153     p += 4;
154     p += 4;
155     p += 4;
156     p += 2;
157     p += 2;
158     p += 2;
159     p += 3;
160     p += 2;
161     p += 4;
162     printf("#SV : %d\n", buf[p]);
163     p += 1;
164     for (i=0; i<12; i++) {
165       printf("CH %d PRN : %d\n", i+1, buf[p+i]);
166     }
167     break;
168
169   case 4: /* Measured Tracker Data Out */
170     printf("Measured Tracker Data Out\n");
171     p += 1;
172     p += 2;
173     p += 4;
174     printf("Chans : %d\n", buf[p]);
175     p += 1;
176     for (;p<n;) {
177       printf("SV ID: %d Azimuth: %d Elevation: %d\n",
178              buf[p], buf[p+1], buf[p+2]);
179       p += 3;
180       p += 2;
181       for (i=0; i<10; i++) {
182         printf("C/No %2d: %d dB-Hz\n", i+1, buf[p+i]);
183       }
184       p+= 10;
185     }
186     
187     break;
188
189   case 6: /* Software Version String */
190     printf("Software Version String : %s\n", buf+p+1);
191     /* for GPS USB : GSW3.5.0_3.5.00.00-SDK-3EP2.01 */
192     p += 1;
193     break;
194
195   case 7: /* Clock Status Data */
196     printf("Clock Status Data\n");
197     break;
198
199   case 8: /* 50 BPS Data */
200     printf("50 BPS Data\n");
201     p += 2;
202     printf("Channel %d, SV# %d\n", buf[p], buf[p+1]);
203     p += 2;
204     break;
205
206   case 9: /* CPU Throughput */
207     printf("CPU Throughput\n");
208     break;
209
210   case 11: /* Command Acknowledgment */
211     printf("Command Acknowledgment ID : %d\n",
212            buf[p+1]);
213     p += 2;
214     break;
215
216   case 28: /* Navigation Library Measurement Data */
217     printf("Navigation Library Measurement Data - Channel %d\n", buf[p+1]);
218     p += 2;
219     printf("  Time Tag = 0x%02x%02x%02x%02x\n", buf[p], buf[p+1], buf[p+2], buf[p+3]);
220     p += 4;
221     printf("  Satellite ID : %d\n", buf[p]);
222     p += 1;
223     printf("  GPS SW Time : %f s\n", get_sirf_dbl(buf+p));
224     p += 8;
225     printf("  Pseudorange = %f m\n", get_sirf_dbl(buf+p));
226     p += 8;
227     printf("  Carrier frequency = %f m/s\n", get_sirf_sgl(buf+p));
228     p += 4;
229     printf("  Carrier phase = %f m\n", get_sirf_dbl(buf+p));
230     p += 8;
231     
232     break;
233
234   case 30: /* Navigation Library SV State Data */
235     printf("Navigation Library SV State Data - Satellite ID : %d\n", buf[p+1]);
236     p += 2;
237     printf("  GPS Time : %f s\n", get_sirf_dbl(buf+p));
238     p += 8;
239     printf("  Position X : %f m\n", get_sirf_dbl(buf+p));
240     p += 8;
241     printf("  Position Y : %f m\n", get_sirf_dbl(buf+p));
242     p += 8;
243     printf("  Position Z : %f m\n", get_sirf_dbl(buf+p));
244     p += 8;
245     printf("  Velocity X : %f m/s\n", get_sirf_dbl(buf+p));
246     p += 8;
247     printf("  Velocity Y : %f m/s\n", get_sirf_dbl(buf+p));
248     p += 8;
249     printf("  Velocity Z : %f m/s\n", get_sirf_dbl(buf+p));
250     p += 8;
251     printf("  Clock bias : %f s\n", get_sirf_dbl(buf+p));
252     p += 8;
253     printf("  Clock drift : %f s/s\n", get_sirf_sgl(buf+p));
254     p += 4;
255     p += 1;
256     p += 4;
257     p += 4;
258     printf("  Ionospheric delay : %f m\n", get_sirf_sgl(buf+p));
259     p += 4;
260     break;
261
262   case 41: /* Geodetic Navigation Data */
263     printf("Geodetic Navigation Data\n");
264     p += 1;
265     p += 2;
266     p += 2;
267     p += 2;
268     p += 4;
269
270     printf("Y-M-D : %04d-%02d-%02d H:M:S : %02d:%02d:%02.3f\n",
271            (buf[p]<<8)|buf[p+1], buf[p+2], buf[p+3],
272            buf[p+4], buf[p+5], ((buf[p+6]<<8)|buf[p+7])/1000.0);
273     break;
274
275   case 255: /* Development Data */
276     printf("Development Data : %s\n", buf+p+1);
277     break;
278   }
279
280   return 0;
281 }
282
283 void dump_msg(unsigned char *buf, int n) {
284   int i;
285
286   printf("%d bytes message :", n);
287   for (i=0; i<n; i++) {
288     printf(" 0x%02x", buf[i]);
289   }
290   printf("\n");
291 }
292
293 int do_read(int fd) {
294   unsigned char buffer[1024]; /* SiRF message size is limited to 912 bytes */
295   int i, n;
296
297   static unsigned char sbuffer[912*2];
298   static int p = 0;
299
300   sleep(1);
301   n = read(fd, buffer, sizeof(buffer));
302   if (n > 0) {
303
304     if (p + n < sizeof(sbuffer)) {
305       memcpy(sbuffer + p, buffer, n);
306       p += n;
307       printf("sbuffer is now %d bytes\n", p);
308       //dump_msg(sbuffer, p);
309     } else {
310       printf("too much data received! (p=%d, n=%d)\n", p, n);
311     }
312
313
314     printf("%d bytes received :", p);
315     for (i=0; i<p; i++) {
316       printf(" 0x%02x", sbuffer[i]);
317     }
318     printf("\n");
319
320     /* small parsing of received data in (sbuffer, p) : 
321        - start pattern = 0xa0 0xa2
322        - stop  pattern = 0xb0 0xb3
323
324        SiRF message format is :
325        <0xa0 0xa2><length (2 bytes)><length bytes><CRC (2 byte)><0xb0 0xb3>
326     */
327
328     for (;;) {
329       int start_found = 0, stop_found = 0;
330
331       /* search for start pattern */
332       for (i=0; i<p-1; i++) {
333         if ((sbuffer[i] == 0xa0) && (sbuffer[i+1] == 0xa2)) {
334           start_found = 1;
335           //printf("start pattern found at offset %d (p=%d)\n", i, p);
336
337           if (i>0) {
338             printf("message: %d bytes skipped\n", i);
339             memmove(sbuffer, sbuffer+i, p-i);
340             p -= i;
341             //      printf("sbuffer is now %d bytes\n", p);
342             //      dump_msg(sbuffer, p);
343           }
344           break;
345         }
346       }
347
348       /* search for stop pattern */
349       for (i=0; i<p-1; i++) {
350         if ((sbuffer[i] == 0xb0) && (sbuffer[i+1] == 0xb3)) {
351           stop_found = 1;
352           //printf("stop pattern found at offset %d\n", i);
353           
354           if (start_found && stop_found) {
355             if (check_sirf_msg(sbuffer, i+2)) {
356               decode_sirf_msg(sbuffer, i+2);
357             }
358           }
359           
360           memmove(sbuffer, sbuffer+(i+2), p-(i+2));
361           p -= (i+2);
362           //      printf("sbuffer is now %d bytes\n", p);
363           //      dump_msg(sbuffer, p);
364           break;
365         }
366       }
367
368       if (!stop_found)
369         break;
370     }
371
372     fflush(stdout);
373   }
374
375   return 0;
376 }
377
378 int main(int argc, const char *argv[])
379 {
380   const char device[] = "/dev/ttyUSB0";
381   int fd;
382   struct termios term;
383
384   fd = open(device, O_RDWR | O_NONBLOCK);
385   if (fd < 0) {
386     perror(device);
387     return -1;
388   }
389
390   if (tcgetattr(fd, &term) != 0) {
391     perror("tcgetattr");
392   }
393   cfsetispeed(&term, B115200);
394   cfsetospeed(&term, B115200);
395   if (tcsetattr(fd, TCSAFLUSH, &term) != 0) {
396     perror("tcsetattr");
397   }
398
399   /* Set Binary Serial Port */
400   /* Tested value : 
401      1200       1200 bit/s
402      2400       1200 bit/s
403      4800       2400 bit/s
404      9600       4800 bit/s
405      19200      4800 bit/s
406      57600      19200 bit/s
407      115200     no effect - 115200 
408   */
409   /*
410   unsigned int baud = 4800;
411   unsigned char msg_134[] = { 0x86, 
412                               (baud >> 24) & 0xff,
413                               (baud >> 16) & 0xff,
414                               (baud >>  8) & 0xff,
415                               (baud >>  0) & 0xff,
416                               8, 1, 0, 0 };
417   sirf_msg_send(fd, msg_134, sizeof(msg_134));
418   */
419   /*
420   if (tcgetattr(fd, &term) != 0) {
421     perror("tcgetattr");
422   }
423   cfsetispeed(&term, B9600);
424   cfsetospeed(&term, B9600);
425   if (tcsetattr(fd, TCSAFLUSH, &term) != 0) {
426     perror("tcsetattr");
427   }
428   */
429
430   /* Poll Software Version */
431   unsigned char msg_132[] = { 0x84, 0x00 };
432   sirf_msg_send(fd, msg_132, sizeof(msg_132));
433
434   /* Initialize GPS/DR Navigation */
435   /*
436   unsigned char msg_172[] = { 0xac, 0x01, 
437                               0x00, 0x00, 0x00, 0x00,
438                               0x00, 0x00, 0x00, 0x00,
439                               0x00, 0x00, 0x00, 0x00,
440                               0x00, 0x00,
441                               0x00, 0x00, 0x00, 0x00,
442                               0x00, 0x00, 0x00, 0x00,
443                               0x00, 0x00,
444                               0x00,
445                               (1<<4)|(1<<5)};
446   sirf_msg_send(fd, msg_172, sizeof(msg_172));
447   */
448
449   /* Initialize Data Source */
450
451   unsigned char msg_128[] = { 128,
452                               0x00, 0x00, 0x00, 0x00,
453                               0x00, 0x00, 0x00, 0x00,
454                               0x00, 0x00, 0x00, 0x00,
455                               0x00, 0x00, 0x00, 0x00,
456                               0x00, 0x00, 0x00, 0x00,
457                               0x00, 0x00,
458                               12,
459                               (1<<4) };
460   sirf_msg_send(fd, msg_128, sizeof(msg_128));
461
462
463   for (;;) {
464     fd_set fdr;
465     int r;
466
467     FD_ZERO(&fdr);
468     FD_SET(fd, &fdr);
469
470     r = select(FD_SETSIZE, &fdr, NULL, NULL, NULL);
471     if (FD_ISSET(fd, &fdr)) {
472       do_read(fd);
473     }
474   }
475
476   close (fd);
477   return 0;
478 }