c83c57ecfc572ab4929d17b161e56fe17d81d7f7
[gps] / sirf2.cpp
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 <stdlib.h>
9 #include <sys/select.h>
10 #include <sys/types.h>
11 #include <sys/stat.h>
12 #include <fcntl.h>
13 #include <termios.h>
14 #include <unistd.h>
15 #include <string.h>
16
17 #include "RinexObsStream.hpp"
18 #include "RinexObsHeader.hpp"
19 #include "RinexObsData.hpp"
20
21 #include "DayTime.hpp"
22 #include "icd_200_constants.hpp"
23
24 using namespace std;
25 using namespace gpstk;
26
27 const char *g_rinex = NULL;
28
29 int     last_ext_gps_week = 0;
30 double  last_gps_seconds = 0.0;
31
32 RinexObsStream  r_out;
33 RinexObsHeader  r_header;
34 RinexObsData    r_data;
35
36 int rinex_open(const char *filename) {
37         r_out.open(filename, ios::out|ios::trunc);
38
39         return 0; 
40 }
41
42 int rinex_header_write() {
43         // fill in the header
44         DayTime now;
45
46         /* for 2.11 header, we need : 
47            versionValid
48            runByValid
49            markerNameValid
50            observerValid
51            receiverValid
52            antennaTypeValid
53            antennaPositionValid
54            antennaOffsetValid
55            waveFactValid
56            obsTypeValid
57            firstTimeValid
58            endValid
59         */
60         r_header.version = 2.11;
61         r_header.fileType = "Observation";
62         r_header.system.system = SatID::systemGPS;
63         r_header.valid |= RinexObsHeader::versionValid;
64
65         r_header.fileProgram = "sirf2";
66         r_header.fileAgency = "Benoit Papillault";
67         r_header.date = now.printf("%Y%m%d");
68         r_header.valid |= RinexObsHeader::runByValid;
69
70         r_header.markerName = "GPSL"; // GPS from LUCEOR
71         r_header.valid |= RinexObsHeader::markerNameValid;
72
73         r_header.observer = "Automatic";
74         r_header.agency = "LUCEOR";
75         r_header.valid |= RinexObsHeader::observerValid;
76
77         r_header.recNo = "BU-353";
78         r_header.recType = "SiRF binary";
79         r_header.recVers = "GSW3.5.0";
80         r_header.valid |= RinexObsHeader::receiverValid;
81
82         r_header.antNo = "USB";
83         r_header.antType = "Integrated";
84         r_header.valid |= RinexObsHeader::antennaTypeValid;
85
86         /* FIXME : should be a real ECEF position ? */
87         r_header.antennaPosition[0] = 0.0;
88         r_header.antennaPosition[1] = 0.0;
89         r_header.antennaPosition[2] = 0.0;
90         r_header.valid |= RinexObsHeader::antennaPositionValid;
91
92         r_header.antennaOffset[0] = 0.0;
93         r_header.antennaOffset[1] = 0.0;
94         r_header.antennaOffset[2] = 0.0;
95         r_header.valid |= RinexObsHeader::antennaOffsetValid;
96
97         r_header.wavelengthFactor[0] = 1;
98         r_header.wavelengthFactor[1] = 1;
99         r_header.valid |= RinexObsHeader::waveFactValid;
100
101         r_header.numObs = 2;
102         r_header.obsTypeList.push_back(RinexObsHeader::C1);
103         r_header.obsTypeList.push_back(RinexObsHeader::S1);
104         r_header.valid |= RinexObsHeader::obsTypeValid;
105
106         r_header.firstObs = now;
107         r_header.firstSystem.system = SatID::systemGPS;
108         r_header.valid |= RinexObsHeader::firstTimeValid;
109
110         r_header.valid |= RinexObsHeader::endValid;
111
112         // write the header
113         r_out << r_header;
114
115         return 0;
116 }
117
118 // write RINEX data record (some part are already filled)
119 int rinex_data_write() {
120
121         // write nothing if no satellite have been seen
122         if (r_data.obs.size() == 0)
123                 return 0;
124
125         // fill in the missing pieces
126         r_data.epochFlag = 0;
127         r_data.time.setGPSfullweek(last_ext_gps_week, last_gps_seconds);
128         r_data.numSvs = r_data.obs.size();
129         r_data.clockOffset = 0.0;
130         /* r_data_.obs = set by rinex_data_add_pseudorange */
131         
132         // write the data epoch
133         r_out << r_data;
134
135         // reset r_data
136         r_data.obs.clear();
137
138         return 0;
139 }
140
141 /* from Message ID 28 */
142 int rinex_data_add_pseudorange_snr(int satNR,
143                                    double gps_seconds,
144                                    double pseudorange,
145                                    double snr) {
146         printf("RINEX range\t| GPS seconds %6f SatID %3u C1 %f\n",
147                gps_seconds, satNR, pseudorange);
148
149         // compute ssi
150         int ssi = (int)(snr / 6.0);
151         if (ssi < 0)
152                 ssi = 0;
153         if (ssi > 9)
154                 ssi = 9;
155
156         // we only set GPS seconds
157         last_gps_seconds = gps_seconds;
158
159         SatID sat(satNR, SatID::systemGPS);
160         r_data.obs[sat][RinexObsHeader::C1].data = pseudorange;
161         r_data.obs[sat][RinexObsHeader::C1].lli = 0;
162         r_data.obs[sat][RinexObsHeader::C1].ssi = ssi;
163
164         r_data.obs[sat][RinexObsHeader::S1].data = (double)snr;
165         r_data.obs[sat][RinexObsHeader::S1].lli = 0;
166         r_data.obs[sat][RinexObsHeader::S1].ssi = ssi;
167
168         return 0;
169 }
170
171 /* from Message ID 7 */
172 int rinex_data_add_time_clock_bias(int ext_gps_week,
173                                    double gps_seconds, double clock_bias) {
174         printf("RINEX time\t| GPS week %5u GPS seconds %6f\n",
175                ext_gps_week, gps_seconds);
176         
177         // set extended GPS week only
178         last_ext_gps_week = ext_gps_week;
179         
180         // adjust using clock bias
181         last_gps_seconds -= clock_bias;
182         
183         RinexObsData::RinexSatMap::iterator it;
184         for (it = r_data.obs.begin(); it != r_data.obs.end(); ++it) {
185                 double old_val = (*it).second[RinexObsHeader::C1].data;
186                 double new_val = old_val - clock_bias * C_GPS_M;
187                 (*it).second[RinexObsHeader::C1].data = new_val;
188
189                 printf("RINEX satID %3u : pseudo range %f => %f\n",
190                        (*it).first.id, old_val, new_val);
191         }
192
193         /* write data */
194         rinex_data_write();
195
196         return 0;
197 }
198
199 /* return 1 if message is a valid SiRF message, 0 if invalid */
200
201 int check_sirf_msg(unsigned char *buf, int n) {
202         int length, crc, computed_crc, i;
203
204         /* check size (at least 8 bytes) */
205         if (n<8) {
206                 printf("msg too small (%d bytes)\n", n);
207                 return 0;
208         }
209
210         /* check start pattern */
211         if ((buf[0] != 0xa0) || (buf[1] != 0xa2)) {
212                 printf("invalid start pattern : 0x%02x 0x%02x\n",
213                        buf[0], buf[1]);
214                 return 0;
215         }
216   
217         /* check length */
218         length = ((buf[2]<<8) | (buf[3]));
219         if (length & 0x8000) {
220                 printf("invalid length : 0x%04x bytes\n", length);
221                 return 0;
222         }
223
224         if (length > 912) {
225                 printf("warning : length (%d bytes) is above 912\n", length);
226         }
227
228         if (4 + length + 4 != n) {
229                 printf("invalid length : %d bytes, buf buffer is %d bytes\n",
230                        length, n);
231                 return 0;
232         }
233   
234         /* check checksum */
235         crc = ((buf[n-4]<<8) | (buf[n-3]));
236         if (crc & 0x8000) {
237                 printf("invalid crc : 0x%04x\n", crc);
238                 return 0;
239         }
240
241         computed_crc = 0;
242         for (i=4; i<n-4; i++) {
243                 computed_crc += buf[i];
244         }
245         computed_crc &= 0x7fff;
246
247         if (computed_crc != crc) {
248                 printf("invalid crc : 0x%04x computed crc : 0x%04x\n",
249                        crc, computed_crc);
250                 return 0;
251         }
252   
253         /* check stop pattern */
254         if ((buf[n-2] != 0xb0) || (buf[n-1] != 0xb3)) {
255                 printf("invalid stop pattern : 0x%02x 0x%02x\n",
256                        buf[n-2], buf[n-1]);
257                 return 0;
258         }
259
260         return 1;
261 }
262
263 double get_sirf_dbl(unsigned char *buf) {
264   double r;
265   unsigned char * ptr = (unsigned char *)&r;
266
267   ptr[0] = buf[3];
268   ptr[1] = buf[2];
269   ptr[2] = buf[1];
270   ptr[3] = buf[0];
271   ptr[4] = buf[7];
272   ptr[5] = buf[6];
273   ptr[6] = buf[5];
274   ptr[7] = buf[4];
275
276   if (sizeof(double) == 8) {
277     return r;
278   } else {
279     printf("get_sirf_dbl implementation error\n");
280   }
281 }
282
283 int get_sirf_2s(unsigned char *buf) {
284         return (((signed char)buf[0])<<8) | buf[1];
285 }
286
287 int get_sirf_4s(unsigned char *buf) {
288         return (((signed char)buf[0])<<24)|(buf[1]<<16)|(buf[2]<<8)|buf[3];
289 }
290
291 unsigned int get_sirf_2u(unsigned char *buf) {
292         return (buf[0]<<8) | buf[1];
293 }
294
295 unsigned int get_sirf_4u(unsigned char *buf) {
296         return (buf[0]<<24) | (buf[1]<<16) | (buf[2]<<8) | buf[3];
297 }
298
299 float get_sirf_sgl(unsigned char *buf) {
300   float r;
301   unsigned char *ptr = (unsigned char *)&r;
302
303   ptr[0] = buf[3];
304   ptr[1] = buf[2];
305   ptr[2] = buf[1];
306   ptr[3] = buf[0];
307
308   if (sizeof(float) == 4) {
309     return r;
310   } else {
311     printf("get_sirf_sgl implementation error\n");
312   }
313 }
314
315 int decode_sirf_msg_2(unsigned char * buf, int n)
316 {
317         int i, p = 0, n_sat;
318         unsigned char mode;
319         const char * altmode = "";
320         const char * pmode = "";
321
322         printf("Measure Navigation Data Out\n");
323         p += 1;
324         printf("  X-position : %d m\n", get_sirf_4s(buf+p));
325         p += 4;
326         printf("  Y-position : %d m\n", get_sirf_4s(buf+p));
327         p += 4;
328         printf("  Z-position : %d m\n", get_sirf_4s(buf+p));
329         p += 4;
330         printf("  X-velocity : %f m/s\n", (double)get_sirf_2s(buf+p) / 8.0);
331         p += 2;
332         printf("  Y-velocity : %f m/s\n", (double)get_sirf_2s(buf+p) / 8.0);
333         p += 2;
334         printf("  Z-velocity : %f m/s\n", (double)get_sirf_2s(buf+p) / 8.0);
335         p += 2;
336         /* Mode 1 */
337         mode = buf[p];
338
339         switch ((mode >> 4) & 3) {
340         case 0:
341                 altmode = "No altitude hold applied"; break;
342         case 1 :
343                 altmode = "Holding of altitude from KF"; break;
344         case 2:
345                 altmode = "Holding of altitude from user input"; break;
346         case 3:
347                 altmode = "Always hold altitude"; break;
348         }
349
350         switch ( mode & 7) {
351         case 0:
352                 pmode = "No navigation solution"; break;
353         case 1:
354                 pmode = "1-SV solution (Kalman filter)"; break;
355         case 2:
356                 pmode = "2-SV solution (Kalman filter)"; break;
357         case 3:
358                 pmode = "3-SV solution (Kalman filter)"; break;
359         case 4:
360                 pmode = ">3S-SV solution (Kalman filter)"; break;
361         case 5:
362                 pmode = "2D solution (least squares)"; break;
363         case 6:
364                 pmode = "3D solution (least squares)"; break;
365         case 7:
366                 pmode = "Dead-Reckoning solution (no satellites)"; break;
367         }
368
369         printf("  Mode 1 : %02x %s %s %s %s %s\n", 
370                mode ,
371                mode & (1<<7) ? "DGPS:Yes" : "DGPS:No",
372                mode & (1<<6) ? "DOP mask exceeded " : "",
373                altmode, 
374                mode & (1<<3) ? "TicklePower position " : "",
375                pmode);
376         p += 1;
377         /* HDOP */
378         printf("  HDOP : %f\n", (double)(unsigned)buf[p] / 5.0);
379         p += 1;
380         /* Mode 2 */
381         mode = buf[p];
382         printf("  Mode 2 : %02x\n", mode);
383         p += 1;
384         printf("  GPS Week : %u\n", get_sirf_2u(buf+p));
385         p += 2;
386         printf("  GPS Time of Week : %f\n", (double)get_sirf_4u(buf+p)/100.0);
387         p += 4;
388         printf("  Satellites used : %d\n", n_sat = buf[p]);
389         p += 1;
390         for (i=0; (i<12) & (i<n_sat); i++) {
391                         printf("  Channel %2d SatID: %3u\n", i+1, buf[p+i]);
392         }
393
394         return 0;
395 }
396
397 int decode_sirf_msg_4(unsigned char *buf, int n)
398 {
399         int i, p = 0;
400
401         printf("Measured Tracker Data Out\n");
402         p += 1;
403         printf("  GPS Week : %d\n", get_sirf_2s(buf + p));
404         p += 2;
405         printf("  GPS Time of Week : %u\n", get_sirf_4u(buf + p));
406         p += 4;
407         printf("  Channels : %d\n", buf[p]);
408         p += 1;
409         for (;p<n;) {
410                 printf("  SatID: %3u Azimuth: %f Elevation: %f\n",
411                        buf[p], (double)buf[p+1]/2.0*3.0, (double)buf[p+2]/2.0);
412                 p += 3;
413                 printf("    State : %02x\n", get_sirf_2u(buf + p));
414                 p += 2;
415                 for (i=0; i<10; i++) {
416                         printf("    SNR %2d: %d dB\n", i+1, buf[p+i]);
417                 }
418                 p+= 10;
419         }
420
421         return 0;
422 }
423
424 int decode_sirf_msg_7(unsigned char *buf, int n)
425 {
426         int p = 0;
427         int ext_gps_week;
428         double gps_seconds;
429         double clock_bias;
430
431         printf("Clock Status Data\n");
432         p += 1;
433         printf("  Extended GPS Week : %u\n",
434                ext_gps_week = get_sirf_2u(buf + p));
435         p += 2;
436         printf("  GPS Time of Week : %f s\n",
437                gps_seconds = (double)get_sirf_4u(buf + p) / 100.0);
438         p += 4;
439         printf("  Satellites used : %u\n", buf[p]);
440         p += 1;
441         printf("  Clock Drift : %u Hz\n", get_sirf_4u(buf + p));
442         p += 4;
443         printf("  Clock Bias : %u ns\n", get_sirf_4u(buf + p));
444         clock_bias = 1e-9 * (double) get_sirf_4u(buf + p);
445         p += 4;
446         printf("  Estimated GPS Time : %u ms\n", get_sirf_4u(buf + p));
447         p += 4;
448
449         rinex_data_add_time_clock_bias(ext_gps_week, gps_seconds, clock_bias);
450
451         return 0;
452 }
453
454 int decode_sirf_msg_8(unsigned char *buf, int n)
455 {
456         int i, p = 0;
457
458         printf("50 BPS Data\n");
459         p += 1;
460         printf("  Channel %d, SV# %d\n", buf[p], buf[p+1]);
461         p += 2;
462         for (i=0; i<10; i++) {
463                 printf("  Word %d : %08x\n", i, get_sirf_4u(buf+p));
464                 p += 4;
465         }
466
467         return 0;
468 }
469
470 int decode_sirf_msg_13(unsigned char *buf, int n)
471 {
472         int p = 0, i, n_sat;
473
474         printf("Visible List\n");
475         p += 1;
476         n_sat = buf[p];
477         p += 1;
478         for (i=0; i<n_sat; i++) {
479                 printf("  SatID %3u : Azimuth %3d Elevation %3d\n",
480                        buf[p], get_sirf_2s(buf+p+1), get_sirf_2s(buf+p+3));
481                 p += 5;
482         }
483
484         return 0;
485 }
486
487 int decode_sirf_msg_27(unsigned char *buf, int n)
488 {
489         int i, p = 0;
490
491         printf("DGPS Status Format\n");
492         p += 1;
493         printf("  DGPS source = %d\n", buf[p]);
494         p += 1;
495         for (i=0; i<12; i++) {
496                 printf("  SatID %3u: Age: %u s, DGPS correction : %d m\n",
497                        buf[p + 14 + 3 * i], buf[p + i],
498                        get_sirf_2s(buf + p + 14 + 3 * i + 1));
499         }
500
501         return 0;
502 }
503
504 int decode_sirf_msg_28(unsigned char *buf, int n)
505 {
506         int p = 0;
507         int satID;
508         double gps_seconds;
509         double pseudorange;
510         double snr_avg = 0.0;
511         int i;
512         int tit;
513
514         printf("Navigation Library Measurement Data - Channel %d\n", buf[p+1]);
515         p += 2;
516         printf("  Time Tag = 0x%08x\n", get_sirf_4u(buf+p));
517         p += 4;
518         printf("  SatID : %3u\n", satID=buf[p]);
519         p += 1;
520         printf("  GPS SW Time : %f s\n", gps_seconds=get_sirf_dbl(buf+p));
521         p += 8;
522         printf("  Pseudorange = %f m\n", pseudorange = get_sirf_dbl(buf+p));
523         p += 8;
524         printf("  Carrier frequency = %f m/s\n", get_sirf_sgl(buf+p));
525         p += 4;
526         printf("  Carrier phase = %f m\n", get_sirf_dbl(buf+p));
527         p += 8;
528         printf("  Time in track = %u ms\n", tit = get_sirf_2u(buf+p));
529         p += 2;
530         printf("  Sync flags = 0x%x\n", buf[p]);
531         p += 1;
532         for (i=0; i<10; i++) {
533                 int snr;
534
535                 printf("  SNR %3u dB-Hz\n", snr = buf[p+i]);
536                 snr_avg += 0.1 * snr;
537         }
538         p += 10;
539         /* ... */
540
541         /* pseudorange is only valid if time in track is > 0 */
542         if (tit > 0) 
543                 rinex_data_add_pseudorange_snr(satID, gps_seconds,
544                                                pseudorange, snr_avg);
545
546         return 0;
547 }
548
549 int decode_sirf_msg_31(unsigned char *buf, int n)
550 {
551         printf("Navigation Library Initialization Data\n");
552         /* TBD */
553
554         return 0;
555 }
556
557 int decode_sirf_msg_50(unsigned char *buf, int n)
558 {
559         int p = 0;
560
561         printf("SBAS Parameters\n");
562         p += 1;
563         printf("  SBAS SatID : %3u\n", buf[p]);
564         p += 1;
565         printf("  SBAS Mode : %02x %s%s\n", buf[p],
566                buf[p] == 0 ? "Testing" : "",
567                buf[p] == 1 ? "Integrity" : "");
568         p += 1;
569         printf("  DGPS Timeout : %u s\n", buf[p]);
570         p += 1;
571         printf("  Flag bits : %02x\n", buf[p]);
572         p += 1;
573         
574         return 0;
575 }
576
577 int decode_sirf_msg_225(unsigned char *buf, int n)
578 {
579         int p = 0;
580
581         printf("Statistics Channel\n");
582         p += 1;
583         /* Message Sub ID : we got 0 or 8 ... undocumented */
584         printf("  Message Sub ID : %d\n", buf[p]);
585         p += 1;
586
587         return 0;
588 }
589
590 int decode_sirf_msg_255(unsigned char *buf, int n)
591 {
592         int p = 0, len;
593         char msg[1024];
594
595         printf("Development Data\n");
596         p += 1;
597
598         if (n < sizeof(msg))
599                 len = n;
600         else    len = sizeof(msg) -1;
601         memcpy(msg, buf + p, len);
602         msg[len] = 0;
603
604         printf("  %s\n", msg);
605
606         return 0;
607 }
608
609 /* (buf,n) is a full SiRF message, including start/stop sequence */
610
611 int decode_sirf_msg(unsigned char *buf, int n, int *ack) {
612         int p = 4;
613
614         switch (buf[p]) {
615         case 2: /* Measured Navigation Data Out */
616                 decode_sirf_msg_2(buf + 4, n - 8);
617                 break;
618     
619         case 4: /* Measured Tracker Data Out */
620                 decode_sirf_msg_4(buf + 4, n - 8);
621                 break;
622                 
623         case 6: /* Software Version String */
624                 printf("Software Version String : %s\n", buf+p+1);
625                 /* for GPS USB : GSW3.5.0_3.5.00.00-SDK-3EP2.01 */
626                 p += 1;
627                 break;
628                 
629         case 7: /* Clock Status Data */
630                 decode_sirf_msg_7(buf + 4, n - 8);
631                 break;
632                 
633         case 8: /* 50 BPS Data */
634                 decode_sirf_msg_8(buf + 4, n - 8);
635                 break;
636                 
637         case 9: /* CPU Throughput */
638                 printf("CPU Throughput\n");
639                 break;
640                 
641         case 11: /* Command Acknowledgment */
642                 printf("Command ACK ID : %d\n",
643                        buf[p+1]);
644                 *ack = buf[p+1];
645                 p += 2;
646                 break;
647                 
648         case 12: /* Command Negative Acknowledgment */
649                 printf("Command NACK ID : %d\n",
650                        buf[p+1]);
651                 *ack = buf[p+1];
652                 p += 2;
653                 break;
654
655         case 13: /* Visible List */
656                 decode_sirf_msg_13(buf + 4, n - 8);
657                 break;
658
659         case 27: /* GPS Status Format */
660                 decode_sirf_msg_27(buf + 4, n - 8);
661                 break;
662                 
663         case 28: /* Navigation Library Measurement Data */
664                 decode_sirf_msg_28(buf + 4, n - 8);
665                 break;
666                 
667         case 30: /* Navigation Library SV State Data */
668                 printf("Navigation Library SV State Data - SatID : %3u\n", buf[p+1]);
669                 p += 2;
670                 printf("  GPS Time : %f s\n", get_sirf_dbl(buf+p));
671                 p += 8;
672                 printf("  Position X : %f m\n", get_sirf_dbl(buf+p));
673                 p += 8;
674                 printf("  Position Y : %f m\n", get_sirf_dbl(buf+p));
675                 p += 8;
676                 printf("  Position Z : %f m\n", get_sirf_dbl(buf+p));
677                 p += 8;
678                 printf("  Velocity X : %f m/s\n", get_sirf_dbl(buf+p));
679                 p += 8;
680                 printf("  Velocity Y : %f m/s\n", get_sirf_dbl(buf+p));
681                 p += 8;
682                 printf("  Velocity Z : %f m/s\n", get_sirf_dbl(buf+p));
683                 p += 8;
684                 printf("  Clock bias : %f s\n", get_sirf_dbl(buf+p));
685                 p += 8;
686                 printf("  Clock drift : %f s/s\n", get_sirf_sgl(buf+p));
687                 p += 4;
688                 p += 1;
689                 p += 4;
690                 p += 4;
691                 printf("  Ionospheric delay : %f m\n", get_sirf_sgl(buf+p));
692                 p += 4;
693                 break;
694
695         case 31:
696                 decode_sirf_msg_31(buf + 4, n - 8);
697                 break;
698                 
699         case 41: /* Geodetic Navigation Data */
700                 printf("Geodetic Navigation Data\n");
701                 p += 1;
702                 p += 2;
703                 p += 2;
704                 p += 2;
705                 p += 4;
706                 
707                 printf("Y-M-D : %04d-%02d-%02d H:M:S : %02d:%02d:%02.3f\n",
708                        (buf[p]<<8)|buf[p+1], buf[p+2], buf[p+3],
709                        buf[p+4], buf[p+5], ((buf[p+6]<<8)|buf[p+7])/1000.0);
710                 break;
711
712         case 50:
713                 decode_sirf_msg_50(buf + 4, n - 8);
714                 break;
715
716         case 225:
717                 decode_sirf_msg_225(buf + 4, n - 8);
718                 break;
719                 
720         case 255: /* Development Data */
721                 decode_sirf_msg_255(buf + 4, n - 8);
722                 break;
723
724         default:
725                 printf("Message ID %d is not decoded yet ...\n", buf[p]);
726                 break;
727         }
728
729         fflush(stdout);
730         return 0;
731 }
732
733 void dump_msg(unsigned char *buf, int n) {
734         int i;
735         
736         printf("%d bytes message :", n);
737         for (i=0; i<n; i++) {
738                 printf(" 0x%02x", buf[i]);
739         }
740         printf("\n");
741 }
742
743 /* return 0 on success, -1 on error */
744
745 int do_read(int fd, int * ack) {
746         /* SiRF message size is limited to 912 bytes */
747         int i, n;
748
749         static unsigned char sbuffer[912*2];
750         static int p = 0;
751
752         if ((sizeof(sbuffer) - p) == 0) {
753                 printf("buffer full -> empty\n");
754                 p = 0;
755                 return -1;
756         }
757
758         n = read(fd, sbuffer + p, sizeof(sbuffer) - p);
759         if (n <= 0) {
760                 perror("read");
761                 return -1;
762         }
763
764         if (n == 0) {
765                 /* nothing to do */
766                 return 0;
767         }
768
769         /* for debug only :
770         printf("%d bytes received (total = %d):", n, p+n);
771         for (i=0; i<n; i++) {
772                 printf(" 0x%02x", sbuffer[p+i]);
773         }
774         printf("\n");
775         */
776
777         /* update p with the bytes just received */
778         p += n;
779
780         /* small parsing of received data in (sbuffer, p) : 
781            - start pattern = 0xa0 0xa2
782            - stop  pattern = 0xb0 0xb3
783
784            SiRF message format is :
785            <0xa0 0xa2><length (2 bytes)><length bytes><CRC (2 byte)><0xb0 0xb3>
786         */
787
788         for (;;) {
789                 int start_found = 0, stop_found = 0;
790
791                 /* search for start pattern */
792                 for (i=0; i<p-1; i++) {
793                         if ((sbuffer[i] == 0xa0) && (sbuffer[i+1] == 0xa2)) {
794                                 start_found = 1;
795
796                                 if (i>0) {
797                                         printf("recv: %d bytes skipped\n", i);
798                                         memmove(sbuffer, sbuffer+i, p-i);
799                                         p -= i;
800                                 }
801                                 break;
802                         }
803                 }
804
805                 /* search for stop pattern */
806                 for (i=0; i<p-1; i++) {
807                         if ((sbuffer[i] == 0xb0) && (sbuffer[i+1] == 0xb3)) {
808                                 stop_found = 1;
809           
810                                 if (start_found && stop_found) {
811                                         printf("recv: Message ID %d (%d bytes)\n",
812                                                sbuffer[4], i+2); 
813                                         if (check_sirf_msg(sbuffer, i+2)) {
814                                                 decode_sirf_msg(sbuffer, i+2,
815                                                                 ack);
816                                         }
817                                 }
818           
819                                 if (!start_found)
820                                         printf("recv: %d bytes skipped\n",i+2);
821                                 memmove(sbuffer, sbuffer+(i+2), p-(i+2));
822                                 p -= (i+2);
823
824                                 break;
825                         }
826                 }
827
828                 if (!stop_found)
829                         break;
830         }
831         
832         return 0;
833 }
834
835
836 /* 
837  * (buf,n) is the payload of the message. This function adds the start/stop
838  * sequences, and the length/checksum fields. Returns 0 on success, -1 on
839  * error.
840  */
841
842 int sirf_msg_send(int fd, unsigned char *buf, int n) {
843         unsigned char sbuf[1024];
844         int i;
845         unsigned int crc;
846         int total_len = 4 + n + 4; 
847         int cmd = buf[0];
848
849         if (total_len > sizeof(sbuf)) {
850                 printf("%s: message too large\n", __func__);
851                 return -1;
852         }
853
854         /* 0xa0, 0xa2 is the start sequence */
855         sbuf[0] = 0xa0;
856         sbuf[1] = 0xa2;
857         sbuf[2] = n>>8;
858         sbuf[3] = n&0xff;
859         memcpy(sbuf+4, buf, n);
860
861         /* compute checksum on the payload */
862         crc = 0;
863         for (i=0; i<n; i++)
864                 crc += buf[i];
865         crc &= 0x7fff;
866         
867         sbuf[4+n+0] = (crc & 0xff00)>>8;
868         sbuf[4+n+1] = (crc & 0x00ff)>>0;
869         /* 0xb0, 0xb3 is the end sequence */
870         sbuf[4+n+2] = 0xb0;
871         sbuf[4+n+3] = 0xb3;
872
873         if ((n=write(fd, sbuf, total_len)) != total_len) {
874                 printf("%s: written only %d bytes out of %d\n", __func__,
875                        n, total_len);
876                 return -1;
877         }
878
879         printf("send: Message ID %d (%d bytes)\n", buf[0], total_len);
880
881         /* wait for ACK */
882         for (;;) {
883                 int ack = 0;
884                 if (do_read(fd, &ack) < 0) {
885                         printf("do_read failed!\n");
886                         return -1;
887                 }
888                 if (ack == cmd) {
889                         printf("got ACK for cmd %d\n", ack);
890                         break;
891                 }
892         }
893
894         return 0;
895 }
896
897 void usage(const char *argv0) {
898         printf("usage: %s [-rinex rinex.obs]\n"
899                "  -rinex rinex.obs : record a RINEX observation file\n",
900                argv0);
901         exit (-1);
902 }
903
904 int main(int argc, const char *argv[])
905 {
906         const char device[] = "/dev/ttyUSB0";
907         int fd;
908         struct termios term;
909         int i;
910
911         for (i=1 ; i<argc; i++) {
912                 if (strcmp(argv[i],"-rinex")==0 && i+1<argc) {
913                         g_rinex = argv[++i];
914                         rinex_open(g_rinex);
915                         rinex_header_write();
916                 } else
917                         usage(argv[0]);
918         }
919
920         /* open serial device */
921         fd = open(device, O_RDWR /*| O_NONBLOCK*/);
922         if (fd < 0) {
923                 perror(device);
924                 return -1;
925         }
926
927         /* switch to 4800 bauds */
928         if (tcgetattr(fd, &term) != 0) {
929                 perror("tcgetattr");
930         }
931         cfmakeraw(&term);
932         cfsetispeed(&term, B115200);
933         cfsetospeed(&term, B115200);
934         /* 8N1 */
935         term.c_cflag &= ~(CSIZE|PARENB|CSTOPB);
936         term.c_cflag |=   CS8;
937         if (tcsetattr(fd, TCSANOW, &term) != 0) {
938                 perror("tcsetattr");
939         }
940
941         /* switch from NMEA to SiRF binary format */
942
943         const char to_sirf[] = "$PSRF100,0,9600,8,1,0*0C\n";
944         /* -1 : we do not want to send the null terminating character */
945         //write(fd, to_sirf, sizeof(to_sirf)-1);
946
947         /* Set Binary Serial Port.  Note : the effect of changing the baud
948            rate is not immediate at all */
949
950         /* Tested value : 
951              1200         1200 bit/s
952              2400         2400 bit/s
953              4800         4800 bit/s
954              9600         9600 bit/s
955             19200        19200 bit/s
956             38400        38400 bit/s
957             57600        57600 bit/s
958            115200       115200 bit/s
959         */
960
961         unsigned int baud = 115200;
962         unsigned char msg_134[] = { 134, 
963                                     (baud >> 24) & 0xff,
964                                     (baud >> 16) & 0xff,
965                                     (baud >>  8) & 0xff,
966                                     (baud >>  0) & 0xff,
967                                     8, 1, 0, 0 };
968         sirf_msg_send(fd, msg_134, sizeof(msg_134));
969         /*
970         if (tcgetattr(fd, &term) != 0) {
971                 perror("tcgetattr");
972         }
973         cfsetispeed(&term, B1200);
974         cfsetospeed(&term, B1200);
975         if (tcsetattr(fd, TCSANOW, &term) != 0) {
976                 perror("tcsetattr");
977         }
978         */
979
980         /* Poll Software Version */
981         unsigned char msg_132[] = { 132, 0x00 };
982         sirf_msg_send(fd, msg_132, sizeof(msg_132));
983
984         /* Initialize GPS/DR Navigation */
985         /*
986         unsigned char msg_172[] = { 0xac, 0x01, 
987                                     0x00, 0x00, 0x00, 0x00,
988                                     0x00, 0x00, 0x00, 0x00,
989                                     0x00, 0x00, 0x00, 0x00,
990                                     0x00, 0x00,
991                                     0x00, 0x00, 0x00, 0x00,
992                                     0x00, 0x00, 0x00, 0x00,
993                                     0x00, 0x00,
994                                     0x00,
995                                     (1<<4)|(1<<5)};
996         sirf_msg_send(fd, msg_172, sizeof(msg_172));
997         */
998
999         /* Initialize Data Source */
1000         unsigned char msg_128[] = { 128,
1001                                     0x00, 0x00, 0x00, 0x00,
1002                                     0x00, 0x00, 0x00, 0x00,
1003                                     0x00, 0x00, 0x00, 0x00,
1004                                     0x00, 0x00, 0x00, 0x00,
1005                                     0x00, 0x00, 0x00, 0x00,
1006                                     0x00, 0x00,
1007                                     12,
1008                                     /* Clear memory */
1009                                     /* (1<<2) | */
1010                                     /* Enable raw track & debug data */
1011                                     (1<<4) | (1<<5) };
1012         sirf_msg_send(fd, msg_128, sizeof(msg_128));
1013
1014         /* Enable MID 29 */
1015         unsigned char msg_166_29[] = { 166, 0, 29, 1, 0, 0, 0, 0 };
1016         sirf_msg_send(fd, msg_166_29, sizeof(msg_166_29));
1017
1018         /* Enable MID 52 : we got a NACK since this feature is not available */
1019         unsigned char msg_166[] = { 166,  0, 52, 1, 0, 0, 0, 0 };
1020         sirf_msg_send(fd, msg_166, sizeof(msg_166));
1021
1022         /* Try to enable all messages. We got a ACK for :
1023              0, Undocumented (never received)
1024              2, Measure Navigation Data Out
1025                 => X,Y,Z,dX,dY,dZ,T, HDOP, sat used
1026                 used by gpsd to report longitude, latitude
1027              4, Measured Tracker Data Out
1028                 => T, SV ID, Azimuth, Elevation, SNR
1029              7, Clock Status Data
1030              8, 50 BPS Data
1031              9, CPU Throughput
1032             13, Visible List
1033                 => SV ID, Azimuth, Elevation
1034             18, OkToSend (never received)
1035             27, DGPS Status Format
1036             28, Navigation Library Measurement Data
1037                 => SV ID, T, Pseudorange, Carrier freq+phase
1038             29, Navigation Library DGPS Data (never received)
1039             30, Navigation Library SV State Data
1040                 => SV ID, T, X,Y,Z,dX,dY,dZ, Clock bias+drift, IONO
1041             31, Navigation Library Initialization Data
1042             41, Geodetic Navigation Data
1043             46, Test Mode 3/4/5/6 (never received)
1044             50, SBAS Parameters
1045             55, Test Mode 4 Track Data (never received)
1046            128, Undocumented (never received)
1047            255, Development Data
1048          */
1049         /*
1050         for (i=0; i<256; i++) {
1051                 printf("Trying to enable message %d\n", i);
1052                 msg_166[2] = i;
1053                 sirf_msg_send(fd, msg_166, sizeof(msg_166));
1054         }
1055         */
1056         for (;;) {
1057                 fd_set fdr;
1058                 int r;
1059
1060                 FD_ZERO(&fdr);
1061                 FD_SET(fd, &fdr);
1062
1063                 r = select(FD_SETSIZE, &fdr, NULL, NULL, NULL);
1064                 if (r < 0) {
1065                         perror("select");
1066                         break;
1067                 }
1068
1069                 if (FD_ISSET(fd, &fdr)) {
1070                         int ack = 0;
1071                         if (do_read(fd, &ack) < 0)
1072                                 break;
1073                         if (ack != 0) {
1074                                 printf("ACK for cmd %d\n", ack);
1075                         }
1076                 }
1077         }
1078
1079         close (fd);
1080         return 0;
1081 }