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.

garmin_xt.cc 12KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437
  1. /*
  2. Copyright (C) 2010 Eriks Zelenka, isindir@users.sourceforge.net
  3. Copyright (C) 2009 jekaeff,
  4. GMXT2GPX ( http://www.geocaching.hu/users.geo?id=9508 ; http://sites.google.com/site/jekaeff/eng-1 )
  5. The original code written in Pascal and does not include specific License, however on the project
  6. webpage it is said to be OpenSource/Libre software
  7. Copyright (C) 2005 Robert Lipe, robertlipe+source@gpsbabel.org
  8. This program is free software; you can redistribute it and/or modify
  9. it under the terms of the GNU General Public License as published by
  10. the Free Software Foundation; either version 2 of the License, or
  11. (at your option) any later version.
  12. This program is distributed in the hope that it will be useful,
  13. but WITHOUT ANY WARRANTY; without even the implied warranty of
  14. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  15. GNU General Public License for more details.
  16. You should have received a copy of the GNU General Public License
  17. along with this program; if not, write to the Free Software
  18. Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA
  19. */
  20. #include "defs.h"
  21. #include <stdlib.h>
  22. #define MYNAME "Garmin_XT"
  23. #define GARMIN_XT_ELE 31500/65536
  24. #define DATABLOCKSIZE 1
  25. #define STRK_BLOCK_SIZE 97
  26. static int colors[] = {
  27. 0x000000, // Black
  28. 0x00008b, // DarkRed
  29. 0x006400, // DarkGreen
  30. 0x00d7ff, // Gold
  31. 0x8b0000, // DarkBlue
  32. 0x8b008b, // DarkMagenta
  33. 0x8b8b00, // DarkCyan
  34. 0xd3d3d3, // LightGray
  35. 0xa9a9a9, // DarkGray
  36. 0x0000ff, // Red
  37. 0x00ff00, // Green
  38. 0x00ffff, // Yellow
  39. 0xff0000, // Blue
  40. 0xff00ff, // Magenta
  41. 0xffff00, // Cyan
  42. 0xffffff // White
  43. };
  44. static gbfile* fin;
  45. static route_head* track;
  46. static char* opt_xt_ftype = NULL;
  47. static char* opt_trk_header = NULL;
  48. static
  49. arglist_t format_garmin_xt_args[] = {
  50. {"ftype", &opt_xt_ftype, "Garmin Mobile XT ([ATRK]/STRK)", "ATRK", ARGTYPE_STRING | ARGTYPE_REQUIRED, ARG_NOMINMAX},
  51. // TODO: SHIFT - can't test behaviour, do not have appropriate files
  52. //{"trk_header_opt", &opt_trk_header, "Track name processing option ([0]-nrm/1-ign/2-sht)", "0", ARGTYPE_INT, ARG_NOMINMAX},
  53. {"trk_header", &opt_trk_header, "Track name processing option ([0]-nrm/1-ign)", "0", ARGTYPE_INT, ARG_NOMINMAX},
  54. ARG_TERMINATOR
  55. };
  56. /*******************************************************************************
  57. * %%% global callbacks called by gpsbabel main process %%% *
  58. *******************************************************************************/
  59. /*******************************************************************************
  60. * %%% Reader callbacks %%% *
  61. *******************************************************************************/
  62. static void
  63. format_garmin_xt_rd_init(const QString& fname)
  64. {
  65. fin = gbfopen(fname, "rb", MYNAME);
  66. }
  67. static void
  68. format_garmin_xt_rd_deinit(void)
  69. {
  70. gbfclose(fin);
  71. }
  72. static uint16_t
  73. format_garmin_xt_rd_st_attrs(char* p_trk_name, uint8_t* p_track_color)
  74. {
  75. int method = 0;
  76. uint16_t trackbytes = 0, TrackPoints = 0;
  77. uint8_t spam = 0;
  78. int32_t TrackMaxLat = 0, TrackMaxLon = 0, TrackMinLat = 0, TrackMinLon = 0;
  79. char trk_name[30]="";
  80. // TODO: SHIFT - can't test behaviour, do not have appropriate files
  81. //int ii;
  82. // get the option for the processing the track name
  83. if (opt_trk_header) {
  84. method = atoi(opt_trk_header);
  85. // if method is out of range set to default
  86. if ((method < 0) || (method > 1)) {
  87. method = 0;
  88. }
  89. }
  90. // set to RED if not specified
  91. *p_track_color=9;
  92. trackbytes = gbfgetuint16(fin);
  93. TrackPoints = gbfgetuint16(fin);
  94. (void) TrackPoints;
  95. switch (method) {
  96. case 1:
  97. break; // IGNORE
  98. /* TODO: SHIFT - can't test behaviour, do not have appropriate files
  99. case 2: { // SHIFTED method
  100. spam = gbfgetc(fin);
  101. gbfread(&trk_name, 30, DATABLOCKSIZE, fin);
  102. gbfseek(fin, -1, SEEK_CUR);
  103. for (ii = 0; ii<29; ii++)
  104. {
  105. trk_name[ii] = (trk_name[ii] >> 2) + ( trk_name[ii+1] % 4 ) * 64;
  106. }
  107. }
  108. break;
  109. */
  110. default: { // NORMAL
  111. spam = gbfgetc(fin);
  112. gbfread(&trk_name, 30, DATABLOCKSIZE, fin);
  113. gbfseek(fin, -1, SEEK_CUR);
  114. }
  115. break;
  116. }
  117. spam = gbfgetc(fin);
  118. gbfread(&TrackMaxLat, 3, DATABLOCKSIZE, fin);
  119. gbfread(&spam, 1, DATABLOCKSIZE, fin);
  120. gbfread(&TrackMaxLon, 3, DATABLOCKSIZE, fin);
  121. gbfread(&spam, 1, DATABLOCKSIZE, fin);
  122. gbfread(&TrackMinLat, 3, DATABLOCKSIZE, fin);
  123. gbfread(&spam, 1, DATABLOCKSIZE, fin);
  124. gbfread(&TrackMinLon, 3, DATABLOCKSIZE, fin);
  125. gbfread(p_track_color, 1, DATABLOCKSIZE, fin);
  126. gbfread(&spam, 1, DATABLOCKSIZE, fin);
  127. strcpy(p_trk_name, trk_name);
  128. return trackbytes;
  129. }
  130. /*
  131. * Function to decrypt track block in saved read from saved tracks file
  132. */
  133. static void
  134. format_garmin_xt_decrypt_trk_blk(int Count, uint8_t TrackBlock[])
  135. {
  136. uint8_t i,j = 12;
  137. while (j<(Count-1)) {
  138. for (i = j; i < Count; i++) {
  139. TrackBlock[i] = TrackBlock[i] >> 1;
  140. if (i<(Count)) {
  141. TrackBlock[i] = TrackBlock[i] + (TrackBlock[i+1] % 2) * 128;
  142. }
  143. }
  144. j+=12;
  145. }
  146. }
  147. /*
  148. * Function to Decompose track block of STRK_BLOCK_SIZE bytes
  149. */
  150. static void
  151. format_garmin_xt_decomp_trk_blk(uint8_t ii, uint8_t TrackBlock[], double* Ele, double* Lat, double* Lon, uint32_t* Time)
  152. {
  153. uint32_t LatLW = 0, LonLW = 0, TimeLW = 0;
  154. double LatF = 0, LonF = 0;
  155. uint16_t PrevEleW;
  156. //printf("%d %d %d %d %d %d\n", TrackBlock[0], TrackBlock[1], TrackBlock[2], TrackBlock[3], TrackBlock[4], TrackBlock[5]);
  157. PrevEleW = TrackBlock[(ii - 1) * 12 + 1 ];
  158. PrevEleW = PrevEleW << 8;
  159. PrevEleW = PrevEleW + TrackBlock[(ii - 1) * 12 ];
  160. *Ele = (double)PrevEleW * GARMIN_XT_ELE - 1500;
  161. LatLW = TrackBlock[(ii - 1) * 12 + 4];
  162. LatLW = LatLW << 8;
  163. LatLW = LatLW + TrackBlock[(ii - 1) * 12 + 3];
  164. LatLW = LatLW << 8;
  165. LatLW = LatLW + TrackBlock[(ii - 1) * 12 + 2];
  166. LatF = (double)LatLW;
  167. if (LatF > 8388608) {
  168. LatF = LatF - 16777216;
  169. }
  170. *Lat = LatF * 360 / 16777216;
  171. LonLW = TrackBlock[(ii-1)*12+7];
  172. LonLW = LonLW << 8;
  173. LonLW = LonLW+TrackBlock[(ii-1)*12+6];
  174. LonLW = LonLW << 8;
  175. LonLW = LonLW+TrackBlock[(ii-1)*12+5];
  176. LonF = (double)LonLW;
  177. if (LonF>8388608) {
  178. LonF = LonF - 16777216;
  179. }
  180. *Lon = LonF * 360 / 16777216;
  181. TimeLW = TrackBlock[(ii - 1) * 12 + 11];
  182. TimeLW = TimeLW << 8;
  183. TimeLW = TimeLW+TrackBlock[(ii - 1) * 12 + 10];
  184. TimeLW = TimeLW << 8;
  185. TimeLW = TimeLW+TrackBlock[(ii - 1) * 12 + 9];
  186. TimeLW = TimeLW << 8;
  187. TimeLW = TimeLW + TrackBlock[(ii - 1) * 12 + 8];
  188. *Time = TimeLW + 631065600;
  189. }
  190. /*
  191. * Decompose Last Waypoint Eleveation
  192. */
  193. static void
  194. format_garmin_xt_decomp_last_ele(uint8_t ii, double* PrevEle, uint8_t TrackBlock[])
  195. {
  196. uint16_t PrevEleW;
  197. PrevEleW = TrackBlock[ii - 1];
  198. PrevEleW = PrevEleW << 8;
  199. PrevEleW = PrevEleW + TrackBlock[ii - 2];
  200. *PrevEle = (double)PrevEleW * GARMIN_XT_ELE - 1500;
  201. }
  202. /*
  203. * Main Function to process Saved tracks file
  204. */
  205. static void
  206. format_garmin_xt_proc_strk(void)
  207. {
  208. int Count = 0; // Used to obtain number of read bytes
  209. int NumberOfTracks = 0, TracksCompleted = 0; // Number of tracks in the file and number of processed tracks
  210. uint16_t trackbytes = 0; // Bytes in track
  211. uint8_t TrackBlock[STRK_BLOCK_SIZE + 1]; // File Block
  212. uint8_t ii; // temp variable
  213. double Lat = 0, Lon = 0; // wpt data
  214. double PrevLat = 0, PrevLon = 0, PrevEle = 0; // wpt data
  215. uint32_t Time = 0; // wpt data
  216. int FirstCoo;
  217. uint8_t trk_color = 0xff;
  218. // Skip 12 bytes from the BOF
  219. gbfseek(fin, 12, SEEK_SET);
  220. // read # of tracks
  221. NumberOfTracks = gbfgetuint16(fin);
  222. // Skip 2 bytes
  223. gbfseek(fin, 2, SEEK_CUR);
  224. // Process all tracks one by one
  225. while ((TracksCompleted < NumberOfTracks) && (!gbfeof(fin))) {
  226. route_head* tmp_track;
  227. Waypoint* wpt;
  228. char* trk_name;
  229. trk_name = (char*) xmalloc(30);
  230. // Generate Track Header
  231. trackbytes = format_garmin_xt_rd_st_attrs(trk_name, &trk_color) - 50;
  232. tmp_track = route_head_alloc();
  233. // update track color
  234. tmp_track->line_color.bbggrr = colors[trk_color];
  235. tmp_track->line_color.opacity = 255;
  236. // update track name
  237. tmp_track->rte_name = trk_name;
  238. xfree(trk_name);
  239. track_add_head(tmp_track);
  240. // This is the 1st coordinate of the track
  241. FirstCoo = TRUE;
  242. while (trackbytes>0) {
  243. if (trackbytes>=STRK_BLOCK_SIZE) {
  244. Count = gbfread(&TrackBlock, DATABLOCKSIZE, STRK_BLOCK_SIZE, fin);
  245. trackbytes -= STRK_BLOCK_SIZE;
  246. } else {
  247. Count = gbfread(&TrackBlock, DATABLOCKSIZE, trackbytes, fin);
  248. trackbytes = 0;
  249. }
  250. // decrypt loaded track block (Count - size of loaded TrackBlock)
  251. format_garmin_xt_decrypt_trk_blk(Count, TrackBlock);
  252. // process each track point in the loaded TrackBlock
  253. for (ii=1; ii <= ((Count-1) / 12); ii++) {
  254. // decompose loaded track block part (track point)
  255. format_garmin_xt_decomp_trk_blk(ii, TrackBlock, &PrevEle, &Lat, &Lon, &Time);
  256. // Add point to the track if not the first point
  257. if (!FirstCoo) {
  258. //create new waypoint
  259. wpt = new Waypoint;
  260. //populate wpt;
  261. wpt->latitude = PrevLat; /* Degrees */
  262. wpt->longitude = PrevLon; /* Degrees */
  263. wpt->altitude = PrevEle; /* Meters. */
  264. wpt->SetCreationTime(Time); /* Unix Time adjusted to Garmin time */
  265. // add way point to the track
  266. track_add_wpt(tmp_track, wpt);
  267. } else {
  268. FirstCoo = FALSE;
  269. }
  270. PrevLat = Lat;
  271. PrevLon = Lon;
  272. }
  273. }
  274. // decompose elevation for the last point
  275. if (Count > 12) {
  276. Count--;
  277. }
  278. format_garmin_xt_decomp_last_ele(Count, &PrevEle, TrackBlock);
  279. //create new waypoint
  280. wpt = new Waypoint;
  281. //populate wpt;
  282. wpt->latitude = PrevLat; /* Degrees */
  283. wpt->longitude = PrevLon; /* Degrees */
  284. wpt->altitude = PrevEle; /* Meters. */
  285. wpt->SetCreationTime(Time); /* Unix Time adjusted to Garmin time */
  286. // add way point to the track
  287. track_add_wpt(tmp_track, wpt);
  288. // update completed tracks counter
  289. TracksCompleted++;
  290. }
  291. }
  292. static void
  293. format_garmin_xt_proc_atrk(void)
  294. {
  295. uint16_t block=0, uu=0;
  296. uint32_t Lat=0, Lon=0;
  297. uint32_t Tim=0;
  298. double LatF = 0, LonF = 0, AltF = 0;
  299. Waypoint* wpt;
  300. int method = 0;
  301. unsigned char buf[3];
  302. int32_t num_trackpoints;
  303. // get the option for the processing the track name
  304. if (opt_trk_header) {
  305. method = atoi(opt_trk_header);
  306. }
  307. if (! track) {
  308. track = route_head_alloc();
  309. // header option was not set to ignore
  310. if (method !=1) {
  311. track->rte_name = "ATRK XT";
  312. }
  313. track_add_head(track);
  314. }
  315. // We think the word at offset 0xc is the trackpoint count.
  316. gbfseek(fin, 12, SEEK_SET);
  317. num_trackpoints = gbfgetuint32(fin);
  318. while (num_trackpoints--) {
  319. block = gbfgetuint16(fin);
  320. if (block != 0x0c) {
  321. break;
  322. }
  323. gbfread(&buf, 3, DATABLOCKSIZE, fin); //1. Lat
  324. Lat = buf[0] | (buf[1] << 8) | (buf[2] << 16);
  325. gbfread(&buf, 3, DATABLOCKSIZE, fin); //2. Lon
  326. Lon = buf[0] | (buf[1] << 8) | (buf[2] << 16);
  327. uu = gbfgetuint16(fin);
  328. Tim = gbfgetuint32(fin);
  329. Tim += 631065600; // adjustment to UnixTime
  330. LatF = Lat;
  331. if (LatF>8388608) {
  332. LatF -= 16777216;
  333. };
  334. LonF = Lon;
  335. if (LonF>8388608) {
  336. LonF -= 16777216;
  337. };
  338. AltF = (double)uu * GARMIN_XT_ELE - 1500;
  339. //create new waypoint
  340. wpt = new Waypoint;
  341. //populate wpt;
  342. wpt->latitude = LatF*180/16777216; /* Degrees */
  343. wpt->longitude = LonF*360/16777216; /* Degrees */
  344. wpt->altitude = AltF; /* Meters. */
  345. wpt->SetCreationTime(Tim); /* Unix Time adjusted to Garmin time */
  346. track_add_wpt(track, wpt);
  347. }
  348. }
  349. static void
  350. format_garmin_xt_read(void)
  351. {
  352. // Saved Tracks file
  353. if (strcmp(opt_xt_ftype, "STRK") == 0) {
  354. format_garmin_xt_proc_strk();
  355. } else { // Active Track file
  356. format_garmin_xt_proc_atrk();
  357. }
  358. }
  359. /**************************************************************************/
  360. ff_vecs_t format_garmin_xt_vecs = {
  361. ff_type_file,
  362. {
  363. ff_cap_none /* waypoints */,
  364. ff_cap_read /* tracks */,
  365. ff_cap_none /* routes */
  366. },
  367. format_garmin_xt_rd_init,
  368. NULL,
  369. format_garmin_xt_rd_deinit,
  370. NULL,
  371. format_garmin_xt_read,
  372. NULL,
  373. NULL,
  374. format_garmin_xt_args,
  375. CET_CHARSET_ASCII, 0 /* ascii is the expected character set */
  376. /* not fixed, can be changed through command line parameter */
  377. };
  378. /**************************************************************************/