forked from GPSBabel/gpsbabel
-
Notifications
You must be signed in to change notification settings - Fork 0
/
delbin.cc
3373 lines (3138 loc) · 83.6 KB
/
delbin.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
/*
DeLorme PN-20/40 USB "DeLBin" protocol
Copyright (C) 2009 Paul Cornett, pc-gpsb at bullseye.com
Copyright (C) 2005-2014 Robert Lipe, [email protected]
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA
*/
#include "defs.h"
#include "src/core/xmltag.h"
#include <assert.h>
#include <stdio.h> /* for atoi, sprintf */
#include <stdlib.h> // atoi
#define MYNAME "delbin"
static short_handle mkshort_handle;
/*
Device documentation:
"DeLorme Binary GPS Format", delbin_user_interface_format_176.pdf
obtained here: http://forum.delorme.com/viewtopic.php?t=13846
Notes:
Initial development was done with a PN-40, firmware 2.4.123299. The test
device was upgraded to firmware 2.5.165506 during development.
The "data size" in the message header includes the 4 trailer bytes, so it
is really the size of the whole message minus the header.
Messages do not always start at the beginning of a packet. Every once in a
while, the start of the next message directly follows the end of the previous
one, in the same packet.
The time before an unacknowledged message will be retransmitted by the
device is on the order of 2 to 4 seconds.
Retrieving all tracks at once (using code 0 in message 0xb031) does not
seem to work, it hangs after the first track, maybe waiting for some
undocumented response message.
Character encoding is not documented, appears to be 8859-1.
The undocumented messages 0xaa01, 0xb015, 0xb016 and the use of the
"reserved" byte in message 0xb012 were discovered by examining the data
transferred between the device and DeLorme Topo 8.0. They may have been
added in the PN-40 2.5 firmware.
*/
//-----------------------------------------------------------------------------
// interface to platform-specific device I/O
typedef struct {
void (*init)(const char* name);
void (*deinit)(void);
unsigned(*packet_read)(void*);
unsigned(*packet_write)(const void*, unsigned);
} delbin_os_ops_t;
// really static, only extern so it can be forward declared
extern delbin_os_ops_t delbin_os_ops;
static unsigned delbin_os_packet_size;
//-----------------------------------------------------------------------------
// number of times to attempt a transfer before giving up
#define ATTEMPT_MAX 2
// seconds to wait for expected message (actual time will be somewhat
// indeterminate, but at least READ_TIMEOUT - 1)
#define READ_TIMEOUT 6
// debug output: low, medium, high, higher
#define DBGLVL_L 1
#define DBGLVL_M 2
#define DBGLVL_H 3
#define DBGLVL_H2 4
// Multiple unit support.
#define DELBIN_MAX_UNITS 32
static struct {
unsigned int unit_number;
const char* unit_serial_number;
const char* unit_name;
} delbin_unit_info[DELBIN_MAX_UNITS];
static int n_delbin_units;
#define UNKNOWN_ELEV -2000000
#define sizeofarray(x) (sizeof(x) / sizeof(x[0]))
static char* opt_getposn = NULL;
static char* opt_logs = NULL;
static char* opt_long_notes = NULL;
static char* opt_nuke_wpt = NULL;
static char* opt_nuke_trk = NULL;
static char* opt_nuke_rte = NULL;
/* If true, Order hint to match Cache Register and Topo 7 */
static char* opt_hint_at_end = NULL;
static char* opt_gcsym = NULL;
static arglist_t delbin_args[] = {
{
"get_posn", &opt_getposn, "Return current position as a waypoint",
NULL, ARGTYPE_BOOL, ARG_NOMINMAX
},
{
"logs", &opt_logs, "Include groundspeak logs when writing",
NULL, ARGTYPE_BOOL, ARG_NOMINMAX
},
{
"long_notes", &opt_long_notes, "Use long waypoint notes regardless of PN version",
NULL, ARGTYPE_BOOL, ARG_NOMINMAX
},
{
"nukewpt", &opt_nuke_wpt, "Delete all waypoints before sending", NULL, ARGTYPE_BOOL,
ARG_NOMINMAX
},
{
"nuketrk", &opt_nuke_trk, "Delete all tracks before sending", NULL, ARGTYPE_BOOL,
ARG_NOMINMAX
},
{
"nukerte", &opt_nuke_rte, "Delete all routes before sending", NULL, ARGTYPE_BOOL,
ARG_NOMINMAX
},
{"hint_at_end", &opt_hint_at_end, "If true, geocache hint at end of text", NULL, ARGTYPE_BOOL, ARG_NOMINMAX },
{"gcsym", &opt_gcsym, "If set to 0, prefer user-provided symbols over Groundspeaks ones for geocaches", NULL, ARGTYPE_BOOL, ARG_NOMINMAX, (char*) "1" },
ARG_TERMINATOR
};
// Whether device understands message 0xb016
static int use_extended_notes;
// Device capabilities
static unsigned device_max_waypoint;
static const char* waypoint_symbol(unsigned index);
static unsigned waypoint_symbol_index(const char* name);
static int track_color(unsigned index);
static unsigned track_color_index(int bgr);
static unsigned waypoint_i;
static unsigned waypoint_n;
static Waypoint** wp_array;
//-----------------------------------------------------------------------------
// Message ids and sizes. Only the needed ones are here.
// Note that "in" and "out" ids are named as in the device documentation,
// so "in" means to the device, "out" means from.
#define MSG_ACK 0xaa00
#define MSG_BREAK 0xaa02
#define MSG_BREAK_SIZE 33
#define MSG_CAPABILITIES 0xb001
#define MSG_DELETE 0xb005
#define MSG_DELETE_SIZE 67
#define MSG_ERROR 0xa003
#define MSG_NAVIGATION 0xa010
#define MSG_REQUEST_ROUTES 0xb051
#define MSG_REQUEST_ROUTES_SIZE 65
#define MSG_REQUEST_TRACKS 0xb031
#define MSG_REQUEST_TRACKS_SIZE 33
#define MSG_REQUEST_WAYPOINTS 0xb012
#define MSG_REQUEST_WAYPOINTS_SIZE 15
#define MSG_ROUTE_COUNT 0xb050
#define MSG_ROUTE_HEADER_IN 0xb055
#define MSG_ROUTE_HEADER_OUT 0xb052
#define MSG_ROUTE_POINT_IN 0xb056
#define MSG_ROUTE_POINT_OUT 0xb053
#define MSG_ROUTE_SHAPE_IN 0xb057
#define MSG_ROUTE_SHAPE_OUT 0xb054
#define MSG_SATELLITE_INFO 0xa020
#define MSG_TRACK_COUNT 0xb030
#define MSG_TRACK_HEADER_IN 0xb035
#define MSG_TRACK_HEADER_OUT 0xb032
#define MSG_TRACK_POINT_IN 0xb036
#define MSG_TRACK_POINT_OUT 0xb033
#define MSG_TRANSFER_COMPLETE 0xaa04
#define MSG_VERSION 0xa001
#define MSG_WAYPOINT_COUNT 0xb010
#define MSG_WAYPOINT_IN 0xb014
#define MSG_WAYPOINT_OUT 0xb013
// Undocumented:
// This one looks like MSG_ACK, except it also has a string in it that says
// something like "device is busy". The expected MSG_ACK usually immediately
// follows it, so the point of this one is unclear.
#define MSG_NACK 0xaa01
// Long waypoint notes
#define MSG_WAYPOINT_NOTE_IN 0xb016
#define MSG_WAYPOINT_NOTE_OUT 0xb015
//-----------------------------------------------------------------------------
// Message structures
// Input Delete Message
// Message ID: 0xB005
typedef enum {
nuke_type_wpt = 0,
nuke_type_trk = 1,
nuke_type_rte = 2,
// int nuke_map = 3;
} nuke_type;
typedef enum {
nuke_mode_all = 0,
nuke_mode_single = 1
} nuke_mode;
typedef enum {
nuke_dest_internal = 0,
nuke_dest_sd = 1
} nuke_dest;
typedef struct {
uint8_t type;
uint8_t mode;
uint8_t location;
char object_name[64];
} msg_delete_t;
// Output Waypoint Message
// Message ID: 0xB013
// Input Waypoint Message
// Message ID: 0xB014
typedef struct {
uint8_t total[4]; // U32
uint8_t index[4]; // U32
uint8_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t minute;
uint8_t second;
uint8_t latitude[4]; // S32 rad * 100000000
uint8_t longitude[4]; // S32 rad * 100000000
uint8_t elevation[4]; // F32 meters
uint8_t color;
uint8_t symbol;
uint8_t name_size;
char name[1];
// note_size[2] U16
// note[note_size]
} msg_waypoint_t;
// undocumented, seen with PN-40 2.5 firmware
// output waypoint note
// Message ID: 0xB015
// input waypoint note
// Message ID: 0xB016
typedef struct {
uint8_t index[2];
uint8_t total[2];
uint8_t name_size;
char name[1];
// note_size[2]
// note[note_size]
} msg_waypoint_note_t;
// Output Track Point Message
// Message ID: 0xB033
// Input Track Point Message
// Message ID: 0xB036
typedef struct {
uint8_t total[4]; // U32
uint8_t index[4]; // U32
uint8_t number;
struct {
uint8_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t minute;
uint8_t second;
uint8_t latitude[4]; // S32 rad * 100000000
uint8_t longitude[4]; // S32 rad * 100000000
uint8_t elevation[4]; // F32 meters
uint8_t speed[2]; // U16 km/h * 10
uint8_t heading[2]; // U16 deg * 100
uint8_t status;
} point[1];
} msg_track_point_t;
// Output Track Header (Name) Message
// Message ID: 0xB032
typedef struct {
uint8_t total_tracks[2]; // U16
uint8_t number[2]; // U16
char name[32];
uint8_t total_points[4]; // U32
uint8_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t minute;
uint8_t second;
uint8_t color[2]; // U16
uint8_t distance[4]; // U32 m
uint8_t duration[4]; // U32 sec
uint8_t comment_size[2]; // U16
char comment[1];
} msg_track_header_t;
// Input Upload Track Header Message
// Message ID: 0xB035
typedef struct {
char name[32];
uint8_t total_points[4]; // U32
uint8_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t minute;
uint8_t second;
uint8_t color[2]; // U16
uint8_t comment_size[2]; // U16
char comment[1];
} msg_track_header_in_t;
// Output Route Shape Message
// Message ID: 0xB054
typedef struct {
uint8_t total[4]; // U32
uint8_t index[4]; // U32
uint8_t number;
uint8_t reserved;
struct {
uint8_t latitude[4]; // S32 rad * 100000000
uint8_t longitude[4]; // S32 rad * 100000000
} point[1];
} msg_route_shape_t;
// Output Route Point Message
// Message ID: 0xB053
// Input Route Itin Point Message
// Message ID: 0xB056
typedef struct {
uint8_t total[4]; // U32
uint8_t index[4]; // U32
char name[32];
uint8_t latitude[4]; // S32 rad * 100000000
uint8_t longitude[4]; // S32 rad * 100000000
uint8_t time_from_start[4]; // U32 sec
uint8_t distance_from_start[4]; // F32 km
uint8_t bearing_in[2]; // U16 deg * 100
uint8_t bearing_out[2]; // U16 deg * 100
uint8_t bearing_next[2]; // U16 deg * 100
uint8_t itinerary_type;
uint8_t turn_type;
uint8_t road_class[2]; // U16
uint8_t feature_code[4]; // U32
uint8_t exit_label_size;
char exit_label[1];
// comment_size U8
// comment[comment_size]
// shape_pt_count U32
} msg_route_point_t;
// Output Route Header (Name) Message
// Message ID: 0xB052
typedef struct {
uint8_t total[2]; // U16
uint8_t index[2]; // U16
char name[64];
uint8_t type;
uint8_t total_route_point[4]; // U32
uint8_t total_shape_point[4]; // U32
} msg_route_header_t;
// Input Upload Route Header Message
// Message ID: 0xB055
typedef struct {
char name[64];
uint8_t type;
uint8_t total_route_point[4]; // U32
uint8_t total_shape_point[4]; // U32
} msg_route_header_in_t;
// Output Navigation Message
// Message ID: 0xA010
typedef struct {
uint8_t gps_week[2]; // U16
uint8_t time_of_week[8]; // D64 sec
uint8_t year[2]; // U16
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t minute;
uint8_t second;
uint8_t satellites;
uint8_t latitude[8]; // D64 deg
uint8_t longitude[8]; // D64 deg
uint8_t elevation[8]; // D64 meters
uint8_t geoid_offset[2]; // S16 meters * 10
uint8_t speed[4]; // F32 km/h
uint8_t heading[2]; // U16 deg * 100
uint8_t magnetic_variation[2]; // S16 deg * 100
uint8_t fix_status;
} msg_navigation_t;
// Output Satellite Info Message
// Message ID: 0xA020
typedef struct {
uint8_t gps_week[2]; // U16
uint8_t time_of_week[8]; // D64 sec
uint8_t hdop[2]; // U16
uint8_t vdop[2]; // U16
uint8_t pdop[2]; // U16
uint8_t number;
struct {
uint8_t prn;
uint8_t azimuth[2]; // S16 deg? * 100
uint8_t elevation[2]; // S16 deg? * 100
uint8_t Cn0[2]; // U16 snr * 100
uint8_t status;
} sat[1];
} msg_satellite_t;
// Output Version Message
// Message ID: 0xA001
typedef struct {
uint8_t firmware_version[4];
char company[32];
char product[32];
char firmware[32];
char gps_firmware[48];
char serial[16];
char extra[16];
} msg_version_t;
// Output Device Capabilities Message
// Message ID: 0xB001
typedef struct {
uint8_t max_waypoints[4]; // U32
uint8_t max_tracks[2]; // U16
uint8_t max_track_points[4]; // U32
uint8_t max_routes[2]; // U16
uint8_t max_route_points[4]; // U32
uint8_t max_route_shape_points[4]; // U32
uint8_t max_maps[2]; // U16
uint8_t min_map_version[2]; // U16
uint8_t max_map_version[2]; // U16
uint8_t total_internal_file_memory[4]; // U32
uint8_t avail_internal_file_memory[4]; // U32
uint8_t total_external_file_memory[4]; // U32
uint8_t avail_external_file_memory[4]; // U32
} msg_capabilities_t;
//-----------------------------------------------------------------------------
#if __APPLE__ || __linux
#include <sys/time.h>
#endif
static void
debug_out(const char* fmt, ...)
{
va_list ap;
va_start(ap, fmt);
fputs(MYNAME ": ", stderr);
vfprintf(stderr, fmt, ap);
va_end(ap);
}
static void
debug_out_time(const char* s)
{
#if __APPLE__ || __linux
struct timeval tv;
gettimeofday(&tv, NULL);
debug_out("%u.%03u %s", (unsigned)tv.tv_sec & 0xf, (unsigned)tv.tv_usec / 1000, s);
#else
debug_out("%u %s", (unsigned)time(NULL) & 0xf, s);
#endif
}
//-----------------------------------------------------------------------------
static uint16_t
checksum(const uint8_t* p, unsigned n)
{
int x = 0;
unsigned i;
for (i = n / 2; i > 0; i--) {
x += *p++;
x += *p++ << 8;
}
if (n & 1) {
x += *p;
}
return (uint16_t)-x;
}
//-----------------------------------------------------------------------------
// OS packet read/write wrappers
static unsigned
packet_read(void* buf)
{
unsigned n = delbin_os_ops.packet_read(buf);
if (n == 0) {
fatal(MYNAME ": read 0\n");
}
if (global_opts.debug_level >= DBGLVL_H) {
unsigned j;
const uint8_t* p = (const uint8_t*) buf;
debug_out_time("pcktrd");
for (j = 0; j < n; j++) {
warning(" %02x", p[j]);
}
if (global_opts.debug_level >= DBGLVL_H2) {
warning(" ");
for (j = 0; j < n; j++) {
int c = p[j];
warning("%c", isprint(c) ? c : '.');
}
}
warning("\n");
}
return n;
}
static void
packet_write(const void* buf, unsigned size)
{
unsigned n;
if (global_opts.debug_level >= DBGLVL_H) {
unsigned j;
const uint8_t* p = (const uint8_t*) buf;
debug_out_time("pcktwr");
for (j = 0; j < size; j++) {
warning(" %02x", p[j]);
}
if (global_opts.debug_level >= DBGLVL_H2) {
warning(" ");
for (j = 0; j < size; j++) {
int c = p[j];
warning("%c", isprint(c) ? c : '.');
}
}
warning("\n");
}
n = delbin_os_ops.packet_write(buf, size);
if (n != size) {
fatal(MYNAME ": short write %u %u\n", size, n);
}
}
//-----------------------------------------------------------------------------
// dynamically sized buffer with space reserved for message header and trailer
typedef struct {
// message data size
unsigned size;
// buffer size
unsigned capacity;
uint8_t* buf;
// convenience pointer to message data area
void* data;
} message_t;
static void
message_init(message_t* m)
{
m->capacity = 100;
m->buf = (uint8_t*)xmalloc(m->capacity);
m->data = m->buf + 2 + 8;
}
static void
message_init_size(message_t* m, unsigned size)
{
m->size = size;
m->capacity = 2 + 8 + size + 4;
m->buf = (uint8_t*)xmalloc(m->capacity);
m->data = m->buf + 2 + 8;
}
static void
message_free(message_t* m)
{
xfree(m->buf);
m->buf = NULL;
m->data = NULL;
}
static void
message_ensure_size(message_t* m, unsigned size)
{
m->size = size;
if (m->capacity < 2 + 8 + size + 4) {
m->capacity = 2 + 8 + size + 4;
xfree(m->buf);
m->buf = (uint8_t*)xmalloc(m->capacity);
m->data = m->buf + 2 + 8;
}
}
static unsigned
message_get_id(const message_t* m)
{
return le_readu16(m->buf + 4);
}
//-----------------------------------------------------------------------------
static void
message_write(unsigned msg_id, message_t* m)
{
unsigned chksum;
unsigned count;
unsigned n;
uint8_t* p = m->buf;
// header (2 start bytes filled in later)
p[2] = 0xdb;
p[3] = 0xfe;
le_write16(p + 4, msg_id);
// "data size" includes 4 trailer bytes
le_write16(p + 6, m->size + 4);
chksum = checksum(p + 2, 6);
le_write16(p + 8, chksum);
// message data (filled in by caller)
chksum = checksum((uint8_t*) m->data, m->size);
n = 2 + 8 + m->size;
// trailer (checksum and marker bytes)
le_write16(p + n, chksum);
p[n + 2] = 0xad;
p[n + 3] = 0xbc;
// size of message not counting packet start bytes
count = 8 + m->size + 4;
do {
const uint8_t save0 = p[0];
const uint8_t save1 = p[1];
n = delbin_os_packet_size - 2;
if (n > count) {
n = count;
}
// doc. says 0x20, device sends 0, probably ignored
p[0] = 0x20;
// valid bytes in packet after first 2
p[1] = n;
packet_write(p, 2 + n);
p[0] = save0;
p[1] = save1;
p += n;
count -= n;
} while (count != 0);
if (global_opts.debug_level >= DBGLVL_M) {
warning(MYNAME ": sent %x\n", msg_id);
}
}
// read from the payload of a single packet
static unsigned
read_depacketize_1(uint8_t** p, unsigned n, int new_packet)
{
static uint8_t buf[256];
static unsigned buf_i, buf_n;
if (new_packet) {
buf_n = 0;
}
while (buf_n == 0) {
packet_read(buf);
if (buf[1] <= delbin_os_packet_size - 2) {
buf_n = buf[1];
buf_i = 2;
}
}
*p = buf + buf_i;
if (n > buf_n) {
n = buf_n;
}
buf_n -= n;
buf_i += n;
return n;
}
// read from packet payloads until request is fulfilled
static void
read_depacketize(uint8_t* buf, unsigned n)
{
while (n) {
uint8_t* p;
unsigned nn = read_depacketize_1(&p, n, FALSE);
memcpy(buf, p, nn);
n -= nn;
buf += nn;
}
}
// Get one valid message.
// If a corrupted message with the right id is seen, return failure (0).
static unsigned
message_read_1(unsigned msg_id, message_t* m)
{
unsigned id;
for (;;) {
unsigned total;
unsigned n;
uint8_t buf[8];
uint8_t* p;
n = read_depacketize_1(&p, 8, FALSE);
memset(buf, 0, 8);
memcpy(buf, p, n);
while (buf[0] != 0xdb || buf[1] != 0xfe || checksum(buf, 6) != le_readu16(buf + 6)) {
// try for a message start at the beginning of next packet
n = read_depacketize_1(&p, 8, TRUE);
memset(buf, 0, 8);
memcpy(buf, p, n);
}
id = le_readu16(buf + 2);
total = le_readu16(buf + 4);
message_ensure_size(m, total - 4);
// copy in message head, really only need id field, do the rest for debugging
m->buf[0] = m->buf[1] = 0;
memcpy(m->buf + 2, buf, 8);
// read message body and trailer
read_depacketize((uint8_t*) m->data, total);
p = (uint8_t*)m->data + m->size;
if (checksum((uint8_t*) m->data, m->size) == le_readu16(p) &&
p[2] == 0xad && p[3] == 0xbc) {
if (global_opts.debug_level >= DBGLVL_M) {
warning(MYNAME ": received %x\n", id);
}
break;
}
if (global_opts.debug_level >= DBGLVL_L) {
warning(MYNAME ": corrupted message %x\n", id);
}
if (id == msg_id) {
id = 0;
break;
}
}
return id;
}
// Send MSG_ACK for given message
static void
message_ack(unsigned id, const message_t* m)
{
message_t ack;
char* p1;
const char* p2 = (const char*) m->data;
switch (id) {
case MSG_ACK:
case MSG_NACK:
case MSG_NAVIGATION:
case MSG_SATELLITE_INFO:
// don't ack these
return;
}
message_init_size(&ack, 4);
p1 = (char*) ack.data;
// ack payload is id and body checksum of acked message
le_write16(p1, id);
p1[2] = p2[m->size];
p1[3] = p2[m->size + 1];
message_write(MSG_ACK, &ack);
message_free(&ack);
}
// Get specific message, ignoring others. Sends ACK for non-interval messages.
// Gives up after at least READ_TIMEOUT-1 seconds have passed.
static int
message_read(unsigned msg_id, message_t* m)
{
unsigned id;
time_t time_start = time(NULL);
if (global_opts.debug_level >= DBGLVL_M) {
warning(MYNAME ": looking for %x\n", msg_id);
}
for (;;) {
id = message_read_1(msg_id, m);
if (id == 0) {
break;
}
if (id == MSG_ERROR) {
const uint8_t* p = (const uint8_t*) m->data;
fatal(MYNAME ": device error %u: \"%s\"\n", *p, p + 1);
}
message_ack(id, m);
if (id == msg_id || time(NULL) - time_start >= READ_TIMEOUT) {
break;
}
}
return id == msg_id;
}
// Read a sequence of messages, up to a MSG_TRANSFER_COMPLETE
static int
get_batch(message_t** array, unsigned* n)
{
int success = 1;
unsigned array_max = 100;
message_t* a = (message_t*) xmalloc(array_max * sizeof(message_t));
unsigned i = 0;
unsigned id;
if (global_opts.debug_level >= DBGLVL_M) {
warning(MYNAME ": begin get_batch\n");
}
do {
time_t time_start = time(NULL);
if (i == array_max) {
message_t* old_a = a;
array_max += array_max;
a = (message_t*) xmalloc(array_max * sizeof(message_t));
memcpy(a, old_a, i * sizeof(message_t));
xfree(old_a);
}
message_init(&a[i]);
for (;;) {
id = message_read_1(0, &a[i]);
switch (id) {
case MSG_NAVIGATION:
if (time(NULL) - time_start >= READ_TIMEOUT) {
success = 0;
break;
}
// fall through
case MSG_ACK:
case MSG_NACK:
case MSG_SATELLITE_INFO:
continue;
}
break;
}
message_ack(id, &a[i]);
i++;
} while (success && id != MSG_TRANSFER_COMPLETE);
if (success) {
*array = a;
*n = i - 1;
message_free(&a[*n]);
if (global_opts.debug_level >= DBGLVL_M) {
warning(MYNAME ": end get_batch, %u messages\n", *n);
}
} else {
while (i--) {
message_free(&a[i]);
}
xfree(a);
*array = NULL;
*n = 0;
if (global_opts.debug_level >= DBGLVL_M) {
warning(MYNAME ": end get_batch, failed\n");
}
}
return success;
}
typedef struct {
unsigned msg_id;
message_t msg;
} batch_array_t;
static batch_array_t* batch_array;
static unsigned batch_array_max;
static unsigned batch_array_i;
// add a message to sequence that will later be sent all at once
static void
add_to_batch(unsigned id, message_t* m)
{
if (batch_array_i == batch_array_max) {
char* old = (char*) batch_array;
if (batch_array_max == 0) {
batch_array_max = 50;
}
batch_array_max += batch_array_max;
batch_array = (batch_array_t*) xmalloc(batch_array_max * sizeof(*batch_array));
if (batch_array_i) {
memcpy(batch_array, old, batch_array_i * sizeof(*batch_array));
xfree(old);
}
}
batch_array[batch_array_i].msg_id = id;
batch_array[batch_array_i].msg = *m;
batch_array_i++;
memset(m, 0, sizeof(*m));
}
// send an accumulated sequence of messages
static void
send_batch(void)
{
message_t m;
const unsigned n = batch_array_i;
unsigned i;
unsigned progress = 0;
message_init(&m);
if (global_opts.debug_level >= DBGLVL_M) {
warning(MYNAME ": begin send_batch, %u messages\n", n);
}
for (i = 0; i < n; i++) {
unsigned timeout_count = 0;
time_t time_start = time(NULL);
// Can't really trigger this off either i or n as we don't
// know how the various packets map to actual waypts.
if (global_opts.verbose_status &&
(batch_array[i].msg_id == MSG_WAYPOINT_IN)) {
waypt_status_disp(waypoint_n, ++progress);
}
message_write(batch_array[i].msg_id, &batch_array[i].msg);
for (;;) {
unsigned id = message_read_1(0, &m);
switch (id) {
case MSG_ACK:
break;
case MSG_NAVIGATION:
if (time(NULL) - time_start >= 2) {
if (timeout_count) {
fatal(MYNAME ": send_batch timed out\n");
}
timeout_count++;
if (global_opts.debug_level >= DBGLVL_M) {
warning(MYNAME ": re-sending %x\n", batch_array[i].msg_id);
}
message_write(batch_array[i].msg_id, &batch_array[i].msg);
time_start = time(NULL);
}
// fall through
case MSG_NACK:
case MSG_SATELLITE_INFO:
continue;
default:
warning(MYNAME ": unexpected response message %x during send_batch\n", id);
continue;
}
break;
}
}
message_read(MSG_TRANSFER_COMPLETE, &m);
if (global_opts.debug_level >= DBGLVL_M) {
warning(MYNAME ": end send_batch\n");
}
for (i = n; i--;) {
message_free(&batch_array[i].msg);
}
xfree(batch_array);
message_free(&m);
batch_array_i = batch_array_max = 0;
}
//-----------------------------------------------------------------------------
// Coordinate conversion
static double
delbin_rad2deg(int32_t x)
{
return x * ((180 / M_PI) / 100000000);
}
static int32_t
delbin_deg2rad(double x)
{
return (int32_t)(x * ((M_PI / 180) * 100000000));
}
//-----------------------------------------------------------------------------
// Waypoint reading
static time_t
decode_time(const uint8_t* p)
{
struct tm t;
t.tm_year = p[0];
t.tm_mon = p[1] - 1;
t.tm_mday = p[2];
t.tm_hour = p[3];
t.tm_min = p[4];
t.tm_sec = p[5];
return mkgmtime(&t);
}
static Waypoint*
decode_waypoint(const void* data)
{
Waypoint* wp = new Waypoint;
const msg_waypoint_t* p = (const msg_waypoint_t*)data;