1 /*
2 Download track data from GPS loggers based in MTK chipset.
3
4 Copyright (C) 2007 Per Borgentun, e4borgen(at)yahoo.com
5 With lot of inspiration from wbt-200.c
6 Copyright (C) 2006 Andy Armstrong
7
8 This program is free software; you can redistribute it and/or modify
9 it under the terms of the GNU General Public License as published by
10 the Free Software Foundation; either version 2 of the License, or
11 (at your option) any later version.
12
13 This program is distributed in the hope that it will be useful,
14 but WITHOUT ANY WARRANTY; without even the implied warranty of
15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 GNU General Public License for more details.
17
18 You should have received a copy of the GNU General Public License
19 along with this program; if not, write to the Free Software
20 Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA
21
22 */
23 /*
24 --------------------------------------------------------------
25 This module will download track data from a MTK chipset based GPS logger.
26 It will also convert the raw data.bin file to MTK compatible CSV and
27 gpsbabel output of your choice.
28 It has been tested with Transystem i-Blue 747 but other devices should
29 work as well (Qstarz BT-Q1000, iTrek Z1, ...)
30
31 For more info and tweaks on MTK based loggers
32 <http://www.gpspassion.com/forumsen/topic.asp?TOPIC_ID=81990>
33 <http://www.gpspassion.com/forumsen/topic.asp?TOPIC_ID=81315>
34 For info about the used log format:
35 <http://spreadsheets.google.com/pub?key=pyCLH-0TdNe-5N-5tBokuOA&gid=5>
36
37 Module updated 2008-2009. Now also handles Holux M-241 and
38 Holux GR-245 aka. GPSport-245 devices. These devices have small differences
39 in the log format and use a lower baudrate to transfer the data.
40
41 Example usage::
42 # Read from USB port, output trackpoints & waypoints in GPX format
43 ./gpsbabel -D 2 -t -w -i mtk -f /dev/ttyUSB0 -o gpx -F out.gpx
44
45 # Parse an existing .bin file (data_2007_09_04.bin), output trackpoints as
46 # both CSV file and GPX, discard points without fix
47 ./gpsbabel -D 2 -t -i mtk-bin,csv=data__2007_09_04.csv -f data_2007_09_04.bin -x discard,fixnone -o gpx -F out.gpx
48
49 Tip: Check out the -x height,wgs84tomsl filter to correct the altitude.
50 Todo:
51 o ....
52
53 */
54
55 #include "defs.h"
56 #include "gbser.h"
57 #include "gbfile.h" /* used for csv output */
58 #include <ctype.h>
59 #include <errno.h>
60
61 #define MYNAME "mtk_logger"
62
63 /* MTK packet id's -- currently unused... */
64 enum MTK_NMEA_PACKET {
65 PMTK_TEST = 0,
66 PMTK_ACK = 1,
67 PMTK_SYS_MSG = 10,
68 PMTK_CMD_HOT_START = 101,
69 PMTK_CMD_WARM_START = 102,
70 PMTK_CMD_COLD_START = 103,
71 PMTK_CMD_FULL_COLD_START = 104,
72 PMTK_CMD_LOG = 182, /* Data log commands */
73 PMTK_SET_NMEA_BAUDRATE = 251,
74 PMTK_API_SET_DGPS_MODE = 301,
75 PMTK_API_SET_SBAS_ENABLED = 313,
76 PMTK_API_SET_NMEA_OUTPUT = 314,
77 PMTK_API_SET_PWR_SAV_MODE = 320,
78 PMTK_API_SET_DATUM = 330,
79 PMTK_API_SET_DATUM_ADVANCE = 331,
80 PMTK_API_SET_USER_OPTION = 390,
81 PMTK_API_Q_FIX_CTL = 400,
82 PMTK_API_Q_DGPS_MODE = 401,
83 PMTK_API_Q_SBAS_ENABLED = 413,
84 PMTK_API_Q_NMEA_OUTPUT = 414,
85 PMTK_API_Q_PWR_SAV_MODE = 420,
86 PMTK_API_Q_DATUM = 430,
87 PMTK_API_Q_DATUM_ADVANCE = 431,
88 PMTK_API_GET_USER_OPTION = 490,
89 PMTK_DT_FIX_CTL = 500,
90 PMTK_DT_DGPS_MODE = 501,
91 PMTK_DT_SBAS_ENABLED = 513,
92 PMTK_DT_NMEA_OUTPUT = 514,
93 PMTK_DT_PWR_SAV_MODE = 520,
94 PMTK_DT_DATUM = 530,
95 PMTK_DT_FLASH_USER_OPTION = 590,
96 PMTK_Q_VERSION = 604,
97 PMTK_Q_RELEASE = 605,
98 PMTK_DT_VERSION = 704,
99 PMTK_DT_RELEASE = 705
100 };
101
102 static const unsigned char LOG_RST[16] = {
103 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, 0xaa, /* start marker */
104 0x00, 0x00, 0x00, 0x00, 0x00, /* data */
105 0xbb, 0xbb, 0xbb, 0xbb
106 }; /* end marker */
107
108 static const char* MTK_ACK[] = { /* Flags returned from PMTK001 ack packet */
109 "Invalid packet", "Unsupported packet type",
110 "Valid packet but action failed", "Valid packet, action success"
111 };
112
113 #define MTK_EVT_BITMASK (1<<0x02)
114 #define MTK_EVT_PERIOD (1<<0x03)
115 #define MTK_EVT_DISTANCE (1<<0x04)
116 #define MTK_EVT_SPEED (1<<0x05)
117 #define MTK_EVT_START (1<<0x07)
118 #define MTK_EVT_WAYPT (1<<0x10) /* Holux waypoint follows... */
119
120 /* *************************************** */
121
122 /* Data id, type, sizes used by MTK chipset - don't touch.... */
123 enum {
124 UTC = 0,
125 VALID,
126 LATITUDE,
127 LONGITUDE,
128 HEIGHT,
129 SPEED,
130 HEADING,
131 DSTA,
132 DAGE,
133 PDOP,
134 HDOP,
135 VDOP,
136 NSAT,
137 SID,
138 ELEVATION,
139 AZIMUTH,
140 SNR,
141 RCR,
142 MILLISECOND,
143 DISTANCE,
144 } DATA_TYPES;
145
146 struct log_type {
147 int id;
148 int size;
149 const char* name;
150 } log_type[32] = {
151 { 0, 4, "UTC" },
152 { 1, 2, "VALID" },
153 { 2, 8, "LATITUDE,N/S"},
154 { 3, 8, "LONGITUDE,E/W"},
155 { 4, 4, "HEIGHT" },
156 { 5, 4, "SPEED" },
157 { 6, 4, "HEADING" },
158 { 7, 2, "DSTA" },
159 { 8, 4, "DAGE" },
160 { 9, 2, "PDOP" },
161 { 10, 2, "HDOP"},
162 { 11, 2, "VDOP"},
163 { 12, 2, "NSAT (USED/VIEW)"},
164 { 13, 4, "SID",},
165 { 14, 2, "ELEVATION" },
166 { 15, 2, "AZIMUTH" },
167 { 16, 2, "SNR"},
168 { 17, 2, "RCR"},
169 { 18, 2, "MILLISECOND"},
170 { 19, 8, "DISTANCE" },
171 { 20, 0, NULL},
172 };
173
174 struct sat_info {
175 char id, used;
176 short elevation, azimut, snr;
177 } sat_info;
178
179 struct data_item {
180 time_t timestamp;
181 short valid;
182 double lat;
183 double lon;
184 float height;
185 float speed;
186 float heading;
187 short dsta; // differential station id
188 float dage; // differential data age
189 float pdop, hdop, vdop;
190 char sat_used, sat_view, sat_count;
191 short rcr;
192 unsigned short timestamp_ms;
193 double distance;
194 struct sat_info sat_data[32];
195 } data_item;
196
197 struct mtk_loginfo {
198 unsigned int bitmask;
199 int logLen;
200 int period, distance, speed; /* in 10:ths of sec, m, km/h */
201 int track_event;
202 } mtk_loginfo;
203
204 /* *************************************** */
205
206 /* MTK chip based devices with different baudrate, tweaks, ... */
207 enum MTK_DEVICE_TYPE {
208 MTK_LOGGER,
209 HOLUX_M241,
210 HOLUX_GR245
211 };
212
213 #define TIMEOUT 1500
214 #define MTK_BAUDRATE 115200
215 #define MTK_BAUDRATE_M241 38400
216
217 #define HOLUX245_MASK (1 << 27)
218
219 static void* fd; /* serial fd */
220 static FILE* fl; /* bin.file fd */
221 static char* port; /* serial port name */
222 static char* OPT_erase; /* erase ? command option */
223 static char* OPT_erase_only; /* erase_only ? command option */
224 static char* OPT_log_enable; /* enable ? command option */
225 static char* csv_file; /* csv ? command option */
226 static enum MTK_DEVICE_TYPE mtk_device = MTK_LOGGER;
227
228 struct mtk_loginfo mtk_info;
229
230 const char LIVE_CHAR[4] = {'-', '\\','|','/'};
231 static const char TEMP_DATA_BIN[]= "data.bin";
232 static const char TEMP_DATA_BIN_OLD[]= "data_old.bin";
233
234
235 const char CMD_LOG_DISABLE[]= "$PMTK182,5*20\r\n";
236 const char CMD_LOG_ENABLE[] = "$PMTK182,4*21\r\n";
237 const char CMD_LOG_FORMAT[] = "$PMTK182,2,2*39\r\n";
238 const char CMD_LOG_ERASE[] = "$PMTK182,6,1*3E\r\n";
239 const char CMD_LOG_STATUS[] = "$PMTK182,2,7*3C\r\n";
240
241 static int mtk_log_len(unsigned int bitmask);
242 static void mtk_rd_init(const char* fname);
243 static void file_init(const char* fname);
244 static void file_deinit(void) ;
245 static void holux245_init(void);
246 static void file_read(void);
247 static int mtk_parse_info(const unsigned char* data, int dataLen);
248
249
250 // Arguments for log fetch 'mtk' command..
251
252 static arglist_t mtk_sargs[] = {
253 {
254 "erase", &OPT_erase, "Erase device data after download",
255 "0", ARGTYPE_BOOL, ARG_NOMINMAX
256 },
257 {
258 "erase_only", &OPT_erase_only, "Only erase device data, do not download anything",
259 "0", ARGTYPE_BOOL, ARG_NOMINMAX
260 },
261 {
262 "log_enable", &OPT_log_enable, "Enable logging after download",
263 "0", ARGTYPE_BOOL, ARG_NOMINMAX
264 },
265 {
266 "csv", &csv_file, "MTK compatible CSV output file",
267 NULL, ARGTYPE_STRING, ARG_NOMINMAX
268 },
269 ARG_TERMINATOR
270 };
271
dbg(int l,const char * msg,...)272 static void dbg(int l, const char* msg, ...)
273 {
274 va_list ap;
275 va_start(ap, msg);
276 if (global_opts.debug_level >= l) {
277 vfprintf(stderr,msg, ap);
278 fflush(stderr);
279 }
280 va_end(ap);
281 }
do_send_cmd(const char * cmd,int cmdLen)282 static int do_send_cmd(const char* cmd, int cmdLen)
283 {
284 int rc;
285
286 dbg(6, "Send %s ", cmd);
287 rc = gbser_print(fd, cmd);
288 if (rc != gbser_OK) {
289 fatal(MYNAME ": Write error (%d)\n", rc);
290 }
291
292 return cmdLen;
293 }
294
295
do_cmd(const char * cmd,const char * expect,char ** rslt,time_t timeout_sec)296 static int do_cmd(const char* cmd, const char* expect, char** rslt, time_t timeout_sec)
297 {
298 char line[256];
299 int len, done, loops, cmd_erase;
300 int expect_len;
301 time_t tout;
302
303 if (expect) {
304 expect_len = strlen(expect);
305 } else {
306 expect_len = 0;
307 }
308
309 time(&tout);
310 if (timeout_sec > 0) {
311 tout += timeout_sec;
312 } else {
313 tout += 1;
314 }
315
316 cmd_erase = 0;
317 if (strncmp(cmd, CMD_LOG_ERASE, 12) == 0) {
318 cmd_erase = 1;
319 if (global_opts.verbose_status || global_opts.debug_level > 0) {
320 fprintf(stderr, "Erasing ");
321 }
322 }
323 // dbg(6, "## Send '%s' -- Expect '%s' in %d sec\n", cmd, expect, timeout_sec);
324
325 do_send_cmd(cmd, strlen(cmd)); // success or fatal()...
326
327 done = 0;
328 loops = 0;
329 memset(line, '\0', sizeof(line));
330 do {
331 int rc;
332 rc = gbser_read_line(fd, line, sizeof(line)-1, TIMEOUT, 0x0A, 0x0D);
333 if (rc != gbser_OK) {
334 if (rc == gbser_TIMEOUT && time(NULL) > tout) {
335 dbg(2, "NMEA command '%s' timeout !\n", cmd);
336 return -1;
337 // fatal(MYNAME "do_cmd(): Read error (%d)\n", rc);
338 }
339 len = -1;
340 } else {
341 len = strlen(line);
342 }
343 loops++;
344 dbg(8, "Read %d bytes: '%s'\n", len, line);
345 if (cmd_erase && (global_opts.verbose_status || (global_opts.debug_level > 0 && global_opts.debug_level <= 3))) {
346 // erase cmd progress wheel -- only for debug level 1-3
347 fprintf(stderr,"\b%c", LIVE_CHAR[loops%4]);
348 fflush(stderr);
349 }
350 if (len > 5 && line[0] == '$') {
351 if (expect_len > 0 && strncmp(&line[1], expect, expect_len) == 0) {
352 if (cmd_erase && (global_opts.verbose_status || global_opts.debug_level > 0)) {
353 fprintf(stderr,"\n");
354 }
355 dbg(6, "NMEA command success !\n");
356 if ((len - 4) > expect_len) { // alloc and copy data segment...
357 if (line[len-3] == '*') {
358 line[len-3] = '\0';
359 }
360 // printf("Data segment: #%s#\n", &line[expect_len+1]);
361 if (rslt) {
362 *rslt = (char*) xmalloc(len-3-expect_len+1);
363 strcpy(*rslt, &line[expect_len+1]);
364 }
365 }
366 done = 1;
367 } else if (strncmp(line, "$PMTK", 5) == 0) {
368 /* A quick parser for ACK packets */
369 if (!cmd_erase && strncmp(line, "$PMTK001,", 9) == 0 && line[9] != '\0') {
370 char* pType, *pRslt;
371 int pAck;
372 pType = &line[9];
373 pRslt = strchr(&line[9], ',') + 1;
374 if (memcmp(&cmd[5], pType, 3) == 0 && pRslt != NULL && *pRslt != '\0') {
375 pAck = *pRslt - '0';
376 if (pAck != 3 && pAck >= 0 && pAck < 4) { // Erase will return '2'
377 dbg(1, "NMEA command '%s' failed - %s\n", cmd, MTK_ACK[pAck]);
378 return -1;
379 }
380 }
381
382 }
383 dbg(6, "RECV: '%s'\n", line);
384 }
385
386 }
387 if (!done && time(NULL) > tout) {
388 dbg(1, "NMEA command '%s' timeout !\n", cmd);
389 return -1;
390 }
391 } while (len != 0 && loops > 0 && !done);
392 return done?0:1;
393 }
394
395 /*******************************************************************************
396 * %%% global callbacks called by gpsbabel main process %%% *
397 *******************************************************************************/
mtk_rd_init_m241(const char * fname)398 static void mtk_rd_init_m241(const char* fname)
399 {
400 mtk_device = HOLUX_M241;
401 mtk_rd_init(fname);
402 }
403
mtk_rd_init(const char * fname)404 static void mtk_rd_init(const char* fname)
405 {
406 int rc;
407 char* model;
408
409 port = xstrdup(fname);
410
411 errno = 0;
412 dbg(1, "Opening port %s...\n", fname);
413 if ((fd = gbser_init(port)) == NULL) {
414 fatal(MYNAME ": Can't initialise port \"%s\" (%s)\n", port, strerror(errno));
415 }
416
417 // verify that we have a MTK based logger...
418 dbg(1, "Verifying MTK based device...\n");
419
420 switch (mtk_device) {
421 case HOLUX_M241:
422 case HOLUX_GR245:
423 log_type[LATITUDE].size = log_type[LONGITUDE].size = 4;
424 log_type[HEIGHT].size = 3;
425 rc = gbser_set_port(fd, MTK_BAUDRATE_M241, 8, 0, 1);
426 break;
427 case MTK_LOGGER:
428 default:
429 rc = gbser_set_port(fd, MTK_BAUDRATE, 8, 0, 1);
430 break;
431 }
432 if (rc) {
433 dbg(1, "Set baud rate to %d failed (%d)\n", MTK_BAUDRATE, rc);
434 fatal(MYNAME ": Failed to set baudrate !\n");
435 }
436
437 rc = do_cmd("$PMTK605*31\r\n", "PMTK705,", &model, 10);
438 if (rc != 0) {
439 fatal(MYNAME ": This is not a MTK based GPS ! (or is device turned off ?)\n");
440 }
441
442 // say hello to GR245 to make it display "USB PROCESSING"
443 if (strstr(model, "GR-245")) {
444 holux245_init(); // remember we have a GR245 for mtk_rd_deinit()
445 rc |= do_cmd("$PHLX810*35\r\n", "PHLX852,", NULL, 10);
446 rc |= do_cmd("$PHLX826*30\r\n", "PHLX859*38", NULL, 10);
447 if (rc != 0) {
448 dbg(2, "Greeting not successfull.\n");
449 }
450 }
451 xfree(model);
452 }
453
mtk_rd_deinit(void)454 static void mtk_rd_deinit(void)
455 {
456 if (mtk_device == HOLUX_GR245) {
457 int rc = do_cmd("$PHLX827*31\r\n", "PHLX860*32", NULL, 10);
458 if (rc != 0) {
459 dbg(2, "Goodbye not successfull.\n");
460 }
461 }
462
463 dbg(3, "Closing port...\n");
464 gbser_deinit(fd);
465 fd = NULL;
466 xfree(port);
467 }
468
mtk_erase(void)469 static int mtk_erase(void)
470 {
471 int log_status, log_mask, err;
472 char* lstatus = NULL;
473
474 log_status = 0;
475 // check log status - is logging disabled ?
476 do_cmd(CMD_LOG_STATUS, "PMTK182,3,7,", &lstatus, 2);
477 if (lstatus) {
478 log_status = atoi(lstatus);
479 dbg(3, "LOG Status '%s'\n", lstatus);
480 xfree(lstatus);
481 lstatus = NULL;
482 }
483
484 do_cmd(CMD_LOG_FORMAT, "PMTK182,3,2,", &lstatus, 2);
485 if (lstatus) {
486 log_mask = strtoul(lstatus, NULL, 16);
487 dbg(3, "LOG Mask '%s' - 0x%.8x \n", lstatus, log_mask);
488 xfree(lstatus);
489 lstatus = NULL;
490 }
491
492 dbg(1, "Start flash erase..\n");
493 do_cmd(CMD_LOG_DISABLE, "PMTK001,182,5,3", NULL, 1);
494 gb_sleep(10*1000);
495
496 // Erase log....
497 do_cmd(CMD_LOG_ERASE, "PMTK001,182,6", NULL, 30);
498 gb_sleep(100*1000);
499
500 if ((log_status & 2)) { // auto-log were enabled before..re-enable log.
501 err = do_cmd(CMD_LOG_ENABLE, "PMTK001,182,4,3", NULL, 2);
502 dbg(3, "re-enable log %s\n", err==0?"Success":"Fail");
503 }
504 return 0;
505 }
506
mtk_read(void)507 static void mtk_read(void)
508 {
509 char cmd[256];
510 char* line = NULL;
511 unsigned char crc, *data = NULL;
512 int cmdLen, j, bsize, i, len, ff_len, null_len, rc, init_scan, retry_cnt, log_enabled;
513 unsigned int line_size, data_size, data_addr, addr, addr_max;
514 unsigned long dsize, dpos = 0;
515 FILE* dout;
516 char* fusage = NULL;
517
518
519 if (*OPT_erase_only != '0') {
520 mtk_erase();
521 return;
522 }
523
524 log_enabled = 0;
525 init_scan = 0;
526 dout = fopen(TEMP_DATA_BIN, "r+b");
527 if (dout == NULL) {
528 dout = fopen(TEMP_DATA_BIN, "wb");
529 if (dout == NULL) {
530 fatal(MYNAME ": Can't create temporary file %s", TEMP_DATA_BIN);
531 return;
532 }
533 }
534 fseek(dout, 0L,SEEK_END);
535 dsize = ftell(dout);
536 if (dsize > 1024) {
537 dbg(1, "Temp %s file exists. with size %d\n", TEMP_DATA_BIN, dsize);
538 dpos = 0;
539 init_scan = 1;
540 }
541 dbg(1, "Download %s -> %s\n", port, TEMP_DATA_BIN);
542
543 // check log status - is logging disabled ?
544 do_cmd(CMD_LOG_STATUS, "PMTK182,3,7,", &fusage, 2);
545 if (fusage) {
546 log_enabled = (atoi(fusage) & 2)?1:0;
547 dbg(3, "LOG Status '%s' -- log %s \n", fusage, log_enabled?"enabled":"disabled");
548 xfree(fusage);
549 fusage = NULL;
550 }
551
552 gb_sleep(10*1000);
553 if (1 || log_enabled) {
554 i = do_cmd(CMD_LOG_DISABLE, "PMTK001,182,5,3", NULL, 2);
555 dbg(3, " ---- LOG DISABLE ---- %s\n", i==0?"Success":"Fail");
556 }
557 gb_sleep(100*1000);
558
559 addr_max = 0;
560 // get flash usage, current log address..cmd only works if log disabled.
561 do_cmd("$PMTK182,2,8*33\r\n", "PMTK182,3,8,", &fusage, 2);
562 if (fusage) {
563 addr_max = strtoul(fusage, NULL, 16);
564 if (addr_max > 0) {
565 addr_max = addr_max - addr_max%65536 + 65535;
566 }
567 xfree(fusage);
568 }
569
570 if (addr_max == 0) { // get flash usage failed...
571 addr_max = 0x200000; // 16Mbit/2Mbyte/32x64kByte block. -- fixme Q1000-ng has 32Mbit
572 init_scan = 1;
573 }
574 dbg(1, "Download %dkB from device\n", (addr_max+1) >> 10);
575
576 if (dsize > addr_max) {
577 dbg(1, "Temp %s file (%ld) is larger than data size %d. Data erased since last download !\n", TEMP_DATA_BIN, dsize, addr_max);
578 fclose(dout);
579 dsize = 0;
580 init_scan = 0;
581 rename(TEMP_DATA_BIN, TEMP_DATA_BIN_OLD);
582 dout = fopen(TEMP_DATA_BIN, "wb");
583 if (dout == NULL) {
584 fatal(MYNAME ": Can't create temporary file %s", TEMP_DATA_BIN);
585 return;
586 }
587 }
588
589 bsize = 0x0400;
590 addr = 0x0000;
591
592 line_size = 2*bsize + 32; // logdata as nmea/hex.
593 data_size = bsize + 32;
594 if ((line = (char*) xmalloc(line_size)) == NULL) {
595 fatal(MYNAME ": Can't allocate %u bytes for NMEA buffer\n", line_size);
596 }
597 if ((data = (unsigned char*) xmalloc(data_size)) == NULL) {
598 fatal(MYNAME ": Can't allocate %u bytes for data buffer\n", data_size);
599 }
600 memset(line, '\0', line_size);
601 memset(data, '\0', data_size);
602 retry_cnt = 0;
603
604 while (init_scan || addr < addr_max) {
605 // generate - read address NMEA command, add crc.
606 crc = 0;
607 cmdLen = snprintf(cmd, sizeof(cmd), "$PMTK182,7,%.8x,%.8x", addr, bsize);
608 for (i=1; i<cmdLen; i++) {
609 crc ^= cmd[i];
610 }
611
612 cmdLen += snprintf(&cmd[cmdLen], sizeof(cmd)-cmdLen, "*%.2X\r\n", crc);
613 mtk_retry:
614 do_send_cmd(cmd, cmdLen);
615
616 memset(line, '\0', line_size);
617 do {
618 rc = gbser_read_line(fd, line, line_size-1, TIMEOUT, 0x0A, 0x0D);
619 if (rc != gbser_OK) {
620 if (rc == gbser_TIMEOUT && retry_cnt < 3) {
621 dbg(2, "\nRetry %d at 0x%.8x\n", retry_cnt, addr);
622 retry_cnt++;
623 goto mtk_retry;
624 } // else
625 fatal(MYNAME "mtk_read(): Read error (%d)\n", rc);
626 }
627 len = strlen(line);
628 dbg(8, "Read %d bytes: '%s'\n", len, line);
629
630 if (len > 0) {
631 line[len] = '\0';
632 if (strncmp(line, "$PMTK182,8", 10) == 0) { // $PMTK182,8,00005000,FFFFFFF
633 retry_cnt = 0;
634 data_addr = strtoul(&line[11], NULL, 16);
635 fseek(dout, data_addr, SEEK_SET);
636 i = 20;
637 j = 0;
638 ff_len = 0; // number of 0xff bytes.
639 null_len = 0; // number of 0x00 bytes.
640 while (i < (len-3)) {
641 data[j] = (isdigit(line[i])?(line[i]-'0'):(line[i]-'A'+0xA))*0x10 +
642 (isdigit(line[i+1])?(line[i+1]-'0'):(line[i+1]-'A'+0xA));
643 if (data[j] == 0xff) {
644 ff_len++;
645 }
646 if (data[j] == 0x00) {
647 null_len++;
648 }
649 i += 2;
650 j++;
651 }
652 if (init_scan) {
653 if (dsize > 0 && addr < dsize) {
654 fseek(dout, addr, SEEK_SET);
655 if (fread(line, 1, bsize, dout) == bsize && memcmp(line, data, bsize) == 0) {
656 dpos = addr;
657 dbg(2, "%s same at %d\n", TEMP_DATA_BIN, addr);
658 } else {
659 dbg(2, "%s differs at %d\n", TEMP_DATA_BIN, addr);
660 }
661 }
662 if (ff_len == j) { // data in sector - we've found max sector..
663 addr_max = addr;
664 dbg(1, "Initial scan done - Download %dkB from device\n", (addr_max+1) >> 10);
665 }
666 len = 0;
667 } else {
668 if (null_len == j) { // 0x00 block - bad block....
669 fprintf(stderr, "FIXME -- read bad block at 0x%.6x - retry ? skip ?\n%s\n", data_addr, line);
670 }
671 if (fwrite(data, 1, j, dout) != j) {
672 fatal(MYNAME ": Failed to write temp. binary file\n");
673 }
674 if (ff_len == j) { // 0xff block - read complete...
675 len = ff_len;
676 addr_max = addr;
677 break;
678 } else {
679 len = 0;
680 }
681 }
682 } else if (strncmp(line, "$PMTK001,182,7,", 15) == 0) { // Command ACK
683 if (line[15] != '3') {
684 // fixme - we should timeout here when no log data has been received...
685 dbg(2, "\nLog req. failed (%c)\n", line[15]);
686 gb_sleep(10*1000);
687 retry_cnt++;
688 goto mtk_retry;
689 }
690 }
691 }
692 } while (len != 0);
693 if (init_scan) {
694 addr += 0x10000;
695 if (addr >= addr_max) { // initial scan complete...
696 init_scan = 0;
697 addr = dpos;
698 }
699 } else {
700 addr += bsize;
701 if (global_opts.verbose_status || (global_opts.debug_level >= 2 && global_opts.debug_level < 5)) {
702 int perc;
703 perc = 100 - 100*(addr_max-addr)/addr_max;
704 if (addr >= addr_max) {
705 perc = 100;
706 }
707 fprintf(stderr, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\bReading 0x%.6x %3d %%", addr, perc);
708 }
709 }
710 }
711 if (dout != NULL) {
712 fclose(dout);
713 }
714 if (global_opts.verbose_status || (global_opts.debug_level >= 2 && global_opts.debug_level < 5)) {
715 fprintf(stderr,"\n");
716 }
717
718 // Fixme - Order or. Enable - parse - erase ??
719 if (log_enabled || *OPT_log_enable=='1') {
720 i = do_cmd(CMD_LOG_ENABLE, "PMTK001,182,4,3", NULL, 2);
721 dbg(3, " ---- LOG ENABLE ----%s\n", i==0?"Success":"Fail");
722 } else {
723 dbg(1, "Note !!! -- Logging is DISABLED !\n");
724 }
725 if (line != NULL) {
726 xfree(line);
727 }
728 if (data != NULL) {
729 xfree(data);
730 }
731
732 file_init(TEMP_DATA_BIN);
733 file_read();
734 file_deinit();
735
736 /* fixme -- we're assuming all went well - erase flash.... */
737 if (*OPT_erase != '0') {
738 mtk_erase();
739 }
740
741 return;
742 }
743
744
745 static route_head* trk_head = NULL;
add_trackpoint(int idx,unsigned long bmask,struct data_item * itm)746 static int add_trackpoint(int idx, unsigned long bmask, struct data_item* itm)
747 {
748 char wp_name[20];
749 waypoint* trk = waypt_new();
750
751 if (global_opts.masked_objective& TRKDATAMASK && (trk_head == NULL || (mtk_info.track_event & MTK_EVT_START))) {
752 char spds[50];
753 trk_head = route_head_alloc();
754 xasprintf(&trk_head->rte_name, "track-%d", 1+track_count());
755
756 spds[0] = '\0';
757 if (mtk_info.speed > 0) {
758 sprintf(spds, " when moving above %.0f km/h", mtk_info.speed/10.);
759 }
760 xasprintf(&trk_head->rte_desc, "Log every %.0f sec, %.0f m%s"
761 , mtk_info.period/10., mtk_info.distance/10., spds);
762 track_add_head(trk_head);
763 }
764
765 if (bmask & (1<<LATITUDE) && bmask & (1<<LONGITUDE)) {
766 trk->latitude = itm->lat;
767 trk->longitude = itm->lon;
768 } else {
769 return -1; // GPX requires lat/lon...
770 }
771
772 if (bmask & (1<<HEIGHT)) {
773 trk->altitude = itm->height;
774 }
775 trk->creation_time = itm->timestamp; // in UTC..
776 if (bmask & (1<<MILLISECOND)) {
777 trk->microseconds = MILLI_TO_MICRO(itm->timestamp_ms);
778 }
779
780 if (bmask & (1<<PDOP)) {
781 trk->pdop = itm->pdop;
782 }
783 if (bmask & (1<<HDOP)) {
784 trk->hdop = itm->hdop;
785 }
786 if (bmask & (1<<VDOP)) {
787 trk->vdop = itm->vdop;
788 }
789
790 if (bmask & (1<<HEADING)) {
791 WAYPT_SET(trk, course, itm->heading);
792 }
793 if (bmask & (1<<SPEED)) {
794 WAYPT_SET(trk, speed, KPH_TO_MPS(itm->speed));
795 }
796 if (bmask & (1<<VALID)) {
797 switch (itm->valid) {
798 case 0x0040:
799 trk->fix = fix_unknown;
800 break; /* Estimated mode */
801 case 0x0001:
802 trk->fix = fix_none;
803 break; /* "No Fix" */
804 case 0x0002:
805 trk->fix = fix_3d;
806 break; /* "SPS" - 2d/3d ?*/
807 case 0x0004:
808 trk->fix = fix_dgps;
809 break;
810 case 0x0008:
811 trk->fix = fix_pps;
812 break; /* Military GPS */
813
814 case 0x0010: /* "RTK" */
815 case 0x0020: /* "FRTK" */
816 case 0x0080: /* "Manual input mode" */
817 case 0x0100: /* "Simulator";*/
818 default:
819 trk->fix = fix_unknown;
820 break;
821 }
822 /* This is a flagrantly bogus position; don't queue it.
823 * The 747 does log some "real" positions with fix_none, though,
824 * so keep those.
825 */
826 if ((trk->fix == fix_unknown || trk->fix == fix_none) &&
827 trk->latitude - 90.0 < .000001 && trk->longitude < 0.000001) {
828 waypt_free(trk);
829 return -1;
830 }
831 }
832 if (bmask & (1<<NSAT)) {
833 trk->sat = itm->sat_used;
834 }
835
836 // RCR is a bitmask of possibly several log reasons..
837 // Holux devics use a Event prefix for each waypt.
838 if (global_opts.masked_objective & WPTDATAMASK
839 && ((bmask & (1<<RCR) && itm->rcr & 0x0008)
840 || (mtk_info.track_event & MTK_EVT_WAYPT)
841 )
842 ) {
843 /* Button press -- create waypoint, start count at 1 */
844 waypoint* w = waypt_dupe(trk);
845
846 sprintf(wp_name, "WP%06d", waypt_count()+1);
847 w->shortname = xstrdup(wp_name);
848 waypt_add(w);
849 }
850 // In theory we would not add the waypoint to the list of
851 // trackpoints. But as the MTK logger restart the
852 // log session from the button press we would loose a
853 // trackpoint unless we include/duplicate it.
854
855 if (global_opts.masked_objective & TRKDATAMASK) {
856 sprintf(wp_name, "TP%06d", idx);
857 trk->shortname = xstrdup(wp_name);
858
859 track_add_wpt(trk_head, trk);
860 }
861 return 0;
862 }
863
864
865 /********************** MTK Logger -- CSV output *************************/
866 static gbfile* cd;
mtk_csv_init(char * csv_fname,unsigned long bitmask)867 static void mtk_csv_init(char* csv_fname, unsigned long bitmask)
868 {
869 int i;
870 FILE* cf;
871
872 dbg(1, "Opening csv output file %s...\n", csv_fname);
873
874 // can't use gbfopen here - it will fatal() if file doesn't exist
875 if ((cf = fopen(csv_fname, "r")) != NULL) {
876 fclose(cf);
877 warning(MYNAME ": CSV file %s already exist ! Cowardly refusing to overwrite.\n", csv_fname);
878 return;
879 }
880
881 if ((cd = gbfopen(csv_fname, "w", MYNAME)) == NULL) {
882 fatal(MYNAME ": Can't open csv file '%s'\n", csv_fname);
883 }
884
885 /* Add the header line */
886 gbfprintf(cd, "INDEX,%s%s", ((1<<RCR) & bitmask)?"RCR,":"",
887 ((1<<UTC) & bitmask)?"DATE,TIME,":"");
888 for (i=0; i<32; i++) {
889 if ((1<<i) & bitmask) {
890 switch (i) {
891 case RCR:
892 case UTC:
893 case MILLISECOND:
894 break;
895 case SID:
896 gbfprintf(cd, "SAT INFO (SID");
897 break;
898 case ELEVATION:
899 gbfprintf(cd, "-ELE");
900 break;
901 case AZIMUTH:
902 gbfprintf(cd, "-AZI");
903 break;
904 case SNR:
905 gbfprintf(cd, "-SNR");
906 break;
907 default:
908 gbfprintf(cd, "%s,", log_type[i].name);
909 break;
910 }
911 }
912 if (i == SNR && (1<<SID) & bitmask) {
913 gbfprintf(cd, "),");
914 }
915 }
916 gbfprintf(cd, "\n");
917 }
918
mtk_csv_deinit(void)919 static void mtk_csv_deinit(void)
920 {
921 if (cd != NULL) {
922 gbfclose(cd);
923 cd = NULL;
924 }
925 }
926
927 /* Output a single data line in MTK application compatible format - i.e ignore any locale settings... */
csv_line(gbfile * csvFile,int idx,unsigned long bmask,struct data_item * itm)928 static int csv_line(gbfile* csvFile, int idx, unsigned long bmask, struct data_item* itm)
929 {
930 struct tm* ts_tm;
931 char ts_str[30];
932 const char* fix_str = "";
933
934 ts_tm = gmtime(&(itm->timestamp));
935 strftime(ts_str, sizeof(ts_str)-1, "%Y/%m/%d,%H:%M:%S", ts_tm);
936
937 if (bmask & (1<<VALID)) {
938 switch (itm->valid) {
939 case 0x0001:
940 fix_str = "No fix";
941 break;
942 case 0x0002:
943 fix_str = "SPS";
944 break;
945 case 0x0004:
946 fix_str = "DGPS";
947 break;
948 case 0x0008:
949 fix_str = "PPS";
950 break; /* Military GPS */
951 case 0x0010:
952 fix_str = "RTK";
953 break; /* RealTime Kinematic */
954 case 0x0020:
955 fix_str = "FRTK";
956 break;
957 case 0x0040:
958 fix_str = "Estimated mode";
959 break;
960 case 0x0080:
961 fix_str = "Manual input mode";
962 break;
963 case 0x0100:
964 fix_str = "Simulator";
965 break;
966 default:
967 fix_str = "???";
968 break;
969 }
970 }
971 gbfprintf(csvFile, "%d,", idx);
972
973 // RCR is a bitmask of possibly several log reasons..
974 if (bmask & (1<<RCR))
975 gbfprintf(csvFile, "%s%s%s%s,"
976 , itm->rcr&0x0001?"T":"",itm->rcr&0x0002?"S":""
977 , itm->rcr&0x0004?"D":"",itm->rcr&0x0008?"B":"");
978
979 if (bmask & (1<<UTC)) {
980 gbfprintf(csvFile, "%s.%.3d,", ts_str, (bmask & (1<<MILLISECOND))?itm->timestamp_ms:0);
981 }
982
983 if (bmask & (1<<VALID)) {
984 gbfprintf(csvFile, "%s,", fix_str);
985 }
986
987 if (bmask & (1<<LATITUDE | 1<<LONGITUDE))
988 gbfprintf(csvFile, "%.6f,%c,%.6f,%c,", fabs(itm->lat), itm->lat>0?'N':'S',
989 fabs(itm->lon), itm->lon>0?'E':'W');
990
991 if (bmask & (1<<HEIGHT)) {
992 gbfprintf(csvFile, "%.3f m,", itm->height);
993 }
994
995 if (bmask & (1<<SPEED)) {
996 gbfprintf(csvFile, "%.3f km/h,", itm->speed);
997 }
998
999 if (bmask & (1<<HEADING)) {
1000 gbfprintf(csvFile, "%.6f,", itm->heading);
1001 }
1002
1003 if (bmask & (1<<DSTA)) {
1004 gbfprintf(csvFile, "%d,", itm->dsta);
1005 }
1006 if (bmask & (1<<DAGE)) {
1007 gbfprintf(csvFile, "%.6f,", itm->dage);
1008 }
1009
1010 if (bmask & (1<<PDOP)) {
1011 gbfprintf(csvFile, "%.2f,", itm->pdop);
1012 }
1013 if (bmask & (1<<HDOP)) {
1014 gbfprintf(csvFile, "%.2f,", itm->hdop); // note bug in MTK appl. 1.02 is output as 1.2 !
1015 }
1016 if (bmask & (1<<VDOP)) {
1017 gbfprintf(csvFile, "%.2f,", itm->vdop);
1018 }
1019 if (bmask & (1<<NSAT)) {
1020 gbfprintf(csvFile, "%d(%d),", itm->sat_used, itm->sat_view);
1021 }
1022
1023 if (bmask & (1<<SID)) {
1024 int l, slen, do_sc = 0;
1025 char sstr[40];
1026 for (l=0; l<itm->sat_count; l++) {
1027 slen = 0;
1028 slen += sprintf(&sstr[slen], "%s%.2d"
1029 , itm->sat_data[l].used?"#":""
1030 , itm->sat_data[l].id);
1031 if (bmask & (1<<ELEVATION)) {
1032 slen += sprintf(&sstr[slen], "-%.2d", itm->sat_data[l].elevation);
1033 }
1034 if (bmask & (1<<AZIMUTH)) {
1035 slen += sprintf(&sstr[slen], "-%.2d", itm->sat_data[l].azimut);
1036 }
1037 if (bmask & (1<<SNR)) {
1038 slen += sprintf(&sstr[slen], "-%.2d", itm->sat_data[l].snr);
1039 }
1040
1041 gbfprintf(csvFile, "%s%s" , do_sc?";":"", sstr);
1042 do_sc = 1;
1043 }
1044 gbfprintf(csvFile, ",");
1045 }
1046
1047 if (bmask & (1<<DISTANCE)) {
1048 gbfprintf(csvFile, "%10.2f m,", itm->distance);
1049 }
1050
1051 gbfprintf(csvFile, "\n");
1052 return 0;
1053 }
1054
1055
1056 /********************* MTK Logger -- Parse functions *********************/
mtk_parse(unsigned char * data,int dataLen,unsigned int bmask)1057 int mtk_parse(unsigned char* data, int dataLen, unsigned int bmask)
1058 {
1059 static int count = 0;
1060 int i, k, sat_id, hspd;
1061 unsigned char crc, hbuf[4];
1062 struct data_item itm;
1063
1064 dbg(5,"Entering mtk_parse, count = %i, dataLen = %i\n", count, dataLen);
1065 if (global_opts.debug_level > 5) {
1066 int j;
1067 fprintf(stderr,"# Data block:");
1068 for (j=0; j<dataLen; j++) {
1069 fprintf(stderr,"%.2x ", data[j]);
1070 }
1071 fprintf(stderr,"\n");
1072 fflush(stderr);
1073 }
1074
1075 memset(&itm, 0, sizeof(itm));
1076 i = 0;
1077 crc = 0;
1078 for (k=0; k<32; k++) {
1079 switch (((1<<k) & bmask)) {
1080 case 1<<UTC:
1081 itm.timestamp = le_read32(data + i);
1082 break;
1083 case 1<<VALID:
1084 itm.valid = le_read16(data + i);
1085 break;
1086 case 1<<LATITUDE:
1087 if (log_type[LATITUDE].size == 4) {
1088 itm.lat = endian_read_float(data + i, 1 /* le */); // M-241
1089 } else {
1090 itm.lat = endian_read_double(data + i, 1 /* le */);
1091 }
1092 break;
1093 case 1<<LONGITUDE:
1094 if (log_type[LONGITUDE].size == 4) {
1095 itm.lon = endian_read_float(data + i, 1 /* le */); // M-241
1096 } else {
1097 itm.lon = endian_read_double(data + i, 1 /* le */);
1098 }
1099 break;
1100 case 1<<HEIGHT:
1101 switch (mtk_device) {
1102 case HOLUX_GR245: // Stupid Holux GPsport 245 - log speed as centimeters/sec. (in height position !)
1103 hspd = data[i] + data[i+1]*0x100 + data[i+2]*0x10000 + data[i+3]*0x1000000;
1104 itm.speed = MPS_TO_KPH(hspd)/100.; // convert to km/h..
1105 break;
1106 case HOLUX_M241:
1107 hbuf[0] = 0x0;
1108 hbuf[1] = *(data + i);
1109 hbuf[2] = *(data + i + 1);
1110 hbuf[3] = *(data + i + 2);
1111 itm.height = endian_read_float(hbuf, 1 /* le */);
1112 break;
1113 case MTK_LOGGER:
1114 default:
1115 itm.height = endian_read_float(data + i, 1 /* le */);
1116 break;
1117 }
1118 break;
1119 case 1<<SPEED:
1120 if (mtk_device == HOLUX_GR245) { // Stupid Holux GPsport 245 - log height in speed position...
1121 hbuf[0] = 0x0;
1122 hbuf[1] = *(data + i);
1123 hbuf[2] = *(data + i + 1);
1124 hbuf[3] = *(data + i + 2);
1125 itm.height = endian_read_float(hbuf, 1 /* le */);
1126 } else {
1127 itm.speed = endian_read_float(data + i, 1 /* le */);
1128 }
1129 break;
1130 case 1<<HEADING:
1131 itm.heading = endian_read_float(data + i, 1 /* le */);
1132 break;
1133 case 1<<DSTA:
1134 itm.dsta = le_read16(data + i);
1135 break;
1136 case 1<<DAGE: // ?? fixme - is this a float ?
1137 itm.dage = endian_read_float(data + i, 1 /* le */);
1138 break;
1139 case 1<<PDOP:
1140 itm.pdop = le_read16(data + i) / 100.;
1141 break;
1142 case 1<<HDOP:
1143 itm.hdop = le_read16(data + i) / 100.;
1144 break;
1145 case 1<<VDOP:
1146 itm.vdop = le_read16(data + i) / 100.;
1147 break;
1148 case 1<<NSAT:
1149 itm.sat_view = data[i];
1150 itm.sat_used = data[i+1];
1151 break;
1152 case 1<<SID: {
1153 int sat_count, sat_idx, sid_size, l;
1154
1155 sat_count = le_read16(data + i + 2);
1156 if (sat_count > 32) {
1157 sat_count = 32; // this can't happen ? or...
1158 }
1159
1160 itm.sat_count = sat_count;
1161 sid_size = log_type[SID].size;
1162 if (sat_count > 0) { // handle 'Zero satellites in view issue'
1163 if (bmask & (1<<ELEVATION)) {
1164 sid_size += log_type[ELEVATION].size;
1165 }
1166 if (bmask & (1<<AZIMUTH)) {
1167 sid_size += log_type[AZIMUTH].size;
1168 }
1169 if (bmask & (1<<SNR)) {
1170 sid_size += log_type[SNR].size;
1171 }
1172 }
1173 l = 0;
1174 sat_idx = 0;
1175 do {
1176 sat_id = data[i];
1177 itm.sat_data[sat_idx].id = sat_id;
1178 itm.sat_data[sat_idx].used = data[i + 1];
1179 // get_word(&data[i+2], &smask); // assume - nr of satellites...
1180
1181 if (sat_count > 0) {
1182 if (bmask & (1<<ELEVATION)) {
1183 itm.sat_data[sat_idx].elevation = le_read16(data + i + 4);
1184 }
1185 if (bmask & (1<<AZIMUTH)) {
1186 itm.sat_data[sat_idx].azimut = le_read16(data + i + 6);
1187 }
1188 if (bmask & (1<<SNR)) {
1189 itm.sat_data[sat_idx].snr = le_read16(data + i + 8);
1190 }
1191 }
1192 sat_idx++;
1193 // duplicated checksum and length calculations...for simplicity...
1194 for (l = 0; l < sid_size; l++) {
1195 crc ^= data[i + l];
1196 }
1197 i += sid_size;
1198 sat_count--;
1199 } while (sat_count > 0);
1200 }
1201 continue; // dont do any more checksum calc..
1202 break;
1203 case 1<<ELEVATION:
1204 case 1<<AZIMUTH:
1205 case 1<<SNR:
1206 // handled in SID
1207 continue; // avoid checksum calc
1208 break;
1209 case 1<<RCR:
1210 itm.rcr = le_read16(data + i);
1211 break;
1212 case 1<<MILLISECOND:
1213 itm.timestamp_ms = le_read16(data + i);
1214 break;
1215 case 1<<DISTANCE:
1216 itm.distance = endian_read_double(data + i, 1 /* le */);
1217 break;
1218 default:
1219 // if ( ((1<<k) & bmask) )
1220 // printf("Unknown ID %d: %.2x %.2x %.2x %.2x\n", k, data[i], data[i+1], data[i+2], data[i+3]);
1221 break;
1222 } /* End: switch (bmap) */
1223
1224 /* update item checksum and length */
1225 if (((1<<k) & bmask)) {
1226 int j;
1227 for (j=0; j<log_type[k].size; j++) {
1228 crc ^= data[i+j];
1229 }
1230 i += log_type[k].size;
1231 }
1232 } /* for (bmap,...) */
1233
1234 if (mtk_device == MTK_LOGGER) { // Holux skips '*' checksum separator
1235 if (data[i] == '*') {
1236 i++; // skip '*' separator
1237 } else {
1238 dbg(1,"Missing '*' !\n");
1239 if (data[i] == 0xff) { // in some case star-crc hasn't been written on power off.
1240 dbg(1, "Bad data point @0x%.6x - skip %d bytes\n", (fl!=NULL)?ftell(fl):-1, i+2);
1241 return i+2; // include '*' and crc
1242 }
1243 }
1244 }
1245 if (memcmp(&data[0], &LOG_RST[0], 6) == 0
1246 && memcmp(&data[12], &LOG_RST[12], 4) == 0) {
1247 mtk_parse_info(data, dataLen);
1248 dbg(1," Missed Log restart ?? skipping 16 bytes\n");
1249 return 16;
1250 }
1251
1252 if (data[i] != crc) {
1253 dbg(0,"%2d: Bad CRC %.2x != %.2x (pos 0x%.6x)\n", count, data[i], crc, (fl!=NULL)?ftell(fl):-1);
1254 }
1255 i++; // crc
1256 count++;
1257
1258 if (cd != NULL) {
1259 csv_line(cd, count, bmask, &itm);
1260 }
1261
1262 add_trackpoint(count, bmask, &itm);
1263
1264 mtk_info.track_event = 0;
1265 return i;
1266 }
1267
1268 /*
1269 Description: Parse an info block
1270 Globals: mtk_info - bitmask/period/speed/... may be affected if updated.
1271 */
mtk_parse_info(const unsigned char * data,int dataLen)1272 static int mtk_parse_info(const unsigned char* data, int dataLen)
1273 {
1274 unsigned short cmd;
1275 unsigned int bm;
1276
1277 if (dataLen >= 16
1278 && memcmp(&data[0], &LOG_RST[0], 6) == 0
1279 && memcmp(&data[12], &LOG_RST[12], 4) == 0) {
1280
1281 cmd = le_read16(data + 8);
1282 switch (data[7]) {
1283 case 0x02:
1284 bm = le_read32(data + 8);
1285 dbg(1, "# Log bitmask is: %.8x\n", bm);
1286 if (mtk_device != MTK_LOGGER) {
1287 bm &= 0x7fffffffU;
1288 }
1289 if (mtk_device == HOLUX_GR245) {
1290 bm &= ~HOLUX245_MASK;
1291 }
1292 if (mtk_info.bitmask != bm) {
1293 dbg(1," ########## Bitmask Change %.8x -> %.8x ###########\n", mtk_info.bitmask, bm);
1294 mtk_info.track_event |= MTK_EVT_BITMASK;
1295 }
1296 mtk_info.bitmask = bm;
1297 mtk_info.logLen = mtk_log_len(mtk_info.bitmask);
1298 break;
1299 case 0x03:
1300 dbg(1, "# Log period change %.0f sec\n", cmd/10.);
1301 mtk_info.track_event |= MTK_EVT_PERIOD;
1302 mtk_info.period = cmd;
1303 break;
1304 case 0x04:
1305 dbg(1, "# Log distance change %.1f m\n", cmd/10.);
1306 mtk_info.track_event |= MTK_EVT_DISTANCE;
1307 mtk_info.distance = cmd;
1308 break;
1309 case 0x05:
1310 dbg(1, "# Log speed change %.1f km/h\n", cmd/10.);
1311 mtk_info.track_event |= MTK_EVT_SPEED;
1312 mtk_info.speed = cmd;
1313 break;
1314 case 0x06:
1315 dbg(1, "# Log policy change 0x%.4x\n", cmd);
1316 if (cmd == 0x01) {
1317 dbg(1, "# Log policy change to OVERWRITE\n");
1318 }
1319 if (cmd == 0x02) {
1320 dbg(1, "# Log policy change to STOP\n");
1321 }
1322 break;
1323 case 0x07:
1324 if (cmd == 0x0106) {
1325 dbg(5, "# GPS Logger# Turned On\n"); // Fixme - start new trk
1326 mtk_info.track_event |= MTK_EVT_START;
1327 }
1328 if (cmd == 0x0104) {
1329 dbg(5, "# GPS Logger# Log disabled\n");
1330 }
1331 break;
1332 default:
1333 dbg(1, "## Unknown INFO 0x%.2x\n", data[7]);
1334 break;
1335 }
1336 } else {
1337 if (global_opts.debug_level > 0) {
1338 fprintf(stderr,"#!! Invalid INFO block !! %d bytes\n >> ", dataLen);
1339 for (bm=0; bm<16; bm++) {
1340 fprintf(stderr, "%.2x ", data[bm]);
1341 }
1342 fprintf(stderr,"\n");
1343 }
1344 return 0;
1345 }
1346 return 16;
1347 }
1348
mtk_log_len(unsigned int bitmask)1349 static int mtk_log_len(unsigned int bitmask)
1350 {
1351 int i, len;
1352
1353 /* calculate the length of a binary log item. */
1354 switch (mtk_device) {
1355 case HOLUX_M241:
1356 case HOLUX_GR245:
1357 len = 1; // add crc
1358 break;
1359 case MTK_LOGGER:
1360 default:
1361 len = 2; // add '*' + crc
1362 break;
1363 }
1364 for (i=0; i<32; i++) {
1365 if ((1<<i) & bitmask) {
1366 if (i > DISTANCE && global_opts.debug_level > 0) {
1367 warning(MYNAME ": Unknown size/meaning of bit %d\n", i);
1368 }
1369 if ((i == SID || i == ELEVATION || i == AZIMUTH || i == SNR) && (1<<SID) & bitmask) {
1370 len += log_type[i].size*32; // worst case, max sat. count..
1371 } else {
1372 len += log_type[i].size;
1373 }
1374 }
1375 }
1376 dbg(3, "Log item size %d bytes\n", len);
1377 return len;
1378 }
1379
1380 /********************** File-in interface ********************************/
1381
file_init_m241(const char * fname)1382 static void file_init_m241(const char* fname)
1383 {
1384 mtk_device = HOLUX_M241;
1385 file_init(fname);
1386 }
1387
file_init(const char * fname)1388 static void file_init(const char* fname)
1389 {
1390 dbg(4, "Opening file %s...\n", fname);
1391 if (fl = fopen(fname, "rb"), NULL == fl) {
1392 fatal(MYNAME ": Can't open file '%s'\n", fname);
1393 }
1394 switch (mtk_device) {
1395 case HOLUX_M241:
1396 case HOLUX_GR245:
1397 log_type[LATITUDE].size = log_type[LONGITUDE].size = 4;
1398 log_type[HEIGHT].size = 3;
1399 break;
1400 default:
1401 break;
1402 }
1403 }
1404
file_deinit(void)1405 static void file_deinit(void)
1406 {
1407 dbg(4, "Closing file...\n");
1408 fclose(fl);
1409 }
1410
holux245_init(void)1411 static void holux245_init(void)
1412 {
1413 mtk_device = HOLUX_GR245;
1414
1415 // stupid workaround for a broken Holux-245 device....
1416 // Height & speed have changed position in bitmask and data on Holux 245 Argh !!!
1417 log_type[HEIGHT].id = SPEED;
1418 log_type[HEIGHT].size = 4; // speed size - unit: cm/sec
1419 log_type[SPEED].id = HEIGHT;
1420 log_type[SPEED].size = 3; // height size..
1421 }
1422
is_holux_string(const unsigned char * data,int dataLen)1423 static int is_holux_string(const unsigned char* data, int dataLen)
1424 {
1425 if (mtk_device != MTK_LOGGER &&
1426 dataLen >= 5 &&
1427 data[0] == (0xff & 'H') &&
1428 data[1] == (0xff & 'O') &&
1429 data[2] == (0xff & 'L') &&
1430 data[3] == (0xff & 'U') &&
1431 data[4] == (0xff & 'X')) {
1432 return 1;
1433 }
1434 return 0;
1435 }
1436
file_read(void)1437 static void file_read(void)
1438 {
1439 long fsize, pos;
1440 int i, j, k, bLen;
1441 unsigned char buf[512];
1442
1443 memset(buf, '\0', sizeof(buf));
1444
1445 /* Get size of file to parse */
1446 fseek(fl, 0L, SEEK_END);
1447 fsize = ftell(fl);
1448 if (fsize <= 0) {
1449 fatal(MYNAME ": File has size %ld\n", fsize);
1450 }
1451
1452 fseek(fl, 0L, SEEK_SET);
1453
1454 /* Header: 20 bytes
1455 47 05 | 7f 1e 0e 00 | 04 01 | 32 00 00 00 e8 03 00 00 00 00 00 00
1456
1457 u16: Log count 'this 64kByte block' - ffff if not complete.
1458 u32: Bitmask for logging. (default mask)
1459 u16; ?? ?? Overwrite/Stop policy
1460 u32: log period, sec*10
1461 u32: log distance , meters*10
1462 u32: log speed , km/h*10
1463 */
1464
1465 bLen = 0;
1466 j = 0;
1467 pos = 0;
1468
1469 /* get default bitmask, log period/speed/distance */
1470 bLen = fread(buf, 1, 20, fl);
1471 if (bLen == 20) {
1472 unsigned int mask, log_period, log_distance, log_speed, log_policy;
1473 log_policy = le_read16(buf + 6);
1474
1475 if (!(log_policy == 0x0104 || log_policy == 0x0106) && fsize > 0x10000) {
1476 dbg(1, "Invalid initial log policy 0x%.4x - check next block\n", log_policy);
1477 fseek(fl, 0x10000, SEEK_SET);
1478 bLen = fread(buf, 1, 20, fl);
1479 log_policy = le_read16(buf + 6);
1480 }
1481 mask = le_read32(buf + 2);
1482 if (mtk_device != MTK_LOGGER) { // clear Holux-specific 'low precision' bit
1483 mask &= 0x7fffffffU;
1484 }
1485 log_period = le_read32(buf + 8);
1486 log_distance = le_read32(buf + 12);
1487 log_speed = le_read32(buf + 16);
1488
1489 dbg(1, "Default Bitmask %.8x, Log every %.0f sec, %.0f m, %.0f km/h\n",
1490 mask, log_period/10., log_distance/10., log_speed/10.);
1491 mtk_info.bitmask = mask;
1492 dbg(3, "Using initial bitmask %.8x for parsing the .bin file\n", mtk_info.bitmask);
1493
1494 mtk_info.period = log_period;
1495 mtk_info.distance = log_distance;
1496 mtk_info.speed = log_speed;
1497 }
1498 mtk_info.track_event = 0;
1499
1500 pos = 0x200; // skip header...first data position
1501 fseek(fl, pos, SEEK_SET);
1502
1503 /* read initial info blocks -- if any */
1504 do {
1505 bLen = fread(buf, 1, 16, fl);
1506 j = 0;
1507 if (buf[0] == 0xaa) { // pre-validate to avoid error...
1508 j = mtk_parse_info(buf, bLen);
1509 pos += j;
1510 } else if (is_holux_string(buf, bLen)) {
1511 pos += j;
1512 // Note -- Holux245 will have <SP><SP><SP><SP> here...handled below..
1513 }
1514 } while (j == 16);
1515 j = bLen;
1516 pos += j;
1517
1518 mtk_info.logLen = mtk_log_len(mtk_info.bitmask);
1519 dbg(3, "Log item size %d bytes\n", mtk_info.logLen);
1520 if (csv_file && *csv_file) {
1521 mtk_csv_init(csv_file, mtk_info.bitmask);
1522 }
1523
1524 while (pos < fsize && (bLen = fread(&buf[j], 1, sizeof(buf)-j, fl)) > 0) {
1525 bLen += j;
1526 i = 0;
1527 while ((bLen - i) >= mtk_info.logLen) {
1528 k = 0;
1529 if ((bLen - i) >= 16 && memcmp(&buf[i], &LOG_RST[0], 6) == 0
1530 && memcmp(&buf[i+12], &LOG_RST[12], 4) == 0) {
1531 mtk_parse_info(&buf[i], (bLen-i));
1532 k = 16;
1533 } else if (is_holux_string(&buf[i], (bLen - i))) {
1534 if (memcmp(&buf[i+10], "WAYPNT", 6) == 0) {
1535 mtk_info.track_event |= MTK_EVT_WAYPT;
1536 }
1537
1538 k = 16;
1539 // m241 - HOLUXGR241LOGGER or HOLUXGR241WAYPNT or HOLUXGR241LOGGER<SP><SP><SP><SP>
1540 // gr245 - HOLUXGR245LOGGER<SP><SP><SP><SP> or HOLUXGR245WAYPNT<SP><SP><SP><SP>
1541 if (memcmp(&buf[i], "HOLUXGR245", 10) == 0) {
1542 dbg(2, "Detected Holux GR245 !\n");
1543 holux245_init();
1544 }
1545
1546 // Tobias Verbree reports that an M-12ee is like a 245.
1547 if (memcmp(&buf[i], "HOLUXM1200", 10) == 0) {
1548 dbg(2, "Detected Holux HOLUXM1200 !\n");
1549 holux245_init();
1550 }
1551
1552 // skip the 4 spaces that may occur on every device
1553 if (memcmp(&buf[i+16], " ", 4) == 0) { // Assume loglen >= 20...
1554 k += 4;
1555 }
1556 } else if (buf[i] == 0xff && buf[i+1] == 0xff && buf[i+2] == 0xff && buf[i+3] == 0xff
1557 /* && ((pos + 2*mtk_info.logLen) & 0xffff) < mtk_info.logLen */) {
1558 /* End of 64k block segment -- realign to next data area */
1559
1560 k = ((pos+mtk_info.logLen+1024)/0x10000) *0x10000 + 0x200;
1561 i = sizeof(buf);
1562 if (k <= pos) {
1563 k += 0x10000;
1564 }
1565 dbg(3, "Jump %ld -> %d / 0x%.6x (fsize %ld) --- \n", pos, k, k, fsize);
1566 if (k > fsize) {
1567 dbg(3, "File parse complete !\n");
1568 pos = k;
1569 break;
1570 } else {
1571 fseek(fl, k, SEEK_SET);
1572 }
1573 pos = k;
1574 continue;
1575 } else {
1576 k = mtk_parse(&buf[i], mtk_info.logLen, mtk_info.bitmask);
1577 }
1578
1579 i += k;
1580 pos += k;
1581 }
1582 memmove(buf, &buf[i], sizeof(buf)-i);
1583 j = sizeof(buf)-i;
1584 }
1585 mtk_csv_deinit();
1586
1587 }
1588
1589
1590 /**************************************************************************/
1591 // GPS logger will only handle tracks - neither waypoints or tracks...
1592
1593 ff_vecs_t mtk_vecs = {
1594 ff_type_serial,
1595 {
1596 ff_cap_none /* waypoints */,
1597 ff_cap_read /* tracks */,
1598 ff_cap_none /* routes */
1599 },
1600 mtk_rd_init,
1601 NULL,
1602 mtk_rd_deinit,
1603 NULL,
1604 mtk_read,
1605 NULL,
1606 NULL,
1607 mtk_sargs,
1608 CET_CHARSET_ASCII, 0 /* ascii is the expected character set */
1609 /* not fixed, can be changed through command line parameter */
1610 };
1611
1612 ff_vecs_t mtk_m241_vecs = {
1613 ff_type_serial,
1614 {
1615 ff_cap_none /* waypoints */,
1616 ff_cap_read /* tracks */,
1617 ff_cap_none /* routes */
1618 },
1619 mtk_rd_init_m241,
1620 NULL,
1621 mtk_rd_deinit,
1622 NULL,
1623 mtk_read,
1624 NULL,
1625 NULL,
1626 mtk_sargs,
1627 CET_CHARSET_ASCII, 0 /* ascii is the expected character set */
1628 /* not fixed, can be changed through command line parameter */
1629 };
1630
1631 /* used for mtk-bin */
1632
1633 static arglist_t mtk_fargs[] = {
1634 {
1635 "csv", &csv_file, "MTK compatible CSV output file",
1636 NULL, ARGTYPE_STRING, ARG_NOMINMAX
1637 },
1638 ARG_TERMINATOR
1639 };
1640
1641 ff_vecs_t mtk_fvecs = {
1642 ff_type_file,
1643 { ff_cap_read, ff_cap_read, ff_cap_none },
1644 file_init,
1645 NULL,
1646 file_deinit,
1647 NULL,
1648 file_read,
1649 NULL,
1650 NULL,
1651 mtk_fargs,
1652 CET_CHARSET_UTF8, 1 /* master process: don't convert anything | CET-REVIEW */
1653 };
1654
1655 ff_vecs_t mtk_m241_fvecs = {
1656 ff_type_file,
1657 { ff_cap_read, ff_cap_read, ff_cap_none },
1658 file_init_m241,
1659 NULL,
1660 file_deinit,
1661 NULL,
1662 file_read,
1663 NULL,
1664 NULL,
1665 mtk_fargs,
1666 CET_CHARSET_UTF8, 1 /* master process: don't convert anything | CET-REVIEW */
1667 };
1668 /* End file: mtk_logger.c */
1669 /**************************************************************************/
1670