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