1 /*
2
3 Format converter for MediaTek Locus-capable devices.
4
5 Copyright (C) 2012 Jeremy Mortis, mortis@tansay.ca
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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
20
21 -------------------------------------------------------------------
22
23 This module will download logged track data (a.k.a. Locus) from a GPS
24 devices based on the MediaTek MT3339 GPS chipset, such as:
25 - GlobalTop PA6H Module
26 - Fastrax IT530
27
28 The MT3339 also emits standard NMEA packets including
29 $GPGGA, $GPGSA, $GPGSV, $GPRMC, and $GPVTG. This module ignores those.
30 If you have an MT3339 and you want to process NMEA packets, simply use
31 the nmea format instead of this one.
32
33 Example usage::
34 # Read from USB port, output trackpoints in GPX format
35 ./gpsbabel -t -i mtk_locus -f /dev/ttyUSB0 -o gpx -F out.gpx
36
37 */
38
39
40 #include "defs.h"
41 #include "gbser.h"
42 #include <cerrno>
43 #include <cstdio>
44 #include <cstdlib>
45
46 static route_head* track;
47
48 static char* opt_baudrate;
49 static char* opt_download;
50 static char* opt_erase;
51 static char* opt_status;
52 static char* opt_enable;
53
54 static QVector<arglist_t> mtk_locus_args = {
55 {"baudrate", &opt_baudrate, "Speed in bits per second of serial port (autodetect=0)", "0", ARGTYPE_INT, ARG_NOMINMAX , nullptr},
56 {"download", &opt_download, "Download logged fixes", "1", ARGTYPE_BOOL, ARG_NOMINMAX, nullptr },
57 {"erase", &opt_erase, "Erase device data after download", "0", ARGTYPE_BOOL, ARG_NOMINMAX, nullptr },
58 {"status", &opt_status, "Show device status", "0", ARGTYPE_BOOL, ARG_NOMINMAX, nullptr },
59 {"enable", &opt_enable, "Enable logging after download", "0", ARGTYPE_BOOL, ARG_NOMINMAX, nullptr },
60 };
61
62 static void mtk_locus_rd_init(const QString& fname);
63 static void mtk_locus_rd_deinit();
64 static void mtk_locus_read();
65
66 ff_vecs_t mtk_locus_vecs = {
67 ff_type_file,
68 {
69 ff_cap_read /* waypoints */,
70 ff_cap_read /* tracks */,
71 ff_cap_none /* routes */
72 },
73 mtk_locus_rd_init,
74 nullptr, // write init
75 mtk_locus_rd_deinit,
76 nullptr, // write deinit
77 mtk_locus_read,
78 nullptr, // write
79 nullptr, // exit
80 &mtk_locus_args,
81 CET_CHARSET_ASCII, 0 /* ascii is the expected character set */
82 , NULL_POS_OPS,
83 nullptr
84 };
85
86 #define MYNAME "mtk_locus"
87 #define TIMEOUT 1500
88
89 #define PMTK_ACK "$PMTK001"
90 #define PMTK_LOCUS_STOP_LOGGER "$PMTK185"
91 #define PMTK_LOCUS_QUERY_STATUS "$PMTK183"
92 #define PMTK_LOCUS_ERASE_FLASH "$PMTK184"
93 #define PMTK_Q_LOCUS_DATA "$PMTK622"
94 #define PMTK_Q_RELEASE "$PMTK605"
95 #define PMTK_DT_RELEASE "$PMTK705"
96
97 static gbfile* ffd; // File access.
98 static void* sfd;
99 static enum {rm_serial, rm_file} read_mode;
100 static int packetnum;
101 static char line[1000];
102 static int download_complete;
103 static int valid_packet_found;
104 static int fixes_found;
105 static int first_loxsequence;
106 static int last_loxsequence;
107 static char waiting_for[20];
108
109
110 static void set_baudrate();
111 static void read_line();
112 static void process_packet();
113 static void process_pmtklox();
114 static void process_pmtklog();
115 static void process_pmtk001();
116 static void process_pmtk705();
117 static void send_command(const char* s, const char*wait_for);
118 static int calculate_checksum(const char* s, int length);
119 static void dbg(int l, const char* msg, ...);
120
121 static void
mtk_locus_rd_init(const QString & fname)122 mtk_locus_rd_init(const QString& fname)
123 {
124 dbg(1, "Opening file: %s\n", qPrintable(fname));
125
126 if (gbser_is_serial(qPrintable(fname))) {
127
128 dbg(1, "Input is a serial port\n");
129 read_mode = rm_serial;
130 if ((sfd = gbser_init(qPrintable(fname))) == nullptr) {
131 fatal(MYNAME ": Can't initialise port \"%s\" (%s)\n", qPrintable(fname), strerror(errno));
132 }
133 set_baudrate();
134 gbser_flush(sfd);
135
136 } else {
137
138 dbg(1, "Input is a normal file\n");
139 read_mode = rm_file;
140 if ((ffd = gbfopen(fname, "rb", MYNAME)) == nullptr) {
141 fatal(MYNAME ": Can't initialise port \"%s\" (%s)\n", qPrintable(fname), strerror(errno));
142 }
143 }
144
145 dbg(1, "File opened\n");
146 }
147
148 static void
mtk_locus_rd_deinit()149 mtk_locus_rd_deinit()
150 {
151 if (read_mode == rm_serial) {
152 gbser_deinit(sfd);
153 } else {
154 gbfclose(ffd);
155 }
156 }
157
158 static void
mtk_locus_read()159 mtk_locus_read()
160 {
161 track = new route_head;
162 track_add_head(track);
163 dbg(1, "Track initialized\n");
164
165 packetnum = 0;
166 valid_packet_found = 0;
167 fixes_found = 0;
168 download_complete = 0;
169 first_loxsequence = -1;
170 last_loxsequence = -1;
171
172 read_line();
173 // initial serial buffer may contain garbage, so read until valid packet found
174 for (int i = 0; i<10; i++) {
175 process_packet();
176 read_line();
177 if (valid_packet_found) {
178 break;
179 }
180 }
181
182 if (! valid_packet_found) {
183 fatal(MYNAME "No valid input data found");
184 }
185
186 if (strcmp(opt_download, "1") == 0) {
187 send_command(PMTK_Q_LOCUS_DATA ",1", nullptr);
188
189 while (! download_complete) {
190 process_packet();
191 read_line();
192 }
193 }
194
195 if (read_mode == rm_serial) {
196 if (strcmp(opt_erase, "1") == 0) {
197 send_command(PMTK_LOCUS_ERASE_FLASH ",1", PMTK_ACK);
198 printf("Flash erased\n");
199 }
200
201 if (strcmp(opt_enable, "1") == 0) {
202 send_command(PMTK_LOCUS_STOP_LOGGER ",0", PMTK_ACK);
203 printf("Logging enabled\n");
204 } else {
205 send_command(PMTK_LOCUS_STOP_LOGGER ",1", PMTK_ACK);
206 printf("Logging disabled\n");
207 }
208
209 if (strcmp(opt_status, "1") == 0) {
210 printf("Device status:\n");
211 send_command(PMTK_Q_RELEASE, PMTK_DT_RELEASE);
212 send_command(PMTK_LOCUS_QUERY_STATUS, PMTK_ACK);
213 }
214 }
215 }
216
217 void
set_baudrate()218 set_baudrate()
219 {
220 int rc;
221 int baudrates[] = { 4800, 9600, 14400, 19200, 38400, 57600, 115200, 0 };
222 int baudrate;
223
224 if (strcmp(opt_baudrate, "0") != 0) {
225
226 baudrate = atoi(opt_baudrate);
227 rc = gbser_set_speed(sfd, baudrate);
228 if (rc != gbser_OK) {
229 fatal(MYNAME ": Set baud rate to %i failed (%i)\n", baudrate, rc);
230 }
231
232 } else {
233
234 dbg(1, "Probing for baudrate...\n");
235 for (int i=0;; i++) {
236 baudrate = baudrates[i];
237 if (baudrate == 0) {
238 fatal(MYNAME ": Autobaud connection failed\n");
239 }
240 dbg(1, MYNAME ": Probing at %i baud...\n", baudrate);
241 rc = gbser_set_speed(sfd, baudrate);
242
243 if (rc != gbser_OK) {
244 dbg(1, "Set speed failed\n");
245 continue;
246 }
247
248 rc = gbser_read_line(sfd, line, sizeof(line)-1, TIMEOUT, 0x0A, 0x0D);
249 if (rc != gbser_OK) {
250 dbg(1, "Read test failed\n");
251 continue;
252 }
253
254 dbg(1, "Port successfully opened\n");
255 break;
256 }
257 }
258 }
259
260 void
read_line()261 read_line()
262 {
263 line[0] = '\0';
264
265 if (read_mode == rm_file) {
266 char* s = gbfgetstr(ffd);
267 if (s == nullptr) {
268 dbg(1, "EOF reached\n");
269 download_complete = 1;
270 return;
271 }
272 strncat(line, s, sizeof(line)-1);
273 } else {
274 int rc = gbser_read_line(sfd, line, sizeof(line)-1, TIMEOUT, 0x0A, 0x0D);
275 if (rc != gbser_OK) {
276 fatal(MYNAME "Serial read failed: %i\n", rc);
277 }
278 }
279
280 packetnum++;
281 dbg(1, "Line %i: %s\n", packetnum, line);
282 }
283
284 void
process_packet()285 process_packet()
286 {
287 int given_checksum;
288
289 if ((strlen(line) < 3) || (line[0] != '$') || (line[strlen(line)-3] != '*')) {
290 dbg(1, "Line %i: Malformed packet\n", packetnum);
291 return;
292 }
293
294 int calculated_checksum = calculate_checksum(&line[1], strlen(line) - 1 - 3);
295 sscanf(&line[strlen(line) - 2], "%02x", &given_checksum);
296 if (calculated_checksum != given_checksum) {
297 dbg(1, "Line %i: NMEA Checksum incorrect, expecting %02X\n", packetnum, calculated_checksum);
298 return;
299 }
300
301 if (strncmp(line, waiting_for, strlen(waiting_for)) == 0) {
302 waiting_for[0] = '\0';
303 }
304
305 valid_packet_found = 1;
306 line[strlen(line) - 3] = '\0'; // remove checksum
307
308 // note that use of strtok in following routines corrupts the value of line
309
310 if (strncmp(line, "$PMTKLOX", 8) == 0) {
311 process_pmtklox();
312 } else if (strncmp(line, "$PMTKLOG", 8) == 0) {
313 process_pmtklog();
314 } else if (strncmp(line, "$PMTK001", 8) == 0) {
315 process_pmtk001();
316 } else if (strncmp(line, "$PMTK705", 8) == 0) {
317 process_pmtk705();
318 } else {
319 dbg(1, "Unknown packet type\n");
320 }
321
322 }
323
324 void
process_pmtklox()325 process_pmtklox()
326 {
327 uint8_t fixbytes[16];
328 static Waypoint* trkpt;
329 static Waypoint* waypt;
330
331 char* token = strtok(line, ",");
332 if ((token == nullptr) || (strcmp(token, "$PMTKLOX") != 0)) {
333 warning("Line %i: Invalid packet id\n", packetnum);
334 return;
335 }
336
337 char* loxtype = strtok(nullptr, ",");
338 if (loxtype == nullptr) {
339 warning("Line %i: Missing lox type\n", packetnum);
340 return;
341 }
342
343 if (strcmp(loxtype, "0") == 0) {
344 last_loxsequence = atoi(strtok(nullptr, "*")) - 1;
345 dbg(1, "Line %i: last sequence will be %i\n", packetnum, last_loxsequence);
346 return;
347 }
348
349 if (strcmp(loxtype, "2") == 0) {
350 printf("Found %i fixes\n", fixes_found);
351 download_complete = 1;
352 return;
353 }
354
355 if (strcmp(loxtype, "1") != 0) {
356 dbg(1, "Line %i: Invalid lox type\n", packetnum);
357 return;
358 }
359
360 int loxsequence = atoi(strtok(nullptr, ","));
361
362 if (first_loxsequence == -1) {
363 first_loxsequence = loxsequence;
364 if (first_loxsequence != 0) {
365 fatal(MYNAME "Dump already in progress (first $PMTKLOX has sequence %i)\n", first_loxsequence);
366 }
367 }
368
369 if (read_mode == rm_serial) {
370 printf("Downloading packet %i of %i\r", loxsequence, last_loxsequence);
371 }
372
373 token = strtok(nullptr, ",");
374 int fixnum = 0;
375 while (token != nullptr) {
376 fixnum++;
377 int bytenum = 0;
378 uint8_t calculated_checksum = 0;
379 for (int wordnum = 0; wordnum<4; wordnum++) { // 4 8-byte hex strings per fix
380 if (token == nullptr) {
381 dbg(1, "Line %i: Fix %i incomplete data\n", packetnum, fixnum);
382 return;
383 }
384 for (int i = 0; i<4; i++) {
385 unsigned int hexval;
386 sscanf(&token[i * 2], "%2x", &hexval);
387 fixbytes[bytenum++] = hexval;
388 calculated_checksum ^= hexval;
389 }
390 token = strtok(nullptr, ",");
391 }
392
393 if (calculated_checksum != 0) {
394 dbg(1, "Line %i: Fix %i failed checksum\n", packetnum, fixnum);
395 continue;
396 }
397
398 uint32_t timestamp = le_read32(&fixbytes[0]);
399 char fixtype = fixbytes[4];
400 float latitude = le_read_float(&fixbytes[5]);
401 float longitude = le_read_float(&fixbytes[9]);
402 int height = le_read16(&fixbytes[13]);
403
404 if (fixtype != '\x02') {
405 dbg(1, "line %i: Fix %i Invalid fix type: %02X\n", packetnum, fixnum, fixtype);
406 continue;
407 }
408
409 if ((latitude < -180.0) || (latitude > 180.0)
410 || (longitude < -180.0) || (longitude > 180.0)
411 || (height < -1000) || (height > 100000)) {
412 dbg(1, "line %i: Fix %i data out of range\n", packetnum, fixnum);
413 continue;
414 }
415
416 if (global_opts.masked_objective & TRKDATAMASK) {
417 trkpt = new Waypoint;
418 trkpt->SetCreationTime(timestamp);
419 trkpt->latitude = latitude;
420 trkpt->longitude = longitude;
421 trkpt->altitude = height;
422 trkpt->sat = 0;
423 trkpt->hdop = 0;
424 trkpt->fix = fix_3d;
425 track_add_wpt(track, trkpt);
426 }
427
428 if (global_opts.masked_objective & WPTDATAMASK) {
429 waypt = new Waypoint;
430 waypt->SetCreationTime(timestamp);
431 waypt->latitude = latitude;
432 waypt->longitude = longitude;
433 waypt->altitude = height;
434 waypt->sat = 0;
435 waypt->hdop = 0;
436 waypt->fix = fix_3d;
437 waypt_add(waypt);
438 }
439
440 dbg(1, "Time: %li Type: %02x Lat: %f Long: %f height: %i\n", (long int)timestamp, fixtype, latitude, longitude, height);
441
442 fixes_found++;
443 }
444 }
445
446 void
process_pmtklog()447 process_pmtklog()
448 {
449 strtok(line, ",");
450
451 printf("Serial#: %s\n", strtok(nullptr, ","));
452
453 int type = atoi(strtok(nullptr, ","));
454 if (type == 0) {
455 printf("Type: %i (wrap around when full)\n", type);
456 } else {
457 printf("Type: %i (stop when full)\n", type);
458 }
459
460 printf("Mode: 0x%02X\n", atoi(strtok(nullptr, ",")));
461 printf("Content: %s\n", strtok(nullptr, ","));
462 printf("Interval: %s seconds\n", strtok(nullptr, ","));
463 printf("Distance: %s\n", strtok(nullptr, ","));
464 printf("Speed: %s\n", strtok(nullptr, ","));
465
466 int status = atoi(strtok(nullptr, ","));
467 if (status == 0) {
468 printf("Status: %i (enabled)\n", status);
469 } else {
470 printf("Status: %i (disabled)\n", status);
471 }
472
473 printf("Number: %s fixes available\n", strtok(nullptr, ","));
474 printf("Percent: %s%% used\n", strtok(nullptr, ","));
475 }
476
477 void
process_pmtk001()478 process_pmtk001()
479 {
480 strtok(line, ",");
481 char* cmd = strtok(nullptr,",");
482 char* flag = strtok(nullptr,",");
483
484 switch (atoi(flag)) {
485 case 0:
486 dbg(1, "Ack: %s %s (Invalid command)\n", cmd, flag);
487 break;
488 case 1:
489 dbg(1, "Ack: %s %s (Unsupported command)\n", cmd, flag);
490 break;
491 case 2:
492 dbg(1, "Ack: %s %s (Action failed)\n", cmd, flag);
493 break;
494 case 3:
495 dbg(1, "Ack: %s %s (Success)\n", cmd, flag);
496 break;
497 default:
498 dbg(1, "Ack: %s %s (Unknown error)\n", cmd, flag);
499 break;
500 }
501 }
502
503 void
process_pmtk705()504 process_pmtk705()
505 {
506 char* token = strtok(line, ",");
507 token = strtok(nullptr,",");
508
509 printf("Firmware: %s\n", token);
510 }
511
512 void
send_command(const char * s,const char * wait_for)513 send_command(const char* s, const char* wait_for)
514 {
515 time_t starttime;
516 time_t currtime;
517 char cmd[100];
518
519 if (read_mode == rm_file) {
520 dbg(1, "Sending device commands ignored when using file input: %s\n", s);
521 return;
522 }
523
524 int checksum = calculate_checksum(&s[1], strlen(s)-1);
525 snprintf(cmd, sizeof(cmd)-1, "%s*%02X\r\n", s, checksum);
526
527 int rc = gbser_print(sfd, cmd);
528 if (rc != gbser_OK) {
529 fatal(MYNAME ": Write error (%d)\n", rc);
530 }
531
532 dbg(1, "Sent command: %s\n", cmd);
533
534 if (wait_for == nullptr) {
535 waiting_for[0] = '\0';
536 return;
537 }
538
539 time(&starttime);
540 cmd[0] = '\0';
541 strncat(cmd, &s[5], 3);
542 waiting_for[0] = '\0';
543 strncat(waiting_for, wait_for, sizeof(waiting_for)-1);
544 dbg(1, "Waiting for: %s\n", waiting_for);
545
546 read_line();
547 while (strlen(waiting_for) > 0) {
548 time(&currtime);
549 if (currtime > starttime + 5) {
550 fatal(MYNAME "Ack not received: %s\n", s);
551 }
552 process_packet();
553 read_line();
554 }
555 }
556
557 int
calculate_checksum(const char * s,int length)558 calculate_checksum(const char* s, int length)
559 {
560 int sum = 0;
561
562 for (int i = 0; i<length; i++) {
563 sum ^= *s++;
564 }
565 return sum;
566 }
567
568 void
dbg(int l,const char * msg,...)569 dbg(int l, const char* msg, ...)
570 {
571 va_list ap;
572 va_start(ap, msg);
573 if (global_opts.debug_level >= l) {
574 vfprintf(stderr,msg, ap);
575 fflush(stderr);
576 }
577 va_end(ap);
578 }
579
580
581