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

wbt-200.cc 26KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133
  1. /*
  2. * Serial download of track data from a Wintec WBT-200.
  3. *
  4. * Copyright (C) 2006 Andy Armstrong
  5. *
  6. * This program is free software; you can redistribute it and/or modify it
  7. * under the terms of the GNU General Public License as published by the
  8. * Free Software Foundation; either version 2 of the License, or (at your
  9. * option) any later version.
  10. *
  11. * This program is distributed in the hope that it will be useful, but
  12. * WITHOUT ANY WARRANTY; without even the implied warranty of
  13. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  14. * General Public License for more details.
  15. *
  16. * You should have received a copy of the GNU General Public License along
  17. * with this program; if not, write to the Free Software Foundation, Inc.,
  18. * 59 Temple Place - Suite 330, Boston, MA 02111 USA
  19. */
  20. #include "defs.h"
  21. #include "gbser.h"
  22. #include "grtcirc.h"
  23. #include <stdio.h>
  24. #include <stdlib.h>
  25. #define MYNAME "WBT-100/200"
  26. #define NL "\x0D\x0A"
  27. #define WBT200BAUD 9600
  28. #define WBT201BAUD 57600
  29. #define WBT201CHUNK 4096
  30. #define TIMEOUT 5000
  31. #define RECLEN_V1 12
  32. #define RECLEN_V2 16
  33. #define RECLEN_WBT201 16
  34. /* tk1 file format stuff */
  35. #define TK1_MAGIC "WintecLogFormat"
  36. #define TK1_DATA_OFFSET 0x0400
  37. #define TK1_END_FLAG 0x04000000ul
  38. /* Used to sanity check data - from
  39. * http://hypertextbook.com/facts/2001/DanaWollman.shtml
  40. * The MAXALT check doesn't need to be enabled unless there's
  41. * a format with larger records than V2.
  42. */
  43. /*#define MAXALT 120000*/
  44. #define _MAX(a, b) ((a) > (b) ? (a) : (b))
  45. #define RECLEN_MAX _MAX(RECLEN_V1, RECLEN_V2)
  46. /* Flags for WBT201 */
  47. enum {
  48. WBT201_TRACK_START = 0x01,
  49. WBT201_WAYPOINT = 0x02,
  50. WBT201_OVER_SPEED = 0x04
  51. };
  52. #define BUFSPEC(b) b, sizeof(b)
  53. /* The formats here must be in ascending record length order so that
  54. * each format identification attempt can read more data from the
  55. * device if necessary. If that proves to be a bad order to try the
  56. * heuristics the format matching code will have to be rejigged.
  57. */
  58. static struct {
  59. size_t reclen;
  60. } fmt_version[] = {
  61. { RECLEN_V1 },
  62. { RECLEN_V2 },
  63. { 0 }
  64. };
  65. /* Number of lines to skip while waiting for an ACK from a command. I've seen
  66. * conversations with up to 30 lines of cruft before the response so 60 isn't
  67. * too crazy.
  68. */
  69. #define RETRIES 60
  70. /*
  71. * For WBT-200 protocol documentation see:
  72. * http://hexten.net/wiki/index.php/WBT-200_Comms_Protocol
  73. */
  74. static void* fd;
  75. static FILE* fl;
  76. static char* port;
  77. static char* erase;
  78. typedef enum {
  79. UNKNOWN, WBT200, WBT201, WSG1000
  80. } wintec_gps_types;
  81. static wintec_gps_types dev_type = UNKNOWN;
  82. struct buf_chunk {
  83. struct buf_chunk* next;
  84. size_t size;
  85. size_t used;
  86. /* data follows in memory */
  87. };
  88. #define buf_CHUNK_DATA(c) \
  89. ((void *) ((struct buf_chunk *) (c) + 1))
  90. #define buf_CHUNK_PTR(c, offset) \
  91. ((void *) ((char *) buf_CHUNK_DATA(c) + (offset)))
  92. struct buf_head {
  93. struct buf_chunk* head;
  94. struct buf_chunk* tail;
  95. size_t alloc;
  96. size_t used;
  97. /* read position */
  98. struct buf_chunk* current;
  99. unsigned long offset;
  100. /* shoehorned in here primarily out of laziness */
  101. unsigned checksum;
  102. };
  103. struct read_state {
  104. route_head* route_head_;
  105. unsigned wpn, tpn;
  106. struct buf_head data;
  107. };
  108. static void db(int l, const char* msg, ...)
  109. {
  110. va_list ap;
  111. va_start(ap, msg);
  112. if (global_opts.debug_level >= l) {
  113. vprintf(msg, ap);
  114. }
  115. va_end(ap);
  116. }
  117. /* Growable buffer support. TODO: Put this in a separate file and
  118. * tidy up its interface.
  119. */
  120. static void buf_init(struct buf_head* h, size_t alloc)
  121. {
  122. h->head = NULL;
  123. h->tail = NULL;
  124. h->alloc = alloc;
  125. h->used = 0;
  126. h->checksum = 0;
  127. h->offset = 0;
  128. }
  129. static void buf_empty(struct buf_head* h)
  130. {
  131. struct buf_chunk* chunk, *next;
  132. for (chunk = h->head; chunk; chunk = next) {
  133. next = chunk->next;
  134. xfree(chunk);
  135. }
  136. h->head = NULL;
  137. h->tail = NULL;
  138. h->used = 0;
  139. h->checksum = 0;
  140. }
  141. static void buf_rewind(struct buf_head* h)
  142. {
  143. h->current = h->head;
  144. h->offset = 0;
  145. }
  146. static size_t buf_read(struct buf_head* h, void* data, size_t len)
  147. {
  148. char* bp = (char*) data;
  149. size_t got = 0;
  150. while (len != 0 && h->current != NULL) {
  151. size_t avail = h->current->used - h->offset;
  152. if (avail > len) {
  153. avail = len;
  154. }
  155. /* Allow NULL buffer pointer to skip bytes */
  156. if (NULL != bp) {
  157. memcpy(bp, buf_CHUNK_PTR(h->current, h->offset), avail);
  158. bp += avail;
  159. }
  160. h->offset += avail;
  161. len -= avail;
  162. got += avail;
  163. if (h->offset == h->current->used) {
  164. h->current = h->current->next;
  165. h->offset = 0;
  166. }
  167. }
  168. return got;
  169. }
  170. static void buf_extend(struct buf_head* h, size_t amt)
  171. {
  172. struct buf_chunk* c;
  173. size_t sz = amt + sizeof(struct buf_chunk);
  174. c = (struct buf_chunk*) xmalloc(sz);
  175. c->next = NULL;
  176. c->size = amt;
  177. c->used = 0;
  178. if (NULL == h->head) {
  179. h->head = c;
  180. } else {
  181. h->tail->next = c;
  182. }
  183. h->tail = c;
  184. }
  185. static void buf_update_checksum(struct buf_head* h, const void* data, size_t len)
  186. {
  187. unsigned char* cp = (unsigned char*) data;
  188. unsigned i;
  189. db(4, "Updating checksum with %p, %lu, before: %02x ",
  190. data, (unsigned long) len, h->checksum);
  191. for (i = 0; i < len; i++) {
  192. h->checksum ^= cp[i];
  193. }
  194. db(4, "after: %02x\n", h->checksum);
  195. }
  196. static void buf_write(struct buf_head* h, const void* data, size_t len)
  197. {
  198. size_t avail;
  199. const char* bp = (const char*) data;
  200. buf_update_checksum(h, data, len);
  201. h->used += len;
  202. if (NULL == h->tail) {
  203. buf_extend(h, h->alloc);
  204. }
  205. for (;;) {
  206. avail = h->tail->size - h->tail->used;
  207. if (avail > len) {
  208. avail = len;
  209. }
  210. memcpy((char*) buf_CHUNK_PTR(h->tail, h->tail->used), bp, avail);
  211. h->tail->used += avail;
  212. bp += avail;
  213. len -= avail;
  214. if (len == 0) {
  215. break;
  216. }
  217. buf_extend(h, h->alloc);
  218. }
  219. }
  220. static void rd_drain()
  221. {
  222. if (gbser_flush(fd)) {
  223. fatal(MYNAME ": Comm error\n");
  224. }
  225. }
  226. static void rd_line(char* buf, int len)
  227. {
  228. int rc;
  229. if (rc = gbser_read_line(fd, buf, len, TIMEOUT, 0x0A, 0x0D), rc != gbser_OK) {
  230. fatal(MYNAME ": Read error (%d)\n", rc);
  231. }
  232. db(3, "Got response: \"%s\"\n", buf);
  233. }
  234. static void wr_cmd(const char* cmd)
  235. {
  236. int rc;
  237. db(3, "Sending: %s\n", cmd);
  238. if (rc = gbser_print(fd, cmd), gbser_OK != rc) {
  239. fatal(MYNAME ": Write error (%d)\n", rc);
  240. }
  241. }
  242. static void wr_cmdl(const char* cmd)
  243. {
  244. wr_cmd(cmd);
  245. wr_cmd(NL);
  246. }
  247. static int expect(const char* str)
  248. {
  249. int state = 0;
  250. int c, i;
  251. int errors = 5; /* allow this many errors */
  252. for (i = 0; i < 5000; i++) {
  253. /* reached end of string */
  254. if (str[state] == '\0') {
  255. return 1;
  256. }
  257. c = gbser_readc_wait(fd, 500);
  258. if (c < 0) {
  259. db(3, "Got error: %d\n", c);
  260. if (--errors <= 0) {
  261. return 0;
  262. }
  263. } else {
  264. db(3, "Got char: %02x '%c'\n", c, isprint(c) ? c : '.');
  265. if (c == str[state]) {
  266. state++; /* carry on */
  267. } else {
  268. state = 0; /* go back to start */
  269. }
  270. }
  271. }
  272. return 0;
  273. }
  274. static int wbt200_try()
  275. {
  276. int rc;
  277. db(1, "Trying WBT100/200\n");
  278. if ((rc = gbser_set_port(fd, WBT200BAUD, 8, 0, 1))) {
  279. db(1, "Set baud rate to %d failed (%d)\n", WBT200BAUD, rc);
  280. return 0;
  281. }
  282. wr_cmdl("$PFST,NORMAL");
  283. return expect("$PFST");
  284. }
  285. static int wbt201_try()
  286. {
  287. int rc;
  288. db(1, "Trying WBT201/G-Rays 2\n");
  289. if ((rc = gbser_set_port(fd, WBT201BAUD, 8, 0, 1))) {
  290. db(1, "Set baud rate to %d failed (%d)\n", WBT201BAUD, rc);
  291. return 0;
  292. }
  293. wr_cmdl("@AL");
  294. return expect("@AL");
  295. }
  296. static int wsg1000_try()
  297. {
  298. int rc;
  299. db(1, "Trying WSG 1000/G-Rays 2\n");
  300. if ((rc = gbser_set_port(fd, WBT201BAUD, 8, 0, 1))) {
  301. db(1, "Set baud rate to %d failed (%d)\n", WBT201BAUD, rc);
  302. return 0;
  303. }
  304. wr_cmdl("@AL,2,3");
  305. return expect("@AL,2,3,OK");
  306. }
  307. static wintec_gps_types guess_device()
  308. {
  309. int i;
  310. db(1, "Guessing device...\n");
  311. for (i = 0; i < 5; i++) {
  312. if (wbt200_try()) {
  313. return WBT200;
  314. }
  315. if (wbt201_try()) {
  316. return WBT201;
  317. }
  318. if (wsg1000_try()) {
  319. return WSG1000;
  320. }
  321. }
  322. return UNKNOWN;
  323. }
  324. static void rd_init(const QString& fname)
  325. {
  326. port = xstrdup(qPrintable(fname));
  327. db(1, "Opening port...\n");
  328. if ((fd = gbser_init(port)) == NULL) {
  329. fatal(MYNAME ": Can't initialise port \"%s\"\n", port);
  330. }
  331. dev_type = guess_device();
  332. if (UNKNOWN == dev_type) {
  333. fatal(MYNAME ": Can't determine device type\n");
  334. }
  335. }
  336. static void rd_deinit(void)
  337. {
  338. db(1, "Closing port...\n");
  339. gbser_deinit(fd);
  340. fd = NULL;
  341. xfree(port);
  342. }
  343. static int rd_buf(void* buf, int len)
  344. {
  345. int rc;
  346. if (rc = gbser_read_wait(fd, buf, len, TIMEOUT), rc < 0) {
  347. fatal(MYNAME ": Read error (%d)\n", rc);
  348. } else if (rc < len) {
  349. db(2, MYNAME ": Read timout, got %i of %i bytes\n", rc, len);
  350. return 0;
  351. }
  352. return 1;
  353. }
  354. static void file_init(const QString& fname)
  355. {
  356. db(1, "Opening file...\n");
  357. if ((fl = fopen(qPrintable(fname), "rb")) == NULL) {
  358. fatal(MYNAME ": Can't open file '%s'\n", qPrintable(fname));
  359. }
  360. }
  361. static void file_deinit(void)
  362. {
  363. db(1, "Closing file...\n");
  364. fclose(fl);
  365. }
  366. static int starts_with(const char* buf, const char* pat)
  367. {
  368. size_t pat_len = strlen(pat);
  369. return (pat_len <= strlen(buf))
  370. ? (memcmp(buf, pat, pat_len) == 0)
  371. : 0;
  372. }
  373. /* Send a command then wait for a line starting with the command string
  374. * to be returned.
  375. */
  376. static int do_cmd(const char* cmd, const char* expect, char* buf, int len)
  377. {
  378. int trycount;
  379. rd_drain();
  380. wr_cmdl(cmd);
  381. db(2, "Cmd: %s\n", cmd);
  382. /* We may need to skip a load of data to start with - the unit streams
  383. * NMEA data all the time so it's highly likely that it'll be in the
  384. * middle of an NMEA sentence when we start listening.
  385. */
  386. for (trycount = 0; trycount < RETRIES; trycount++) {
  387. rd_line(buf, len);
  388. db(3, "Got: %s\n", buf);
  389. if (starts_with(buf, expect)) {
  390. db(2, "Matched: %s\n", buf);
  391. return strlen(expect);
  392. }
  393. db(2, "Skip %d: %s\n", trycount, buf);
  394. }
  395. fatal(MYNAME ": Bad response from unit\n");
  396. return 0; /* keep compiler quiet */
  397. }
  398. /* Issue a command that expects the same string to be echoed
  399. * back as an ACK
  400. */
  401. static int do_simple(const char* cmd, char* buf, int len)
  402. {
  403. return do_cmd(cmd, cmd, buf, len);
  404. }
  405. static char* get_param(const char* cmd, char* buf, int len)
  406. {
  407. int cl = do_simple(cmd, buf, len);
  408. return buf + cl + 1;
  409. }
  410. static int get_param_int(const char* cmd)
  411. {
  412. char buf[80];
  413. return atoi(get_param(cmd, buf, sizeof(buf)));
  414. }
  415. static double get_param_float(const char* cmd)
  416. {
  417. char buf[80];
  418. return atof(get_param(cmd, buf, sizeof(buf)));
  419. }
  420. /* Decompose binary date into discreet fields */
  421. #define _SPLIT_DATE(tim) \
  422. int sec = (((tim) >> 0) & 0x3F); \
  423. int min = (((tim) >> 6) & 0x3F); \
  424. int hour = (((tim) >> 12) & 0x1F); \
  425. int mday = (((tim) >> 17) & 0x1F); \
  426. int mon = (((tim) >> 22) & 0x0F); \
  427. int year = (((tim) >> 26) & 0x3F);
  428. static time_t decode_date(uint32_t tim)
  429. {
  430. _SPLIT_DATE(tim)
  431. struct tm t;
  432. t.tm_sec = sec;
  433. t.tm_min = min;
  434. t.tm_hour = hour;
  435. t.tm_mday = mday;
  436. t.tm_mon = mon - 1;
  437. t.tm_year = year + 100;
  438. return mkgmtime(&t);
  439. }
  440. static int check_date(uint32_t tim)
  441. {
  442. _SPLIT_DATE(tim)
  443. /* Sanity check the date. We don't allow years prior to 2004 because zero in
  444. * those bits will usually indicate that we have an altitude instead of a
  445. * date (i.e. that the data is the new format that uses 16 byte records).
  446. */
  447. return sec < 60 && min < 60 && hour < 24 &&
  448. mday > 0 && mday <= 31 && mon > 0 && mon <= 12 && year >= 4;
  449. }
  450. static Waypoint* make_point(double lat, double lon, double alt, time_t tim, const char* fmt, int index)
  451. {
  452. char wp_name[20];
  453. Waypoint* wpt = new Waypoint;
  454. sprintf(wp_name, fmt, index);
  455. wpt->latitude = lat;
  456. wpt->longitude = lon;
  457. wpt->altitude = alt;
  458. wpt->SetCreationTime(tim);
  459. wpt->shortname = wp_name;
  460. return wpt;
  461. }
  462. static Waypoint* make_waypoint(struct read_state* st, double lat, double lon, double alt, time_t tim)
  463. {
  464. return make_point(lat, lon, alt, tim, "WP%04d", ++st->wpn);
  465. }
  466. static Waypoint* make_trackpoint(struct read_state* st, double lat, double lon, double alt, time_t tim)
  467. {
  468. return make_point(lat, lon, alt, tim, "TP%04d", ++st->tpn);
  469. }
  470. static int wbt200_data_chunk(struct read_state* st, const void* buf, int fmt)
  471. {
  472. uint32_t tim;
  473. double lat, lon, alt;
  474. time_t rtim;
  475. Waypoint* tpt = NULL;
  476. const char* bp = (const char*) buf;
  477. size_t buf_used = fmt_version[fmt].reclen;
  478. tim = le_read32(bp + 0);
  479. lat = (double)((int32_t) le_read32(bp + 4)) / 10000000;
  480. lon = (double)((int32_t) le_read32(bp + 8)) / 10000000;
  481. /* Handle extra fields in longer records here. */
  482. if (buf_used >= 16) {
  483. alt = (double) le_read32(bp + 12) / 10;
  484. } else {
  485. alt = unknown_alt;
  486. }
  487. rtim = decode_date(tim);
  488. if (lat >= 100) {
  489. /* Start new track in the northern hemisphere */
  490. lat -= 100;
  491. st->route_head_ = NULL;
  492. } else if (lat <= -100) {
  493. /* Start new track in the southern hemisphere */
  494. /* This fix courtesy of Anton Frolich */
  495. lat += 100;
  496. st->route_head_ = NULL;
  497. }
  498. tpt = make_trackpoint(st, lat, lon, alt, rtim);
  499. if (NULL == st->route_head_) {
  500. db(1, "New Track\n");
  501. st->route_head_ = route_head_alloc();
  502. track_add_head(st->route_head_);
  503. }
  504. track_add_wpt(st->route_head_, tpt);
  505. return 1;
  506. }
  507. /* Return true iff the data appears valid with the specified record length */
  508. static int is_valid(struct buf_head* h, int fmt)
  509. {
  510. char buf[RECLEN_MAX];
  511. size_t reclen = fmt_version[fmt].reclen;
  512. buf_rewind(h);
  513. db(2, "Checking %lu bytes of data against format %d\n", h->used, fmt);
  514. for (;;) {
  515. size_t got = buf_read(h, buf, reclen);
  516. uint32_t tim;
  517. /* Don't mind odd bytes at the end - we may
  518. * be examining an incomplete dataset.
  519. */
  520. if (got != reclen) {
  521. break;
  522. }
  523. tim = le_read32(buf + 0);
  524. if (!check_date(tim)) {
  525. return 0;
  526. }
  527. if (reclen > 12) {
  528. #ifdef MAXALT
  529. uint32_t alt = le_read32(buf + 12);
  530. if (alt > MAXALT * 10) {
  531. return 0;
  532. }
  533. #endif
  534. }
  535. }
  536. return 1;
  537. }
  538. static void wbt200_process_data(struct read_state* pst, int fmt)
  539. {
  540. char buf[RECLEN_MAX];
  541. size_t reclen = fmt_version[fmt].reclen;
  542. buf_rewind(&pst->data);
  543. db(2, "Processing %lu bytes of data using format %d\n", pst->data.used, fmt);
  544. for (;;) {
  545. size_t got = buf_read(&pst->data, buf, reclen);
  546. if (got != reclen) {
  547. break;
  548. }
  549. wbt200_data_chunk(pst, buf, fmt);
  550. }
  551. }
  552. static void state_init(struct read_state* pst)
  553. {
  554. pst->route_head_ = NULL;
  555. pst->wpn = 0;
  556. pst->tpn = 0;
  557. buf_init(&pst->data, RECLEN_V1 * RECLEN_V2);
  558. }
  559. static void state_empty(struct read_state* pst)
  560. {
  561. buf_empty(&pst->data);
  562. state_init(pst);
  563. }
  564. static int want_bytes(struct buf_head* h, size_t len)
  565. {
  566. char buf[512];
  567. db(3, "Reading %lu bytes from device\n", (unsigned long) len);
  568. while (len > 0) {
  569. size_t want = sizeof(buf);
  570. if (want > len) {
  571. want = len;
  572. }
  573. if (!rd_buf(buf, want)) {
  574. return 0;
  575. }
  576. buf_write(h, buf, want);
  577. len -= want;
  578. }
  579. return 1;
  580. }
  581. static void wbt200_data_read(void)
  582. {
  583. /* Awooga! Awooga! Statically allocated buffer danger!
  584. * Actually, it's OK because rd_line can read arbitrarily
  585. * long lines returning only the first N characters
  586. */
  587. char line_buf[100];
  588. int fmt;
  589. unsigned long count;
  590. struct read_state st;
  591. state_init(&st);
  592. /* We could potentially parse the version string to find out which
  593. * data format to use - but it's not clear how the version string
  594. * will increment in the future - so just now it's more future-
  595. * proof to rely on analysing the data. We need to be able to do
  596. * that with files anyway - because they're not versioned.
  597. */
  598. do_simple("$PFST,FIRMWAREVERSION", BUFSPEC(line_buf));
  599. do_simple("$PFST,NORMAL", BUFSPEC(line_buf));
  600. do_simple("$PFST,READLOGGER", BUFSPEC(line_buf));
  601. /* Now we're into binary mode */
  602. rd_buf(line_buf, 6); /* six byte header */
  603. count = le_read16(line_buf + 2) + 1;
  604. if (count == 0x10000) {
  605. count = 0;
  606. }
  607. db(3, "%lu points available\n", count);
  608. /* Loop through the known formats requesting more data from the
  609. * device each time. When the device contains only a single
  610. * point the first format will get a false positive - so we'll
  611. * lose the altitude data.
  612. */
  613. for (fmt = 0; fmt_version[fmt].reclen != 0; fmt++) {
  614. size_t reclen = fmt_version[fmt].reclen;
  615. size_t want = reclen * count;
  616. if (want < st.data.used) {
  617. fatal(MYNAME ": Internal error: formats not ordered in ascending size order\n");
  618. }
  619. db(3, "Want %lu bytes of data\n", (unsigned long) want);
  620. /* Top up the buffer */
  621. want_bytes(&st.data, want - st.data.used);
  622. /* And see if it's valid */
  623. if (is_valid(&st.data, fmt)) {
  624. break;
  625. }
  626. }
  627. if (fmt_version[fmt].reclen == 0) {
  628. fatal(MYNAME ": Can't autodetect data format\n");
  629. }
  630. wbt200_process_data(&st, fmt);
  631. /* Erase data? */
  632. if (*erase != '0') {
  633. int f;
  634. db(1, "Erasing data\n");
  635. for (f = 27; f <= 31; f++) {
  636. sprintf(line_buf, "$PFST,REMOVEFILE,%d", f);
  637. do_cmd(line_buf, "$PFST,REMOVEFILE", BUFSPEC(line_buf));
  638. }
  639. db(1, "Reclaiming free space\n");
  640. for (f = 0; f <= 3; f++) {
  641. sprintf(line_buf, "$PFST,FFSRECLAIM,%d", f);
  642. do_cmd(line_buf, "$PFST,FFSRECLAIM", BUFSPEC(line_buf));
  643. }
  644. }
  645. do_simple("$PFST,NORMAL", BUFSPEC(line_buf));
  646. state_empty(&st);
  647. }
  648. static int all_null(const void* buf, const int len)
  649. {
  650. const char* bp = (const char*) buf;
  651. int i;
  652. for (i = 0; i < len; i++) {
  653. if (bp[i]) {
  654. return 0;
  655. }
  656. }
  657. return 1;
  658. }
  659. static int wbt201_data_chunk(struct read_state* st, const void* buf)
  660. {
  661. uint32_t tim;
  662. uint16_t flags;
  663. double lat, lon, alt;
  664. time_t rtim;
  665. Waypoint* tpt = NULL;
  666. const char* bp = (const char*) buf;
  667. /* Zero records are skipped */
  668. if (all_null(buf, RECLEN_WBT201)) {
  669. return 1;
  670. }
  671. flags = le_read16(bp + 0);
  672. tim = le_read32(bp + 2);
  673. if (TK1_END_FLAG == tim) {
  674. /* EOF? (TK1 files only as far as I know) */
  675. return 0;
  676. }
  677. lat = (double)((int32_t) le_read32(bp + 6)) / 10000000;
  678. lon = (double)((int32_t) le_read32(bp + 10)) / 10000000;
  679. alt = (double)((int16_t) le_read16(bp + 14));
  680. rtim = decode_date(tim);
  681. if ((flags & WBT201_WAYPOINT) && (global_opts.masked_objective & WPTDATAMASK)) {
  682. Waypoint* wpt = make_waypoint(st, lat, lon, alt, rtim);
  683. waypt_add(wpt);
  684. }
  685. if (global_opts.masked_objective & TRKDATAMASK) {
  686. if (flags & WBT201_TRACK_START) {
  687. st->route_head_ = NULL;
  688. }
  689. tpt = make_trackpoint(st, lat, lon, alt, rtim);
  690. if (NULL == st->route_head_) {
  691. db(1, "New Track\n");
  692. st->route_head_ = route_head_alloc();
  693. track_add_head(st->route_head_);
  694. }
  695. track_add_wpt(st->route_head_, tpt);
  696. }
  697. return 1;
  698. }
  699. static void wbt201_process_chunk(struct read_state* st)
  700. {
  701. char buf[RECLEN_WBT201];
  702. db(2, "Processing %lu bytes of data\n", st->data.used);
  703. while (buf_read(&st->data, buf, sizeof(buf)) == sizeof(buf)
  704. && wbt201_data_chunk(st, buf)) {
  705. /* do nothing */
  706. }
  707. }
  708. static int wbt201_read_chunk(struct read_state* st, unsigned pos, unsigned limit)
  709. {
  710. char cmd_buf[30];
  711. char line_buf[100];
  712. unsigned long cs;
  713. char* lp, *op;
  714. static const char* cs_prefix = "@AL,CS,";
  715. unsigned want = limit - pos;
  716. if (want > WBT201CHUNK) {
  717. want = WBT201CHUNK;
  718. }
  719. db(3, "Reading bytes at %u (0x%x), limit = %u (0x%x), want = %u (0x%x)\n",
  720. pos, pos, limit, limit, want, want);
  721. buf_empty(&st->data);
  722. rd_drain();
  723. sprintf(cmd_buf, "@AL,5,3,%d", pos);
  724. wr_cmdl(cmd_buf);
  725. if (!want_bytes(&st->data, want)) {
  726. return 0;
  727. }
  728. /* checksum */
  729. rd_line(BUFSPEC(line_buf));
  730. if (!starts_with(line_buf, cs_prefix)) {
  731. db(2, "Bad checksum response\n");
  732. return 0;
  733. }
  734. lp = line_buf + strlen(cs_prefix);
  735. cs = strtoul(lp, &op, 16);
  736. if (*lp == ',' || *op != ',') {
  737. db(2, "Badly formed checksum\n");
  738. return 0;
  739. }
  740. if (cs != st->data.checksum) {
  741. db(2, "Checksums don't match. Got %02x, expected %02\n", cs, st->data.checksum);
  742. return 0;
  743. }
  744. /* ack */
  745. /* rd_line(BUFSPEC(line_buf));
  746. return starts_with(line_buf, cmd_buf);
  747. */
  748. if (dev_type == WBT201) {
  749. rd_line(BUFSPEC(line_buf));
  750. return starts_with(line_buf, cmd_buf);
  751. }
  752. return 1;
  753. }
  754. static void wbt201_data_read(void)
  755. {
  756. char line_buf[100];
  757. struct read_state st;
  758. unsigned tries;
  759. const char* tmp;
  760. double ver_hw;
  761. double ver_sw;
  762. double ver_fmt;
  763. unsigned log_addr_start;
  764. unsigned log_addr_end;
  765. unsigned log_area_start;
  766. unsigned log_area_end;
  767. unsigned wantbytes;
  768. unsigned read_pointer;
  769. unsigned read_limit;
  770. /* Read various device information. We don't use much of this yet -
  771. * just log_addr_start and log_addr_end - but it's useful to have it
  772. * here for debug and documentation purposes.
  773. */
  774. tmp = get_param("@AL,7,1", BUFSPEC(line_buf));
  775. db(1, "Reading device \"%s\"\n", tmp);
  776. ver_hw = get_param_float("@AL,8,1");
  777. ver_sw = get_param_float("@AL,8,2");
  778. ver_fmt = get_param_float("@AL,8,3");
  779. db(2, "versions: hw=%f, sw=%f, fmt=%f\n",
  780. ver_hw, ver_sw, ver_fmt);
  781. log_addr_start = get_param_int("@AL,5,1"); /* we read from here... */
  782. log_addr_end = get_param_int("@AL,5,2"); /* ...to here, but ... */
  783. log_area_start = get_param_int("@AL,5,9"); /* ...we need these when ... */
  784. log_area_end = get_param_int("@AL,5,10"); /* ...the gps wrote more then it fits in memory */
  785. db(2, "Log addr=(%d..%d), area=(%d..%d)\n",
  786. log_addr_start, log_addr_end,
  787. log_area_start, log_area_end);
  788. state_init(&st);
  789. tries = 10;
  790. /* If the WBT-201 device logs more then the memory can handle it continues to write at the beginning of the memory,
  791. * thus overwriting the oldest tracks. In this case log_addr_end is smaller then log_addr_start and we need to read
  792. * from log_addr_start to log_area_end and then from log_area_start to log_addr_end.
  793. */
  794. wantbytes = (log_addr_start < log_addr_end) ? log_addr_end - log_addr_start : log_area_end - (log_addr_start - log_addr_end);
  795. read_pointer = log_addr_start;
  796. read_limit = (log_addr_start < log_addr_end) ? log_addr_end : log_area_end;
  797. db(2, "Want %d bytes from device\n", wantbytes);
  798. while (wantbytes > 0) {
  799. db(2, "Read params: Want %d bytes, read_pointer = %d, read_limit = %d\n", wantbytes, read_pointer, read_limit);
  800. if (wbt201_read_chunk(&st, read_pointer, read_limit)) {
  801. buf_rewind(&st.data);
  802. wbt201_process_chunk(&st);
  803. wantbytes -= st.data.used;
  804. read_pointer += st.data.used;
  805. if (read_pointer >= log_area_end) {
  806. read_pointer = log_area_start;
  807. read_limit = log_addr_end;
  808. }
  809. } else {
  810. if (--tries <= 0) {
  811. fatal(MYNAME ": Too many data errors during read\n");
  812. }
  813. }
  814. }
  815. if (*erase != '0') {
  816. /* erase device */
  817. do_simple("@AL,5,6", BUFSPEC(line_buf));
  818. }
  819. state_empty(&st);
  820. do_simple("@AL,2,1", BUFSPEC(line_buf));
  821. }
  822. static void file_read(void)
  823. {
  824. char buf[512];
  825. size_t rc;
  826. struct read_state st;
  827. int fmt;
  828. const char* tk1_magic = TK1_MAGIC;
  829. size_t tk1_magic_len = strlen(tk1_magic) + 1;
  830. state_init(&st);
  831. /* Read the whole file into the buffer */
  832. rc = fread(buf, 1, sizeof(buf), fl);
  833. while (rc != 0) {
  834. buf_write(&st.data, buf, rc);
  835. rc = fread(buf, 1, sizeof(buf), fl);
  836. }
  837. if (!feof(fl)) {
  838. fatal(MYNAME ": Read error\n");
  839. }
  840. /* Although wbt-tk1 and wbt-bin are enumerated as distinct formats
  841. * we handle them both here and autodetect which type we have.
  842. */
  843. /* WBT201 TK1 format? */
  844. buf_rewind(&st.data);
  845. buf_read(&st.data, buf, tk1_magic_len);
  846. if (memcmp(buf, tk1_magic, tk1_magic_len) == 0) {
  847. db(1, "Got TK1 file\n");
  848. buf_rewind(&st.data);
  849. /* Seek */
  850. buf_read(&st.data, NULL, TK1_DATA_OFFSET);
  851. wbt201_process_chunk(&st);
  852. } else {
  853. db(1, "Got bin file\n");
  854. /* Try to guess the data format */
  855. for (fmt = 0; fmt_version[fmt].reclen != 0; fmt++) {
  856. size_t reclen = fmt_version[fmt].reclen;
  857. if ((st.data.used % reclen) == 0 && is_valid(&st.data, fmt)) {
  858. break;
  859. }
  860. }
  861. if (fmt_version[fmt].reclen == 0) {
  862. fatal(MYNAME ": Can't autodetect data format\n");
  863. }
  864. wbt200_process_data(&st, fmt);
  865. }
  866. state_empty(&st);
  867. }
  868. static void data_read(void)
  869. {
  870. switch (dev_type) {
  871. case WBT200:
  872. wbt200_data_read();
  873. break;
  874. case WBT201:
  875. wbt201_data_read();
  876. break;
  877. case WSG1000:
  878. wbt201_data_read();
  879. break;
  880. default:
  881. fatal(MYNAME ": Unknown device type (internal)\n");
  882. break;
  883. }
  884. }
  885. /* wbt */
  886. static arglist_t wbt_sargs[] = {
  887. {
  888. "erase", &erase, "Erase device data after download",
  889. "0", ARGTYPE_BOOL, ARG_NOMINMAX
  890. },
  891. ARG_TERMINATOR
  892. };
  893. ff_vecs_t wbt_svecs = {
  894. ff_type_serial,
  895. { ff_cap_read, ff_cap_read, ff_cap_none },
  896. rd_init,
  897. NULL,
  898. rd_deinit,
  899. NULL,
  900. data_read,
  901. NULL,
  902. NULL,
  903. wbt_sargs,
  904. CET_CHARSET_UTF8, 1 /* master process: don't convert anything | CET-REVIEW */
  905. };
  906. /* used for wbt-bin /and/ wbt-tk1 */
  907. static arglist_t wbt_fargs[] = {
  908. ARG_TERMINATOR
  909. };
  910. ff_vecs_t wbt_fvecs = {
  911. ff_type_file,
  912. { ff_cap_none, ff_cap_read, ff_cap_none },
  913. file_init,
  914. NULL,
  915. file_deinit,
  916. NULL,
  917. file_read,
  918. NULL,
  919. NULL,
  920. wbt_fargs,
  921. CET_CHARSET_UTF8, 1 /* master process: don't convert anything | CET-REVIEW */
  922. };