You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

615 lines
15KB

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