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.

vitosmt.cc 9.3KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371
  1. /*
  2. Read Vito Navigator .SMT tracks
  3. Copyright (C) 2005 Etienne TASSE
  4. This program is free software; you can redistribute it and/or modify
  5. it under the terms of the GNU General Public License as published by
  6. the Free Software Foundation; either version 2 of the License, or
  7. (at your option) any later version.
  8. This program is distributed in the hope that it will be useful,
  9. but WITHOUT ANY WARRANTY; without even the implied warranty of
  10. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  11. GNU General Public License for more details.
  12. You should have received a copy of the GNU General Public License
  13. along with this program; if not, write to the Free Software
  14. Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA
  15. */
  16. #include "defs.h"
  17. #include "grtcirc.h"
  18. #include <cmath>
  19. #include <errno.h>
  20. #define MYNAME "vitosmt"
  21. static gbfile* infile =NULL;
  22. static gbfile* ofs =NULL;
  23. static long count =0;
  24. const long vitosmt_version =2;
  25. const long vitosmt_subversion =1000;
  26. const size_t vitosmt_headersize =24;
  27. const size_t vitosmt_datasize =64;
  28. static unsigned char*
  29. ReadRecord(gbfile* f, gbsize_t size)
  30. {
  31. unsigned char* result = (unsigned char*) xmalloc(size);
  32. gbfread(result, size, 1, f);
  33. return result;
  34. }
  35. static void
  36. WriteDouble(void* ptr, double d)
  37. {
  38. unsigned char result[9]="\0\0\0\0\0\0\0\0";
  39. le_write_double(result,d);
  40. memcpy(ptr, result, 8);
  41. }
  42. static void
  43. rd_init(const QString& fname)
  44. {
  45. infile = gbfopen_le(fname, "rb", MYNAME);
  46. }
  47. static void
  48. rd_deinit(void)
  49. {
  50. gbfclose(infile);
  51. }
  52. static void
  53. vitosmt_read(void)
  54. {
  55. long version =0;
  56. long subversion =0;
  57. long check1 =-1;
  58. long check2 =-2;
  59. long check3 =-3;
  60. route_head* route_head =0;
  61. Waypoint* wpt_tmp =0;
  62. double latrad =0;
  63. double lonrad =0;
  64. double elev =0;
  65. unsigned char* timestamp =0;
  66. struct tm tmStruct;
  67. double seconds =0.0;
  68. double speed =0.0;
  69. double course =0.0;
  70. double pdop =0.0;
  71. unsigned char gpsfix =0;
  72. unsigned char gpsvalid =0;
  73. unsigned char gpssats =0;
  74. int serial =0;
  75. memset(&tmStruct, 0, sizeof(tmStruct));
  76. /*
  77. * 24 bytes header
  78. */
  79. version = gbfgetint32(infile); /* 2 */
  80. subversion = gbfgetint32(infile); /* 1000 */
  81. count = gbfgetint32(infile); /* n */
  82. check1 = gbfgetint32(infile); /* 0 */
  83. check2 = gbfgetint32(infile); /* not sure */
  84. (void) check2; // silence warning.
  85. check3 = gbfgetint32(infile); /* n */
  86. if (version!=vitosmt_version) {
  87. fatal("%s (%d) reading file. Unsupported version %ld.%ld\n",
  88. MYNAME, __LINE__, version, subversion);
  89. }
  90. if (subversion!=vitosmt_subversion) {
  91. warning("%s (%d) reading file. Unsafe version %ld.%ld\n",
  92. MYNAME, __LINE__, version, subversion);
  93. }
  94. if ((count!=check3) ||
  95. (check1!=count-1) ||
  96. (check3!=count)) {
  97. fatal("%s (%d) reading file. Invalid file header\n",
  98. MYNAME, __LINE__);
  99. }
  100. while (count) {
  101. /*
  102. * 64 bytes of data
  103. */
  104. if (gbfeof(infile)||gbferror(infile)) {
  105. warning("%s (%d) reading file. Unexpected end of file %s\n",
  106. MYNAME, __LINE__, strerror(errno));
  107. break;
  108. }
  109. #if 0
  110. fprintf(stderr, "Looptop %d\n", gbftell(infile));
  111. #endif
  112. latrad =gbfgetdbl(infile); /* WGS84 latitude in radians */
  113. lonrad =gbfgetdbl(infile); /* WGS84 longitude in radians */
  114. elev =gbfgetdbl(infile); /* elevation in meters */
  115. #if 0
  116. fprintf(stderr, "before %d\n", gbftell(infile));
  117. #endif
  118. timestamp =ReadRecord(infile,5); /* UTC time yr/mo/dy/hr/mi */
  119. #if 0
  120. fprintf(stderr, "%d latrad %f/%f ele %f\n", gbftell(infile),latrad, DEG(latrad), elev);
  121. #endif
  122. seconds =gbfgetdbl(infile); /* seconds */
  123. speed =gbfgetdbl(infile); /* speed in knots */
  124. course =gbfgetdbl(infile); /* course in degrees */
  125. pdop =gbfgetdbl(infile); /* dilution of precision */
  126. gpsfix =gbfgetc(infile); /* fix type x08,x10, x20 */
  127. gpsvalid =gbfgetc(infile); /* fix is valid */
  128. gpssats =gbfgetc(infile); /* number of sats */
  129. wpt_tmp = new Waypoint;
  130. wpt_tmp->latitude =DEG(latrad);
  131. wpt_tmp->longitude =DEG(lonrad);
  132. wpt_tmp->altitude =elev;
  133. tmStruct.tm_year =timestamp[0]+100;
  134. tmStruct.tm_mon =timestamp[1]-1;
  135. tmStruct.tm_mday =timestamp[2];
  136. tmStruct.tm_hour =timestamp[3];
  137. tmStruct.tm_min =timestamp[4];
  138. tmStruct.tm_sec =(int)floor(seconds);
  139. tmStruct.tm_isdst =-1;
  140. double usec = fmod(1000000*seconds+0.5,1000000);
  141. wpt_tmp->SetCreationTime(mkgmtime(&tmStruct), lround(usec/1000.0));
  142. wpt_tmp->shortname = QString().sprintf("WP%04d", ++serial);
  143. WAYPT_SET(wpt_tmp, speed, KNOTS_TO_MPS(speed)); /* meters per second */
  144. WAYPT_SET(wpt_tmp, course, course);
  145. wpt_tmp->pdop = pdop;
  146. /*
  147. GPS Fix data
  148. */
  149. if (gpsvalid&0x7) {
  150. if (gpsfix==0) {
  151. wpt_tmp->fix =fix_none;
  152. }
  153. if (gpsfix&0x8) {
  154. wpt_tmp->fix =fix_2d;
  155. } else if (gpsfix&0x10) {
  156. wpt_tmp->fix =fix_3d;
  157. } else if (gpsfix&0x20) {
  158. wpt_tmp->fix =fix_dgps;
  159. } else {
  160. wpt_tmp->fix =fix_unknown;
  161. }
  162. /* <sat> */
  163. wpt_tmp->sat = gpssats;
  164. } else {
  165. wpt_tmp->fix =fix_unknown;
  166. }
  167. if (doing_wpts) { /* process as waypoints */
  168. waypt_add(wpt_tmp);
  169. } else if (doing_rtes) { /* process as route */
  170. if (route_head == NULL) {
  171. route_head = route_head_alloc();
  172. route_add_head(route_head);
  173. }
  174. route_add_wpt(route_head, wpt_tmp);
  175. } else { /* default track mode */
  176. if (route_head == NULL) {
  177. route_head = route_head_alloc();
  178. track_add_head(route_head);
  179. }
  180. track_add_wpt(route_head, wpt_tmp);
  181. }
  182. xfree(timestamp);
  183. count--;
  184. }
  185. }
  186. static void
  187. wr_init(const QString& fname)
  188. {
  189. warning(MYNAME " write: format is experimental and may crash Vito Navigator II.\n");
  190. ofs = gbfopen_le(fname, "wb", MYNAME);
  191. }
  192. static void
  193. wr_deinit(void)
  194. {
  195. gbfclose(ofs);
  196. }
  197. static void
  198. vitosmt_waypt_pr(const Waypoint* waypointp)
  199. {
  200. unsigned char* workbuffer =0;
  201. size_t position =0;
  202. double seconds =0;
  203. ++count;
  204. workbuffer = (unsigned char*) xcalloc(vitosmt_datasize,1);
  205. WriteDouble(&workbuffer[position], RAD(waypointp->latitude));
  206. position += sizeof(double);
  207. WriteDouble(&workbuffer[position], RAD(waypointp->longitude));
  208. position += sizeof(double);
  209. if (waypointp->altitude-1 > unknown_alt) {
  210. WriteDouble(&workbuffer[position], waypointp->altitude);
  211. }
  212. position += sizeof(double);
  213. QDate date(waypointp->GetCreationTime().date());
  214. QTime time(waypointp->GetCreationTime().time());
  215. workbuffer[position++] = date.year()-100;
  216. workbuffer[position++] = date.month();
  217. workbuffer[position++] = date.day();
  218. workbuffer[position++] = time.hour();
  219. workbuffer[position++] = time.minute();
  220. WriteDouble(&workbuffer[position], seconds);
  221. position += sizeof(double);
  222. /* speed */
  223. if (waypointp->speed>0) {
  224. WriteDouble(&workbuffer[position], MPS_TO_MPH(waypointp->speed));
  225. }
  226. position += sizeof(double);
  227. /* course */
  228. if ((waypointp->course>=-360.0)&&(waypointp->course<=360.0)) {
  229. WriteDouble(&workbuffer[position], waypointp->course);
  230. }
  231. position += sizeof(double);
  232. /* pdop */
  233. if (waypointp->pdop>0) {
  234. WriteDouble(&workbuffer[position], waypointp->pdop);
  235. }
  236. position += sizeof(double);
  237. /* fix type */
  238. switch (waypointp->fix) {
  239. case fix_2d:
  240. workbuffer[position++] = 0x08;
  241. break;
  242. case fix_3d:
  243. workbuffer[position++] = 0x10;
  244. break;
  245. case fix_dgps:
  246. workbuffer[position++] = 0x20;
  247. break;
  248. default:
  249. workbuffer[position++] = 0;
  250. break;
  251. }
  252. /* Assume position is valid */
  253. workbuffer[position++] = 0x07;
  254. if ((waypointp->sat>0)&&(waypointp->sat<128)) {
  255. workbuffer[position++] = waypointp->sat;
  256. } else {
  257. workbuffer[position++] = 0;
  258. }
  259. (void)gbfwrite(workbuffer,vitosmt_datasize,1,ofs);
  260. xfree(workbuffer);
  261. }
  262. static void
  263. vitosmt_write(void)
  264. {
  265. unsigned char* workbuffer =0;
  266. size_t position =0;
  267. workbuffer = (unsigned char*) xcalloc(vitosmt_headersize,1);
  268. count = 0;
  269. position = 0;
  270. /* leave a spacer for the header */
  271. memset(workbuffer,0,vitosmt_headersize);
  272. (void)gbfwrite(workbuffer,vitosmt_headersize,1,ofs);
  273. if (doing_wpts) { /* process as waypoints */
  274. waypt_disp_all(vitosmt_waypt_pr);
  275. } else if (doing_rtes) { /* process as route */
  276. route_disp_all(NULL, NULL, vitosmt_waypt_pr);
  277. } else { /* default track mode */
  278. track_disp_all(NULL, NULL, vitosmt_waypt_pr);
  279. }
  280. /* write the complete the header */
  281. le_write32(&workbuffer[position],vitosmt_version);
  282. position += sizeof(uint32_t);
  283. le_write32(&workbuffer[position],vitosmt_subversion);
  284. position += sizeof(uint32_t);
  285. le_write32(&workbuffer[position],count);
  286. position += sizeof(uint32_t);
  287. le_write32(&workbuffer[position],0);
  288. position += sizeof(uint32_t);
  289. le_write32(&workbuffer[position],count-1);
  290. position += sizeof(uint32_t);
  291. le_write32(&workbuffer[position],count);
  292. position += sizeof(uint32_t);
  293. gbfrewind(ofs);
  294. (void)gbfwrite(workbuffer,vitosmt_headersize,1,ofs);
  295. xfree(workbuffer);
  296. }
  297. ff_vecs_t vitosmt_vecs = {
  298. ff_type_file,
  299. FF_CAP_RW_ALL,
  300. rd_init,
  301. wr_init,
  302. rd_deinit,
  303. wr_deinit,
  304. vitosmt_read,
  305. vitosmt_write,
  306. NULL,
  307. NULL,
  308. CET_CHARSET_UTF8, 1 /* do nothing | CET-REVIEW */
  309. };