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.

navilink.cc 30KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250
  1. /*
  2. NaviGPS serial protocol.
  3. Copyright (C) 2007 Tom Hughes, tom@compton.nu
  4. Copyright (C) 2008 Rodney Lorrimar, rodney@rodney.id.au
  5. This program is free software; you can redistribute it and/or modify
  6. it under the terms of the GNU General Public License as published by
  7. the Free Software Foundation; either version 2 of the License, or
  8. (at your option) any later version.
  9. This program is distributed in the hope that it will be useful,
  10. but WITHOUT ANY WARRANTY; without even the implied warranty of
  11. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12. GNU General Public License for more details.
  13. You should have received a copy of the GNU General Public License
  14. along with this program; if not, write to the Free Software
  15. Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA
  16. */
  17. /* Based on description at http://wiki.splitbrain.org/navilink */
  18. #include "defs.h"
  19. #include "gbser.h"
  20. #include "jeeps/gpsmath.h"
  21. #include "navilink.h"
  22. #define MYNAME "NAVILINK"
  23. static char* nuketrk = NULL;
  24. static char* nukerte = NULL;
  25. static char* nukewpt = NULL;
  26. static char* nukedlg = NULL;
  27. static char* poweroff = NULL;
  28. static char* datalog = NULL;
  29. static void* serial_handle = NULL;
  30. static gbfile* file_handle = NULL;
  31. static unsigned char* track_data;
  32. static unsigned char* track_data_ptr;
  33. static unsigned char* track_data_end;
  34. static unsigned track_serial;
  35. static Waypoint** route_waypts;
  36. static unsigned* route_ids;
  37. static unsigned route_id_ptr;
  38. static enum {
  39. READING,
  40. WRITING
  41. } operation = READING;
  42. #define SERIAL_TIMEOUT 8000
  43. #define CLEAR_DATALOG_TIME 7000
  44. #define MAX_WAYPOINTS 1000
  45. #define MAX_SUBROUTES 9
  46. #define MAX_SUBROUTE_LENGTH 14
  47. #define MAX_ROUTE_LENGTH (MAX_SUBROUTES * MAX_SUBROUTE_LENGTH - 1)
  48. #define MAX_READ_TRACKPOINTS 512
  49. #define MAX_WRITE_TRACKPOINTS 127
  50. #define MAX_READ_LOGPOINTS 256
  51. #define PID_SYNC 0xd6
  52. #define PID_ACK 0x0c
  53. #define PID_NAK 0x00
  54. #define PID_QRY_INFORMATION 0x20
  55. #define PID_QRY_FW_VERSION 0xfe
  56. #define PID_DATA 0x03
  57. #define PID_ADD_A_WAYPOINT 0x3c
  58. #define PID_QRY_WAYPOINTS 0x28
  59. #define PID_QRY_ROUTE 0x24
  60. #define PID_DEL_WAYPOINT 0x36
  61. #define PID_DEL_ALL_WAYPOINT 0x37
  62. #define PID_DEL_ROUTE 0x34
  63. #define PID_DEL_ALL_ROUTE 0x35
  64. #define PID_ADD_A_ROUTE 0x3d
  65. #define PID_ERASE_TRACK 0x11
  66. #define PID_READ_TRACKPOINTS 0x14
  67. #define PID_WRITE_TRACKPOINTS 0x16
  68. #define PID_CMD_OK 0xf3
  69. #define PID_CMD_FAIL 0xf4
  70. #define PID_QUIT 0xf2
  71. #define PID_INFO_DATALOG 0x1c
  72. #define PID_READ_DATALOG 0x14
  73. #define PID_CLEAR_DATALOG 0x1b
  74. static
  75. const char* const icon_table[] = {
  76. "Star",
  77. "Flag",
  78. "House",
  79. "Left Sign",
  80. "Telegraph Pole",
  81. "People",
  82. "Fuel",
  83. "Phone",
  84. "Pole",
  85. "Mountain",
  86. "Water",
  87. "Tree",
  88. "Road Narrows",
  89. "Crossroads",
  90. "Road Fork",
  91. "Turn Right",
  92. "Turn Left",
  93. "Bird",
  94. "3D House",
  95. "Trig Point",
  96. "Tower",
  97. "Cable Car",
  98. "Church",
  99. "Telegraph Pole",
  100. "Skier",
  101. "Anchor",
  102. "Fish",
  103. "Fishes",
  104. "Rain",
  105. "Fisherman",
  106. "Tower",
  107. "Boats",
  108. "Boat",
  109. "Bicycle",
  110. "Railway Track",
  111. "Dollar Sign",
  112. "Bus",
  113. "Camera",
  114. "Fuel Pump",
  115. "Cup",
  116. "Merging Road",
  117. "Plane",
  118. "Red Cross",
  119. "House",
  120. "Parking"
  121. };
  122. static
  123. arglist_t navilink_args[] = {
  124. {
  125. "nuketrk", &nuketrk, "Delete all track points", NULL, ARGTYPE_BOOL,
  126. ARG_NOMINMAX
  127. },
  128. {
  129. "nukerte", &nukerte, "Delete all routes", NULL, ARGTYPE_BOOL,
  130. ARG_NOMINMAX
  131. },
  132. {
  133. "nukewpt", &nukewpt, "Delete all waypoints", NULL, ARGTYPE_BOOL,
  134. ARG_NOMINMAX
  135. },
  136. {
  137. "nukedlg", &nukedlg, "Clear the datalog", NULL, ARGTYPE_BOOL,
  138. ARG_NOMINMAX
  139. },
  140. {
  141. "datalog", &datalog, "Read from datalogger buffer",
  142. NULL, ARGTYPE_BOOL, ARG_NOMINMAX
  143. },
  144. {
  145. "power_off", &poweroff, "Command unit to power itself down",
  146. NULL, ARGTYPE_BOOL, ARG_NOMINMAX
  147. },
  148. ARG_TERMINATOR
  149. };
  150. static void (*write_waypoint)(const Waypoint*) = NULL;
  151. static void (*write_track_start)(const route_head* track) = NULL;
  152. static void (*write_track_point)(const Waypoint* waypt) = NULL;
  153. static void (*write_track_end)(const route_head* track) = NULL;
  154. static void (*write_route_start)(const route_head* track) = NULL;
  155. static void (*write_route_point)(const Waypoint* waypt) = NULL;
  156. static void (*write_route_end)(const route_head* track) = NULL;
  157. static int
  158. find_icon_from_descr(const QString& descr)
  159. {
  160. unsigned int i;
  161. for (i = 0; i < sizeof(icon_table) / sizeof(const char*); i++) {
  162. if (0 == descr.compare(icon_table[i])) {
  163. return i;
  164. }
  165. }
  166. return 0;
  167. }
  168. static void
  169. free_waypoints(Waypoint** waypts)
  170. {
  171. Waypoint** wayptp;
  172. for (wayptp = waypts; wayptp < waypts + MAX_WAYPOINTS; wayptp++) {
  173. if (*wayptp) {
  174. delete *wayptp;
  175. }
  176. }
  177. xfree(waypts);
  178. }
  179. static unsigned
  180. compare_waypoints(const Waypoint* waypt1, const Waypoint* waypt2)
  181. {
  182. return waypt1->latitude == waypt2->latitude &&
  183. waypt1->longitude == waypt2->longitude &&
  184. waypt1->altitude == waypt2->altitude &&
  185. waypt1->shortname == waypt2->shortname;
  186. }
  187. unsigned
  188. navilink_checksum_packet(const unsigned char* packet, unsigned length)
  189. {
  190. unsigned checksum = 0;
  191. while (length-- > 0) {
  192. checksum += *packet++;
  193. }
  194. return checksum & 0x7fff;
  195. }
  196. #ifdef NAVILINK_DEBUG
  197. static void
  198. dump_packet(char* prefix, unsigned char* packet, unsigned length)
  199. {
  200. unsigned i;
  201. for (i = 0; i < length; i++) {
  202. if ((i % 16) == 0) {
  203. fprintf(stderr, "%s %08x :", prefix, i);
  204. }
  205. fprintf(stderr, " %02x", packet[i]);
  206. if ((i % 16) == 15 || i == length - 1) {
  207. fprintf(stderr, "\n");
  208. }
  209. }
  210. fprintf(stderr, "\n");
  211. }
  212. #endif
  213. static void
  214. write_packet(unsigned type, const void* payload, unsigned length)
  215. {
  216. unsigned char* packet = (unsigned char*) xmalloc(length + 9);
  217. packet[0] = 0xa0;
  218. packet[1] = 0xa2;
  219. le_write16(packet + 2, length + 1);
  220. packet[4] = type;
  221. memcpy(packet + 5, payload, length);
  222. le_write16(packet + length + 5, navilink_checksum_packet(packet + 4, length + 1));
  223. packet[length + 7] = 0xb0;
  224. packet[length + 8] = 0xb3;
  225. #ifdef NAVILINK_DEBUG
  226. dump_packet(">>>", packet + 4, length + 1);
  227. #endif
  228. if (gbser_write(serial_handle, packet, length + 9) != gbser_OK) {
  229. fatal(MYNAME ": Write error\n");
  230. }
  231. xfree(packet);
  232. }
  233. static unsigned
  234. read_word(void)
  235. {
  236. unsigned char buffer[2];
  237. if (gbser_read_wait(serial_handle, buffer, 2, SERIAL_TIMEOUT) != 2) {
  238. fatal(MYNAME ": Read error\n");
  239. }
  240. return (buffer[1] << 8) | buffer[0];
  241. }
  242. /*
  243. * Read a protocol packet into payload.
  244. *
  245. * handle_nak determines behaviour when a PID_NAK packet is read from
  246. * the device:
  247. * - if handle_nak is FALSE, a fatal error will be raised.
  248. * - if handle_nak is TRUE, read_packet will simply return FALSE.
  249. *
  250. * Returns TRUE if the packet was successfully read into payload.
  251. */
  252. static int
  253. read_packet(unsigned type, void* payload,
  254. unsigned minlength, unsigned maxlength,
  255. int handle_nak)
  256. {
  257. unsigned size;
  258. unsigned char* data;
  259. unsigned checksum;
  260. if (read_word() != 0xa2a0) {
  261. fatal(MYNAME ": Protocol error: Bad packet header."
  262. " Is your NaviGPS in NAVILINK mode?\n");
  263. }
  264. if ((size = read_word()) <= minlength) {
  265. fatal(MYNAME ": Protocol error: Packet too short\n");
  266. }
  267. data = (unsigned char*) xmalloc(size);
  268. if (gbser_read_wait(serial_handle, data, size, SERIAL_TIMEOUT) != size) {
  269. fatal(MYNAME ": Read error reading %d byte payload\n", size);
  270. }
  271. #ifdef NAVILINK_DEBUG
  272. dump_packet("<<<", data, size);
  273. #endif
  274. if (data[0] != type) {
  275. if (handle_nak && data[0] == PID_NAK) {
  276. return FALSE;
  277. }
  278. fatal(MYNAME ": Protocol error: Bad packet type (expected 0x%02x but got 0x%02x)\n", type, data[0]);
  279. }
  280. if ((checksum = read_word()) != navilink_checksum_packet(data, size)) {
  281. fatal(MYNAME ": Checksum error - expected %x got %x\n",
  282. navilink_checksum_packet(data, size), checksum);
  283. }
  284. if (read_word() != 0xb3b0) {
  285. fatal(MYNAME ": Protocol error: Bad packet trailer\n");
  286. }
  287. if (size - 1 > maxlength) {
  288. memcpy(payload, data + 1, maxlength);
  289. } else {
  290. memcpy(payload, data + 1, size - 1);
  291. }
  292. xfree(data);
  293. return TRUE;
  294. }
  295. static QDateTime
  296. decode_datetime(const unsigned char* buffer)
  297. {
  298. QTime tm(buffer[3], buffer[4], buffer[5]);
  299. QDate dt(2000 + buffer[0], buffer[1], buffer[2]);
  300. return QDateTime(dt, tm, Qt::UTC);
  301. }
  302. static void
  303. encode_datetime(time_t datetime, unsigned char* buffer)
  304. {
  305. struct tm* tm;
  306. if ((tm = gmtime(&datetime)) != NULL) {
  307. buffer[0] = tm->tm_year - 100;
  308. buffer[1] = tm->tm_mon + 1;
  309. buffer[2] = tm->tm_mday;
  310. buffer[3] = tm->tm_hour;
  311. buffer[4] = tm->tm_min;
  312. buffer[5] = tm->tm_sec;
  313. } else {
  314. memset(buffer, 0, 6);
  315. }
  316. }
  317. static void
  318. decode_position(const unsigned char* buffer, Waypoint* waypt)
  319. {
  320. waypt->latitude = le_read32(buffer + 0) / 10000000.0;
  321. waypt->longitude = le_read32(buffer + 4) / 10000000.0;
  322. waypt->altitude = FEET_TO_METERS(le_read16(buffer + 8));
  323. }
  324. static void
  325. encode_position(const Waypoint* waypt, unsigned char* buffer)
  326. {
  327. le_write32(buffer + 0, (int)(waypt->latitude * 10000000));
  328. le_write32(buffer + 4, (int)(waypt->longitude * 10000000));
  329. le_write16(buffer + 8, METERS_TO_FEET(waypt->altitude));
  330. }
  331. static unsigned
  332. decode_waypoint_id(const unsigned char* buffer)
  333. {
  334. unsigned id = le_read16(buffer + 2);
  335. if (id >= MAX_WAYPOINTS) {
  336. fatal(MYNAME ": Invalid waypoint ID\n");
  337. }
  338. return id;
  339. }
  340. static Waypoint*
  341. decode_waypoint(const unsigned char* buffer)
  342. {
  343. Waypoint* waypt = new Waypoint;
  344. decode_position(buffer + 12, waypt);
  345. char* s = xstrdup((char*)buffer + 4);
  346. waypt->shortname = s;
  347. xfree(s);
  348. waypt->icon_descr = icon_table[buffer[28]];
  349. waypt->SetCreationTime(decode_datetime(buffer + 22));
  350. return waypt;
  351. }
  352. static void
  353. encode_waypoint(const Waypoint* waypt, unsigned char* buffer)
  354. {
  355. buffer[0] = 0x00;
  356. buffer[1] = 0x40;
  357. le_write16(buffer + 2, 0);
  358. strncpy((char*)buffer + 4, CSTRc(waypt->shortname), 6);
  359. buffer[10] = 0;
  360. buffer[11] = 0;
  361. encode_position(waypt, buffer + 12);
  362. encode_datetime(waypt->GetCreationTime().toTime_t(), buffer + 22);
  363. buffer[28] = find_icon_from_descr(waypt->icon_descr);
  364. buffer[29] = 0;
  365. buffer[30] = 0x00;
  366. buffer[31] = 0x7e;
  367. }
  368. static Waypoint*
  369. decode_trackpoint(const unsigned char* buffer)
  370. {
  371. Waypoint* waypt = new Waypoint;
  372. decode_position(buffer + 12, waypt);
  373. waypt->SetCreationTime(decode_datetime(buffer + 22));
  374. WAYPT_SET(waypt, course, le_read16(buffer + 2));
  375. WAYPT_SET(waypt, speed, KPH_TO_MPS(buffer[29] * 2));
  376. return waypt;
  377. }
  378. static void
  379. encode_trackpoint(const Waypoint* waypt, unsigned serial, unsigned char* buffer)
  380. {
  381. double x;
  382. double y;
  383. int32 z;
  384. char zc;
  385. GPS_Math_WGS84_To_UTM_EN(waypt->latitude, waypt->longitude, &x, &y, &z, &zc);
  386. le_write16(buffer + 0, serial);
  387. le_write16(buffer + 2, WAYPT_GET(waypt, course, 0));
  388. le_write32(buffer + 4, x);
  389. le_write32(buffer + 8, y);
  390. encode_position(waypt, buffer + 12);
  391. encode_datetime(waypt->GetCreationTime().toTime_t(), buffer + 22);
  392. buffer[28] = z;
  393. buffer[29] = MPS_TO_KPH(WAYPT_GET(waypt, speed, 0) / 2);
  394. buffer[30] = 0x5a;
  395. buffer[31] = 0x7e;
  396. }
  397. static Waypoint**
  398. serial_read_waypoints(void)
  399. {
  400. Waypoint** waypts = NULL;
  401. unsigned char information[32];
  402. unsigned short total;
  403. unsigned short start;
  404. if (global_opts.masked_objective & RTEDATAMASK) {
  405. waypts = (Waypoint**) xcalloc(MAX_WAYPOINTS, sizeof(Waypoint*));
  406. }
  407. write_packet(PID_QRY_INFORMATION, NULL, 0);
  408. read_packet(PID_DATA, information,
  409. sizeof(information), sizeof(information),
  410. FALSE);
  411. total = le_read16(information + 0);
  412. for (start = 0; start < total; start += 32) {
  413. unsigned short count = total - start;
  414. unsigned char payload[7];
  415. unsigned char* waypoints;
  416. unsigned char* w;
  417. if (count > 32) {
  418. count = 32;
  419. }
  420. le_write32(payload + 0, start);
  421. le_write16(payload + 4, count);
  422. payload[6] = 1;
  423. write_packet(PID_QRY_WAYPOINTS, payload, sizeof(payload));
  424. waypoints = (unsigned char*) xmalloc(count * 32);
  425. read_packet(PID_DATA, waypoints, count * 32, count * 32, FALSE);
  426. for (w = waypoints; w < waypoints + count * 32; w = w + 32) {
  427. if (global_opts.masked_objective & WPTDATAMASK) {
  428. waypt_add(decode_waypoint(w));
  429. }
  430. if (global_opts.masked_objective & RTEDATAMASK) {
  431. waypts[decode_waypoint_id(w)] = decode_waypoint(w);
  432. }
  433. }
  434. xfree(waypoints);
  435. if (global_opts.verbose_status) {
  436. waypt_status_disp(total, start + count);
  437. }
  438. }
  439. return waypts;
  440. }
  441. static unsigned int
  442. serial_write_waypoint_packet(const Waypoint* waypt)
  443. {
  444. unsigned char data[32];
  445. unsigned char id[2];
  446. encode_waypoint(waypt, data);
  447. write_packet(PID_ADD_A_WAYPOINT, data, sizeof(data));
  448. if (!read_packet(PID_DATA, id, sizeof(id), sizeof(id), TRUE)) {
  449. fatal(MYNAME ": Could not write waypoint.\n");
  450. }
  451. return le_read16(id);
  452. }
  453. static void
  454. serial_write_waypoint(const Waypoint* waypt)
  455. {
  456. serial_write_waypoint_packet(waypt);
  457. }
  458. static void
  459. serial_read_track(void)
  460. {
  461. unsigned char information[32];
  462. unsigned int address;
  463. unsigned short total;
  464. route_head* track;
  465. write_packet(PID_QRY_INFORMATION, NULL, 0);
  466. read_packet(PID_DATA, information,
  467. sizeof(information), sizeof(information),
  468. FALSE);
  469. address = le_read32(information + 4);
  470. total = le_read16(information + 12);
  471. track = route_head_alloc();
  472. track_add_head(track);
  473. while (total > 0) {
  474. unsigned short count = total < MAX_READ_TRACKPOINTS ? total : MAX_READ_TRACKPOINTS;
  475. unsigned char payload[7];
  476. unsigned char* trackpoints;
  477. unsigned char* t;
  478. le_write32(payload + 0, address);
  479. le_write16(payload + 4, count * 32);
  480. payload[6] = 0x00;
  481. write_packet(PID_READ_TRACKPOINTS, payload, sizeof(payload));
  482. trackpoints = (unsigned char*) xmalloc(count * 32);
  483. read_packet(PID_DATA, trackpoints, count * 32, count * 32, FALSE);
  484. write_packet(PID_ACK, NULL, 0);
  485. for (t = trackpoints; t < trackpoints + count * 32; t = t + 32) {
  486. track_add_wpt(track, decode_trackpoint(t));
  487. }
  488. xfree(trackpoints);
  489. address = address + count * 32;
  490. total = total - count;
  491. }
  492. }
  493. static void
  494. serial_write_track(void)
  495. {
  496. unsigned char information[32];
  497. unsigned int address;
  498. unsigned short total;
  499. unsigned char data[7];
  500. write_packet(PID_QRY_INFORMATION, NULL, 0);
  501. read_packet(PID_DATA, information,
  502. sizeof(information), sizeof(information),
  503. FALSE);
  504. address = le_read32(information + 4);
  505. total = le_read16(information + 12);
  506. le_write32(data + 0, address + total * 32);
  507. le_write16(data + 4, track_data_ptr - track_data);
  508. data[6] = 0x00;
  509. write_packet(PID_WRITE_TRACKPOINTS, data, sizeof(data));
  510. gb_sleep(10000);
  511. write_packet(PID_DATA, track_data, track_data_ptr - track_data);
  512. read_packet(PID_CMD_OK, NULL, 0, 0, FALSE);
  513. track_data_ptr = track_data;
  514. }
  515. static void
  516. serial_write_track_start(const route_head* track)
  517. {
  518. track_data = (unsigned char*) xmalloc(MAX_WRITE_TRACKPOINTS * 32);
  519. track_data_ptr = track_data;
  520. track_data_end = track_data + MAX_WRITE_TRACKPOINTS * 32;
  521. }
  522. static void
  523. serial_write_track_point(const Waypoint* waypt)
  524. {
  525. if (track_data_ptr >= track_data_end) {
  526. serial_write_track();
  527. }
  528. encode_trackpoint(waypt, 0, track_data_ptr);
  529. track_data_ptr += 32;
  530. }
  531. static void
  532. serial_write_track_end(const route_head* track)
  533. {
  534. if (track_data_ptr > track_data) {
  535. serial_write_track();
  536. }
  537. xfree(track_data);
  538. }
  539. static void
  540. serial_read_routes(Waypoint** waypts)
  541. {
  542. unsigned char information[32];
  543. unsigned char routec;
  544. unsigned char r;
  545. write_packet(PID_QRY_INFORMATION, NULL, 0);
  546. read_packet(PID_DATA, information,
  547. sizeof(information), sizeof(information),
  548. FALSE);
  549. routec = information[2];
  550. for (r = 0; r < routec; r++) {
  551. unsigned char payload[7];
  552. unsigned char routedata[320];
  553. route_head* route;
  554. int sr;
  555. le_write32(payload + 0, r);
  556. le_write16(payload + 2, 0);
  557. payload[6] = 0x01;
  558. write_packet(PID_QRY_ROUTE, payload, sizeof(payload));
  559. read_packet(PID_DATA, routedata, 64, sizeof(routedata), FALSE);
  560. route = route_head_alloc();
  561. route->rte_num = routedata[2];
  562. route->rte_name = xstrdup((char*)routedata + 4);
  563. route_add_head(route);
  564. for (sr = 0; sr < MAX_SUBROUTES; sr++) {
  565. int w;
  566. for (w = 0; w < MAX_SUBROUTE_LENGTH; w++) {
  567. unsigned short id = le_read16(routedata + 34 + 32 * sr + 2 *w);
  568. if (id == 0xffffu) {
  569. w = MAX_SUBROUTE_LENGTH;
  570. sr = MAX_SUBROUTES;
  571. } else if (id >= MAX_WAYPOINTS) {
  572. fatal(MYNAME ": Invalid waypoint ID in route\n");
  573. } else if (waypts[id] == NULL) {
  574. fatal(MYNAME ": Non-existent waypoint in route\n");
  575. } else {
  576. route_add_wpt(route, new Waypoint(*waypts[id]));
  577. }
  578. }
  579. }
  580. }
  581. }
  582. static void
  583. serial_write_route_start(const route_head* route)
  584. {
  585. route_ids = (unsigned int*) xmalloc(route->rte_waypt_ct * sizeof(unsigned));
  586. route_id_ptr = 0;
  587. }
  588. static void
  589. serial_write_route_point(const Waypoint* waypt)
  590. {
  591. unsigned w;
  592. for (w = 0; w < MAX_WAYPOINTS; w++) {
  593. if (route_waypts[w] && compare_waypoints(waypt, route_waypts[w])) {
  594. break;
  595. }
  596. }
  597. if (w == MAX_WAYPOINTS) {
  598. w = serial_write_waypoint_packet(waypt);
  599. route_waypts[w] = new Waypoint(*waypt);
  600. }
  601. route_ids[route_id_ptr++] = w;
  602. }
  603. static void
  604. serial_write_route_end(const route_head* route)
  605. {
  606. unsigned char* data;
  607. unsigned src;
  608. unsigned sr;
  609. unsigned char id[1];
  610. QString rte_name;
  611. rte_name = route->rte_name;
  612. if (rte_name == NULL) {
  613. rte_name = "NO NAME";
  614. }
  615. if (route_id_ptr > MAX_ROUTE_LENGTH) {
  616. fatal(MYNAME ": Route %s too long\n", qPrintable(route->rte_name));
  617. }
  618. src = (route_id_ptr + MAX_SUBROUTE_LENGTH) / MAX_SUBROUTE_LENGTH;
  619. data = (unsigned char*) xmalloc(32 + src * 32);
  620. le_write16(data + 0, 0x2000);
  621. data[2] = 0;
  622. data[3] = 0x20;
  623. memset(data + 4, 0, 14);
  624. strncpy((char*)data + 4, CSTR(rte_name), 13);
  625. data[18] = 0;
  626. data[19] = 0;
  627. le_write32(data + 20, 0);
  628. le_write32(data + 24, 0);
  629. le_write16(data + 28, 0);
  630. data[30] = 0x7b;
  631. data[31] = 0x77;
  632. for (sr = 0; sr < src; sr++) {
  633. unsigned char* srdata = data + 32 + 32 * sr;
  634. unsigned pt_offset = MAX_SUBROUTE_LENGTH * sr;
  635. unsigned pt;
  636. le_write16(srdata + 0, 0x2010);
  637. for (pt = 0; pt < MAX_SUBROUTE_LENGTH; pt++) {
  638. if (pt_offset + pt < route_id_ptr) {
  639. le_write16(srdata + 2 + 2 * pt, route_ids[pt_offset + pt]);
  640. } else {
  641. le_write16(srdata + 2 + 2 * pt, 0xffffu);
  642. }
  643. }
  644. srdata[30] = 0x7f;
  645. srdata[31] = 0x77;
  646. }
  647. write_packet(PID_ADD_A_ROUTE, data, 32 + src * 32);
  648. if (!read_packet(PID_DATA, id, sizeof(id), sizeof(id), TRUE)) {
  649. fatal(MYNAME ": Could not add route.\n");
  650. }
  651. xfree(data);
  652. xfree(route_ids);
  653. }
  654. static int
  655. decode_sbp_msec(const unsigned char* buffer)
  656. {
  657. int msec = le_read16(buffer);
  658. return (msec % 1000);
  659. }
  660. static time_t
  661. decode_sbp_datetime_packed(const unsigned char* buffer)
  662. {
  663. /*
  664. * Packed_Date_Time_UTC:
  665. * bit 31..22 :year*12+month (10 bits) : real year= year+2000
  666. * bit17.21: day (5bits)
  667. * bit12.16: hour (5bits)
  668. * bit6..11: min (6bits)
  669. * bit0..5 : sec (6bits)
  670. *
  671. * 0 1 2 3
  672. * 01234567 01234567 01234567 01234567
  673. * ........ ........ ........ ........
  674. * SSSSSSMM MMMMHHHH Hdddddmm mmmmmmmm
  675. */
  676. int months;
  677. struct tm tm;
  678. memset(&tm, 0, sizeof(tm));
  679. tm.tm_sec = buffer[0] & 0x3F;
  680. tm.tm_min = ((buffer[0] & 0xC0) >> 6) | ((buffer[1] & 0x0F) << 2);
  681. tm.tm_hour = ((buffer[1] & 0xF0) >> 4) | ((buffer[2] & 0x01) << 4);
  682. tm.tm_mday = (buffer[2] & 0x3E) >> 1;
  683. months = ((buffer[2] & 0xC0) >> 6) | buffer[3] << 2;
  684. tm.tm_mon = months % 12 - 1;
  685. tm.tm_year = 100 + months / 12;
  686. return mkgmtime(&tm);
  687. }
  688. static void
  689. decode_sbp_position(const unsigned char* buffer, Waypoint* waypt)
  690. {
  691. waypt->latitude = le_read32(buffer + 0) / 10000000.0;
  692. waypt->longitude = le_read32(buffer + 4) / 10000000.0;
  693. waypt->altitude = le_read32(buffer + 8) / 100.0;
  694. }
  695. Waypoint*
  696. navilink_decode_logpoint(const unsigned char* buffer)
  697. {
  698. Waypoint* waypt = NULL;
  699. waypt = new Waypoint;
  700. waypt->hdop = ((unsigned char)buffer[0]) * 0.2f;
  701. waypt->sat = buffer[1];
  702. waypt->SetCreationTime(decode_sbp_datetime_packed(buffer + 4),
  703. decode_sbp_msec(buffer + 2));
  704. decode_sbp_position(buffer + 12, waypt);
  705. WAYPT_SET(waypt, speed, le_read16(buffer + 24) * 0.01f);
  706. WAYPT_SET(waypt, course, le_read16(buffer + 26) * 0.01f);
  707. return waypt;
  708. }
  709. /*
  710. * The datalog is a circular buffer, so it may be necessary to glue
  711. * together two segments. This function queries the device for the
  712. * circular buffer pointers, and returns two pairs of address/length.
  713. * If there is only one segment (i.e. the datalog has not yet wrapped
  714. * around), then seg2_addr and seg2_len will be zero.
  715. */
  716. static void
  717. read_datalog_info(unsigned int* seg1_addr, unsigned int* seg1_len,
  718. unsigned int* seg2_addr, unsigned int* seg2_len)
  719. {
  720. unsigned char info[16];
  721. unsigned int flash_start_addr;
  722. unsigned int flash_length;
  723. unsigned int data_start_addr;
  724. unsigned int next_blank_addr;
  725. write_packet(PID_INFO_DATALOG, NULL, 0);
  726. read_packet(PID_DATA, info, sizeof(info), sizeof(info), FALSE);
  727. flash_start_addr = le_read32(info);
  728. flash_length = le_read32(info + 4);
  729. data_start_addr = le_read32(info + 8);
  730. next_blank_addr = le_read32(info + 12);
  731. if (data_start_addr > next_blank_addr) {
  732. /* usually there are two segments to be read */
  733. *seg1_addr = data_start_addr;
  734. *seg1_len = flash_start_addr + flash_length - *seg1_addr;
  735. *seg2_addr = flash_start_addr;
  736. *seg2_len = next_blank_addr - flash_start_addr;
  737. } else {
  738. /* hasn't wrapped around yet, only one segment */
  739. *seg1_addr = data_start_addr;
  740. *seg1_len = next_blank_addr - data_start_addr;
  741. *seg2_addr = 0;
  742. *seg2_len = 0;
  743. }
  744. if (*seg1_len & 0x1F || *seg2_len & 0x1F) {
  745. fatal(MYNAME ": Protocol error: datalog lengths %u, %u "
  746. "not aligned to 32 bytes\n", *seg1_len, *seg2_len);
  747. }
  748. }
  749. static void
  750. read_datalog_records(route_head* track,
  751. unsigned int start_addr, unsigned int len)
  752. {
  753. unsigned char logpoints[MAX_READ_LOGPOINTS * SBP_RECORD_LEN];
  754. unsigned int logpoints_len;
  755. unsigned char payload[7];
  756. unsigned char* p;
  757. /* The protocol only supports reading 256 logpoints at once, so
  758. * read small chunks until none left. */
  759. while (len > 0) {
  760. logpoints_len = len > MAX_READ_LOGPOINTS ? MAX_READ_LOGPOINTS : len;
  761. le_write32(payload, start_addr);
  762. le_write16(payload + 4, logpoints_len);
  763. payload[6] = 0x01;
  764. write_packet(PID_READ_DATALOG, payload, sizeof(payload));
  765. read_packet(PID_DATA, logpoints, logpoints_len, logpoints_len, FALSE);
  766. write_packet(PID_ACK, NULL, 0);
  767. for (p = logpoints; p < logpoints + logpoints_len; p += 32) {
  768. track_add_wpt(track, navilink_decode_logpoint(p));
  769. }
  770. len -= logpoints_len;
  771. start_addr += logpoints_len;
  772. }
  773. }
  774. static void
  775. serial_read_datalog(void)
  776. {
  777. route_head* track;
  778. unsigned int seg1_addr;
  779. unsigned int seg1_len;
  780. unsigned int seg2_addr;
  781. unsigned int seg2_len;
  782. read_datalog_info(&seg1_addr, &seg1_len, &seg2_addr, &seg2_len);
  783. track = route_head_alloc();
  784. track_add_head(track);
  785. if (seg1_len) {
  786. read_datalog_records(track, seg1_addr, seg1_len);
  787. }
  788. if (seg2_len) {
  789. read_datalog_records(track, seg2_addr, seg2_len);
  790. }
  791. }
  792. static void
  793. file_read(void)
  794. {
  795. unsigned char data[32];
  796. route_head* track = NULL;
  797. while (gbfread(data, sizeof(data), 1, file_handle) == 1) {
  798. switch (le_read16(data)) {
  799. case 0x2000:
  800. fatal(MYNAME ": Route objects not supported in file sources\n");
  801. break;
  802. case 0x2010:
  803. fatal(MYNAME ": Subroute objects not supported in file sources\n");
  804. break;
  805. case 0x4000:
  806. if (global_opts.masked_objective & WPTDATAMASK) {
  807. waypt_add(decode_waypoint(data));
  808. }
  809. break;
  810. default:
  811. if (global_opts.masked_objective & TRKDATAMASK) {
  812. if (track == NULL) {
  813. track = route_head_alloc();
  814. track_add_head(track);
  815. }
  816. track_add_wpt(track, decode_trackpoint(data));
  817. }
  818. break;
  819. }
  820. }
  821. }
  822. static void
  823. file_write_waypoint(const Waypoint* waypt)
  824. {
  825. unsigned char data[32];
  826. encode_waypoint(waypt, data);
  827. gbfwrite(data, sizeof(data), 1, file_handle);
  828. }
  829. static void
  830. file_write_track_start(const route_head* track)
  831. {
  832. track_serial = 1;
  833. }
  834. static void
  835. file_write_track_point(const Waypoint* waypt)
  836. {
  837. unsigned char data[32];
  838. encode_trackpoint(waypt, track_serial++, data);
  839. gbfwrite(data, sizeof(data), 1, file_handle);
  840. }
  841. static void
  842. file_write_track_end(const route_head* track)
  843. {
  844. }
  845. static void
  846. file_write_route_start(const route_head* track)
  847. {
  848. fatal(MYNAME ": Can't write routes to a file\n");
  849. }
  850. static void
  851. file_write_route_point(const Waypoint* waypt)
  852. {
  853. }
  854. static void
  855. file_write_route_end(const route_head* track)
  856. {
  857. }
  858. static void
  859. nuke(void)
  860. {
  861. if (nuketrk) {
  862. unsigned char information[32];
  863. unsigned char data[7];
  864. write_packet(PID_QRY_INFORMATION, NULL, 0);
  865. read_packet(PID_DATA, information,
  866. sizeof(information), sizeof(information),
  867. FALSE);
  868. le_write32(data + 0, le_read32(information + 4));
  869. le_write16(data + 4, 0);
  870. data[6] = 0;
  871. write_packet(PID_ERASE_TRACK, data, sizeof(data));
  872. read_packet(PID_CMD_OK, NULL, 0, 0, FALSE);
  873. }
  874. if (nukerte) {
  875. unsigned char data[4];
  876. le_write32(data, 0x00f00000);
  877. write_packet(PID_DEL_ALL_ROUTE, data, sizeof(data));
  878. if (!read_packet(PID_ACK, NULL, 0, 0, TRUE)) {
  879. fatal(MYNAME ": Could not nuke all routes.\n");
  880. }
  881. }
  882. if (nukewpt) {
  883. unsigned char data[4];
  884. le_write32(data, 0x00f00000);
  885. write_packet(PID_DEL_ALL_WAYPOINT, data, sizeof(data));
  886. if (!read_packet(PID_ACK, NULL, 0, 0, TRUE)) {
  887. fatal(MYNAME ": You must nuke all routes before nuking waypoints.\n");
  888. /* perhaps a better action would be to nuke routes for user.
  889. * i.e. set nukerte when nukewpt is set */
  890. }
  891. }
  892. if (nukedlg) {
  893. write_packet(PID_CLEAR_DATALOG, NULL, 0);
  894. /* The flash erase operation is time-consuming. Each sector (64KB)
  895. * takes around 1 second. The total sectors for SBP is 10.
  896. * So give the device some time to clear its datalog, in addition
  897. * to SERIAL_TIMEOUT, which applies to read_packet() */
  898. gb_sleep(CLEAR_DATALOG_TIME * 1000);
  899. read_packet(PID_ACK, NULL, 0, 0, FALSE);
  900. }
  901. }
  902. static void
  903. navilink_common_init(const QString& name)
  904. {
  905. if (gbser_is_serial(qPrintable(name))) {
  906. if ((serial_handle = gbser_init(qPrintable(name))) == NULL) {
  907. fatal(MYNAME ": Could not open serial port %s\n", qPrintable(name));
  908. }
  909. if (gbser_set_port(serial_handle, 115200, 8, 0, 1) != gbser_OK) {
  910. fatal(MYNAME ": Can't configure port\n");
  911. }
  912. write_packet(PID_SYNC, NULL, 0);
  913. read_packet(PID_ACK, NULL, 0, 0, FALSE);
  914. /* nuke data before writing */
  915. if (operation == WRITING) {
  916. nuke();
  917. }
  918. write_waypoint = serial_write_waypoint;
  919. write_track_start = serial_write_track_start;
  920. write_track_point = serial_write_track_point;
  921. write_track_end = serial_write_track_end;
  922. write_route_start = serial_write_route_start;
  923. write_route_point = serial_write_route_point;
  924. write_route_end = serial_write_route_end;
  925. } else {
  926. const char* mode = operation == READING ? "r" : "w+";
  927. file_handle = gbfopen(name, mode, MYNAME);
  928. write_waypoint = file_write_waypoint;
  929. write_track_start = file_write_track_start;
  930. write_track_point = file_write_track_point;
  931. write_track_end = file_write_track_end;
  932. write_route_start = file_write_route_start;
  933. write_route_point = file_write_route_point;
  934. write_route_end = file_write_route_end;
  935. }
  936. return;
  937. }
  938. static void
  939. navilink_rd_init(const QString& name)
  940. {
  941. operation = READING;
  942. navilink_common_init(name);
  943. }
  944. static void
  945. navilink_wr_init(const QString& name)
  946. {
  947. operation = WRITING;
  948. navilink_common_init(name);
  949. }
  950. static void
  951. navilink_deinit(void)
  952. {
  953. if (serial_handle) {
  954. /* nuke data after reading */
  955. if (operation == READING) {
  956. nuke();
  957. }
  958. if (poweroff) {
  959. write_packet(PID_QUIT, NULL, 0);
  960. }
  961. gbser_deinit(serial_handle);
  962. }
  963. if (file_handle) {
  964. gbfclose(file_handle);
  965. }
  966. return;
  967. }
  968. static void
  969. navilink_read(void)
  970. {
  971. if (datalog) {
  972. if (global_opts.masked_objective & TRKDATAMASK) {
  973. if (serial_handle) {
  974. serial_read_datalog();
  975. } else if (file_handle) {
  976. fatal(MYNAME ": Not supported. Use SBP format.\n");
  977. }
  978. }
  979. } else {
  980. if (serial_handle) {
  981. Waypoint** waypts = NULL;
  982. if (global_opts.masked_objective & (WPTDATAMASK|RTEDATAMASK)) {
  983. waypts = serial_read_waypoints();
  984. }
  985. if (global_opts.masked_objective & TRKDATAMASK) {
  986. serial_read_track();
  987. }
  988. if (global_opts.masked_objective & RTEDATAMASK) {
  989. serial_read_routes(waypts);
  990. }
  991. if (waypts) {
  992. free_waypoints(waypts);
  993. }
  994. } else if (file_handle) {
  995. file_read();
  996. }
  997. }
  998. }
  999. static void
  1000. navilink_write(void)
  1001. {
  1002. if (datalog) {
  1003. fatal(MYNAME ": Writing to datalog not supported.\n");
  1004. }
  1005. switch (global_opts.objective) {
  1006. case trkdata:
  1007. track_disp_all(write_track_start,
  1008. write_track_end,
  1009. write_track_point);
  1010. break;
  1011. case wptdata:
  1012. waypt_disp_all(write_waypoint);
  1013. break;
  1014. case rtedata:
  1015. if (serial_handle) {
  1016. route_waypts = serial_read_waypoints();
  1017. }
  1018. route_disp_all(write_route_start,
  1019. write_route_end,
  1020. write_route_point);
  1021. if (route_waypts) {
  1022. free_waypoints(route_waypts);
  1023. route_waypts = NULL;
  1024. }
  1025. break;
  1026. default:
  1027. fatal(MYNAME ": Unknown objective.\n");
  1028. }
  1029. }
  1030. ff_vecs_t navilink_vecs = {
  1031. ff_type_serial,
  1032. FF_CAP_RW_ALL,
  1033. navilink_rd_init,
  1034. navilink_wr_init,
  1035. navilink_deinit,
  1036. navilink_deinit,
  1037. navilink_read,
  1038. navilink_write,
  1039. NULL,
  1040. navilink_args,
  1041. CET_CHARSET_ASCII, 0 /* CET-REVIEW */
  1042. };