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_fit.cc 18KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623
  1. /*
  2. Support for FIT track files.
  3. Copyright (C) 2011 Paul Brook, paul@nowt.org
  4. Copyright (C) 2003-2011 Robert Lipe, robertlipe+source@gpsbabel.org
  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. #include "defs.h"
  18. #include <stdio.h>
  19. #define MYNAME "fit"
  20. static char* opt_allpoints = NULL;
  21. static int lap_ct = 0;
  22. static
  23. arglist_t fit_args[] = {
  24. {
  25. "allpoints", &opt_allpoints,
  26. "Read all points even if latitude or longitude is missing",
  27. NULL, ARGTYPE_BOOL, ARG_NOMINMAX, NULL
  28. },
  29. ARG_TERMINATOR
  30. };
  31. typedef struct {
  32. int id;
  33. int size;
  34. int type;
  35. } fit_field_t;
  36. typedef struct {
  37. int endian;
  38. int global_id;
  39. int num_fields;
  40. fit_field_t* fields;
  41. } fit_message_def;
  42. static struct {
  43. int len;
  44. int endian;
  45. route_head* track;
  46. uint32_t last_timestamp;
  47. fit_message_def message_def[16];
  48. } fit_data;
  49. static gbfile* fin;
  50. /*******************************************************************************
  51. * %%% global callbacks called by gpsbabel main process %%% *
  52. *******************************************************************************/
  53. static void
  54. fit_rd_init(const QString& fname)
  55. {
  56. fin = gbfopen_le(fname, "rb", MYNAME);
  57. }
  58. static void
  59. fit_rd_deinit(void)
  60. {
  61. int local_id;
  62. for (local_id=0; local_id<16; local_id++) {
  63. fit_message_def* def = &fit_data.message_def[local_id];
  64. if (def->fields) {
  65. xfree(def->fields);
  66. def->fields = NULL;
  67. }
  68. }
  69. gbfclose(fin);
  70. }
  71. /*******************************************************************************
  72. * fit_parse_header- parse the global FIT header
  73. *******************************************************************************/
  74. static void
  75. fit_parse_header(void)
  76. {
  77. int len;
  78. int ver;
  79. char sig[4];
  80. len = gbfgetc(fin);
  81. if (len == EOF || len < 12) {
  82. fatal(MYNAME ": Bad header\n");
  83. }
  84. if (global_opts.debug_level >= 1) {
  85. debug_print(1,"%s: header len=%d\n", MYNAME, len);
  86. }
  87. ver = gbfgetc(fin);
  88. if (ver == EOF || (ver >> 4) > 1)
  89. fatal(MYNAME ": Unsupported protocol version %d.%d\n",
  90. ver >> 4, ver & 0xf);
  91. if (global_opts.debug_level >= 1) {
  92. debug_print(1,"%s: protocol version=%d\n", MYNAME, ver);
  93. }
  94. // profile version
  95. ver = gbfgetuint16(fin);
  96. // data length
  97. fit_data.len = gbfgetuint32(fin);
  98. // File signature
  99. is_fatal(gbfread(sig, 4, 1, fin) != 1,
  100. MYNAME ": Unexpected end of file\n");
  101. if (sig[0] != '.' || sig[1] != 'F' || sig[2] != 'I' || sig[3] != 'T') {
  102. fatal(MYNAME ": .FIT signature missing\n");
  103. }
  104. if (global_opts.debug_level >= 1) {
  105. debug_print(1,"%s: profile version=%d\n", MYNAME, ver);
  106. debug_print(1,"%s: fit_data.len=%d\n", MYNAME, fit_data.len);
  107. }
  108. if (len > 12) {
  109. // Unused according to Ingo Arndt
  110. gbfgetuint16(fin);
  111. }
  112. }
  113. static uint8_t
  114. fit_getuint8(void)
  115. {
  116. int val;
  117. if (fit_data.len == 0) {
  118. // fail gracefully for GARMIN Edge 800 with newest firmware, seems to write a wrong record length
  119. // for the last record.
  120. //fatal(MYNAME ": record truncated: fit_data.len=0\n");
  121. if (global_opts.debug_level >= 1) {
  122. warning("%s: record truncated: fit_data.len=0\n", MYNAME);
  123. }
  124. return 0;
  125. }
  126. val = gbfgetc(fin);
  127. if (val == EOF) {
  128. fatal(MYNAME ": unexpected end of file with fit_data.len=%d\n",fit_data.len);
  129. }
  130. fit_data.len--;
  131. return (uint8_t)val;
  132. }
  133. static uint16_t
  134. fit_getuint16(void)
  135. {
  136. char buf[2];
  137. if (fit_data.len < 2) {
  138. fatal(MYNAME ": record truncated: expecting char[2], but only got %d\n",fit_data.len);
  139. }
  140. is_fatal(gbfread(buf, 2, 1, fin) != 1,
  141. MYNAME ": unexpected end of file with fit_data.len=%d\n",fit_data.len);
  142. fit_data.len -= 2;
  143. if (fit_data.endian) {
  144. return be_read16(buf);
  145. } else {
  146. return le_read16(buf);
  147. }
  148. }
  149. static uint32_t
  150. fit_getuint32(void)
  151. {
  152. char buf[4];
  153. if (fit_data.len < 4) {
  154. fatal(MYNAME ": record truncated: expecting char[4], but only got %d\n",fit_data.len);
  155. }
  156. is_fatal(gbfread(buf, 4, 1, fin) != 1,
  157. MYNAME ": unexpected end of file with fit_data.len=%d\n",fit_data.len);
  158. fit_data.len -= 4;
  159. if (fit_data.endian) {
  160. return be_read32(buf);
  161. } else {
  162. return le_read32(buf);
  163. }
  164. }
  165. static void
  166. fit_parse_definition_message(uint8_t header)
  167. {
  168. int local_id = header & 0x0f;
  169. fit_message_def* def = &fit_data.message_def[local_id];
  170. int i;
  171. if (def->fields) {
  172. xfree(def->fields);
  173. }
  174. // first byte is reserved. It's usually 0 and we don't know what it is,
  175. // but we've seen some files that are 0x40. So we just read it and toss it.
  176. i = fit_getuint8();
  177. // second byte is endianness
  178. def->endian = fit_getuint8();
  179. if (def->endian > 1) {
  180. fatal(MYNAME ": Bad endian field\n");
  181. }
  182. fit_data.endian = def->endian;
  183. // next two bytes are the global message number
  184. def->global_id = fit_getuint16();
  185. // byte 5 has the number of records in the remainder of the definition message
  186. def->num_fields = fit_getuint8();
  187. if (global_opts.debug_level >= 8) {
  188. debug_print(8,"%s: definition message contains %d records\n",MYNAME, def->num_fields);
  189. }
  190. if (def->num_fields == 0) {
  191. def->fields = (fit_field_t*) xmalloc(sizeof(fit_field_t));
  192. return;
  193. }
  194. // remainder of the definition message is data at one byte per field * 3 fields
  195. def->fields = (fit_field_t*) xmalloc(def->num_fields * sizeof(fit_field_t));
  196. for (i = 0; i < def->num_fields; i++) {
  197. def->fields[i].id = fit_getuint8();
  198. def->fields[i].size = fit_getuint8();
  199. def->fields[i].type = fit_getuint8();
  200. if (global_opts.debug_level >= 8) {
  201. debug_print(8,"%s: record %d ID: %d SIZE: %d TYPE: %d fit_data.len=%d\n",
  202. MYNAME, i, def->fields[i].id, def->fields[i].size, def->fields[i].type,fit_data.len);
  203. }
  204. }
  205. }
  206. static uint32_t
  207. fit_read_field(fit_field_t* f)
  208. {
  209. /* https://forums.garmin.com/showthread.php?223645-Vivoactive-problems-plus-suggestions-for-future-firmwares&p=610929#post610929
  210. * Per section 4.2.1.4.2 of the FIT Protocol the size of a field may be a
  211. * multiple of the size of the underlying type, indicating the field
  212. * contains multiple elements represented as an array.
  213. *
  214. * Garmin Product Support
  215. */
  216. // In the case that the field contains one value of the indicated type we return that value,
  217. // otherwise we just skip over the data.
  218. int i;
  219. if (global_opts.debug_level >= 8) {
  220. debug_print(8,"%s: fit_read_field: read data field with f->type=0x%X and f->size=%d fit_data.len=%d\n",
  221. MYNAME, f->type, f->size, fit_data.len);
  222. }
  223. switch (f->type) {
  224. case 1: // sint8
  225. case 2: // uint8
  226. if (f->size == 1) {
  227. return fit_getuint8();
  228. } else { // ignore array data
  229. for (i = 0; i < f->size; i++) {
  230. fit_getuint8();
  231. }
  232. if (global_opts.debug_level >= 8) {
  233. debug_print(8, "%s: fit_read_field: skipping 1-byte array data\n", MYNAME);
  234. }
  235. return -1;
  236. }
  237. case 0x83: // sint16
  238. case 0x84: // uint16
  239. if (f->size == 2) {
  240. return fit_getuint16();
  241. } else { // ignore array data
  242. for (i = 0; i < f->size; i++) {
  243. fit_getuint8();
  244. }
  245. if (global_opts.debug_level >= 8) {
  246. debug_print(8, "%s: fit_read_field: skipping 2-byte array data\n", MYNAME);
  247. }
  248. return -1;
  249. }
  250. case 0x85: // sint32
  251. case 0x86: // uint32
  252. if (f->size == 4) {
  253. return fit_getuint32();
  254. } else { // ignore array data
  255. for (i = 0; i < f->size; i++) {
  256. fit_getuint8();
  257. }
  258. if (global_opts.debug_level >= 8) {
  259. debug_print(8, "%s: fit_read_field: skipping 4-byte array data\n", MYNAME);
  260. }
  261. return -1;
  262. }
  263. default: // Ignore everything else for now.
  264. for (i = 0; i < f->size; i++) {
  265. fit_getuint8();
  266. }
  267. if (global_opts.debug_level >= 8) {
  268. debug_print(8, "%s: fit_read_field: skipping unrecognized data type\n", MYNAME);
  269. }
  270. return -1;
  271. }
  272. }
  273. static void
  274. fit_parse_data(fit_message_def* def, int time_offset)
  275. {
  276. fit_field_t* f;
  277. uint32_t timestamp = fit_data.last_timestamp + time_offset;
  278. uint32_t val;
  279. int32_t lat = 0x7fffffff;
  280. int32_t lon = 0x7fffffff;
  281. uint16_t alt = 0xffff;
  282. uint16_t speed = 0xffff;
  283. uint8_t heartrate = 0xff;
  284. uint8_t cadence = 0xff;
  285. uint16_t power = 0xffff;
  286. int8_t temperature = 0x7f;
  287. int i;
  288. Waypoint* waypt;
  289. int32_t startlat = 0x7fffffff;
  290. int32_t startlon = 0x7fffffff;
  291. int32_t endlat = 0x7fffffff;
  292. int32_t endlon = 0x7fffffff;
  293. uint32_t starttime = 0; // ??? default ?
  294. char cbuf[10];
  295. Waypoint* lappt; // WptPt in gpx
  296. if (global_opts.debug_level >= 7) {
  297. debug_print(7,"%s: parsing fit data ID %d with num_fields=%d\n", MYNAME, def->global_id, def->num_fields);
  298. }
  299. for (i = 0; i < def->num_fields; i++) {
  300. if (global_opts.debug_level >= 7) {
  301. debug_print(7,"%s: parsing field %d\n", MYNAME, i);
  302. }
  303. f = &def->fields[i];
  304. val = fit_read_field(f);
  305. if (f->id == 253) {
  306. if (global_opts.debug_level >= 7) {
  307. debug_print(7,"%s: parsing fit data: timestamp=%d\n", MYNAME, val);
  308. }
  309. fit_data.last_timestamp = timestamp = val;
  310. } else {
  311. switch (def->global_id) {
  312. case 20: // record message - trkType is a track
  313. switch (f->id) {
  314. case 0:
  315. if (global_opts.debug_level >= 7) {
  316. debug_print(7,"%s: parsing fit data: lat=%d\n", MYNAME, val);
  317. }
  318. lat = val;
  319. break;
  320. case 1:
  321. if (global_opts.debug_level >= 7) {
  322. debug_print(7,"%s: parsing fit data: lon=%d\n", MYNAME, val);
  323. }
  324. lon = val;
  325. break;
  326. case 2:
  327. if (global_opts.debug_level >= 7) {
  328. debug_print(7,"%s: parsing fit data: alt=%d\n", MYNAME, val);
  329. }
  330. alt = val;
  331. break;
  332. case 3:
  333. if (global_opts.debug_level >= 7) {
  334. debug_print(7,"%s: parsing fit data: heartrate=%d\n", MYNAME, val);
  335. }
  336. heartrate = val;
  337. break;
  338. case 4:
  339. if (global_opts.debug_level >= 7) {
  340. debug_print(7,"%s: parsing fit data: cadence=%d\n", MYNAME, val);
  341. }
  342. cadence = val;
  343. break;
  344. case 5:
  345. // NOTE: 5 is DISTANCE in cm ... unused.
  346. if (global_opts.debug_level >= 7) {
  347. debug_print(7, "%s: unrecognized data type in GARMIN FIT record: f->id=%d\n", MYNAME, f->id);
  348. }
  349. break;
  350. case 6:
  351. if (global_opts.debug_level >= 7) {
  352. debug_print(7,"%s: parsing fit data: speed=%d\n", MYNAME, val);
  353. }
  354. speed = val;
  355. break;
  356. case 7:
  357. if (global_opts.debug_level >= 7) {
  358. debug_print(7,"%s: parsing fit data: power=%d\n", MYNAME, val);
  359. }
  360. power = val;
  361. break;
  362. case 13:
  363. if (global_opts.debug_level >= 7) {
  364. debug_print(7,"%s: parsing fit data: temperature=%d\n", MYNAME, val);
  365. }
  366. temperature = val;
  367. break;
  368. case 19: // lap wptType , endlat+lon is wpt
  369. switch (f->id) {
  370. case 2:
  371. if (global_opts.debug_level >= 7) {
  372. debug_print(7,"%s: parsing fit data: starttime=%d\n", MYNAME, val);
  373. }
  374. starttime = val;
  375. break;
  376. case 3:
  377. if (global_opts.debug_level >= 7) {
  378. debug_print(7,"%s: parsing fit data: startlat=%d\n", MYNAME, val);
  379. }
  380. startlat = val;
  381. break;
  382. case 4:
  383. if (global_opts.debug_level >= 7) {
  384. debug_print(7,"%s: parsing fit data: startlon=%d\n", MYNAME, val);
  385. }
  386. startlon = val;
  387. break;
  388. case 5:
  389. if (global_opts.debug_level >= 7) {
  390. debug_print(7,"%s: parsing fit data: endlat=%d\n", MYNAME, val);
  391. }
  392. endlat = val;
  393. break;
  394. case 6:
  395. if (global_opts.debug_level >= 7) {
  396. debug_print(7,"%s: parsing fit data: endlon=%d\n", MYNAME, val);
  397. }
  398. endlon = val;
  399. break;
  400. case 7:
  401. if (global_opts.debug_level >= 7) {
  402. debug_print(7,"%s: parsing fit data: elapsedtime=%d\n", MYNAME, val);
  403. }
  404. //elapsedtime = val;
  405. break;
  406. case 9:
  407. if (global_opts.debug_level >= 7) {
  408. debug_print(7,"%s: parsing fit data: totaldistance=%d\n", MYNAME, val);
  409. }
  410. //totaldistance = val;
  411. break;
  412. default:
  413. if (global_opts.debug_level >= 1) {
  414. debug_print(1, "%s: unrecognized data type in GARMIN FIT lap: f->id=%d\n", MYNAME, f->id);
  415. }
  416. break;
  417. } // switch (f->id)
  418. break;
  419. default:
  420. if (global_opts.debug_level >= 1) {
  421. debug_print(1, "%s: unrecognized data type in GARMIN FIT record: f->id=%d\n", MYNAME, f->id);
  422. }
  423. break;
  424. }
  425. }
  426. }
  427. }
  428. if (global_opts.debug_level >= 7) {
  429. debug_print(7,"%s: storing fit data with num_fields=%d\n", MYNAME, def->num_fields);
  430. }
  431. switch (def->global_id) {
  432. case 19: // lap message
  433. if (endlat == 0x7fffffff || endlon == 0x7fffffff) {
  434. break;
  435. }
  436. if (global_opts.debug_level >= 7) {
  437. debug_print(7,"%s: storing fit data LAP %d\n", MYNAME, def->global_id);
  438. }
  439. lappt = new Waypoint;
  440. lappt->latitude = (endlat / (double)0x7fffffff) * 180;
  441. lappt->longitude = (endlon / (double)0x7fffffff) * 180;
  442. lap_ct++;
  443. snprintf(cbuf, sizeof(cbuf), "LAP%03d", lap_ct);
  444. lappt->shortname = cbuf;
  445. waypt_add(lappt);
  446. break;
  447. case 20: // record message
  448. if ((lat == 0x7fffffff || lon == 0x7fffffff) && !opt_allpoints) {
  449. break;
  450. }
  451. waypt = new Waypoint;
  452. if (lat != 0x7fffffff) {
  453. waypt->latitude = (lat / (double)0x7fffffff) * 180;
  454. }
  455. if (lon != 0x7fffffff) {
  456. waypt->longitude = (lon / (double)0x7fffffff) * 180;
  457. }
  458. if (alt != 0xffff) {
  459. waypt->altitude = (alt / 5.0) - 500;
  460. }
  461. waypt->SetCreationTime(QDateTime::fromTime_t(timestamp + 631065600));
  462. if (speed != 0xffff) {
  463. WAYPT_SET(waypt, speed, speed / 1000.0f);
  464. }
  465. if (heartrate != 0xff) {
  466. waypt->heartrate = heartrate;
  467. }
  468. if (cadence != 0xff) {
  469. waypt->cadence = cadence;
  470. }
  471. if (power != 0xffff) {
  472. waypt->power = power;
  473. }
  474. if (temperature != 0x7f) {
  475. WAYPT_SET(waypt, temperature, temperature);
  476. }
  477. track_add_wpt(fit_data.track, waypt);
  478. break;
  479. }
  480. }
  481. static void
  482. fit_parse_data_message(uint8_t header)
  483. {
  484. int local_id = header & 0x1f;
  485. fit_message_def* def = &fit_data.message_def[local_id];
  486. fit_parse_data(def, 0);
  487. }
  488. static void
  489. fit_parse_compressed_message(uint8_t header)
  490. {
  491. int local_id = (header >> 5) & 3;
  492. fit_message_def* def = &fit_data.message_def[local_id];
  493. fit_parse_data(def, header & 0x1f);
  494. }
  495. /*******************************************************************************
  496. * fit_parse_record- parse each record in the file
  497. *******************************************************************************/
  498. static void
  499. fit_parse_record(void)
  500. {
  501. uint8_t header;
  502. header = fit_getuint8();
  503. // high bit 7 set -> compressed message (0 for normal)
  504. // second bit 6 set -> 0 for data message, 1 for definition message
  505. // bits 5, 4 -> reserved
  506. // bits 3..0 -> local message type
  507. if (header & 0x80) {
  508. if (global_opts.debug_level >= 6) {
  509. debug_print(6,"%s: got compressed message at fit_data.len=%d", MYNAME, fit_data.len);
  510. debug_print(0," ...local message type 0x%X\n", header&0x0f);
  511. }
  512. fit_parse_compressed_message(header);
  513. } else if (header & 0x40) {
  514. if (global_opts.debug_level >= 6) {
  515. debug_print(6,"%s: got definition message at fit_data.len=%d", MYNAME, fit_data.len);
  516. debug_print(0," ...local message type 0x%X\n", header&0x0f);
  517. }
  518. fit_parse_definition_message(header);
  519. } else {
  520. if (global_opts.debug_level >= 6) {
  521. debug_print(6,"%s: got data message at fit_data.len=%d", MYNAME, fit_data.len);
  522. debug_print(0," ...local message type 0x%X\n", header&0x0f);
  523. }
  524. fit_parse_data_message(header);
  525. }
  526. }
  527. /*******************************************************************************
  528. * fit_read- global entry point
  529. * - parse the header
  530. * - parse all the records in the file
  531. *******************************************************************************/
  532. static void
  533. fit_read(void)
  534. {
  535. fit_parse_header();
  536. fit_data.track = route_head_alloc();
  537. track_add_head(fit_data.track);
  538. if (global_opts.debug_level >= 1) {
  539. debug_print(1,"%s: starting to read data with fit_data.len=%d\n", MYNAME, fit_data.len);
  540. }
  541. while (fit_data.len) {
  542. fit_parse_record();
  543. }
  544. }
  545. /**************************************************************************/
  546. // capabilities below means: we can only read and write waypoints
  547. // please change this depending on your new module
  548. ff_vecs_t format_fit_vecs = {
  549. ff_type_file,
  550. {
  551. ff_cap_none /* waypoints */,
  552. ff_cap_read /* tracks */,
  553. ff_cap_none /* routes */
  554. },
  555. fit_rd_init,
  556. NULL,
  557. fit_rd_deinit,
  558. NULL,
  559. fit_read,
  560. NULL,
  561. NULL,
  562. fit_args,
  563. CET_CHARSET_ASCII, 0 /* ascii is the expected character set */
  564. /* not fixed, can be changed through command line parameter */
  565. };
  566. /**************************************************************************/