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