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