1 /*
2 Read files containing selected NMEA 0183 sentences.
3 Based on information by Eino Uikkanenj
4
5 Copyright (C) 2004-2006 Robert Lipe, robertlipe@usa.net
6
7 This program is free software; you can redistribute it and/or modify
8 it under the terms of the GNU General Public License as published by
9 the Free Software Foundation; either version 2 of the License, or
10 (at your option) any later version.
11
12 This program is distributed in the hope that it will be useful,
13 but WITHOUT ANY WARRANTY; without even the implied warranty of
14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 GNU General Public License for more details.
16
17 You should have received a copy of the GNU General Public License
18 along with this program; if not, write to the Free Software
19 Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA
20
21 */
22
23 #include <ctype.h>
24 #include <time.h>
25
26 #include "defs.h"
27 #include "gbser.h"
28 #include "strptime.h"
29 #include "jeeps/gpsmath.h"
30
31 /**********************************************************
32
33 ' 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
34 ' $GPGGA - Global Positioning System Fix Data
35 ' $GPGGA,155537,6006.718,N,02426.290,E,1,05,2.4,50.5,M,19.7,M,,*79
36 ' 2 123519 Fix taken at 12:35:19 UTC
37 ' 3,4 4807.038,N Latitude 48 deg 07.038' N
38 ' 5,6 01131.324,E Longitude 11 deg 31.324' E
39 ' 7 1 Fix quality: 0 = invalid
40 ' 1 = GPS fix
41 ' 2 = DGPS fix
42 ' 8 08 Number of satellites being tracked
43 ' 9 0.9 Horizontal dilution of position
44 ' 10,11 545.4,M Altitude, Metres, above mean sea level
45 ' 12,13 46.9,M Height of geoid (mean sea level) above WGS84 ellipsoid
46 ' 14 (empty field) time in seconds since last DGPS update
47 ' 15 (empty field) DGPS station ID number
48
49 ' $GPWPL - waypoint location
50 ' $GPWPL,4917.16,N,12310.64,W,003*65
51 ' 2,3 4917.16,N Latitude of waypoint
52 ' 4,5 12310.64,W Longitude of waypoint
53 ' 6 003 Waypoint ID
54
55 ' $GPGLL - Geographic position, Latitude and Longitude
56 ' $GPGLL,4916.45,N,12311.12,W,225444,A
57 ' 2,3 4916.46,N Latitude 49 deg. 16.45 min. North
58 ' 4,5 12311.12,W Longitude 123 deg. 11.12 min. West
59 ' 6 225444 Fix taken at 22:54:44 UTC
60 ' 7 A Data valid
61
62 ' $GPRMC - Recommended minimum specific GNSS Data
63 ' $GPRMC,085721.194,A,5917.7210,N,01103.9227,E,21.42,50.33,300504,,*07
64 ' 2 085721 Fix taken at 08:57:21 UTC
65 ' 3 A Fix valid (this field reads V if fix is not valid)
66 ' 4,5 5917.7210,N Latitude 59 deg 17.7210' N
67 ' 6,7 01103.9227,E Longitude 11 deg 03.9227' E
68 ' 8 21.42 Speed over ground (knots)
69 ' 9 50.33 Course over ground (true)
70 ' 10 300504 Date 30/05-2004
71 ' 11 Empty field Magnetic variation
72
73 GSA - GPS DOP and active satellites
74 $GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39
75 A Auto selection of 2D or 3D fix (M = manual)
76 3 3D fix
77 04,05... PRNs of satellites used for fix (space for 12)
78 2.5 PDOP (dilution of precision)
79 1.3 Horizontal dilution of precision (HDOP)
80 2.1 Vertical dilution of precision (VDOP)
81 DOP is an indication of the effect of satellite geometry on
82 the accuracy of the fix.
83
84 VTG - Track made good and ground speed
85 $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K
86 054.7,T True track made good
87 034.4,M Magnetic track made good
88 005.5,N Ground speed, knots
89 010.2,K Ground speed, Kilometers per hour
90
91 WPL - waypoint location
92 $GPWPL,4917.16,N,12310.64,W,003*65
93 4917.16,N Latitude of waypoint
94 12310.64,W Longitude of waypoint
95 003 Waypoint ID
96 When a route is active, this sentence is sent once for each
97 waypoint in the route, in sequence. When all waypoints have
98 been reported, GPR00 is sent in the next data set. In any
99 group of sentences, only one WPL sentence, or an R00
100 sentence, will be sent.
101
102
103 ' The optional checksum field consists of a "*" and two hex digits repre-
104 ' senting the exclusive OR of all characters between, but not including,
105 ' the "$" and "*". A checksum is required on some sentences.
106
107 ****************************************/
108
109 /*
110 * An input file may have both GGA and GLL and RMC sentences for the exact
111 * same position fix. If we see a single GGA, start ignoring GLL's and RMC's.
112 * GLL's will also be ignored if RMC's are found and GGA's not found.
113 */
114
115 /*
116 Zmarties notes:
117
118 In practice, all fields of the NMEA sentences should be treated as optional -
119 if the data is not available, then the field can be omitted (hence leading
120 to the output of two consecutive commas).
121
122 An NMEA recording can start anywhere in the stream of data. It is therefore
123 necessary to discard sentences until sufficient data has been processed to
124 have all the necessary data to construct a waypoint. In practice, this means
125 discarding data until we have had the first sentence that provides the date.
126 (We could scan forwards in the stream of data to find the first date, and
127 then back apply it to all previous sentences, but that is probably more
128 complexity that is necessary - the lost of one waypoint at the start of the
129 stream can normally be tolerated.)
130
131 If a sentence is received without a checksum, but previous sentences have
132 had checksums, it is best to discard that sentence. In practice, the only
133 time I have seen this is when the recording stops suddenly, where the last
134 sentence is truncated - and missing part of the line, including the checksum.
135 */
136
137 typedef enum {
138 gp_unknown = 0,
139 gpgga,
140 gplgll,
141 gprmc
142 } preferred_posn_type;
143
144 enum {
145 rm_unknown = 0,
146 rm_serial,
147 rm_file
148 } read_mode;
149
150 static gbfile* file_in, *file_out;
151 static route_head* trk_head;
152 static short_handle mkshort_handle;
153 static preferred_posn_type posn_type;
154 static struct tm tm;
155 static waypoint* curr_waypt;
156 static waypoint* last_waypt;
157 static void* gbser_handle;
158 static const char* posn_fname;
159 static queue pcmpt_head;
160
161 static int without_date; /* number of created trackpoints without a valid date */
162 static struct tm opt_tm; /* converted "date" parameter */
163
164 #define MYNAME "nmea"
165
166 static char* opt_gprmc;
167 static char* opt_gpgga;
168 static char* opt_gpvtg;
169 static char* opt_gpgsa;
170 static char* snlenopt;
171 static char* optdate;
172 static char* getposnarg;
173 static char* opt_sleep;
174 static char* opt_baud;
175 static char* opt_append;
176 static char* opt_gisteq;
177 static char* opt_ignorefix;
178
179 static long sleepus;
180 static int getposn;
181 static int append_output;
182 static int amod_waypoint;
183
184 static time_t last_time;
185 static double last_read_time; /* Last timestamp of GGA or PRMC */
186 static int datum;
187 static int had_checksum;
188
189 static waypoint* nmea_rd_posn(posn_status*);
190 static void nmea_rd_posn_init(const char* fname);
191
192 arglist_t nmea_args[] = {
193 {"snlen", &snlenopt, "Max length of waypoint name to write", "6", ARGTYPE_INT, "1", "64" },
194 {"gprmc", &opt_gprmc, "Read/write GPRMC sentences", "1", ARGTYPE_BOOL, ARG_NOMINMAX },
195 {"gpgga", &opt_gpgga, "Read/write GPGGA sentences", "1", ARGTYPE_BOOL, ARG_NOMINMAX },
196 {"gpvtg", &opt_gpvtg, "Read/write GPVTG sentences", "1", ARGTYPE_BOOL, ARG_NOMINMAX },
197 {"gpgsa", &opt_gpgsa, "Read/write GPGSA sentences", "1", ARGTYPE_BOOL, ARG_NOMINMAX },
198 {"date", &optdate, "Complete date-free tracks with given date (YYYYMMDD).", NULL, ARGTYPE_INT, ARG_NOMINMAX },
199 {
200 "get_posn", &getposnarg, "Return current position as a waypoint",
201 NULL, ARGTYPE_BOOL, ARG_NOMINMAX
202 },
203 {"pause", &opt_sleep, "Decimal seconds to pause between groups of strings", NULL, ARGTYPE_INT, ARG_NOMINMAX },
204 {"append_positioning", &opt_append, "Append realtime positioning data to the output file instead of truncating", "0", ARGTYPE_BOOL, ARG_NOMINMAX },
205 {"baud", &opt_baud, "Speed in bits per second of serial port (baud=4800)", NULL, ARGTYPE_INT, ARG_NOMINMAX },
206 {"gisteq", &opt_gisteq, "Write tracks for Gisteq Phototracker", "0", ARGTYPE_BOOL, ARG_NOMINMAX },
207 {"ignore_fix", &opt_ignorefix, "Accept position fixes in gpgga marked invalid", "0", ARGTYPE_BOOL, ARG_NOMINMAX },
208 ARG_TERMINATOR
209 };
210
211 #define CHECK_BOOL(a) if (a && (*a == '0')) a = NULL
212
213 /*
214 * Slightly different than the Magellan checksum fn.
215 */
216 int
nmea_cksum(const char * const buf)217 nmea_cksum(const char* const buf)
218 {
219 int x = 0 ;
220 const char* p;
221
222 for (p = buf; *p; p++) {
223 x ^= *p;
224 }
225 return x;
226 }
227
228 static void
nmea_add_wpt(waypoint * wpt,route_head * trk)229 nmea_add_wpt(waypoint* wpt, route_head* trk)
230 {
231 if (datum != DATUM_WGS84) {
232 double lat, lon, alt;
233 GPS_Math_Known_Datum_To_WGS84_M(
234 wpt->latitude, wpt->longitude, 0,
235 &lat, &lon, &alt, datum);
236 wpt->latitude = lat;
237 wpt->longitude = lon;
238 }
239 if (trk != NULL) {
240 track_add_wpt(trk, wpt);
241 } else {
242 waypt_add(wpt);
243 }
244 }
245
246 static void
nmea_release_wpt(waypoint * wpt)247 nmea_release_wpt(waypoint* wpt)
248 {
249 if (wpt && ((wpt->Q.next == NULL) || (wpt->Q.next == &wpt->Q))) {
250 /* This waypoint isn't queued.
251 Release it, because we don't have any reference to this
252 waypoint (! memory leak !) */
253 waypt_free(wpt);
254 }
255 }
256
257 static void
nmea_rd_init(const char * fname)258 nmea_rd_init(const char* fname)
259 {
260 curr_waypt = NULL;
261 last_waypt = NULL;
262 last_time = -1;
263 datum = DATUM_WGS84;
264 had_checksum = 0;
265
266 CHECK_BOOL(opt_gprmc);
267 CHECK_BOOL(opt_gpgga);
268 CHECK_BOOL(opt_gpvtg);
269 CHECK_BOOL(opt_gpgsa);
270 CHECK_BOOL(opt_gisteq);
271
272 QUEUE_INIT(&pcmpt_head);
273
274 if (getposnarg) {
275 getposn = 1;
276 }
277
278 /* A special case hack that gets our current position and returns
279 * it as one waypoint.
280 */
281 if (getposn) {
282 waypoint* wpt;
283 posn_status st;
284 nmea_rd_posn_init(fname);
285 wpt = nmea_rd_posn(&st);
286 if (!wpt) {
287 return;
288 }
289 if (wpt->shortname) {
290 xfree(wpt->shortname);
291 }
292 wpt->shortname = xstrdup("Position");
293 nmea_add_wpt(wpt, NULL);
294 return;
295 }
296
297 read_mode = rm_file;
298 file_in = gbfopen(fname, "rb", MYNAME);
299 }
300
301 static void
nmea_rd_deinit(void)302 nmea_rd_deinit(void)
303 {
304 switch (read_mode) {
305 case rm_serial:
306 gbser_deinit(gbser_handle);
307 break;
308 case rm_file:
309 gbfclose(file_in);
310 file_in = NULL;
311 break;
312 default:
313 fatal("nmea_rd_deinit: illegal read_mode.\n");
314 break;
315 }
316 }
317
318 static void
nmea_wr_init(const char * portname)319 nmea_wr_init(const char* portname)
320 {
321 CHECK_BOOL(opt_gprmc);
322 CHECK_BOOL(opt_gpgga);
323 CHECK_BOOL(opt_gpvtg);
324 CHECK_BOOL(opt_gpgsa);
325 CHECK_BOOL(opt_gisteq);
326
327 append_output = atoi(opt_append);
328
329 file_out = gbfopen(portname, append_output ? "a+" : "w+", MYNAME);
330
331 sleepus = -1;
332 if (opt_sleep) {
333 if (*opt_sleep) {
334 sleepus = 1e6 * atof(opt_sleep);
335 } else {
336 sleepus = -1;
337 }
338 }
339
340 mkshort_handle = mkshort_new_handle();
341 setshort_length(mkshort_handle, atoi(snlenopt));
342
343 if (opt_gisteq) {
344 opt_gpgga = NULL;
345 opt_gpvtg = NULL;
346 opt_gpgsa = NULL;
347 }
348 }
349
350 static void
nmea_wr_deinit(void)351 nmea_wr_deinit(void)
352 {
353 gbfclose(file_out);
354 mkshort_del_handle(&mkshort_handle);
355 }
356
357 static void
nmea_set_waypoint_time(waypoint * wpt,struct tm * time,int microseconds)358 nmea_set_waypoint_time(waypoint* wpt, struct tm* time, int microseconds)
359 {
360 if (time->tm_year == 0) {
361 wpt->creation_time = ((((time_t)time->tm_hour * 60) + time->tm_min) * 60) + time->tm_sec;
362 wpt->microseconds = microseconds;
363 if (wpt->wpt_flags.fmt_use == 0) {
364 wpt->wpt_flags.fmt_use = 1;
365 without_date++;
366 }
367 } else {
368 wpt->creation_time = mkgmtime(time);
369 wpt->microseconds = microseconds;
370 if (wpt->wpt_flags.fmt_use != 0) {
371 wpt->wpt_flags.fmt_use = 0;
372 without_date--;
373 }
374 }
375 }
376
377 static void
gpgll_parse(char * ibuf)378 gpgll_parse(char* ibuf)
379 {
380 double latdeg, lngdeg, microseconds;
381 char lngdir, latdir;
382 double hmsd;
383 int hms;
384 char valid = 0;
385 waypoint* waypt;
386
387 if (trk_head == NULL) {
388 trk_head = route_head_alloc();
389 track_add_head(trk_head);
390 }
391
392 sscanf(ibuf,"$GPGLL,%lf,%c,%lf,%c,%lf,%c,",
393 &latdeg,&latdir,
394 &lngdeg,&lngdir,
395 &hmsd,&valid);
396
397 if (valid != 'A') {
398 return;
399 }
400
401 hms = (int) hmsd;
402 last_read_time = hms;
403 microseconds = MILLI_TO_MICRO(1000 * (hmsd - hms));
404
405 tm.tm_sec = hms % 100;
406 hms = hms / 100;
407 tm.tm_min = hms % 100;
408 hms = hms / 100;
409 tm.tm_hour = hms % 100;
410
411 waypt = waypt_new();
412
413 nmea_set_waypoint_time(waypt, &tm, microseconds);
414
415 if (latdir == 'S') {
416 latdeg = -latdeg;
417 }
418 waypt->latitude = ddmm2degrees(latdeg);
419
420 if (lngdir == 'W') {
421 lngdeg = -lngdeg;
422 }
423 waypt->longitude = ddmm2degrees(lngdeg);
424
425 nmea_release_wpt(curr_waypt);
426 curr_waypt = waypt;
427 }
428
429 static void
gpgga_parse(char * ibuf)430 gpgga_parse(char* ibuf)
431 {
432 double latdeg, lngdeg;
433 char lngdir, latdir;
434 double hms;
435 double alt;
436 int fix = fix_unknown;
437 int nsats = 0;
438 double hdop;
439 char altunits;
440 waypoint* waypt;
441 double microseconds;
442
443 if (trk_head == NULL) {
444 trk_head = route_head_alloc();
445 track_add_head(trk_head);
446 }
447
448 sscanf(ibuf,"$GPGGA,%lf,%lf,%c,%lf,%c,%d,%d,%lf,%lf,%c",
449 &hms, &latdeg,&latdir,
450 &lngdeg,&lngdir,
451 &fix,&nsats,&hdop,&alt,&altunits);
452
453 /*
454 * In serial mode, allow the fix with an invalid position through
455 * as serial units will often spit a remembered position up and
456 * that is more comfortable than nothing at all...
457 */
458 CHECK_BOOL(opt_ignorefix);
459 if ((fix <= 0) && (read_mode != rm_serial) && (!opt_ignorefix)) {
460 return;
461 }
462
463 last_read_time = hms;
464 microseconds = MILLI_TO_MICRO(1000 * (hms - (int)hms));
465
466 tm.tm_sec = (long) hms % 100;
467 hms = hms / 100;
468 tm.tm_min = (long) hms % 100;
469 hms = hms / 100;
470 tm.tm_hour = (long) hms % 100;
471
472 waypt = waypt_new();
473
474 nmea_set_waypoint_time(waypt, &tm, microseconds);
475
476 if (latdir == 'S') {
477 latdeg = -latdeg;
478 }
479 waypt->latitude = ddmm2degrees(latdeg);
480
481 if (lngdir == 'W') {
482 lngdeg = -lngdeg;
483 }
484 waypt->longitude = ddmm2degrees(lngdeg);
485
486 waypt->altitude = alt;
487
488 waypt->sat = nsats;
489
490 waypt->hdop = hdop;
491
492 switch (fix) {
493 case 0:
494 waypt->fix = fix_none;
495 break;
496 case 1:
497 waypt->fix = (nsats>3)?(fix_3d):(fix_2d);
498 break;
499 case 2:
500 waypt->fix = fix_dgps;
501 break;
502 case 3:
503 waypt->fix = fix_pps;
504 break;
505 }
506
507 nmea_release_wpt(curr_waypt);
508 curr_waypt = waypt;
509 }
510
511 static void
gprmc_parse(char * ibuf)512 gprmc_parse(char* ibuf)
513 {
514 double latdeg, lngdeg;
515 char lngdir, latdir;
516 double hms;
517 char fix;
518 unsigned int dmy;
519 double speed,course;
520 waypoint* waypt;
521 double microseconds;
522 char* dmybuf;
523 int i;
524
525 if (trk_head == NULL) {
526 trk_head = route_head_alloc();
527 track_add_head(trk_head);
528 }
529
530 /*
531 * Read everything except the dmy, in case lngdeg
532 * and lngdir are missing.
533 */
534 sscanf(ibuf,"$GPRMC,%lf,%c,%lf,%c,%lf,%c,%lf,%lf",
535 &hms, &fix, &latdeg, &latdir,
536 &lngdeg, &lngdir,
537 &speed, &course);
538
539 if (fix != 'A') {
540 /* ignore this fix - it is invalid */
541 return;
542 }
543
544 /* Skip past nine commas in ibuf to reach the dmy value */
545 for (dmybuf=ibuf,i=0; i<9; i++) {
546 dmybuf= strchr(dmybuf, ',');
547 if (dmybuf==NULL) {
548 /* If we run out of commas, the sentence is invalid. */
549 return;
550 }
551 dmybuf++;
552 }
553
554 /* Now read dmy from the correct position */
555 sscanf(dmybuf,"%u", &dmy);
556
557 last_read_time = hms;
558 microseconds = MILLI_TO_MICRO(1000 * (hms - (int)hms));
559
560 tm.tm_sec = (long) hms % 100;
561 hms = hms / 100;
562 tm.tm_min = (long) hms % 100;
563 hms = hms / 100;
564 tm.tm_hour = (long) hms % 100;
565
566 tm.tm_year = dmy % 100 + 100;
567 dmy = dmy / 100;
568 tm.tm_mon = dmy % 100 - 1;
569 dmy = dmy / 100;
570 tm.tm_mday = dmy;
571
572 if (posn_type == gpgga) {
573 /* capture useful data update and exit */
574 if (curr_waypt) {
575 if (! WAYPT_HAS(curr_waypt, speed)) {
576 WAYPT_SET(curr_waypt, speed, KNOTS_TO_MPS(speed));
577 }
578 if (! WAYPT_HAS(curr_waypt, course)) {
579 WAYPT_SET(curr_waypt, course, course);
580 }
581 /* The change of date wasn't recorded when
582 * going from 235959 to 000000. */
583 nmea_set_waypoint_time(curr_waypt, &tm, microseconds);
584 }
585 /* This point is both a waypoint and a trackpoint. */
586 if (amod_waypoint) {
587 waypt_add(waypt_dupe(curr_waypt));
588 amod_waypoint = 0;
589 }
590 return;
591 }
592
593 waypt = waypt_new();
594
595 WAYPT_SET(waypt, speed, KNOTS_TO_MPS(speed));
596
597 WAYPT_SET(waypt, course, course);
598
599 nmea_set_waypoint_time(waypt, &tm, microseconds);
600
601 if (latdir == 'S') {
602 latdeg = -latdeg;
603 }
604 waypt->latitude = ddmm2degrees(latdeg);
605
606 if (lngdir == 'W') {
607 lngdeg = -lngdeg;
608 }
609 waypt->longitude = ddmm2degrees(lngdeg);
610
611 nmea_release_wpt(curr_waypt);
612 curr_waypt = waypt;
613
614 /* This point is both a waypoint and a trackpoint. */
615 if (amod_waypoint) {
616 waypt_add(waypt_dupe(waypt));
617 amod_waypoint = 0;
618 }
619 }
620
621 static void
gpwpl_parse(char * ibuf)622 gpwpl_parse(char* ibuf)
623 {
624 waypoint* waypt;
625 double latdeg, lngdeg;
626 char latdir, lngdir;
627 char sname[99];
628
629 sscanf(ibuf,"$GPWPL,%lf,%c,%lf,%c,%[^*]",
630 &latdeg,&latdir,
631 &lngdeg,&lngdir,
632 sname);
633
634 waypt = waypt_new();
635
636 if (latdir == 'S') {
637 latdeg = -latdeg;
638 }
639 waypt->latitude = ddmm2degrees(latdeg);
640 if (lngdir == 'W') {
641 lngdeg = -lngdeg;
642 }
643 waypt->longitude = ddmm2degrees(lngdeg);
644
645 waypt->shortname = xstrdup(sname);
646
647 curr_waypt = NULL; /* waypoints won't be updated with GPS fixes */
648 nmea_add_wpt(waypt, NULL);
649 }
650
651 static void
gpzda_parse(char * ibuf)652 gpzda_parse(char* ibuf)
653 {
654 double hms;
655 int dd, mm, yy, lclhrs, lclmins;
656
657 sscanf(ibuf,"$GPZDA,%lf,%d,%d,%d,%d,%d",
658 &hms, &dd, &mm, &yy, &lclhrs, &lclmins);
659 tm.tm_sec = (int) hms % 100;
660 tm.tm_min = (((int) hms - tm.tm_sec) / 100) % 100;
661 tm.tm_hour = (int) hms / 10000;
662 tm.tm_mday = dd;
663 tm.tm_mon = mm - 1;
664 tm.tm_year = yy - 1900;
665 }
666
667 static void
gpgsa_parse(char * ibuf)668 gpgsa_parse(char* ibuf)
669 {
670 char fixauto;
671 char fix;
672 int prn[12];
673 int scn,cnt;
674 float pdop=0,hdop=0,vdop=0;
675 char* tok=0;
676
677 memset(prn,0xff,sizeof(prn));
678
679 scn = sscanf(ibuf,"$GPGSA,%c,%c,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",
680 &fixauto, &fix,
681 &prn[0],&prn[1],&prn[2],&prn[3],&prn[4],&prn[5],
682 &prn[6],&prn[7],&prn[8],&prn[9],&prn[10],&prn[11]);
683
684 /*
685 sscanf has scanned all the leftmost elements
686 we'll rescan by skipping 15 commas to the dops
687 */
688 tok = ibuf;
689 for (cnt=0; (tok)&&(cnt<15); cnt++) {
690 tok = strchr(tok,',');
691 if (!tok) {
692 break;
693 }
694 tok++;
695 }
696 if (tok) {
697 sscanf(tok,"%f,%f,%f",&pdop,&hdop,&vdop);
698 }
699
700
701 if (curr_waypt) {
702
703 if (curr_waypt->fix!=fix_dgps) {
704 if (fix=='3') {
705 curr_waypt->fix=fix_3d;
706 } else if (fix=='2') {
707 curr_waypt->fix=fix_2d;
708 }
709 }
710
711 curr_waypt->pdop = pdop;
712 curr_waypt->hdop = hdop;
713 curr_waypt->vdop = vdop;
714
715 if (curr_waypt->sat <= 0) {
716 for (cnt=0; cnt<12; cnt++) {
717 curr_waypt->sat += (prn[cnt]>0)?(1):(0);
718 }
719 }
720 }
721
722 }
723
724 static void
gpvtg_parse(char * ibuf)725 gpvtg_parse(char* ibuf)
726 {
727 float course;
728 char ct;
729 float magcourse;
730 char cm;
731 double speed_n;
732 char cn;
733 double speed_k;
734 char ck;
735
736 sscanf(ibuf,"$GPVTG,%f,%c,%f,%c,%lf,%c,%lf,%c",
737 &course,&ct,&magcourse,&cm,&speed_n,&cn,&speed_k,&ck);
738
739 if (curr_waypt) {
740 WAYPT_SET(curr_waypt, course, course);
741
742 if (speed_k>0)
743 WAYPT_SET(curr_waypt, speed, KPH_TO_MPS(speed_k))
744 else {
745 WAYPT_SET(curr_waypt, speed, KNOTS_TO_MPS(speed_n));
746 }
747
748 }
749
750 }
751
752 /*
753 * AVMAP EKP-IV Tracks - a proprietary (and very weird) extended NMEA.
754 * https://sourceforge.net/tracker/?func=detail&atid=489478&aid=1640814&group_id=58972
755 */
756 static
pcmpt_deg(int d)757 double pcmpt_deg(int d)
758 {
759 int deg;
760 double minutes;
761
762 deg = d / 100000;
763 minutes = (((d / 100000.0) - deg) * 100) / 60.0;
764 return (double) deg + minutes;
765 }
766
767 void
pcmpt_parse(char * ibuf)768 pcmpt_parse(char* ibuf)
769 {
770 int i, j1, j2, j3, j4, j5, j6;
771 int lat, lon;
772 char altflag, u1, u2;
773 float alt, f1, f2;
774 char coords[20] = {0};
775 int dmy, hms;
776
777 dmy = hms = 0;
778
779 sscanf(ibuf,"$PCMPT,%d,%d,%d,%c,%f,%d,%[^,],%d,%f,%d,%f,%c,%d,%c,%d",
780 &j1, &j2, &j3, &altflag, &alt, &j4, (char*) &coords,
781 &j5, &f1, &j6, &f2, &u1, &dmy, &u2, &hms);
782
783 if (altflag == 'D' && curr_waypt && alt > 0) {
784 curr_waypt->altitude = alt /*+ 500*/;
785 return;
786 }
787
788 /*
789 * There are a couple of different second line records, but we
790 * don't care about them.
791 */
792 if (j2 != 1) {
793 return;
794 }
795
796 sscanf(coords, "%d%n", &lat, &i);
797 if (coords[i] == 'S') {
798 lat = -lat;
799 }
800 sscanf(coords + i + 1, "%d%n", &lon, &i);
801 if (coords[i] == 'W') {
802 lon= -lon;
803 }
804
805 if (lat || lon) {
806 curr_waypt = waypt_new();
807 curr_waypt->longitude = pcmpt_deg(lon);
808 curr_waypt->latitude = pcmpt_deg(lat);
809
810 tm.tm_sec = (long) hms % 100;
811 hms = hms / 100;
812 tm.tm_min = (long) hms % 100;
813 hms = hms / 100;
814 tm.tm_hour = (long) hms % 100;
815
816 tm.tm_year = dmy % 10000 - 1900;
817 dmy = dmy / 10000;
818 tm.tm_mon = dmy % 100 - 1;
819 dmy = dmy / 100;
820 tm.tm_mday = dmy;
821 nmea_set_waypoint_time(curr_waypt, &tm, 0);
822 ENQUEUE_HEAD(&pcmpt_head, &curr_waypt->Q);
823 } else {
824 queue* elem, *tmp;
825 route_head* trk_head;
826
827 if (QUEUE_EMPTY(&pcmpt_head)) {
828 return;
829 }
830
831 /*
832 * Since we oh-so-cleverly inserted points at the head,
833 * we can rip through the queue forward now to get our
834 ` * handy-dandy reversing effect.
835 */
836 trk_head = route_head_alloc();
837 track_add_head(trk_head);
838 QUEUE_FOR_EACH(&pcmpt_head, elem, tmp) {
839 waypoint* wpt = (waypoint*) dequeue(elem);
840 nmea_add_wpt(wpt, trk_head);
841 }
842 }
843 }
844
845 static void
nmea_fix_timestamps(route_head * track)846 nmea_fix_timestamps(route_head* track)
847 {
848 if ((trk_head == NULL) || (without_date == 0)) {
849 return;
850 }
851
852 if (tm.tm_year == 0) {
853 queue* elem, *temp;
854 waypoint* prev = NULL;
855 time_t delta_tm;
856
857 if (optdate == NULL) {
858 warning(MYNAME ": No date found within track (all points dropped)!\n");
859 warning(MYNAME ": Please use option \"date\" to preset a valid date for thoose tracks.\n");
860 track_del_head(track);
861 return;
862 }
863 delta_tm = mkgmtime(&opt_tm);
864
865 QUEUE_FOR_EACH(&track->waypoint_list, elem, temp) {
866 waypoint* wpt = (waypoint*)elem;
867
868 wpt->creation_time += delta_tm;
869 if ((prev != NULL) && (prev->creation_time > wpt->creation_time)) { /* go over midnight ? */
870 delta_tm += SECONDS_PER_DAY;
871 wpt->creation_time += SECONDS_PER_DAY;
872 }
873 prev = wpt;
874 }
875 } else {
876 time_t prev;
877 queue* elem;
878
879 tm.tm_hour = 23; /* last date found */
880 tm.tm_min = 59;
881 tm.tm_sec = 59;
882
883 prev = mkgmtime(&tm);
884
885 /* go backward through the track and complete timestamps */
886
887 for (elem = QUEUE_LAST(&track->waypoint_list); elem != &track->waypoint_list; elem=elem->prev) {
888 waypoint* wpt = (waypoint*)elem;
889
890 if (wpt->wpt_flags.fmt_use != 0) {
891 time_t dt;
892
893 wpt->wpt_flags.fmt_use = 0; /* reset flag */
894
895 dt = (prev / SECONDS_PER_DAY) * SECONDS_PER_DAY;
896 wpt->creation_time += dt;
897 if (wpt->creation_time > prev) {
898 wpt->creation_time+=SECONDS_PER_DAY;
899 }
900 }
901 prev = wpt->creation_time;
902 }
903 }
904 }
905
906 void
nmea_parse_one_line(char * ibuf)907 nmea_parse_one_line(char* ibuf)
908 {
909 char* ck;
910 int ckval, ckcmp;
911 char* tbuf = lrtrim(ibuf);
912
913 /*
914 * GISTEQ PhotoTracker (stupidly) puts a bogus field in front
915 * of the line. Look for it and toss it.
916 */
917 if (0 == strncmp(tbuf, "---,", 4)) {
918 tbuf += 4;
919 }
920
921 if (*tbuf != '$') {
922 return;
923 }
924
925 ck = strrchr(tbuf, '*');
926 if (ck != NULL) {
927 *ck = '\0';
928 ckval = nmea_cksum(&tbuf[1]);
929 *ck = '*';
930 ck++;
931 sscanf(ck, "%2X", &ckcmp);
932 if (ckval != ckcmp) {
933 #if 0
934 printf("ckval %X, %X, %s\n", ckval, ckcmp, ck);
935 printf("NMEA %s\n", tbuf);
936 #endif
937 return;
938 }
939
940 had_checksum = 1;
941 } else if (had_checksum) {
942 /* we have had a checksum on all previous sentences, but not on this
943 one, which probably indicates this line is truncated */
944 had_checksum = 0;
945 return;
946 }
947
948 if (strstr(tbuf+1,"$")!=NULL) {
949 /* If line has more than one $, there is probably an error in it. */
950 return;
951 }
952
953 /* @@@ zmarties: The parse routines all assume all fields are present, but
954 the NMEA format allows any field to be missed out if there is no data
955 for that field. Rather than change all the parse routines, we first
956 substitute a default value of zero for any missing field.
957 */
958 if (strstr(tbuf, ",,")) {
959 tbuf = gstrsub(tbuf, ",,", ",0,");
960 }
961
962 if (0 == strncmp(tbuf, "$GPWPL,", 7)) {
963 gpwpl_parse(tbuf);
964 } else if (opt_gpgga && (0 == strncmp(tbuf, "$GPGGA,", 7))) {
965 posn_type = gpgga;
966 gpgga_parse(tbuf);
967 } else if (opt_gprmc && (0 == strncmp(tbuf, "$GPRMC,", 7))) {
968 if (posn_type != gpgga) {
969 posn_type = gprmc;
970 }
971 /*
972 * Always call gprmc_parse() because like GPZDA
973 * it contains the full date.
974 */
975 gprmc_parse(tbuf);
976 } else if (0 == strncmp(tbuf, "$GPGLL,", 7)) {
977 if ((posn_type != gpgga) && (posn_type != gprmc)) {
978 gpgll_parse(tbuf);
979 }
980 } else if (0 == strncmp(tbuf, "$GPZDA,",7)) {
981 gpzda_parse(tbuf);
982 } else if (0 == strncmp(tbuf, "$PCMPT,", 7)) {
983 pcmpt_parse(tbuf);
984 } else if (opt_gpvtg && (0 == strncmp(tbuf, "$GPVTG,",7))) {
985 gpvtg_parse(tbuf); /* speed and course */
986 } else if (opt_gpgsa && (0 == strncmp(tbuf, "$GPGSA,",7))) {
987 gpgsa_parse(tbuf); /* GPS fix */
988 } else if (0 == strncmp(tbuf, "$ADPMB,5,0", 10)) {
989 amod_waypoint = 1;
990 }
991
992 if (tbuf != ibuf) {
993 /* clear up the dynamic buffer we used because substition was required */
994 xfree(tbuf);
995 }
996 }
997
998 static void
nmea_read(void)999 nmea_read(void)
1000 {
1001 char* ibuf;
1002 char* ck;
1003 double lt = -1;
1004 int line = -1;
1005
1006 posn_type = gp_unknown;
1007 trk_head = NULL;
1008 without_date = 0;
1009 memset(&tm, 0, sizeof(tm));
1010 opt_tm = tm;
1011
1012 /* This was done in rd_init() */
1013 if (getposn) {
1014 return;
1015 }
1016
1017 if (optdate) {
1018 memset(&opt_tm, 0, sizeof(opt_tm));
1019
1020 ck = (char*)strptime(optdate, "%Y%m%d", &opt_tm);
1021 if ((ck == NULL) || (*ck != '\0') || (strlen(optdate) != 8)) {
1022 fatal(MYNAME ": Invalid date \"%s\"!\n", optdate);
1023 } else if (opt_tm.tm_year < 70) {
1024 fatal(MYNAME ": Date \"%s\" is out of range (have to be 19700101 or later)!\n", optdate);
1025 }
1026 }
1027
1028 curr_waypt = NULL;
1029
1030 while ((ibuf = gbfgetstr(file_in))) {
1031 char* sdatum, *cx;
1032
1033 line++;
1034
1035 if ((line == 0) & file_in->unicode) {
1036 cet_convert_init(CET_CHARSET_UTF8, 1);
1037 }
1038
1039 if ((line == 0) && (case_ignore_strncmp(ibuf, "@SonyGPS/ver", 12) == 0)) {
1040 /* special hack for Sony GPS-CS1 files:
1041 they are fully (?) nmea compatible, but come with a header line like
1042 "@Sonygps/ver1.0/wgs-84". */
1043 /* The Sony GPS-CS3KA extends that line even further
1044 so we now look for the second field to be /
1045 delimited.
1046 @Sonygps/ver1.0/wgs-84/gps-cs3.0
1047 */
1048
1049 /* Check the GPS datum */
1050 cx = strchr(&ibuf[12], '/');
1051 if (cx != NULL) {
1052 char* edatum;
1053 sdatum = cx + 1;
1054 edatum = strchr(sdatum, '/');
1055 if (edatum) {
1056 *edatum = 0;
1057 }
1058 datum = GPS_Lookup_Datum_Index(sdatum);
1059 if (datum < 0) {
1060 fatal(MYNAME "/SonyGPS: Unsupported datum \"%s\" in source data!\n", sdatum);
1061 }
1062 }
1063 continue;
1064 }
1065
1066 nmea_parse_one_line(ibuf);
1067 if (lt != last_read_time && curr_waypt && trk_head) {
1068 if (curr_waypt != last_waypt) {
1069 nmea_add_wpt(curr_waypt, trk_head);
1070 last_waypt = curr_waypt;
1071 }
1072 lt = last_read_time;
1073 }
1074 }
1075
1076 /* try to complete date-less trackpoints */
1077 nmea_fix_timestamps(trk_head);
1078 }
1079
1080 void
nmea_rd_posn_init(const char * fname)1081 nmea_rd_posn_init(const char* fname)
1082 {
1083 if ((gbser_handle = gbser_init(fname)) != NULL) {
1084 read_mode = rm_serial;
1085 gbser_set_speed(gbser_handle, 4800);
1086 } else {
1087 fatal(MYNAME ": Could not open '%s' for position tracking.\n", fname);
1088 }
1089
1090 gbser_flush(gbser_handle);
1091
1092 if (opt_baud) {
1093 if (!gbser_set_speed(gbser_handle, atoi(opt_baud))) {
1094 fatal(MYNAME ": Unable to set baud rate %s\n", opt_baud);
1095 }
1096 }
1097 posn_fname = fname;
1098 }
1099
1100 static void
safe_print(int cnt,const char * b)1101 safe_print(int cnt, const char* b)
1102 {
1103 int i;
1104 for (i = 0; i < cnt; i++) {
1105 char c = isprint(b[i]) ? b[i] : '.';
1106 fputc(c, stderr);
1107 }
1108 }
1109
1110 static void reset_sirf_to_nmea(int br);
1111
1112 static
hunt_sirf(void)1113 int hunt_sirf(void)
1114 {
1115 /* Try to place the common BR's first to speed searching */
1116 static int br[] = {38400, 9600, 57600, 115200, 19200, 4800, -1};
1117 static int* brp = &br[0];
1118 char ibuf[1024];
1119
1120 for (brp = br; *brp > 0; brp++) {
1121 int rv;
1122 if (global_opts.debug_level > 1) {
1123 fprintf(stderr, "Trying %d\n", *brp);
1124 }
1125
1126 /*
1127 * Cycle our port's data speed and spray the "change to NMEA
1128 * mode to the device.
1129 */
1130 gbser_set_speed(gbser_handle, *brp);
1131 reset_sirf_to_nmea(*brp);
1132
1133 rv = gbser_read_line(gbser_handle, ibuf, sizeof(ibuf),
1134 1000, 0x0a, 0x0d);
1135 /*
1136 * If we didn't get a read error but did get a string that
1137 * started with a dollar sign, we're probably in NMEA mode
1138 * now.
1139 */
1140 if ((rv > -1) && (strlen(ibuf) > 0) && ibuf[0] == '$') {
1141 return 1;
1142 }
1143
1144 /*
1145 * If nothing was received, it's not a sirf part. Fast exit.
1146 */
1147 if (rv < 0) {
1148 return 0;
1149 }
1150 }
1151 return 0;
1152 }
1153
1154 static waypoint*
nmea_rd_posn(posn_status * posn_status)1155 nmea_rd_posn(posn_status* posn_status)
1156 {
1157 char ibuf[1024];
1158 static double lt = -1;
1159 int i;
1160 int am_sirf = 0;
1161
1162 /*
1163 * Read a handful of sentences, collecting the best info we
1164 * can. If the timestamp changes (indicating the sequence is
1165 * about to restart and thus the one we're collecting isn't going
1166 * to get any better than we now have) hand that back to the caller.
1167 */
1168
1169 for (i = 0; i < 10; i++) {
1170 int rv;
1171 ibuf[0] = 0;
1172 rv = gbser_read_line(gbser_handle, ibuf, sizeof(ibuf), 2000, 0x0a, 0x0d);
1173 if (global_opts.debug_level > 1) {
1174 safe_print(strlen(ibuf), ibuf);
1175 }
1176 if (rv < 0) {
1177 if (am_sirf == 0) {
1178 if (global_opts.debug_level > 1) {
1179 warning(MYNAME ": Attempting sirf mode.\n");
1180 }
1181 /* This is tacky, we have to change speed
1182 * to 9600bps to tell it to speak NMEA at
1183 * 4800.
1184 */
1185 am_sirf = hunt_sirf();
1186 if (am_sirf) {
1187 i = 0;
1188 continue;
1189 }
1190 }
1191 fatal(MYNAME ": No data received on %s.\n", posn_fname);
1192 }
1193 nmea_parse_one_line(ibuf);
1194 if (lt != last_read_time) {
1195 if (last_read_time) {
1196 waypoint* w = curr_waypt;
1197
1198 lt = last_read_time;
1199 curr_waypt = NULL;
1200
1201 return w;
1202 }
1203 }
1204 }
1205 return NULL;
1206 }
1207
1208 static void
nmea_wayptpr(const waypoint * wpt)1209 nmea_wayptpr(const waypoint* wpt)
1210 {
1211 char obuf[200];
1212 double lat,lon;
1213 char* s;
1214 int cksum;
1215
1216 lat = degrees2ddmm(wpt->latitude);
1217 lon = degrees2ddmm(wpt->longitude);
1218 if (global_opts.synthesize_shortnames) {
1219 s = mkshort_from_wpt(mkshort_handle, wpt);
1220 } else {
1221 s = mkshort(mkshort_handle, wpt->shortname);
1222 }
1223
1224 snprintf(obuf, sizeof(obuf), "GPWPL,%08.3f,%c,%09.3f,%c,%s",
1225 fabs(lat), lat < 0 ? 'S' : 'N',
1226 fabs(lon), lon < 0 ? 'W' : 'E', s
1227
1228 );
1229 cksum = nmea_cksum(obuf);
1230 gbfprintf(file_out, "$%s*%02X\n", obuf, cksum);
1231 if (sleepus >= 0) {
1232 gbfflush(file_out);
1233 gb_sleep(sleepus);
1234 }
1235
1236 xfree(s);
1237
1238 }
1239
1240 void
nmea_track_init(const route_head * rte)1241 nmea_track_init(const route_head* rte)
1242 {
1243 last_time = -1;
1244 }
1245
1246 void
nmea_trackpt_pr(const waypoint * wpt)1247 nmea_trackpt_pr(const waypoint* wpt)
1248 {
1249 char obuf[200];
1250 char fix='0';
1251 double lat,lon;
1252 int cksum;
1253 struct tm* tm;
1254 time_t hms;
1255 time_t ymd;
1256
1257 if (opt_sleep) {
1258 gbfflush(file_out);
1259 if (last_time > 0) {
1260 if (sleepus >= 0) {
1261 gb_sleep(sleepus);
1262 } else {
1263 long wait_time = wpt->creation_time - last_time;
1264 if (wait_time > 0) {
1265 gb_sleep(wait_time * 1000000);
1266 }
1267 }
1268 }
1269 last_time = wpt->creation_time;
1270 }
1271
1272 lat = degrees2ddmm(wpt->latitude);
1273 lon = degrees2ddmm(wpt->longitude);
1274
1275 tm = gmtime(&wpt->creation_time);
1276 if (tm) {
1277 hms = tm->tm_hour * 10000 + tm->tm_min * 100 + tm->tm_sec;
1278 ymd = tm->tm_mday * 10000 + tm->tm_mon * 100 + tm->tm_year;
1279 } else {
1280 hms = 0;
1281 ymd = 0;
1282 }
1283
1284 switch (wpt->fix) {
1285 case fix_dgps:
1286 fix='2';
1287 /* or */
1288 case fix_3d:
1289 case fix_2d:
1290 fix='1';
1291 break;
1292 case fix_pps:
1293 fix='3';
1294 break;
1295 default:
1296 fix='0';
1297 }
1298
1299 if (opt_gprmc) {
1300 snprintf(obuf, sizeof(obuf), "GPRMC,%010.3f,%c,%08.3f,%c,%09.3f,%c,%.2f,%.2f,%06d,,",
1301 (double) hms + (wpt->microseconds / 1000000.0),
1302 fix=='0' ? 'V' : 'A',
1303 fabs(lat), lat < 0 ? 'S' : 'N',
1304 fabs(lon), lon < 0 ? 'W' : 'E',
1305 WAYPT_HAS(wpt, speed) ? MPS_TO_KNOTS(wpt->speed):(0),
1306 WAYPT_HAS(wpt, course) ? (wpt->course):(0),
1307 (int) ymd);
1308 cksum = nmea_cksum(obuf);
1309
1310 /* GISTeq doesn't care about the checksum, but wants this prefixed, so
1311 * we can write it with abandon.
1312 */
1313 if (opt_gisteq) {
1314 gbfprintf(file_out, "---,");
1315 }
1316 gbfprintf(file_out, "$%s*%02X\n", obuf, cksum);
1317 }
1318 if (opt_gpgga) {
1319 snprintf(obuf, sizeof(obuf), "GPGGA,%010.3f,%08.3f,%c,%09.3f,%c,%c,%02d,%.1f,%.3f,M,0.0,M,,",
1320 (double) hms + (wpt->microseconds / 1000000.0),
1321 fabs(lat), lat < 0 ? 'S' : 'N',
1322 fabs(lon), lon < 0 ? 'W' : 'E',
1323 fix,
1324 (wpt->sat>0)?(wpt->sat):(0),
1325 (wpt->hdop>0)?(wpt->hdop):(0.0),
1326 wpt->altitude == unknown_alt ? 0 : wpt->altitude);
1327 cksum = nmea_cksum(obuf);
1328 gbfprintf(file_out, "$%s*%02X\n", obuf, cksum);
1329 }
1330 if ((opt_gpvtg) && (WAYPT_HAS(wpt, course) || WAYPT_HAS(wpt, speed))) {
1331 snprintf(obuf,sizeof(obuf),"GPVTG,%.3f,T,0,M,%.3f,N,%.3f,K",
1332 WAYPT_HAS(wpt, course) ? (wpt->course):(0),
1333 WAYPT_HAS(wpt, speed) ? MPS_TO_KNOTS(wpt->speed):(0),
1334 WAYPT_HAS(wpt, speed) ? MPS_TO_KPH(wpt->speed):(0));
1335
1336 cksum = nmea_cksum(obuf);
1337 gbfprintf(file_out, "$%s*%02X\n", obuf, cksum);
1338 }
1339
1340 if ((opt_gpgsa) && (wpt->fix!=fix_unknown)) {
1341
1342 switch (wpt->fix) {
1343 case fix_dgps:
1344 /* or */
1345 case fix_3d:
1346 fix='3';
1347 break;
1348 case fix_2d:
1349 fix='2';
1350 break;
1351 default:
1352 fix=0;
1353 }
1354 snprintf(obuf,sizeof(obuf),"GPGSA,A,%c,,,,,,,,,,,,,%.1f,%.1f,%.1f",
1355 fix,
1356 (wpt->pdop>0)?(wpt->pdop):(0),
1357 (wpt->hdop>0)?(wpt->hdop):(0),
1358 (wpt->vdop>0)?(wpt->vdop):(0));
1359 cksum = nmea_cksum(obuf);
1360 gbfprintf(file_out, "$%s*%02X\n", obuf, cksum);
1361 }
1362 gbfflush(file_out);
1363 }
1364
1365 static void
nmea_write(void)1366 nmea_write(void)
1367 {
1368 waypt_disp_all(nmea_wayptpr);
1369 track_disp_all(nmea_track_init, NULL, nmea_trackpt_pr);
1370 }
1371
1372 static void
nmea_wr_posn_init(const char * fname)1373 nmea_wr_posn_init(const char* fname)
1374 {
1375 nmea_wr_init(fname);
1376 }
1377
1378 static void
nmea_wr_posn(waypoint * wpt)1379 nmea_wr_posn(waypoint* wpt)
1380 {
1381 nmea_trackpt_pr(wpt);
1382 }
1383
1384 static void
nmea_wr_posn_deinit(void)1385 nmea_wr_posn_deinit(void)
1386 {
1387 // nmea_wr_deinit();
1388 }
1389
1390
1391 ff_vecs_t nmea_vecs = {
1392 ff_type_file,
1393 {
1394 (ff_cap)(ff_cap_read | ff_cap_write),
1395 (ff_cap)(ff_cap_read | ff_cap_write),
1396 ff_cap_none
1397 },
1398 nmea_rd_init,
1399 nmea_wr_init,
1400 nmea_rd_deinit,
1401 nmea_wr_deinit,
1402 nmea_read,
1403 nmea_write,
1404 NULL,
1405 nmea_args,
1406 CET_CHARSET_ASCII, 0, /* CET-REVIEW */
1407 {
1408 nmea_rd_posn_init, nmea_rd_posn, nmea_rd_deinit,
1409 nmea_wr_posn_init, nmea_wr_posn, nmea_wr_posn_deinit
1410 }
1411 };
1412
1413 /*
1414 * If we later decide to implement a "real" Sirf module, this code should
1415 * go there. For now, we try a kind of heavy handed thing - if we don't
1416 * see NMEA-isms from the device, we'll go on the premise that it MAY be
1417 * a SiRF Star device and send it the "speak NMEA, please" command.
1418 */
1419
1420 static void
sirf_write(unsigned char * buf)1421 sirf_write(unsigned char* buf)
1422 {
1423 int i, chksum = 0;
1424 int len = buf[2] << 8 | buf[3];
1425
1426 for (i = 0; i < len; i++) {
1427 chksum += buf[4 + i];
1428 }
1429 chksum &= 0x7fff;
1430
1431 buf[len + 4] = chksum >> 8;
1432 buf[len + 5] = chksum & 0xff;
1433
1434 gbser_write(gbser_handle, buf, len + 8); /* 4 at front, 4 at back */
1435 }
1436
1437 static
reset_sirf_to_nmea(int br)1438 void reset_sirf_to_nmea(int br)
1439 {
1440 static unsigned char pkt[] = {0xa0, 0xa2, 0x00, 0x18,
1441 0x81, 0x02,
1442 0x01, 0x01, /* GGA */
1443 0x00, 0x00, /* suppress GLL */
1444 0x01, 0x00, /* suppress GSA */
1445 0x05, 0x00, /* suppress GSV */
1446 0x01, 0x01, /* use RMC for date*/
1447 0x00, 0x00, /* suppress VTG */
1448 0x00, 0x01, /* output rate */
1449 0x00, 0x01, /* unused recommended values */
1450 0x00, 0x01,
1451 0x00, 0x01, /* ZDA */
1452 0x12, 0xc0, /* 4800 bps */
1453 0x00, 0x00, /* checksum */
1454 0xb0, 0xb3
1455 }; /* packet end */
1456 /* repopulate bit rate */
1457 pkt[26] = br >> 8;
1458 pkt[27] = br & 0xff;
1459
1460 sirf_write(pkt);
1461 gb_sleep(250 * 1000);
1462 gbser_flush(gbser_handle);
1463 }
1464