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