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.

skytraq.cc 49KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658
  1. /*
  2. Serial download of track data from GPS loggers with Skytraq chipset.
  3. Copyright (C) 2008-2012 Mathias Adam, m.adam (at) adamis.de
  4. 2008 J.C Haessig, jean-christophe.haessig (at) dianosis.org
  5. 2009-09-06 | Josef Reisinger | Added "set target location", i.e. -i skytrag,targetlocation=<lat>:<lng>
  6. 2010-10-23 | Josef Reisinger | Added read/write for miniHomer POI
  7. This program is free software; you can redistribute it and/or modify
  8. it under the terms of the GNU General Public License as published by
  9. the Free Software Foundation; either version 2 of the License, or
  10. (at your option) any later version.
  11. This program is distributed in the hope that it will be useful,
  12. but WITHOUT ANY WARRANTY; without even the implied warranty of
  13. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  14. GNU General Public License for more details.
  15. You should have received a copy of the GNU General Public License
  16. along with this program; if not, write to the Free Software
  17. Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA
  18. */
  19. #include "defs.h"
  20. #include "gbser.h"
  21. #include <math.h>
  22. #include <stdio.h>
  23. #include <stdlib.h>
  24. #define MYNAME "skytraq"
  25. #define TIMEOUT 5000
  26. #define SECTOR_SIZE 4096
  27. #define FULL_ITEM_LEN 18
  28. #define COMPACT_ITEM_LEN 8
  29. #define MULTI_HZ_ITEM_LEN 20
  30. /* Maximum number of chars to skip while waiting for a reply: */
  31. #define RETRIES 250
  32. /* Maximum number of messages to read while expecting a specific message or ACK/NACK: */
  33. #define MSG_RETRIES 3
  34. /* Abort when reading a specific sector fails this many times: */
  35. #define SECTOR_RETRIES 3
  36. #define res_OK 0
  37. #define res_ERROR -1
  38. #define res_NACK -2
  39. #define res_PROTOCOL_ERR -3
  40. #define res_NOTFOUND -4
  41. #define MIN(X,Y) ((X) < (Y) ? (X) : (Y))
  42. #define MAX(X,Y) ((X) > (Y) ? (X) : (Y))
  43. static void* serial_handle = 0; /* IO file descriptor */
  44. static int skytraq_baud = 0; /* detected baud rate */
  45. static gbfile* file_handle = 0; /* file descriptor (used by skytraq-bin format) */
  46. static char* opt_erase = 0; /* erase after read? (0/1) */
  47. static char* opt_initbaud = 0; /* baud rate used to init device */
  48. static char* opt_dlbaud = 0; /* baud rate used for downloading tracks */
  49. static char* opt_read_at_once = 0; /* number of sectors to read at once (Venus6 only) */
  50. static char* opt_first_sector = 0; /* first sector to be read from the device (default: 0) */
  51. static char* opt_last_sector = 0; /* last sector to be read from the device (default: smart read everything) */
  52. static char* opt_dump_file = 0; /* dump raw data to this file (optional) */
  53. static char* opt_no_output = 0; /* disable output? (0/1) */
  54. static char* opt_set_location = 0; /* set if the "targetlocation" options was used */
  55. static char* opt_configure_logging = 0;
  56. static
  57. arglist_t skytraq_args[] = {
  58. {
  59. "erase", &opt_erase, "Erase device data after download",
  60. "0", ARGTYPE_BOOL, ARG_NOMINMAX
  61. },
  62. {
  63. "targetlocation", &opt_set_location, "Set location finder target location as lat,lng",
  64. NULL, ARGTYPE_STRING, "", ""
  65. },
  66. {
  67. "configlog", &opt_configure_logging, "Configure logging parameter as tmin:tmax:dmin:dmax",
  68. NULL, ARGTYPE_STRING, "", ""
  69. },
  70. {
  71. "baud", &opt_dlbaud, "Baud rate used for download",
  72. "230400", ARGTYPE_INT, "0", "230400"
  73. },
  74. {
  75. "initbaud", &opt_initbaud, "Baud rate used to init device (0=autodetect)",
  76. "0", ARGTYPE_INT, "4800", "230400"
  77. },
  78. {
  79. "read-at-once", &opt_read_at_once, "Number of sectors to read at once (0=use single sector mode)",
  80. "255", ARGTYPE_INT, "0", "255"
  81. },
  82. {
  83. "first-sector", &opt_first_sector, "First sector to be read from the device",
  84. "0", ARGTYPE_INT, "0", "65535"
  85. },
  86. {
  87. "last-sector", &opt_last_sector, "Last sector to be read from the device (-1: smart read everything)",
  88. "-1", ARGTYPE_INT, "-1", "65535"
  89. },
  90. {
  91. "dump-file", &opt_dump_file, "Dump raw data to this file",
  92. NULL, ARGTYPE_OUTFILE, ARG_NOMINMAX
  93. },
  94. {
  95. "no-output", &opt_no_output, "Disable output (useful with erase)",
  96. "0", ARGTYPE_BOOL, ARG_NOMINMAX
  97. },
  98. ARG_TERMINATOR
  99. };
  100. static
  101. arglist_t skytraq_fargs[] = {
  102. {
  103. "first-sector", &opt_first_sector, "First sector to be read from the file",
  104. "0", ARGTYPE_INT, "0", "65535"
  105. },
  106. {
  107. "last-sector", &opt_last_sector, "Last sector to be read from the file (-1: read till empty sector)",
  108. "-1", ARGTYPE_INT, "-1", "65535"
  109. },
  110. ARG_TERMINATOR
  111. };
  112. static void
  113. db(int l, const char* msg, ...)
  114. {
  115. va_list ap;
  116. va_start(ap, msg);
  117. if (global_opts.debug_level >= l) {
  118. vprintf(msg, ap);
  119. }
  120. va_end(ap);
  121. }
  122. static void
  123. rd_drain(void)
  124. {
  125. if (gbser_flush(serial_handle)) {
  126. db(1, MYNAME ": rd_drain(): Comm error\n");
  127. }
  128. }
  129. static int
  130. rd_char(int* errors)
  131. {
  132. int c;
  133. while (*errors > 0) {
  134. c = gbser_readc_wait(serial_handle, TIMEOUT);
  135. if (c < 0) {
  136. db(1, MYNAME ": rd_char(): Got error: %d\n", c);
  137. (*errors)--;
  138. } else {
  139. db(4, "rd_char(): Got char: %02x '%c'\n", c, isprint(c) ? c : '.');
  140. return c;
  141. }
  142. }
  143. fatal(MYNAME ": Too many read errors on serial port\n");
  144. return -1;
  145. }
  146. static int
  147. rd_buf(const uint8_t* buf, int len)
  148. {
  149. int rc, timeout, i;
  150. char dump[16*3+16+2];
  151. /* Allow TIMEOUT plus the time needed to actually receive the data bytes:
  152. * baudrate/10 bytes per second (8 data bits, start and stop bit)
  153. * TODO: use dlbaud if selected.
  154. */
  155. timeout = TIMEOUT + len;//*1000/(skytraq_baud/10);
  156. /*TODO: timeout gets <0 e.g. when len~=250000 --> 32bit signed int is too small.
  157. if (skytraq_baud > 0) timeout = TIMEOUT + (long long int)len*1000*10/(long long int)skytraq_baud;
  158. printf("len=%i skytraq_baud=%i timeout=%i\n", len, skytraq_baud, timeout);*/
  159. rc = gbser_read_wait(serial_handle, (void*)buf, len, timeout);
  160. if (rc < 0) {
  161. db(1, MYNAME ": rd_buf(): Read error (%d)\n", rc);
  162. return res_ERROR;
  163. } else if (rc < len) {
  164. db(1, MYNAME ": rd_buf(): Read timout\n");
  165. return res_ERROR;
  166. }
  167. if (global_opts.debug_level >= 4) {
  168. db(4, "rd_buf(): dump follows:\n");
  169. dump[sizeof(dump)-1] = 0;
  170. for (i = 0; i < (len+15)/16*16; i++) { // count to next 16-byte boundary
  171. if (i < len) {
  172. snprintf(&dump[(i%16)*3], 4, "%02x ", buf[i]);
  173. snprintf(&dump[3*16+1+(i%16)], 2, "%c", isprint(buf[i]) ? buf[i] : '.');
  174. } else {
  175. memset(&dump[(i%16)*3], ' ', 3);
  176. dump[3*16+1+(i%16)] = ' ';
  177. }
  178. if ((i+1)%16 == 0) {
  179. dump[16*3] = ' '; // gets overwritten with 0 by snprintf
  180. db(4, "%s\n", dump);
  181. }
  182. }
  183. }
  184. return res_OK;
  185. }
  186. static unsigned int
  187. rd_word(void)
  188. {
  189. int errors = 5; /* allow this many errors */
  190. uint8_t buffer[2];
  191. buffer[0] = rd_char(&errors);
  192. buffer[1] = rd_char(&errors);
  193. /* if (rd_buf(buffer, 2) != res_OK) {
  194. db(1, MYNAME ": rd_word(): Read error\n");
  195. return res_ERROR;
  196. }*/
  197. return (buffer[0] << 8) | buffer[1];
  198. }
  199. static void
  200. wr_char(int c)
  201. {
  202. int rc;
  203. db(4, "Sending: %02x '%c'\n", (unsigned)c, isprint(c) ? c : '.');
  204. if (rc = gbser_writec(serial_handle, c), gbser_OK != rc) {
  205. fatal(MYNAME ": Write error (%d)\n", rc);
  206. }
  207. }
  208. static void
  209. wr_buf(const unsigned char* str, int len)
  210. {
  211. int i;
  212. for (i = 0; i < len; i++) {
  213. wr_char(str[i]);
  214. }
  215. }
  216. /*******************************************************************************
  217. * %%% SkyTraq protocol implementation %%% *
  218. *******************************************************************************/
  219. uint8_t NL[2] = { 0x0D, 0x0A };
  220. uint8_t MSG_START[2] = { 0xA0, 0xA1 };
  221. uint8_t SECTOR_READ_END[13] = { 'E','N','D', 0, 'C','H','E','C','K','S','U','M','=' };
  222. static int
  223. skytraq_calc_checksum(const unsigned char* buf, int len)
  224. {
  225. int i, cs = 0;
  226. for (i = 0; i < len; i++) {
  227. cs ^= buf[i];
  228. }
  229. return cs;
  230. }
  231. static int
  232. skytraq_rd_msg(const void* payload, unsigned int len)
  233. {
  234. int errors = 5; /* allow this many errors */
  235. unsigned int c, i, state;
  236. signed int rcv_len;
  237. unsigned int calc_cs, rcv_cs;
  238. for (i = 0, state = 0; i < RETRIES && state < sizeof(MSG_START); i++) {
  239. c = rd_char(&errors);
  240. if (c == MSG_START[state]) {
  241. state++;
  242. } else if (c == MSG_START[0]) {
  243. state = 1;
  244. } else {
  245. state = 0;
  246. }
  247. }
  248. if (state < sizeof(MSG_START)) {
  249. db(1, MYNAME ": Didn't get message start tag\n");
  250. return res_ERROR;
  251. }
  252. if ((rcv_len = rd_word()) < len) {
  253. if (rcv_len >= 0) { /* negative values indicate receive errors */
  254. db(1, MYNAME ": Received message too short (got %i bytes, expected %i)\n",
  255. rcv_len, len);
  256. return res_PROTOCOL_ERR;
  257. }
  258. return res_ERROR;
  259. }
  260. db(2, "Receiving message with %i bytes of payload (expected >=%i)\n", rcv_len, len);
  261. rd_buf((const unsigned char*) payload, MIN(rcv_len, len));
  262. calc_cs = skytraq_calc_checksum((const unsigned char*) payload, MIN(rcv_len, len));
  263. for (i = 0; i < rcv_len-len; i++) {
  264. c = rd_char(&errors);
  265. calc_cs ^= c;
  266. }
  267. rcv_cs = rd_char(&errors);
  268. if (rcv_cs != calc_cs) {
  269. fatal(MYNAME ": Checksum error: got 0x%02x, expected 0x%02x\n", rcv_cs, calc_cs);
  270. }
  271. if (rd_word() != 0x0D0A) {
  272. fatal(MYNAME ": Didn't get message end tag (CR/LF)\n");
  273. }
  274. // return MIN(rcv_len, len);
  275. return res_OK;
  276. }
  277. static void
  278. skytraq_wr_msg(const uint8_t* payload, int len)
  279. {
  280. int cs;
  281. rd_drain();
  282. wr_buf(MSG_START, sizeof(MSG_START));
  283. wr_char((len>>8) & 0x0FF);
  284. wr_char(len & 0x0FF);
  285. wr_buf(payload, len);
  286. cs = skytraq_calc_checksum(payload, len);
  287. wr_char(cs);
  288. wr_buf(NL, sizeof(NL));
  289. }
  290. static int
  291. skytraq_expect_ack(uint8_t id)
  292. {
  293. uint8_t ack_msg[2];
  294. int i/*, rcv_len*/;
  295. for (i = 0; i < MSG_RETRIES; i++) {
  296. // rcv_len = skytraq_rd_msg(ack_msg, sizeof(ack_msg));
  297. // if (rcv_len == sizeof(ack_msg)) {
  298. if (skytraq_rd_msg(ack_msg, sizeof(ack_msg)) == res_OK) {
  299. if (ack_msg[0] == 0x83) {
  300. if (ack_msg[1] == id) {
  301. db(3, "Got ACK (id=0x%02x)\n", id);
  302. return res_OK;
  303. } else if (ack_msg[1] == 0) {
  304. /* some (all?) devices first send an ACK with id==0, skip that */
  305. continue;
  306. } else {
  307. db(1, MYNAME ": Warning: Got unexpected ACK (id=0x%02x)\n", ack_msg[1]);
  308. continue;
  309. }
  310. } else if (ack_msg[0] == 0x84) {
  311. db(3, "Warning: Got NACK (id=0x%02x)\n", ack_msg[1]);
  312. return res_NACK;
  313. } else {
  314. db(3, "Warning: Got unexpected message (id=0x%02x), expected ACK (id=0x%02x)\n",
  315. ack_msg[0], id);
  316. }
  317. } else {
  318. /* payload too short or didn't receive a message at all
  319. -> caller should either resend request or give up.
  320. */
  321. break;
  322. }
  323. }
  324. return res_PROTOCOL_ERR;
  325. }
  326. static int
  327. skytraq_expect_msg(uint8_t id, const uint8_t* payload, int len)
  328. {
  329. int i, rc;
  330. for (i = 0; i < MSG_RETRIES; i++) {
  331. rc = skytraq_rd_msg(payload, len);
  332. if (rc < 0) {
  333. return rc;
  334. }
  335. if (payload[0] == id) {
  336. return len;
  337. }
  338. }
  339. return res_PROTOCOL_ERR;
  340. }
  341. static int
  342. skytraq_wr_msg_verify(const uint8_t* payload, int len)
  343. {
  344. int i, rc;
  345. for (i = 0; i < MSG_RETRIES; i++) {
  346. if (i > 0) {
  347. db(1, "resending msg (id=0x%02x)...\n", payload[0]);
  348. }
  349. skytraq_wr_msg(payload, len);
  350. rc = skytraq_expect_ack(payload[0]);
  351. if (rc == res_OK || rc == res_NACK) {
  352. return rc;
  353. }
  354. db(1, MYNAME ": Got neither ACK nor NACK, ");
  355. }
  356. db(1, "aborting (msg id was 0x%02x).\n", payload[0]);
  357. return res_ERROR;
  358. }
  359. static int
  360. skytraq_system_restart(void)
  361. {
  362. uint8_t MSG_SYSTEM_RESTART[15] =
  363. { 0x01, 0x01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
  364. db(2, "restart system\n");
  365. return skytraq_wr_msg_verify(MSG_SYSTEM_RESTART, sizeof(MSG_SYSTEM_RESTART));
  366. }
  367. static int
  368. skytraq_set_baud(int baud)
  369. {
  370. /* Note: according to AN0003_v3.pdf, attrib == 0x00 means write to SRAM only, however
  371. * it seems to write to flash too. The Windows software sends 0x02 so we do here too.
  372. */
  373. uint8_t MSG_CONFIGURE_SERIAL_PORT[4]
  374. = { 0x05, 0x00, 0x00, 0x02 };
  375. int rc;
  376. db(2, "Setting baud rate to %i\n", baud);
  377. switch (baud) {
  378. case 4800:
  379. MSG_CONFIGURE_SERIAL_PORT[2] = 0;
  380. break;
  381. case 9600:
  382. MSG_CONFIGURE_SERIAL_PORT[2] = 1;
  383. break;
  384. case 19200:
  385. MSG_CONFIGURE_SERIAL_PORT[2] = 2;
  386. break;
  387. case 38400:
  388. MSG_CONFIGURE_SERIAL_PORT[2] = 3;
  389. break;
  390. case 57600:
  391. MSG_CONFIGURE_SERIAL_PORT[2] = 4;
  392. break;
  393. case 115200:
  394. MSG_CONFIGURE_SERIAL_PORT[2] = 5;
  395. break;
  396. case 230400:
  397. MSG_CONFIGURE_SERIAL_PORT[2] = 6;
  398. break;
  399. default:
  400. fatal(MYNAME ": Unsupported baud rate: %ibd\n", baud);
  401. }
  402. rc = skytraq_wr_msg_verify(MSG_CONFIGURE_SERIAL_PORT, sizeof(MSG_CONFIGURE_SERIAL_PORT));
  403. if (rc != res_OK) {
  404. db(2, "Warning: error setting skytraq device baud rate\n");
  405. return rc;
  406. }
  407. db(3, "Now setting UART baud rate to %i\n", baud);
  408. rd_drain();
  409. if (gbser_set_speed(serial_handle, baud) != gbser_OK) {
  410. db(2, "Warning: error setting uart baud rate\n");
  411. return res_ERROR;
  412. }
  413. gb_sleep(50); /* allow UART to settle. */
  414. return res_OK;
  415. }
  416. static int
  417. skytraq_configure_logging(void)
  418. {
  419. // an0008-1.4.14: logs if
  420. // (dt > tmin & dd >= dmin & v >= vmin) | dt > tmax | dd > dmax | v > vmax
  421. unsigned int tmin=6, tmax=3600, dmin=0, dmax=10000, nn=0;
  422. uint8_t MSG_LOG_CONFIGURE_CONTROL[] = {
  423. 0x18, // message_id
  424. 0x00, 0x00, 0x0e, 0x10, // max_time: was 0x0000ffff (big endian!)
  425. 0x00, 0x00, 0x00, 0x06, // min_time: was 0x00000005
  426. 0x00, 0x00, 0x27, 0x10, // max_distance: was 0x0000ffff
  427. 0x00, 0x00, 0x00, 0x00, // min_distance
  428. 0x00, 0x00, 0xff, 0xff, // max_speed
  429. 0x00, 0x00, 0x00, 0x00, // min_speed
  430. 0x01, // datalog_enable: NOTE: always ON
  431. 0x00 // reserved
  432. };
  433. if (opt_configure_logging) {
  434. if (*opt_configure_logging) {
  435. nn = sscanf(opt_configure_logging, "%u:%u:%u:%u", &tmin, &tmax, &dmin, &dmax);
  436. if (nn>3) {
  437. db(0, "Reconfiguring logging to: tmin=%u, tmax=%u, dmin=%u, dmax=%u\n", tmin, tmax, dmin, dmax);
  438. be_write32(MSG_LOG_CONFIGURE_CONTROL+5, tmin);
  439. be_write32(MSG_LOG_CONFIGURE_CONTROL+1, tmax);
  440. be_write32(MSG_LOG_CONFIGURE_CONTROL+13, dmin);
  441. be_write32(MSG_LOG_CONFIGURE_CONTROL+9, dmax);
  442. } else {
  443. db(1, MYNAME "Option usage: configlog=tmin:tmax:dmin:dmax");
  444. return -1;
  445. }
  446. }
  447. }
  448. return skytraq_wr_msg_verify(MSG_LOG_CONFIGURE_CONTROL, sizeof(MSG_LOG_CONFIGURE_CONTROL));
  449. }
  450. static int
  451. skytraq_get_log_buffer_status(uint32_t* log_wr_ptr, uint16_t* sectors_free, uint16_t* sectors_total)
  452. {
  453. uint8_t MSG_LOG_STATUS_CONTROL = 0x17;
  454. struct {
  455. uint8_t id[1];
  456. uint8_t log_wr_ptr[4];
  457. uint8_t sectors_free[2];
  458. uint8_t sectors_total[2];
  459. uint8_t max_time[4], min_time[4], max_dist[4], min_dist[4], max_speed[4], min_speed[4];
  460. uint8_t datalog_enable[1], log_fifo_mode[1];
  461. } MSG_LOG_STATUS_OUTPUT;
  462. unsigned int rc;
  463. if ((rc = skytraq_wr_msg_verify(&MSG_LOG_STATUS_CONTROL, 1)) != res_OK) { /* get memory status */
  464. db(1, MYNAME ": Error sending LOG STATUS CONTROL message (%d)\n", rc);
  465. return res_ERROR;
  466. }
  467. rc = skytraq_expect_msg(0x94, (uint8_t*)&MSG_LOG_STATUS_OUTPUT, sizeof(MSG_LOG_STATUS_OUTPUT));
  468. if (rc < sizeof(MSG_LOG_STATUS_OUTPUT)) {
  469. db(1, MYNAME ": Didn't receive expected reply (%d)\n", rc);
  470. return res_ERROR;
  471. }
  472. *log_wr_ptr = le_readu32(&MSG_LOG_STATUS_OUTPUT.log_wr_ptr);
  473. *sectors_free = le_readu16(&MSG_LOG_STATUS_OUTPUT.sectors_free);
  474. *sectors_total = le_readu16(&MSG_LOG_STATUS_OUTPUT.sectors_total);
  475. // print logging parameters -- useful, but does this belong here?
  476. unsigned int tmax, tmin, dmax, dmin, vmax, vmin;
  477. // unsigned char log_bool, fifo_mode;
  478. char* mystatus;
  479. tmax = le_readu32(&MSG_LOG_STATUS_OUTPUT.max_time);
  480. tmin = le_readu32(&MSG_LOG_STATUS_OUTPUT.min_time);
  481. dmax = le_readu32(&MSG_LOG_STATUS_OUTPUT.max_dist);
  482. dmin = le_readu32(&MSG_LOG_STATUS_OUTPUT.min_dist);
  483. vmax = le_readu32(&MSG_LOG_STATUS_OUTPUT.max_speed);
  484. vmin = le_readu32(&MSG_LOG_STATUS_OUTPUT.min_speed);
  485. // log_bool = *(MSG_LOG_STATUS_OUTPUT.datalog_enable);
  486. // fifo_mode = *(MSG_LOG_STATUS_OUTPUT.log_fifo_mode);
  487. xasprintf(&mystatus, "#logging: tmin=%u, tmax=%u, dmin=%u, dmax=%u, vmin=%u, vmax=%u\n", tmin, tmax, dmin, dmax, vmin, vmax);
  488. db(1, mystatus);
  489. xfree(mystatus);
  490. return res_OK;
  491. }
  492. /* reads 32-bit "middle-endian" fields */
  493. static unsigned int me_read32(const unsigned char* p)
  494. {
  495. return ((unsigned)be_read16(p+2) << 16) | ((unsigned)be_read16(p));
  496. }
  497. static time_t
  498. gpstime_to_timet(int week, int sec)
  499. {
  500. /* Notes:
  501. * * assumes we're between the 1st and 2nd week rollover
  502. * (i.e. between 22 Aug 1999 and 7 April 2019), so this
  503. * should be taken care of before the next rollover...
  504. * * list of leap seconds taken from
  505. * <http://maia.usno.navy.mil/ser7/tai-utc.dat>
  506. * as of 2012-10-12. Please update when necessary.
  507. * Announcement of leap seconds:
  508. * <http://hpiers.obspm.fr/iers/bul/bulc/bulletinc.dat>
  509. * * leap seconds of 1999 JAN 1 and before are not reflected
  510. * here, beware when using this for really old data
  511. * * overflow of sec into next week is allowed
  512. * (i.e. sec >= 7*24*3600 = 604800 is allowed)
  513. */
  514. time_t gps_timet = 315964800; /* Jan 06 1980 0:00 UTC */
  515. gps_timet += (week+1024)*7*SECONDS_PER_DAY + sec;
  516. /* leap second compensation: */
  517. gps_timet -= 13; /* diff GPS-UTC=13s (valid from Jan 01 1999 on) */
  518. if (gps_timet >= 1136073600) { /* Jan 01 2006 0:00 UTC */
  519. gps_timet--; /* GPS-UTC = 14s */
  520. }
  521. if (gps_timet >= 1230768000) { /* Jan 01 2009 0:00 UTC */
  522. gps_timet--; /* GPS-UTC = 15s */
  523. }
  524. if (gps_timet >= 1341100800) { /* Jul 01 2012 0:00 UTC */
  525. gps_timet--; /* GPS-UTC = 16s */
  526. }
  527. return gps_timet; /* returns UTC time */
  528. }
  529. static void
  530. ECEF_to_LLA(double x, double y, long z, double* lat, double* lon, double* alt)
  531. {
  532. /* constants: */
  533. const double CA = 6378137.0;
  534. const double CB = 6356752.31424518;
  535. const double CE2 = (CA*CA - CB*CB) / (CA*CA); /* =e^2 */
  536. const double CE_2 = (CA*CA - CB*CB) / (CB*CB); /* =e'^2 */
  537. /* auxiliary values: */
  538. double AP = sqrt(x*x + y*y);
  539. double ATHETA = atan2(z*CA, AP*CB);
  540. /* latitude (in radians): */
  541. *lat = atan2(z + CE_2 * CB * pow(sin(ATHETA), 3), AP - CE2 * CA * pow(cos(ATHETA), 3));
  542. /* longitude (in radians): */
  543. *lon = atan2(y, x);
  544. /* height above ellipsoid (in meters): */
  545. *alt = AP/cos(*lat) - CA/sqrt(1 - CE2 * pow(sin(*lat), 2));
  546. *lat = *lat /M_PI*180;
  547. *lon = *lon /M_PI*180;
  548. }
  549. struct read_state {
  550. route_head* route_head_;
  551. unsigned wpn, tpn;
  552. unsigned gps_week;
  553. unsigned gps_sec;
  554. long x, y, z;
  555. };
  556. static void
  557. state_init(struct read_state* pst)
  558. {
  559. route_head* track;
  560. track = route_head_alloc();
  561. track->rte_name = "SkyTraq tracklog";
  562. track->rte_desc = "SkyTraq GPS tracklog data";
  563. track_add_head(track);
  564. pst->route_head_ = track;
  565. pst->wpn = 0;
  566. pst->tpn = 0;
  567. pst->gps_week = 0;
  568. pst->gps_sec = 0;
  569. pst->x = 0;
  570. pst->y = 0;
  571. pst->z = 0;
  572. }
  573. static Waypoint*
  574. make_trackpoint(struct read_state* st, double lat, double lon, double alt)
  575. {
  576. Waypoint* wpt = new Waypoint;
  577. wpt->shortname = QString().sprintf("TP%04d", ++st->tpn);
  578. wpt->latitude = lat;
  579. wpt->longitude = lon;
  580. wpt->altitude = alt;
  581. wpt->SetCreationTime(gpstime_to_timet(st->gps_week, st->gps_sec));
  582. return wpt;
  583. }
  584. typedef struct {
  585. uint32_t gps_week;
  586. uint32_t gps_sec;
  587. int32_t x;
  588. int32_t y;
  589. int32_t z;
  590. } full_item;
  591. typedef struct {
  592. uint16_t dt;
  593. int16_t dx;
  594. int16_t dy;
  595. int16_t dz;
  596. } compact_item;
  597. typedef struct {
  598. uint32_t gps_week;
  599. uint32_t gps_sec;
  600. int32_t lat;
  601. int32_t lon;
  602. int32_t alt;
  603. } multi_hz_item;
  604. struct full_item_frame {
  605. unsigned char ts[4];
  606. unsigned char x[4];
  607. unsigned char y[4];
  608. unsigned char z[4];
  609. };
  610. struct compact_item_frame {
  611. unsigned char dt[2]; /* big endian unsigned short */
  612. unsigned char dpos[4];
  613. };
  614. struct multi_hz_item_frame {
  615. unsigned char v_kmh[2];
  616. unsigned char ts[4];
  617. unsigned char lat[4];
  618. unsigned char lon[4];
  619. unsigned char alt[4];
  620. };
  621. typedef struct {
  622. unsigned char type_and_speed[2];
  623. union {
  624. struct multi_hz_item_frame multi_hz;
  625. struct full_item_frame full;
  626. struct compact_item_frame comp;
  627. };
  628. } item_frame;
  629. #define ITEM_WEEK_NUMBER(item) (item->type_and_speed[1] | ((item->type_and_speed[0] & 0x03) << 8))
  630. #define POW_2_M20 0.000000953674316
  631. #define POW_2_M7 0.0078125
  632. #define ITEM_TYPE(item) (item->type_and_speed[0] >> 4)
  633. #define ITEM_SPEED(item) (item->type_and_speed[1] | ((item->type_and_speed[0] & 0x0F) << 8))
  634. static int
  635. process_data_item(struct read_state* pst, const item_frame* pitem, int len)
  636. {
  637. int res = 0;
  638. double lat, lon, alt, spe;
  639. unsigned int ts;
  640. int poi = 0;
  641. full_item f;
  642. compact_item c;
  643. multi_hz_item m;
  644. Waypoint* tpt = NULL;
  645. switch (ITEM_TYPE(pitem)) {
  646. case 0xc: /* POI item (same structure as full) */
  647. poi = 1;
  648. /* fall through: */
  649. case 0x2: /* Multi HZ item */
  650. if (len < MULTI_HZ_ITEM_LEN) {
  651. db(1, MYNAME ": Not enough bytes in sector for a full item.\n");
  652. return res_ERROR;
  653. }
  654. m.gps_week = ITEM_WEEK_NUMBER(pitem);
  655. ts = me_read32(pitem->multi_hz.ts);
  656. m.gps_sec = ((int)(ts & 0x3FFFFFFF)) / 1000;
  657. m.lat = me_read32(pitem->multi_hz.lat);
  658. m.lon = me_read32(pitem->multi_hz.lon);
  659. m.alt = me_read32(pitem->multi_hz.alt);
  660. pst->gps_week = m.gps_week;
  661. pst->gps_sec = m.gps_sec;
  662. spe = KPH_TO_MPS(be_read16(pitem->multi_hz.v_kmh));
  663. db(4, "Got multi hz item: week=%i sec=%i lat=%i lon=%i alt=%i speed=%f\n",
  664. m.gps_week, m.gps_sec,
  665. m.lat, m.lon, m.alt,
  666. spe);
  667. lat = m.lat * POW_2_M20;
  668. lon = m.lon * POW_2_M20;
  669. alt = m.alt * POW_2_M7;
  670. tpt = make_trackpoint(pst, lat, lon, alt);
  671. WAYPT_SET(tpt, speed, spe); /* convert speed to m/s */
  672. track_add_wpt(pst->route_head_, tpt);
  673. res = MULTI_HZ_ITEM_LEN;
  674. break;
  675. case 0x6: /* POI item (same structure as full) */
  676. poi = 1;
  677. /* fall through: */
  678. case 0x4: /* full item */
  679. if (len < FULL_ITEM_LEN) {
  680. db(1, MYNAME ": Not enough bytes in sector for a full item.\n");
  681. return res_ERROR;
  682. }
  683. ts = me_read32(pitem->full.ts);
  684. f.gps_week = ts & 0x000003FF;
  685. f.gps_sec = ts >> 12;
  686. f.x = me_read32(pitem->full.x);
  687. f.y = me_read32(pitem->full.y);
  688. f.z = me_read32(pitem->full.z);
  689. pst->gps_week = f.gps_week;
  690. pst->gps_sec = f.gps_sec;
  691. pst->x = f.x;
  692. pst->y = f.y;
  693. pst->z = f.z;
  694. db(4, "Got %s item: week=%i sec=%i x=%i y=%i z=%i speed=%i\n",
  695. poi ? "POI" : "full",
  696. f.gps_week, f.gps_sec,
  697. f.x, f.y, f.z,
  698. ITEM_SPEED(pitem));
  699. res = FULL_ITEM_LEN;
  700. break;
  701. case 0x8: /* compact item */
  702. if (len < COMPACT_ITEM_LEN) {
  703. db(1, MYNAME ": Not enough bytes in sector for a compact item.\n");
  704. return res_ERROR;
  705. }
  706. c.dx = (pitem->comp.dpos[1] >> 6) | (pitem->comp.dpos[0] << 2);
  707. c.dy = (pitem->comp.dpos[1] & 0x3F) | ((pitem->comp.dpos[2] & 0xF0) << 2);
  708. c.dz = pitem->comp.dpos[3] | ((pitem->comp.dpos[2] & 0x03) << 8);
  709. if (c.dx > 511) {
  710. c.dx = 511-c.dx; /* make proper signed values */
  711. }
  712. if (c.dy > 511) {
  713. c.dy = 511-c.dy;
  714. }
  715. if (c.dz > 511) {
  716. c.dz = 511-c.dz;
  717. }
  718. c.dt = (pitem->comp.dt[0] << 8) | pitem->comp.dt[1];
  719. db(4, "Got compact item: dt=%i dx=%i dy=%i dz=%i speed=%i uu=%i\n",
  720. c.dt, c.dx, c.dy, c.dz,
  721. ITEM_SPEED(pitem), (pitem->comp.dpos[2] & 0x0F)>>2);
  722. pst->gps_sec += c.dt;
  723. pst->x += c.dx;
  724. pst->y += c.dy;
  725. pst->z += c.dz;
  726. res = COMPACT_ITEM_LEN;
  727. break;
  728. default:
  729. db(1, MYNAME ": Unknown item type encountered: 0x%02x\n", ITEM_TYPE(pitem));
  730. return 0;
  731. }
  732. if (res == COMPACT_ITEM_LEN || res == FULL_ITEM_LEN) {
  733. ECEF_to_LLA(pst->x, pst->y, pst->z, &lat, &lon, &alt);
  734. // GPS_Math_XYZ_To_WGS84LatLonH(&lat, &lon, &alt, pst->x, pst->y, pst->z);
  735. tpt = make_trackpoint(pst, lat, lon, alt);
  736. WAYPT_SET(tpt, speed, KPH_TO_MPS(ITEM_SPEED(pitem))); /* convert speed to m/s */
  737. if (poi) {
  738. waypt_add(new Waypoint(*tpt));
  739. }
  740. if (0 == pst->route_head_) {
  741. db(1, MYNAME ": New Track\n");
  742. pst->route_head_ = route_head_alloc();
  743. track_add_head(pst->route_head_);
  744. }
  745. track_add_wpt(pst->route_head_, tpt);
  746. }
  747. return res;
  748. }
  749. static int /* returns number of bytes processed (terminates on 0xFF i.e. empty or padding bytes) */
  750. process_data_sector(struct read_state* pst, const uint8_t* buf, int len)
  751. {
  752. int plen, ilen;
  753. for (plen = 0; plen < len && buf[plen] != 0xFF; plen += ilen) {
  754. ilen = process_data_item(pst, (item_frame*)&buf[plen], len-plen);
  755. if (ilen <= 0) {
  756. fatal(MYNAME ": Error %i while processing data item #%i (starts at %i)\n",
  757. ilen, pst->tpn, plen);
  758. }
  759. }
  760. return plen;
  761. }
  762. /* Note: the buffer is being padded with 0xFFs if necessary so there are always SECTOR_SIZE valid bytes */
  763. static int
  764. skytraq_read_single_sector(unsigned int sector, uint8_t* buf)
  765. {
  766. uint8_t MSG_LOG_SECTOR_READ_CONTROL[2] = { 0x1B, (uint8_t)(sector) };
  767. int errors = 5; /* allow this many errors */
  768. unsigned int c, i, j, cs;
  769. uint8_t buffer[16];
  770. if (sector > 0xFF) {
  771. fatal(MYNAME ": Invalid sector number (%i)\n", sector);
  772. }
  773. db(2, "Reading sector #%i...\n", sector);
  774. if (skytraq_wr_msg_verify((uint8_t*)&MSG_LOG_SECTOR_READ_CONTROL, sizeof(MSG_LOG_SECTOR_READ_CONTROL)) != res_OK) {
  775. db(1, MYNAME ": Didn't receive ACK\n");
  776. return res_ERROR;
  777. }
  778. #ifdef READ_SINGLE_CHARS
  779. for (i = 0, j = 0; i-j < SECTOR_SIZE && j < sizeof(SECTOR_READ_END); i++) {
  780. c = rd_char(&errors);
  781. buf[i] = c;
  782. if (c == SECTOR_READ_END[j]) {
  783. j++;
  784. } else if (c == SECTOR_READ_END[0]) {
  785. j = 1;
  786. } else {
  787. j = 0;
  788. }
  789. }
  790. if (j < sizeof(SECTOR_READ_END)) {
  791. db(1, MYNAME ": Didn't get sector end tag\n");
  792. return res_ERROR;
  793. }
  794. c = rd_char(&errors); /* read checksum byte */
  795. buf[i] = c;
  796. #else
  797. for (i = 0, j = 0; i-j < SECTOR_SIZE && j < sizeof(SECTOR_READ_END); i+=c) {
  798. rd_buf(buffer, 16);
  799. for (c = 0; c < 16 && j < sizeof(SECTOR_READ_END); c++) {
  800. buf[i+c] = buffer[c];
  801. if (buffer[c] == SECTOR_READ_END[j]) {
  802. j++;
  803. } else if (buffer[c] == SECTOR_READ_END[0]) {
  804. j = 1;
  805. } else {
  806. j = 0;
  807. }
  808. }
  809. }
  810. if (j < sizeof(SECTOR_READ_END)) {
  811. db(1, MYNAME ": Didn't get sector end tag\n");
  812. return res_ERROR;
  813. }
  814. if (c < 16) {
  815. buf[i] = buffer[c];
  816. } else {
  817. c = rd_char(&errors); /* read checksum byte */
  818. buf[i] = c;
  819. }
  820. #endif
  821. i = i-j;
  822. db(3, "Received %i bytes of log data\n", i);
  823. //#define SINGLE_READ_WORKAROUND
  824. #ifdef SINGLE_READ_WORKAROUND
  825. gbser_set_speed(serial_handle, skytraq_baud);
  826. rd_char(&errors);
  827. rd_char(&errors);
  828. rd_char(&errors);
  829. rd_char(&errors);
  830. rd_char(&errors);
  831. rd_char(&errors);
  832. skytraq_set_baud(atoi(opt_dlbaud));
  833. #endif
  834. cs = skytraq_calc_checksum(buf, i);
  835. if (cs != buf[i+sizeof(SECTOR_READ_END)]) {
  836. db(1, MYNAME ": Checksum error while reading sector: got 0x%02x, expected 0x%02x\n",
  837. buf[i+sizeof(SECTOR_READ_END)], cs);
  838. return res_ERROR;
  839. }
  840. for (; i < SECTOR_SIZE; i++) {
  841. buf[i] = 0xFF;
  842. }
  843. return res_OK;
  844. }
  845. static int
  846. skytraq_read_multiple_sectors(int first_sector, unsigned int sector_count, uint8_t* buf)
  847. {
  848. uint8_t MSG_LOG_READ_MULTI_SECTORS[5] = { 0x1D };
  849. uint8_t* buf_end_tag;
  850. unsigned int cs, i, read_result;
  851. if (first_sector < 0 || first_sector > 0xFFFF) {
  852. fatal(MYNAME ": Invalid sector number (%i)\n", first_sector);
  853. }
  854. be_write16(&MSG_LOG_READ_MULTI_SECTORS[1], first_sector);
  855. if (sector_count > 0xFFFF) {
  856. fatal(MYNAME ": Invalid sector count (%i)\n", sector_count);
  857. }
  858. be_write16(&MSG_LOG_READ_MULTI_SECTORS[3], sector_count);
  859. db(2, "Reading %i sectors beginning from #%i...\n", sector_count, first_sector);
  860. read_result = skytraq_wr_msg_verify((uint8_t*)&MSG_LOG_READ_MULTI_SECTORS, sizeof(MSG_LOG_READ_MULTI_SECTORS));
  861. if (read_result != res_OK) {
  862. return read_result;
  863. }
  864. for (i = 0; i < sector_count; i++) {
  865. db(2, "Receiving data of sector #%i...\n", first_sector+i);
  866. rd_buf(buf+i*SECTOR_SIZE, SECTOR_SIZE);
  867. }
  868. rd_buf(buf+SECTOR_SIZE*sector_count, sizeof(SECTOR_READ_END)+6);
  869. buf_end_tag = buf + SECTOR_SIZE*sector_count;
  870. for (i = 0; i < sizeof(SECTOR_READ_END); i++) {
  871. if (buf_end_tag[i] != SECTOR_READ_END[i]) {
  872. db(1, MYNAME ": Wrong end tag: got 0x%02x ('%c'), expected 0x%02x ('%c')\n",
  873. buf_end_tag[i], isprint(buf_end_tag[i]) ? buf_end_tag[i] : '.',
  874. SECTOR_READ_END[i], isprint(SECTOR_READ_END[i]) ? SECTOR_READ_END[i] : '.');
  875. return res_ERROR;
  876. }
  877. }
  878. cs = skytraq_calc_checksum(buf, SECTOR_SIZE*sector_count);
  879. if (cs != buf_end_tag[sizeof(SECTOR_READ_END)]) {
  880. db(1, MYNAME ": Checksum error while reading sector: got 0x%02x, expected 0x%02x\n",
  881. buf_end_tag[sizeof(SECTOR_READ_END)], cs);
  882. return res_ERROR;
  883. }
  884. return res_OK;
  885. }
  886. static void
  887. skytraq_read_tracks(void)
  888. {
  889. struct read_state st;
  890. uint32_t log_wr_ptr;
  891. uint16_t sectors_free, sectors_total, /*sectors_used_a, sectors_used_b,*/ sectors_used;
  892. int i, t, s, rc, got_sectors, total_sectors_read = 0;
  893. int read_at_once = MAX(atoi(opt_read_at_once), 1);
  894. int opt_first_sector_val = atoi(opt_first_sector);
  895. int opt_last_sector_val = atoi(opt_last_sector);
  896. int multi_read_supported = 1;
  897. uint8_t* buffer = NULL;
  898. gbfile* dumpfile = NULL;
  899. state_init(&st);
  900. if (skytraq_get_log_buffer_status(&log_wr_ptr, &sectors_free, &sectors_total) != res_OK) {
  901. fatal(MYNAME ": Can't get log buffer status\n");
  902. }
  903. db(1, MYNAME ": Device status: free sectors: %i / total sectors: %i / %i%% used / write ptr: %i\n",
  904. sectors_free, sectors_total, 100 - sectors_free*100 / sectors_total, log_wr_ptr);
  905. if (opt_first_sector_val >= sectors_total) {
  906. db(1, "Warning: sector# specified by option first-sector (%i) is beyond reported total sector count (%i)",
  907. opt_first_sector_val, sectors_total);
  908. }
  909. /* Workaround: sectors_free is sometimes reported wrong. Tried to use log_wr_ptr as an
  910. indicator for how many sectors are currently used. However this isn't correct in every case too.
  911. The current read logic is aware of that so this shouldn't be necessary anymore.
  912. sectors_used_a = sectors_total - sectors_free;
  913. sectors_used_b = (log_wr_ptr + SECTOR_SIZE - 1) / SECTOR_SIZE;
  914. if (sectors_used_a != sectors_used_b) {
  915. db(1, "Warning: device reported inconsistent number of used sectors (a=%i, b=%i), "\
  916. "using max=%i\n", sectors_used_a, sectors_used_b, MAX(sectors_used_a, sectors_used_b));
  917. }
  918. sectors_used = MAX(sectors_used_a, sectors_used_b);
  919. */
  920. if (opt_last_sector_val < 0) {
  921. sectors_used = sectors_total - sectors_free + 1 /*+5*/;
  922. if (opt_first_sector_val >= sectors_used) {
  923. sectors_used = opt_first_sector_val + 1;
  924. }
  925. } else {
  926. sectors_used = opt_last_sector_val;
  927. if (opt_last_sector_val >= sectors_total) {
  928. db(1, "Warning: sector# specified by option last-sector (%i) is beyond reported total sector count (%i)",
  929. opt_last_sector_val, sectors_total);
  930. }
  931. }
  932. buffer = (uint8_t*) xmalloc(SECTOR_SIZE*read_at_once+sizeof(SECTOR_READ_END)+6);
  933. // m.ad/090930: removed code that tried reducing read_at_once if necessary since doesn't work with xmalloc
  934. if (opt_dump_file) {
  935. dumpfile = gbfopen(opt_dump_file, "w", MYNAME);
  936. }
  937. db(1, MYNAME ": Reading log data from device...\n");
  938. db(1, MYNAME ": start=%d used=%d\n", opt_first_sector_val, sectors_used);
  939. db(1, MYNAME ": opt_last_sector_val=%d\n", opt_last_sector_val);
  940. for (i = opt_first_sector_val; i < sectors_used; i += got_sectors) {
  941. for (t = 0, got_sectors = 0; (t < SECTOR_RETRIES) && (got_sectors <= 0); t++) {
  942. if (atoi(opt_read_at_once) == 0 || multi_read_supported == 0) {
  943. rc = skytraq_read_single_sector(i, buffer);
  944. if (rc == res_OK) {
  945. got_sectors = 1;
  946. }
  947. } else {
  948. /* Try to read read_at_once sectors at once.
  949. * If tere aren't any so many interesting ones, read the remainder (sectors_used-i).
  950. * And read at least 1 sector.
  951. */
  952. read_at_once = MAX(MIN(read_at_once, sectors_used-i), 1);
  953. rc = skytraq_read_multiple_sectors(i, read_at_once, buffer);
  954. switch (rc) {
  955. case res_OK:
  956. got_sectors = read_at_once;
  957. read_at_once = MIN(read_at_once*2, atoi(opt_read_at_once));
  958. break;
  959. case res_NACK:
  960. db(1, MYNAME ": Device doesn't seem to support reading multiple "
  961. "sectors at once, falling back to single read.\n");
  962. multi_read_supported = 0;
  963. break;
  964. default:
  965. /* On failure, try with less sectors */
  966. read_at_once = MAX(read_at_once/2, 1);
  967. }
  968. }
  969. }
  970. if (got_sectors <= 0) {
  971. fatal(MYNAME ": Error reading sector %i\n", i);
  972. }
  973. total_sectors_read += got_sectors;
  974. if (dumpfile) {
  975. gbfwrite(buffer, SECTOR_SIZE, got_sectors, dumpfile);
  976. }
  977. if (*opt_no_output == '1') {
  978. continue; // skip decoding
  979. }
  980. for (s = 0; s < got_sectors; s++) {
  981. db(4, MYNAME ": Decoding sector #%i...\n", i+s);
  982. rc = process_data_sector(&st, buffer+s*SECTOR_SIZE, SECTOR_SIZE);
  983. if (rc == 0) {
  984. db(1, MYNAME ": Empty sector encountered: apparently only %i sectors are "
  985. "used but device reported %i.\n",
  986. i+s, sectors_used);
  987. i = sectors_used; /* terminate to avoid reading stale data still in the logger */
  988. break;
  989. } else if (rc >= (4096-FULL_ITEM_LEN) && i+s+1 >= sectors_used && i+s+1 < sectors_total) {
  990. db(1, MYNAME ": Last sector is nearly full, reading one more sector\n");
  991. sectors_used++;
  992. }
  993. }
  994. }
  995. free(buffer);
  996. db(1, MYNAME ": Got %i trackpoints from %i sectors.\n", st.tpn, total_sectors_read);
  997. if (dumpfile) {
  998. gbfclose(dumpfile);
  999. }
  1000. }
  1001. static int
  1002. skytraq_probe(void)
  1003. {
  1004. int baud_rates[] = { 9600, 230400, 115200, 57600, 4800, 19200, 38400 };
  1005. int baud_rates_count = sizeof(baud_rates)/sizeof(baud_rates[0]);
  1006. int initbaud = atoi(opt_initbaud);
  1007. uint8_t MSG_QUERY_SOFTWARE_VERSION[2] = { 0x02, 0x01 };
  1008. struct {
  1009. uint8_t id;
  1010. uint8_t sw_type;
  1011. uint8_t kernel_ver[4];
  1012. uint8_t odm_ver[4];
  1013. uint8_t revision[4];
  1014. } MSG_SOFTWARE_VERSION;
  1015. int i, rc;
  1016. // TODO: get current serial port baud rate and try that first
  1017. // (only sensible if init to 4800 can be disabled...)
  1018. if (initbaud > 0) {
  1019. baud_rates[0] = initbaud;
  1020. baud_rates_count = 1;
  1021. }
  1022. for (i = 0; i < baud_rates_count; i++) {
  1023. db(1, MYNAME ": Probing SkyTraq Venus at %ibaud...\n", baud_rates[i]);
  1024. rd_drain();
  1025. if ((rc = gbser_set_speed(serial_handle, baud_rates[i])) != gbser_OK) {
  1026. db(1, MYNAME ": Set baud rate to %d failed (%d), retrying...\n", baud_rates[i], rc);
  1027. if ((rc = gbser_set_speed(serial_handle, baud_rates[i])) != gbser_OK) {
  1028. db(1, MYNAME ": Set baud rate to %d failed (%d)\n", baud_rates[i], rc);
  1029. continue;
  1030. }
  1031. }
  1032. gb_sleep(50); /* allow UART to settle. */
  1033. skytraq_wr_msg(MSG_QUERY_SOFTWARE_VERSION, /* get firmware version */
  1034. sizeof(MSG_QUERY_SOFTWARE_VERSION));
  1035. if ((rc = skytraq_expect_ack(0x02)) != res_OK) {
  1036. db(2, "Didn't receive ACK (%d), retrying...\n", rc);
  1037. skytraq_wr_msg(MSG_QUERY_SOFTWARE_VERSION, /* get firmware version */
  1038. sizeof(MSG_QUERY_SOFTWARE_VERSION));
  1039. if ((rc = skytraq_expect_ack(0x02)) != res_OK) {
  1040. db(2, "Didn't receive ACK (%d)\n", rc);
  1041. continue;
  1042. }
  1043. }
  1044. /* note: _verify retries on errors, probe takes too long.
  1045. if (skytraq_wr_msg_verify(MSG_QUERY_SOFTWARE_VERSION,
  1046. sizeof(MSG_QUERY_SOFTWARE_VERSION)) != res_OK)
  1047. {
  1048. continue;
  1049. }*/
  1050. rc = skytraq_expect_msg(0x80, (uint8_t*)&MSG_SOFTWARE_VERSION, sizeof(MSG_SOFTWARE_VERSION));
  1051. if (rc < (int)sizeof(MSG_SOFTWARE_VERSION)) {
  1052. db(2, "Didn't receive expected reply (%d)\n", rc);
  1053. } else {
  1054. db(1, MYNAME ": Venus device found: Kernel version = %i.%i.%i, ODM version = %i.%i.%i, "\
  1055. "revision (Y/M/D) = %02i/%02i/%02i\n",
  1056. MSG_SOFTWARE_VERSION.kernel_ver[1], MSG_SOFTWARE_VERSION.kernel_ver[2],
  1057. MSG_SOFTWARE_VERSION.kernel_ver[3],
  1058. MSG_SOFTWARE_VERSION.odm_ver[1], MSG_SOFTWARE_VERSION.odm_ver[2],
  1059. MSG_SOFTWARE_VERSION.odm_ver[3],
  1060. MSG_SOFTWARE_VERSION.revision[1], MSG_SOFTWARE_VERSION.revision[2],
  1061. MSG_SOFTWARE_VERSION.revision[3]);
  1062. return baud_rates[i];
  1063. }
  1064. }
  1065. return res_NOTFOUND;
  1066. }
  1067. static int
  1068. skytraq_erase()
  1069. {
  1070. uint8_t MSG_LOG_ERASE = 0x19;
  1071. db(1, MYNAME ": Erasing logger memory...\n");
  1072. if (skytraq_wr_msg_verify(&MSG_LOG_ERASE, sizeof(MSG_LOG_ERASE)) != res_OK) {
  1073. db(1, MYNAME ": Didn't receive ACK\n");
  1074. return res_ERROR;
  1075. }
  1076. return res_OK;
  1077. }
  1078. static void
  1079. skytraq_set_location(void)
  1080. {
  1081. double lat, lng;
  1082. unsigned int i;
  1083. uint8_t MSG_SET_LOCATION[17] = { 0x36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
  1084. uint8_t MSG_GET_LOCATION = 0x35;
  1085. db(3, MYNAME ": set_location='%s'\n", opt_set_location);
  1086. sscanf(opt_set_location, "%lf:%lf", &lat, &lng);
  1087. le_write_double(&MSG_SET_LOCATION[1], lat);
  1088. le_write_double(&MSG_SET_LOCATION[9], lng);
  1089. for (i=0; i<sizeof MSG_SET_LOCATION; i++) {
  1090. db(3, "%02x ", MSG_SET_LOCATION[i]);
  1091. }
  1092. db(3, "\n");
  1093. if (skytraq_wr_msg_verify((uint8_t*)&MSG_SET_LOCATION, sizeof(MSG_SET_LOCATION)) != res_OK) {
  1094. fatal(MYNAME ": cannot set new location\n");
  1095. }
  1096. {
  1097. char buf[32];
  1098. skytraq_wr_msg_verify(&MSG_GET_LOCATION, 1);
  1099. skytraq_rd_msg(buf, 32);
  1100. }
  1101. }
  1102. /*******************************************************************************
  1103. * %%% global callbacks called by gpsbabel main process %%% *
  1104. *******************************************************************************/
  1105. static void
  1106. skytraq_rd_init(const QString& fname)
  1107. {
  1108. if ((serial_handle = gbser_init(qPrintable(fname))) == NULL) {
  1109. fatal(MYNAME ": Can't open port '%s'\n", qPrintable(fname));
  1110. }
  1111. if ((skytraq_baud = skytraq_probe()) <= 0) {
  1112. fatal(MYNAME ": Can't find skytraq device on '%s'\n", qPrintable(fname));
  1113. }
  1114. }
  1115. static void
  1116. skytraq_rd_deinit(void)
  1117. {
  1118. gbser_deinit(serial_handle);
  1119. serial_handle = NULL;
  1120. }
  1121. static void
  1122. skytraq_read(void)
  1123. {
  1124. int dlbaud;
  1125. if (opt_set_location) {
  1126. skytraq_set_location();
  1127. return;
  1128. }
  1129. if (opt_configure_logging) {
  1130. skytraq_configure_logging();
  1131. return;
  1132. }
  1133. dlbaud = atoi(opt_dlbaud);
  1134. if (dlbaud != 0 && dlbaud != skytraq_baud) {
  1135. skytraq_set_baud(dlbaud);
  1136. }
  1137. // read device unless no-output=1 and dump-file=0 (i.e. no data needed at all)
  1138. if (*opt_no_output == '0' || opt_dump_file != NULL) {
  1139. skytraq_read_tracks();
  1140. }
  1141. if (*opt_erase == '1') {
  1142. skytraq_erase();
  1143. }
  1144. if (dlbaud != 0 && dlbaud != skytraq_baud) {
  1145. skytraq_set_baud(skytraq_baud); // note that _system_restart resets baud rate anyway...
  1146. }
  1147. skytraq_system_restart();
  1148. }
  1149. static void
  1150. file_init(const QString& fname)
  1151. {
  1152. db(1, "Opening file...\n");
  1153. if ((file_handle = gbfopen(fname, "rb", MYNAME)) == NULL) {
  1154. fatal(MYNAME ": Can't open file '%s'\n", qPrintable(fname));
  1155. }
  1156. }
  1157. static void
  1158. file_deinit(void)
  1159. {
  1160. db(1, "Closing file...\n");
  1161. gbfclose(file_handle);
  1162. file_handle = NULL;
  1163. }
  1164. static void
  1165. file_read(void)
  1166. {
  1167. struct read_state st;
  1168. int rc, got_bytes;
  1169. int opt_first_sector_val = atoi(opt_first_sector);
  1170. int opt_last_sector_val = atoi(opt_last_sector);
  1171. int sectors_read;
  1172. uint8_t* buffer;
  1173. state_init(&st);
  1174. buffer = (uint8_t*) xmalloc(SECTOR_SIZE);
  1175. if (opt_first_sector_val > 0) {
  1176. db(4, MYNAME ": Seeking to first-sector index %i\n", opt_first_sector_val*SECTOR_SIZE);
  1177. gbfseek(file_handle, opt_first_sector_val*SECTOR_SIZE, SEEK_SET);
  1178. }
  1179. db(1, MYNAME ": Reading log data from file...\n");
  1180. sectors_read = 0;
  1181. while ((got_bytes = gbfread(buffer, 1, SECTOR_SIZE, file_handle)) > 0) {
  1182. db(4, MYNAME ": Decoding sector #%i...\n", sectors_read++);
  1183. rc = process_data_sector(&st, buffer, got_bytes);
  1184. if (opt_last_sector_val < 0) {
  1185. if (rc < (4096-FULL_ITEM_LEN)) {
  1186. db(1, MYNAME ": Empty sector encountered, terminating.\n");
  1187. break;
  1188. }
  1189. } else if (sectors_read-1 >= opt_last_sector_val) {
  1190. db(1, MYNAME ": desired last-sector #%i reached, terminating.\n", sectors_read-1);
  1191. break;
  1192. }
  1193. }
  1194. xfree(buffer);
  1195. db(1, MYNAME ": Got %i trackpoints from %i sectors.\n", st.tpn, sectors_read);
  1196. }
  1197. /**************************************************************************/
  1198. // capabilities below means: we can only read tracks
  1199. ff_vecs_t skytraq_vecs = {
  1200. ff_type_serial,
  1201. {
  1202. ff_cap_read /* waypoints */,
  1203. ff_cap_read /* tracks */,
  1204. ff_cap_none /* routes */
  1205. },
  1206. skytraq_rd_init,
  1207. NULL,
  1208. skytraq_rd_deinit,
  1209. NULL,
  1210. skytraq_read,
  1211. NULL,
  1212. NULL,
  1213. skytraq_args,
  1214. CET_CHARSET_UTF8, 1 /* master process: don't convert anything */
  1215. };
  1216. ff_vecs_t skytraq_fvecs = {
  1217. ff_type_file,
  1218. {
  1219. ff_cap_read /* waypoints */,
  1220. ff_cap_read /* tracks */,
  1221. ff_cap_none /* routes */
  1222. },
  1223. file_init,
  1224. NULL,
  1225. file_deinit,
  1226. NULL,
  1227. file_read,
  1228. NULL,
  1229. NULL,
  1230. skytraq_fargs,
  1231. CET_CHARSET_UTF8, 1 /* master process: don't convert anything */
  1232. };
  1233. /**************************************************************************/
  1234. /*
  1235. * support POI of skytraq based miniHomer device
  1236. * http://navin.com.tw/miniHomer.htm
  1237. * 2010-10-23 Josef Reisinger
  1238. */
  1239. #ifdef MYNAME
  1240. #undef MYNAME
  1241. #endif
  1242. #define MYNAME "miniHomer"
  1243. static char* opt_set_poi_home = NULL; /* set if a "poi" option was used */
  1244. static char* opt_set_poi_car = NULL; /* set if a "poi" option was used */
  1245. static char* opt_set_poi_boat = NULL; /* set if a "poi" option was used */
  1246. static char* opt_set_poi_heart = NULL; /* set if a "poi" option was used */
  1247. static char* opt_set_poi_bar = NULL; /* set if a "poi" option was used */
  1248. arglist_t miniHomer_args[] = {
  1249. { "baud", &opt_dlbaud, "Baud rate used for download", "115200", ARGTYPE_INT, "0", "115200" },
  1250. { "dump-file", &opt_dump_file, "Dump raw data to this file", NULL, ARGTYPE_OUTFILE, ARG_NOMINMAX },
  1251. { "erase", &opt_erase, "Erase device data after download", "0", ARGTYPE_BOOL, ARG_NOMINMAX },
  1252. { "first-sector", &opt_first_sector, "First sector to be read from the device", "0", ARGTYPE_INT, "0", "65535" },
  1253. { "initbaud", &opt_initbaud, "Baud rate used to init device (0=autodetect)", "38400", ARGTYPE_INT, "38400", "38400" },
  1254. { "last-sector", &opt_last_sector, "Last sector to be read from the device (-1: smart read everything)", "-1", ARGTYPE_INT, "-1", "65535" },
  1255. { "no-output", &opt_no_output, "Disable output (useful with erase)", "0", ARGTYPE_BOOL, ARG_NOMINMAX },
  1256. { "read-at-once", &opt_read_at_once, "Number of sectors to read at once (0=use single sector mode)", "255", ARGTYPE_INT, "0", "255" },
  1257. { "Home", &opt_set_poi_home, "POI for Home Symbol as lat:lng[:alt]", NULL, ARGTYPE_STRING, "", "" },
  1258. { "Car", &opt_set_poi_car, "POI for Car Symbol as lat:lng[:alt]", NULL, ARGTYPE_STRING, "", "" },
  1259. { "Boat", &opt_set_poi_boat, "POI for Boat Symbol as lat:lng[:alt]", NULL, ARGTYPE_STRING, "", "" },
  1260. { "Heart", &opt_set_poi_heart, "POI for Heart Symbol as lat:lng[:alt]", NULL, ARGTYPE_STRING, "", "" },
  1261. { "Bar", &opt_set_poi_bar, "POI for Bar Symbol as lat:lng[:alt]", NULL, ARGTYPE_STRING, "", "" },
  1262. ARG_TERMINATOR
  1263. };
  1264. /*
  1265. * Names of the POIs on miniHomer
  1266. */
  1267. static const char* poinames[] = {
  1268. "Home", "Car", "Boat", "Heart", "Bar"
  1269. };
  1270. #define NUMPOI (sizeof poinames/sizeof poinames[0])
  1271. int getPoiByName(char* name)
  1272. {
  1273. unsigned int i;
  1274. for (i=0; i<NUMPOI; i++) {
  1275. if (strcmp(poinames[i], name) == 0) {
  1276. return i;
  1277. }
  1278. }
  1279. return -1;
  1280. }
  1281. // Convert lla (lat, lng, alt) to ECEF
  1282. // Algorith taken from these sources:
  1283. // http://www.mathworks.com/matlabcentral/fileexchange/7942-covert-lat-lon-alt-to-ecef-cartesian
  1284. // http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_geodetic
  1285. // http://earth-info.nga.mil/GandG/publications/tr8350.2/wgs84fin.pdf
  1286. void lla2ecef(double lat, double lng, double alt, double* ecef_x, double* ecef_y, double* ecef_z)
  1287. {
  1288. long double n;
  1289. long double a = 6378137.0;
  1290. long double esqr = 6.69437999014e-3;
  1291. long double s;
  1292. long double llat, llng, lalt;
  1293. llat=lat*M_PI/180;
  1294. llng=lng*M_PI/180;
  1295. lalt=alt;
  1296. s=sin(llat);
  1297. n = a / sqrt(1 - esqr * s*s);
  1298. *ecef_x = (double)((n+lalt) * cos(llat) * cos(llng));
  1299. *ecef_y = (double)((n+lalt) * cos(llat) * sin(llng));
  1300. *ecef_z = (double)((n*(1-esqr) + lalt)* sin(llat));
  1301. }
  1302. static void miniHomer_get_poi()
  1303. {
  1304. uint8_t MSG_GET_POI[3] = { 0x4D, 0, 0};
  1305. uint8_t buf[32];
  1306. unsigned int poi;
  1307. double lat, lng, alt;
  1308. double ecef_x, ecef_y, ecef_z;
  1309. Waypoint* wpt;
  1310. for (poi=0; poi<NUMPOI; poi++) {
  1311. MSG_GET_POI[1]=(poi>>8)&0xff;
  1312. MSG_GET_POI[2]=(poi)&0xff;
  1313. if (skytraq_wr_msg_verify((uint8_t*)&MSG_GET_POI, sizeof(MSG_GET_POI)) != res_OK) {
  1314. warning(MYNAME ": cannot read poi %d '%s'\n", poi, poinames[poi]);
  1315. }
  1316. skytraq_rd_msg(buf, 25);
  1317. ecef_x=be_read_double(buf+1);
  1318. ecef_y=be_read_double(buf+9);
  1319. ecef_z=be_read_double(buf+17);
  1320. // todo - how to determine not-set POIs ?
  1321. if (ecef_x < 100.0 && ecef_y < 100.0 && ecef_z < 100.0) {
  1322. db(2, MYNAME" : skipped poi %d for X=%f, y=%f, Z=%f\n", ecef_x, ecef_y, ecef_z);
  1323. } else {
  1324. ECEF_to_LLA(ecef_x, ecef_y, ecef_z, &lat, &lng, &alt);
  1325. wpt = new Waypoint;
  1326. wpt->shortname = QString().sprintf("POI_%s", poinames[poi]);
  1327. wpt->description = QString().sprintf("miniHomer points to this coordinates if the %s symbol is on", poinames[poi]);
  1328. wpt->latitude = lat;
  1329. wpt->longitude = lng;
  1330. wpt->altitude = alt;
  1331. waypt_add(wpt);
  1332. db(1, MYNAME ": got POI[%s]='%f %f %f/%f %f %f'\n", poinames[poi], lat, lng, alt, ecef_x, ecef_y, ecef_z);
  1333. }
  1334. }
  1335. }
  1336. /*
  1337. * set lla (lat/lng/alt) specified as <lat>:<lng>[:<alt] for a given poi [0..4] in miniHomer
  1338. * returns
  1339. * 1 if poi was set
  1340. * 0 if opt_poi was not set
  1341. * -1 in case of errors
  1342. * the number of the POI will not be checked - if it is not correct, miniHome will send NACK
  1343. */
  1344. static int miniHomer_set_poi(uint16_t poinum, const char* opt_poi)
  1345. {
  1346. #define MSG_SET_POI_SIZE (sizeof(uint8_t)+sizeof(uint16_t)+3*sizeof(double)+sizeof(uint8_t))
  1347. uint8_t MSG_SET_POI[MSG_SET_POI_SIZE] = {
  1348. 0x4C, 0, 0, // cmd + poi (u16)
  1349. 0, 0, 0, 0, 0, 0, 0, 0, //lat (double ecef)
  1350. 0, 0, 0, 0, 0, 0, 0, 0, //lng (double ecef)
  1351. 0, 0, 0, 0, 0, 0, 0, 0, //alt (double ecef)
  1352. 0 // attr (u8, 1-> to flash, 0->ro sram)
  1353. };
  1354. int n, result;
  1355. double lat, lng, alt;
  1356. double ecef_x, ecef_y, ecef_z;
  1357. result=0; // result will be 0 if opt_poi isn't set
  1358. if (opt_poi) { // first check opt_poi
  1359. if (*opt_poi) {
  1360. lat=lng=alt=0.0;
  1361. /*
  1362. * parse format of <lat>:<lng>[:alt]
  1363. * we assume at least two elements in the value string
  1364. */
  1365. n = sscanf(opt_poi, "%lf:%lf:%lf", &lat, &lng, &alt);
  1366. if (n >= 2) {
  1367. db(3, "found %d elems '%s':poi=%s@%d, lat=%f, lng=%f, alt=%f over=%s\n", n, opt_poi, poinames[poinum], poinum, lat, lng, alt);
  1368. lla2ecef(lat, lng, alt, &ecef_x, &ecef_y, &ecef_z);
  1369. db(1, MYNAME ": set POI[%s]='%f %f %f/%f %f %f'\n", poinames[poinum], lat, lng, alt, ecef_x, ecef_y, ecef_z);
  1370. be_write16(MSG_SET_POI+1, poinum);
  1371. be_write_double(MSG_SET_POI+3, ecef_x);
  1372. be_write_double(MSG_SET_POI+11, ecef_y);
  1373. be_write_double(MSG_SET_POI+19, ecef_z);
  1374. MSG_SET_POI[27]=0;
  1375. if (skytraq_wr_msg_verify((uint8_t*)&MSG_SET_POI, sizeof(MSG_SET_POI)) == res_OK) {
  1376. result=1;
  1377. } else {
  1378. warning(MYNAME ": cannot set poi %d '%s'\n", poinum, poinames[poinum]);
  1379. result=-1;
  1380. }
  1381. } else {
  1382. warning(MYNAME ": argument to %s needs to be like <lat>:<lng>[:<alt>]\n", poinames[poinum]);
  1383. result=-1;
  1384. }
  1385. }
  1386. }
  1387. return result;
  1388. }
  1389. static QString mhport;
  1390. static void
  1391. miniHomer_rd_init(const QString& fname)
  1392. {
  1393. opt_set_location=NULL; // otherwise it will lead to bus error
  1394. skytraq_rd_init(fname); // sets global var serial_handle
  1395. mhport=fname;
  1396. }
  1397. static void
  1398. miniHomer_rd_deinit(void)
  1399. {
  1400. skytraq_rd_deinit();
  1401. mhport.clear();
  1402. }
  1403. #define SETPOI(poinum, poiname) if (opt_set_poi_##poiname ) {miniHomer_set_poi(poinum, opt_set_poi_##poiname);}
  1404. static void
  1405. miniHomer_read(void)
  1406. {
  1407. int npoi=0;
  1408. /*
  1409. * read tracks and POI from miniHomer
  1410. */
  1411. if (miniHomer_set_poi(0, opt_set_poi_home) > 0) {
  1412. npoi++;
  1413. }
  1414. if (miniHomer_set_poi(1, opt_set_poi_car) > 0) {
  1415. npoi++;
  1416. }
  1417. if (miniHomer_set_poi(2, opt_set_poi_boat) > 0) {
  1418. npoi++;
  1419. }
  1420. if (miniHomer_set_poi(3, opt_set_poi_heart) > 0) {
  1421. npoi++;
  1422. }
  1423. if (miniHomer_set_poi(4, opt_set_poi_bar) > 0) {
  1424. npoi++;
  1425. }
  1426. if (npoi == 0) { // do not read if POIs are set (consider set & read distinct operations)
  1427. skytraq_read(); // first read tracks (if not supressed by cmd line params)
  1428. // we need this call it initialized waypoint list etc...
  1429. skytraq_rd_deinit(); // skytraq_read called system_reset, which changes the baud rate.
  1430. skytraq_rd_init(mhport); // Lets start from scratch and re-init the port
  1431. miniHomer_get_poi(); // add POI as waypoints to the waypoints of the track
  1432. }
  1433. }
  1434. ff_vecs_t miniHomer_vecs = {
  1435. ff_type_serial,
  1436. {
  1437. ff_cap_read /* waypoints */,
  1438. ff_cap_read /* tracks */,
  1439. ff_cap_none /* routes */
  1440. },
  1441. miniHomer_rd_init,
  1442. NULL,
  1443. miniHomer_rd_deinit,
  1444. NULL,
  1445. miniHomer_read,
  1446. NULL,
  1447. NULL,
  1448. miniHomer_args,
  1449. CET_CHARSET_UTF8, 1 /* master process: don't convert anything */
  1450. };