1 /********************************************************************
2 ** @source JEEPS application and data functions
3 **
4 ** @author Copyright (C) 1999 Alan Bleasby
5 ** @version 1.0
6 ** @modified Dec 28 1999 Alan Bleasby. First version
7 ** @modified Copyright (C) 2004, 2005, 2006 Robert Lipe
8 ** @modified Copyright (C) 2007 Achim Schumacher
9 ** @@
10 **
11 ** This library is free software; you can redistribute it and/or
12 ** modify it under the terms of the GNU Library General Public
13 ** License as published by the Free Software Foundation; either
14 ** version 2 of the License, or (at your option) any later version.
15 **
16 ** This library is distributed in the hope that it will be useful,
17 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
18 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
19 ** Library General Public License for more details.
20 **
21 ** You should have received a copy of the GNU Library General Public
22 ** License along with this library; if not, write to the
23 ** Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
24 ** Boston, MA 02110-1301, USA.
25 ********************************************************************/
26 #include "gps.h"
27 #include <cctype>
28 #include <cstdio>
29 #include <cstdlib>
30 #include <cstring>
31 #include <ctime>
32
33 #include <QtCore/QDateTime>
34
35 /*
36 * This violates the layering design, but is needed for device discovery.
37 * See the use of gps_is_usb and GPS_Packet_Read_usb below.
38 */
39 #include "garminusb.h"
40 #include "gpsserial.h"
41 #include "gpsusbint.h"
42
43 time_t gps_save_time;
44 double gps_save_lat;
45 double gps_save_lon;
46
47 #define XMIN(a,b) (a < b? a : b)
48
49 static int32 GPS_A000(const char* port);
50 static void GPS_A001(GPS_PPacket& packet);
51
52
53 static void GPS_A500_Translate(UC* s, GPS_PAlmanac* alm);
54 static void GPS_A500_Encode(UC* s, GPS_PAlmanac alm);
55 static void GPS_A300_Translate(UC* s, GPS_PTrack* trk);
56 static void GPS_A300_Encode(UC* s, GPS_PTrack trk);
57
58
59 static void GPS_D100_Get(GPS_PWay* way, UC* s);
60 static void GPS_D101_Get(GPS_PWay* way, UC* s);
61 static void GPS_D102_Get(GPS_PWay* way, UC* s);
62 static void GPS_D103_Get(GPS_PWay* way, UC* s);
63 static void GPS_D104_Get(GPS_PWay* way, UC* s);
64 static void GPS_D105_Get(GPS_PWay* way, UC* s);
65 static void GPS_D106_Get(GPS_PWay* way, UC* s);
66 static void GPS_D107_Get(GPS_PWay* way, UC* s);
67 static void GPS_D108_Get(GPS_PWay* way, UC* s);
68 static void GPS_D109_Get(GPS_PWay* way, UC* s, int protoid);
69 static void GPS_D150_Get(GPS_PWay* way, UC* s);
70 static void GPS_D151_Get(GPS_PWay* way, UC* s);
71 static void GPS_D152_Get(GPS_PWay* way, UC* s);
72 static void GPS_D154_Get(GPS_PWay* way, UC* s);
73 static void GPS_D155_Get(GPS_PWay* way, UC* s);
74
75 static void GPS_D100_Send(UC* data, GPS_PWay way, int32* len);
76 static void GPS_D101_Send(UC* data, GPS_PWay way, int32* len);
77 static void GPS_D102_Send(UC* data, GPS_PWay way, int32* len);
78 static void GPS_D103_Send(UC* data, GPS_PWay way, int32* len);
79 static void GPS_D104_Send(UC* data, GPS_PWay way, int32* len);
80 static void GPS_D105_Send(UC* data, GPS_PWay way, int32* len);
81 static void GPS_D106_Send(UC* data, GPS_PWay way, int32* len);
82 static void GPS_D107_Send(UC* data, GPS_PWay way, int32* len);
83 static void GPS_D108_Send(UC* data, GPS_PWay way, int32* len);
84 static void GPS_D109_Send(UC* data, GPS_PWay way, int32* len, int protoid);
85 static void GPS_D150_Send(UC* data, GPS_PWay way, int32* len);
86 static void GPS_D151_Send(UC* data, GPS_PWay way, int32* len);
87 static void GPS_D152_Send(UC* data, GPS_PWay way, int32* len);
88 static void GPS_D154_Send(UC* data, GPS_PWay way, int32* len);
89 static void GPS_D155_Send(UC* data, GPS_PWay way, int32* len);
90
91 static void GPS_D120_Get(int cat_num, char*s);
92
93 static void GPS_D200_Get(GPS_PWay* way, const UC* s);
94 static void GPS_D201_Get(GPS_PWay* way, UC* s);
95 static void GPS_D202_Get(GPS_PWay* way, UC* s);
96 static void GPS_D210_Get(GPS_PWay* way, UC* s);
97 static void GPS_D200_Send(UC* data, GPS_PWay way, int32* len);
98 static void GPS_D201_Send(UC* data, GPS_PWay way, int32* len);
99 static void GPS_D202_Send(UC* data, GPS_PWay way, int32* len);
100 static void GPS_D210_Send(UC* data, GPS_PWay way, int32* len);
101
102 static void GPS_D400_Get(GPS_PWay* way, UC* s);
103 static void GPS_D403_Get(GPS_PWay* way, UC* s);
104 static void GPS_D450_Get(GPS_PWay* way, UC* s);
105 static void GPS_D400_Send(UC* data, GPS_PWay way, int32* len);
106 static void GPS_D403_Send(UC* data, GPS_PWay way, int32* len);
107 static void GPS_D450_Send(UC* data, GPS_PWay way, int32* len);
108
109 static void GPS_D500_Send(UC* data, GPS_PAlmanac alm);
110 static void GPS_D501_Send(UC* data, GPS_PAlmanac alm);
111 static void GPS_D550_Send(UC* data, GPS_PAlmanac alm);
112 static void GPS_D551_Send(UC* data, GPS_PAlmanac alm);
113
114 static UC Is_Trackpoint_Invalid(GPS_PTrack trk);
115
116
117 int32 gps_save_id;
118 int gps_is_usb;
119 double gps_save_version;
120 char gps_save_string[GPS_ARB_LEN];
121
122 /*
123 * Internal function to copy what Garmin describes as a "Character Array".
124 * Dest buffer is padded with spaces and must not contain nulls. Optionally
125 * we uppercase the string because some models (III's and 12's) react
126 * violently to lower case data.
127 */
128 typedef enum { UpperNo = 0, UpperYes = 1 } copycase;
129
130 static
copy_char_array(UC ** dst,char * src,int count,copycase mustupper)131 void copy_char_array(UC** dst, char* src, int count, copycase mustupper)
132 {
133 UC* d = *dst;
134 int ocount = count;
135 do {
136 UC sc = *src++;
137 if (sc == 0) {
138 while (count--) {
139 *d++ = ' ';
140 }
141 break;
142 }
143 if (!isalnum(sc)) {
144 continue;
145 } else {
146 *d++ = mustupper == UpperYes ? toupper(sc) : sc;
147 }
148 } while (--count) ;
149 *dst += ocount;
150 }
151
152
153 /* @func GPS_Init ******************************************************
154 **
155 ** Initialise GPS communication
156 ** Get capabilities and store time lat/lon in case GPS requests
157 ** it later.
158 ** Find endian nature of hardware and store
159 **
160 ** @param [r] port [const char *] serial port
161 **
162 ** @return [int32] 1 if success -ve if error
163 ************************************************************************/
GPS_Init(const char * port)164 int32 GPS_Init(const char* port)
165 {
166 int32 ret;
167
168 (void) GPS_Util_Little();
169
170 ret = GPS_A000(port);
171 if (ret<0) {
172 return ret;
173 }
174 gps_save_time = GPS_Command_Get_Time(port);
175
176 /*
177 * Some units may be unable to return time, such as a C320 when in
178 * charging mode. Only consider it fatal if the unit returns an error,
179 * not just absence of returning a time.
180 */
181 if (gps_save_time < 0) {
182 return FRAMING_ERROR;
183 }
184
185 if (0 == strncmp(gps_save_string, "GPilotS", 7)) {
186 return 1;
187 }
188
189 return GPS_Command_Get_Position(port,&gps_save_lat,&gps_save_lon);
190 }
191
192
193 /* @funcstatic GPS_A000 ************************************************
194 **
195 ** Return product ID, version and description. Turn off PVT transfer
196 **
197 ** @param [r] port [const char *] serial port
198 **
199 ** @return [int32] 1 if success -ve if error
200 ************************************************************************/
GPS_A000(const char * port)201 static int32 GPS_A000(const char* port)
202 {
203 gpsdevh* fd;
204 GPS_PPacket tra;
205 GPS_PPacket rec;
206 int16 version;
207 int16 id;
208
209 if (!GPS_Device_On(port, &fd)) {
210 return gps_errno;
211 }
212
213 if (!GPS_Device_Flush(fd)) {
214 return gps_errno;
215 }
216
217 GPS_Make_Packet(&tra, LINK_ID[0].Pid_Product_Rqst,nullptr,0);
218 if (!GPS_Write_Packet(fd,tra)) {
219 return SERIAL_ERROR;
220 }
221
222 if (!GPS_Get_Ack(fd, &tra, &rec)) {
223 return SERIAL_ERROR;
224 }
225
226 if (!GPS_Packet_Read(fd, &rec)) {
227 return gps_errno;
228 }
229 GPS_Send_Ack(fd, &tra, &rec);
230
231 id = GPS_Util_Get_Short(rec.data);
232 version = GPS_Util_Get_Short((rec.data)+2);
233
234 (void) strcpy(gps_save_string,(char*)rec.data+4);
235 gps_save_id = id;
236 gps_save_version = ((double)version/100.);
237
238 GPS_User("Unit:\t%s\nID:\t%d\nVersion:\t%.2f\n",
239 gps_save_string, gps_save_id, gps_save_version);
240
241 #if 0
242 gps_date_time_transfer = pA600;
243 gps_date_time_type = pD600; /* All models so far */
244 gps_position_transfer = pA700;
245 gps_position_type = pD700; /* All models so far */
246 #else
247 gps_date_time_transfer = -1;
248 gps_date_time_type = -1;
249 gps_position_transfer = -1;
250 gps_position_type = -1;
251 #endif
252 gps_pvt_transfer = -1;
253 gps_pvt_type = -1;
254 gps_trk_transfer = -1;
255 gps_trk_type = -1;
256 gps_trk_hdr_type = -1;
257 gps_rte_link_type = -1;
258
259 gps_waypt_transfer = -1;
260 gps_waypt_type = -1;
261 gps_route_transfer = -1;
262 gps_rte_hdr_type = -1;
263 gps_rte_type = -1;
264
265 gps_prx_waypt_transfer = -1;
266 gps_prx_waypt_type = -1;
267 gps_almanac_transfer = -1;
268 gps_almanac_type = -1;
269
270 gps_lap_transfer = -1;
271 gps_lap_type = -1;
272 gps_run_transfer = -1;
273 gps_run_type = -1;
274 gps_run_crs_trk_type = -1;
275 gps_run_crs_trk_hdr_type = -1;
276 gps_workout_transfer = -1;
277 gps_workout_type = -1;
278 gps_workout_occurrence_type = -1;
279 gps_user_profile_transfer = -1;
280 gps_user_profile_type = -1;
281 gps_workout_limits_transfer = -1;
282 gps_workout_limits_type = -1;
283 gps_course_transfer = -1;
284 gps_course_type = -1;
285 gps_course_lap_transfer = -1;
286 gps_course_lap_type = -1;
287 gps_course_point_transfer = -1;
288 gps_course_point_type = -1;
289 gps_course_limits_transfer = -1;
290 gps_course_limits_type = -1;
291 gps_course_trk_transfer = -1;
292
293 gps_device_command = -1;
294 gps_link_type = -1;
295
296 if (!GPS_Device_Wait(fd)) {
297 GPS_Warning("A001 protocol not supported");
298 id = GPS_Protocol_Version_Change(id,version);
299 if (GPS_Protocol_Table_Set(id)<0) {
300 return GPS_UNSUPPORTED;
301 }
302 } else {
303 int i;
304 /*
305 * The unit may return more than one packet, so read and
306 * discard all but the product inquiry response. We have
307 * no way of knowing how many we'll get, so we have to keep
308 * reading until we incur a timeout.
309 * Worse still, the serial layer assumes a read timeout is a
310 * fatal error, while the USB layer (correctly) returns that error
311 * to the caller. So we call GPS_Device_Wait which spins into
312 * a delay/select for the serial system and a NOP for USB.
313 *
314 * Worse _yet_, this is the one place in all of Garmin Protocolsville
315 * where we don't know a priori how many packets will be sent in
316 * response. Since we want the lower levels of the USB handler
317 * to handle the ugliness of the "return to interrupt" packets, we
318 * reach behind that automation here and hand that ourselves.
319 */
320 for (i = 0; i < 25; i++) {
321 rec.type = 0;
322
323 if (gps_is_usb) {
324 GPS_Packet_Read_usb(fd, &rec, 0);
325 } else {
326 if (!GPS_Device_Wait(fd)) {
327 goto carry_on;
328 }
329
330 // Garmin 276C serial - not USB - sees a zero here, so we changed
331 // <= 0 to <0 on 2014-06-29 per Pierre Brial.
332
333 if (GPS_Packet_Read(fd, &rec) < 0) {
334 goto carry_on;
335 }
336
337 GPS_Send_Ack(fd, &tra, &rec);
338 }
339
340 if (rec.type == 0xfd) {
341 GPS_A001(rec);
342 goto carry_on;
343 }
344
345 /*
346 * If a 296 has previously been interrupted, it's going to
347 * ignore the session request (grrrr) and continue to send
348 * us left over packets. So if we see anything that isn't
349 * part of our A000 discovery cycle, reset the counter and
350 * continue to loop.
351 *
352 * Garmin acknowledges this is a firmware defect.
353 */
354 if (rec.type < 0xf8) {
355 i = 0;
356 }
357 }
358 fatal("Failed to find a product inquiry response.\n");
359 }
360
361 carry_on:
362 /* Make sure PVT is off as some GPS' have it on by default */
363 if (gps_pvt_transfer != -1) {
364 GPS_A800_Off(port,&fd);
365 }
366
367 if (!GPS_Device_Off(fd)) {
368 return gps_errno;
369 }
370
371 return 1;
372 }
373
374
375
376
377 /* @funcstatic GPS_A001 ************************************************
378 **
379 ** Extract protocol capabilities
380 ** This routine could do with re-writing. It's too long and obtuse.
381 **
382 ** @param [r] packet [GPS_PPacket] A001 protocol packet
383 **
384 ** @return [void]
385 ************************************************************************/
GPS_A001(GPS_PPacket & packet)386 static void GPS_A001(GPS_PPacket& packet)
387 {
388 int32 entries;
389 int32 i;
390 UC* p;
391 US tag;
392 US data;
393 US lasta=0;
394
395 entries = packet.n / 3;
396 p = packet.data;
397
398 for (i=0; i<entries; ++i,p+=3) {
399 tag = *p;
400 data = GPS_Util_Get_Short(p+1);
401
402 switch (tag) {
403 /* Only one type of P[hysical] so far */
404 case 'P':
405 if (data!=0) {
406 GPS_Protocol_Error(tag,data);
407 }
408 break;
409 case 'L':
410 gps_link_type = data;
411 break;
412 case 'A':
413 GPS_User("\nCapability %c%d:", tag, data);
414 lasta = data;
415 switch (data) {
416 case 10:
417 gps_device_command = pA010-10;
418 break;
419 case 11:
420 gps_device_command = pA011-10;
421 break;
422 case 100:
423 gps_waypt_transfer = pA100;
424 break;
425 case 101:
426 gps_category_transfer = pA101;
427 break;
428 case 200:
429 gps_route_transfer = pA200;
430 break;
431 case 201:
432 gps_route_transfer = pA201;
433 break;
434 case 300:
435 gps_trk_transfer = pA300;
436 break;
437 case 301:
438 gps_trk_transfer = pA301;
439 break;
440 case 302:
441 gps_trk_transfer = pA302;
442 /* Use A302 for course track transfer only if we don't
443 * have another protocol for it yet. This is in accordance
444 * with the Garmin docs for A1006 which say to use A302
445 * in this context only if A1012 is not reported.
446 */
447 if (gps_course_trk_transfer == -1) {
448 gps_course_trk_transfer = pA302;
449 }
450 break;
451 case 400:
452 gps_prx_waypt_transfer = pA400;
453 break;
454 case 500:
455 gps_almanac_transfer = pA500;
456 break;
457 case 600:
458 gps_date_time_transfer = pA600;
459 break;
460 case 650:
461 /* FlightBook Transfer Protocol */
462 break;
463 case 700:
464 gps_position_transfer = pA700;
465 break;
466 case 800:
467 gps_pvt_transfer = pA800;
468 break;
469 case 906:
470 gps_lap_transfer = pA906;
471 break;
472 case 1000:
473 gps_run_transfer = pA1000;
474 break;
475 case 1002:
476 gps_workout_transfer = pA1002;
477 break;
478 case 1004:
479 gps_user_profile_transfer = pA1004;
480 break;
481 case 1005:
482 gps_workout_limits_transfer = pA1005;
483 break;
484 case 1006:
485 gps_course_transfer = pA1006;
486 break;
487 case 1007:
488 gps_course_lap_transfer = pA1007;
489 break;
490 case 1008:
491 gps_course_point_transfer = pA1008;
492 break;
493 case 1009:
494 gps_course_limits_transfer = pA1009;
495 break;
496 case 1012:
497 gps_course_trk_transfer = pA1012;
498 break;
499 }
500 break;
501
502 case 'D':
503 GPS_User(" %c%d", tag, data);
504 if (lasta<200) {
505 switch (data) {
506 case 100:
507 case 101:
508 case 102:
509 case 103:
510 case 104:
511 case 105:
512 case 106:
513 case 107:
514 case 108:
515 case 109:
516 case 110:
517 /* 15x is panel-mount aviation */
518 case 150:
519 case 151:
520 case 152:
521 /* 153 not documented */
522 case 154:
523 case 155:
524 gps_waypt_type = data;
525 break;
526
527 /*
528 * Observed on Quest 3.0, 27xx, 27x, 29x.
529 */
530 case 120:
531 gps_category_type = data;
532 break;
533
534 case 200:
535 case 201:
536 case 202:
537 gps_rte_hdr_type = data;
538 break;
539
540 /* 210 Link packets appear in newer models, but the
541 * doc isn't sufficiently clear on what they really
542 * mean.
543 */
544 case 210:
545 gps_rte_link_type = data;
546 break;
547
548 }
549 }
550
551
552 else if (lasta<300) {
553 if (data>=200 && data <=202) {
554 gps_rte_hdr_type = data;
555 continue;
556 }
557 if (data==210) {
558 gps_rte_link_type = data;
559 continue;
560 }
561
562 if (data<=110 && data>=100) {
563 gps_rte_type = data;
564 continue;
565 }
566 if (data<153 && data>=150) {
567 gps_rte_type = data;
568 continue;
569 }
570 if (data<156 && data>=154) {
571 gps_rte_type = data;
572 continue;
573 }
574 if (data<451) {
575 if (data==400) {
576 gps_rte_type = pD400;
577 } else if (data==403) {
578 gps_rte_type = pD403;
579 } else if (data==450) {
580 gps_rte_type = pD450;
581 } else {
582 GPS_Protocol_Error(tag,data);
583 }
584 continue;
585 }
586 }
587
588 else if (lasta<400) {
589 switch (data) {
590 case 300:
591 gps_trk_type = pD300;
592 break;
593 case 301:
594 gps_trk_type = pD301;
595 break;
596 case 302:
597 gps_trk_type = pD302;
598 break;
599 case 303:
600 gps_trk_type = pD303;
601 break;
602 case 304:
603 gps_trk_type = pD304;
604 break;
605 case 310:
606 gps_trk_hdr_type = pD310;
607 break;
608 case 311:
609 gps_trk_hdr_type = pD311;
610 break;
611 case 312:
612 gps_trk_hdr_type = pD312;
613 break;
614 default:
615 GPS_Protocol_Error(tag,data);
616 break;
617 }
618 if (lasta==302 && gps_course_trk_transfer == pA302)
619 switch (data) {
620 case 300:
621 gps_run_crs_trk_type = pD300;
622 break;
623 case 301:
624 gps_run_crs_trk_type = pD301;
625 break;
626 case 302:
627 gps_run_crs_trk_type = pD302;
628 break;
629 case 303:
630 gps_run_crs_trk_type = pD303;
631 break;
632 case 304:
633 gps_run_crs_trk_type = pD304;
634 break;
635 case 310:
636 gps_run_crs_trk_hdr_type = pD310;
637 break;
638 case 311:
639 gps_run_crs_trk_hdr_type = pD311;
640 break;
641 case 312:
642 gps_run_crs_trk_hdr_type = pD312;
643 break;
644 default:
645 GPS_Protocol_Error(tag,data);
646 break;
647 }
648 continue;
649 }
650
651 else if (lasta<500) {
652 if ((data<=110 && data>=100) ||
653 (data<153 && data>=150) ||
654 (data<156 && data>=154)) {
655 gps_prx_waypt_type = data;
656 } else if (data==400) {
657 gps_prx_waypt_type = pD400;
658 } else if (data==403) {
659 gps_prx_waypt_type = pD403;
660 } else if (data==450) {
661 gps_prx_waypt_type = pD450;
662 } else {
663 GPS_Protocol_Error(tag,data);
664 }
665 continue;
666 }
667
668 else if (lasta<600) {
669 if (data==500) {
670 gps_almanac_type = pD500;
671 } else if (data==501) {
672 gps_almanac_type = pD501;
673 } else if (data==550) {
674 gps_almanac_type = pD550;
675 } else if (data==551) {
676 gps_almanac_type = pD551;
677 } else {
678 GPS_Protocol_Error(tag,data);
679 }
680 continue;
681 }
682
683 else if (lasta<650) {
684 if (data == 600) {
685 gps_date_time_type = pD600;
686 } else {
687 /* Stupid undocumented 60 D601 packets */
688 /* GPS_Protocol_Error(tag,data); */
689 continue;
690 }
691 continue;
692 }
693
694 else if (lasta<651) {
695 /* FlightBook Transfer Protocol, not handled */
696 continue;
697 }
698
699 else if (lasta<800) {
700 if (data!=700) {
701 GPS_Protocol_Error(tag,data);
702 } else {
703 gps_position_type = pD700;
704 }
705 continue;
706 }
707
708 else if (lasta<900) {
709 if (data == 800) {
710 gps_pvt_type = pD800;
711 }
712 /*
713 * Stupid, undocumented Vista 3.60 D802 packets
714 else
715 GPS_Protocol_Error(tag,data);
716 */
717 continue;
718 }
719
720 else if (lasta < 1000) {
721 if (data == 906) {
722 gps_lap_type = pD906;
723 } else if (data == 1001) {
724 gps_lap_type = pD1001;
725 } else if (data == 1011) {
726 gps_lap_type = pD1011;
727 } else if (data == 1015) {
728 gps_lap_type = pD1015;
729 }
730 continue;
731 }
732
733 else if (lasta < 1002) {
734 if (data == 1000) {
735 gps_run_type = pD1000;
736 } else if (data == 1009) {
737 gps_run_type = pD1009;
738 } else if (data == 1010) {
739 gps_run_type = pD1010;
740 }
741 continue;
742 }
743
744 else if (lasta < 1003) {
745 if (data == 1002) {
746 gps_workout_type = pD1002;
747 } else if (data == 1008) {
748 gps_workout_type = pD1008;
749 }
750 continue;
751 }
752
753 else if (lasta < 1004) {
754 if (data == 1003) {
755 gps_workout_occurrence_type = pD1003;
756 }
757 continue;
758 }
759
760 else if (lasta < 1005) {
761 if (data == 1004) {
762 gps_user_profile_type = pD1004;
763 }
764 continue;
765 }
766
767 else if (lasta < 1006) {
768 if (data == 1005) {
769 gps_workout_limits_type = pD1005;
770 }
771 continue;
772 }
773
774 else if (lasta < 1007) {
775 if (data == 1006) {
776 gps_course_type = pD1006;
777 }
778 continue;
779 }
780
781 else if (lasta < 1008) {
782 if (data == 1007) {
783 gps_course_lap_type = pD1007;
784 }
785 continue;
786 }
787
788 else if (lasta < 1009) {
789 if (data == 1012) {
790 gps_course_point_type = pD1012;
791 }
792 continue;
793 }
794
795 else if (lasta < 1010) {
796 if (data == 1013) {
797 gps_course_limits_type = pD1013;
798 }
799 continue;
800 } else if (lasta == 1012) {
801 /* We don't know which data types to expect for A1012. For now,
802 * accept the same ones as for A302 since it is used as a
803 * replacement for this.
804 */
805 switch (data) {
806 case 300:
807 gps_run_crs_trk_type = pD300;
808 break;
809 case 301:
810 gps_run_crs_trk_type = pD301;
811 break;
812 case 302:
813 gps_run_crs_trk_type = pD302;
814 break;
815 case 303:
816 gps_run_crs_trk_type = pD303;
817 break;
818 case 304:
819 gps_run_crs_trk_type = pD304;
820 break;
821 case 310:
822 gps_run_crs_trk_hdr_type = pD310;
823 break;
824 case 311:
825 gps_run_crs_trk_hdr_type = pD311;
826 break;
827 case 312:
828 gps_run_crs_trk_hdr_type = pD312;
829 break;
830 default:
831 GPS_Protocol_Error(tag,data);
832 break;
833 }
834 continue;
835 }
836 }
837 }
838
839 GPS_User("\nLink_type %d Device_command %d\n",
840 gps_link_type, gps_device_command);
841 GPS_User("Waypoint: Transfer %d Type %d\n",
842 gps_waypt_transfer, gps_waypt_type);
843 GPS_User("Route: Transfer %d Header %d Type %d\n",
844 gps_route_transfer, gps_rte_hdr_type, gps_rte_type);
845 GPS_User("Track: Transfer %d Type %d\n",
846 gps_trk_transfer, gps_trk_type);
847 QDateTime dt = QDateTime::fromMSecsSinceEpoch(gps_save_time * 1000);
848 GPS_User("GPS Time: %s\n", CSTR(dt.toString()));
849
850 return;
851 }
852
853
854
855
856 /* @func GPS_A100_Get ******************************************************
857 **
858 ** Get waypoint data from GPS
859 **
860 ** @param [r] port [const char *] serial port
861 ** @param [w] way [GPS_PWay **] waypoint array
862 **
863 ** @return [int32] number of waypoint entries
864 ************************************************************************/
GPS_A100_Get(const char * port,GPS_PWay ** way,int (* cb)(int,GPS_PWay *))865 int32 GPS_A100_Get(const char* port, GPS_PWay** way, int (*cb)(int, GPS_PWay*))
866 {
867 static UC data[2];
868 gpsdevh* fd;
869 GPS_PPacket tra;
870 GPS_PPacket rec;
871 int32 n;
872 int32 i;
873
874
875 if (!GPS_Device_On(port,&fd)) {
876 return gps_errno;
877 }
878
879 GPS_Util_Put_Short(data,
880 COMMAND_ID[gps_device_command].Cmnd_Transfer_Wpt);
881 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
882 data,2);
883
884 if (!GPS_Write_Packet(fd,tra)) {
885 GPS_Error("A100_Get: Cannot write packet");
886 return FRAMING_ERROR;
887 }
888
889 if (!GPS_Get_Ack(fd, &tra, &rec)) {
890 GPS_Error("A100_Get: No acknowledge");
891 return FRAMING_ERROR;
892 }
893
894 if (!GPS_Packet_Read(fd, &rec)) {
895 return gps_errno;
896 }
897 GPS_Send_Ack(fd, &tra, &rec);
898
899 n = GPS_Util_Get_Short(rec.data);
900
901 if (n)
902 if (!((*way)=(GPS_PWay*)malloc(n*sizeof(GPS_PWay)))) {
903 GPS_Error("A100_Get: Insufficient memory");
904 return MEMORY_ERROR;
905 }
906
907 for (i=0; i<n; ++i) {
908 if (!((*way)[i]=GPS_Way_New())) {
909 return MEMORY_ERROR;
910 }
911
912 if (!GPS_Packet_Read(fd, &rec)) {
913 return gps_errno;
914 }
915
916 if (!GPS_Send_Ack(fd, &tra, &rec)) {
917 return gps_errno;
918 }
919
920 switch (gps_waypt_type) {
921 case pD100:
922 GPS_D100_Get(&((*way)[i]),rec.data);
923 break;
924 case pD101:
925 GPS_D101_Get(&((*way)[i]),rec.data);
926 break;
927 case pD102:
928 GPS_D102_Get(&((*way)[i]),rec.data);
929 break;
930 case pD103:
931 GPS_D103_Get(&((*way)[i]),rec.data);
932 break;
933 case pD104:
934 GPS_D104_Get(&((*way)[i]),rec.data);
935 break;
936 case pD105:
937 GPS_D105_Get(&((*way)[i]),rec.data);
938 break;
939 case pD106:
940 GPS_D106_Get(&((*way)[i]),rec.data);
941 break;
942 case pD107:
943 GPS_D107_Get(&((*way)[i]),rec.data);
944 break;
945 case pD108:
946 GPS_D108_Get(&((*way)[i]),rec.data);
947 break;
948 case pD109:
949 GPS_D109_Get(&((*way)[i]),rec.data, 109);
950 break;
951 case pD110:
952 GPS_D109_Get(&((*way)[i]),rec.data, 110);
953 break;
954 case pD150:
955 GPS_D150_Get(&((*way)[i]),rec.data);
956 break;
957 case pD151:
958 GPS_D151_Get(&((*way)[i]),rec.data);
959 break;
960 case pD152:
961 GPS_D152_Get(&((*way)[i]),rec.data);
962 break;
963 case pD154:
964 GPS_D154_Get(&((*way)[i]),rec.data);
965 break;
966 case pD155:
967 GPS_D155_Get(&((*way)[i]),rec.data);
968 break;
969 default:
970 GPS_Error("A100_GET: Unknown waypoint protocol: %d", gps_waypt_type);
971 return PROTOCOL_ERROR;
972 }
973 /* Issue callback for status updates. */
974 if (cb) {
975 cb(n, &((*way)[i]));
976 }
977 }
978
979 if (!GPS_Packet_Read(fd, &rec)) {
980 return gps_errno;
981 }
982 if (!GPS_Send_Ack(fd, &tra, &rec)) {
983 return gps_errno;
984 }
985
986 if (rec.type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) {
987 GPS_Error("A100_GET: Error transferring waypoints. Expected %d completion code. Got %d. %d of %d received", LINK_ID[gps_link_type].Pid_Xfer_Cmplt, rec.type, i, n);
988 return FRAMING_ERROR;
989 }
990
991 if (i != n) {
992 GPS_Error("A100_GET: Waypoint entry number mismatch");
993 return FRAMING_ERROR;
994 }
995
996 if (!GPS_Device_Off(fd)) {
997 return gps_errno;
998 }
999
1000 return n;
1001 }
1002
1003
1004
1005
1006
1007 /* @func GPS_A100_Send **************************************************
1008 **
1009 ** Send waypoints to GPS
1010 **
1011 ** @param [r] port [const char *] serial port
1012 ** @param [r] trk [GPS_PWay *] waypoint array
1013 ** @param [r] n [int32] number of waypoint entries
1014 **
1015 ** @return [int32] success
1016 ************************************************************************/
GPS_A100_Send(const char * port,GPS_PWay * way,int32 n,int (* cb)(GPS_PWay *))1017 int32 GPS_A100_Send(const char* port, GPS_PWay* way, int32 n, int (*cb)(GPS_PWay*))
1018 {
1019 UC data[GPS_ARB_LEN];
1020 gpsdevh* fd;
1021 GPS_PPacket tra;
1022 GPS_PPacket rec;
1023 int32 i;
1024 int32 len;
1025
1026 if (!GPS_Device_On(port,&fd)) {
1027 return gps_errno;
1028 }
1029
1030 GPS_Util_Put_Short(data, (short) n);
1031 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records,
1032 data,2);
1033 if (!GPS_Write_Packet(fd,tra)) {
1034 return gps_errno;
1035 }
1036 if (!GPS_Get_Ack(fd, &tra, &rec)) {
1037 GPS_Error("Waypoint start data not acknowledged");
1038 return gps_errno;
1039 }
1040
1041
1042 for (i=0; i<n; ++i) {
1043 if (cb) {
1044 if (cb((GPS_PWay*) way[i])) { /* BUGBUG Wrong level of indirection */
1045 break;
1046 }
1047 }
1048
1049 switch (gps_waypt_type) {
1050 case pD100:
1051 GPS_D100_Send(data,way[i],&len);
1052 break;
1053 case pD101:
1054 GPS_D101_Send(data,way[i],&len);
1055 break;
1056 case pD102:
1057 GPS_D102_Send(data,way[i],&len);
1058 break;
1059 case pD103:
1060 GPS_D103_Send(data,way[i],&len);
1061 break;
1062 case pD104:
1063 GPS_D104_Send(data,way[i],&len);
1064 break;
1065 case pD105:
1066 GPS_D105_Send(data,way[i],&len);
1067 break;
1068 case pD106:
1069 GPS_D106_Send(data,way[i],&len);
1070 break;
1071 case pD107:
1072 GPS_D107_Send(data,way[i],&len);
1073 break;
1074 case pD108:
1075 GPS_D108_Send(data,way[i],&len);
1076 break;
1077 case pD109:
1078 GPS_D109_Send(data,way[i],&len, 109);
1079 break;
1080 case pD110:
1081 GPS_D109_Send(data,way[i],&len, 110);
1082 break;
1083 case pD150:
1084 GPS_D150_Send(data,way[i],&len);
1085 break;
1086 case pD151:
1087 GPS_D151_Send(data,way[i],&len);
1088 break;
1089 case pD152:
1090 GPS_D152_Send(data,way[i],&len);
1091 break;
1092 case pD154:
1093 GPS_D154_Send(data,way[i],&len);
1094 break;
1095 case pD155:
1096 GPS_D155_Send(data,way[i],&len);
1097 break;
1098 default:
1099 GPS_Error("Unknown waypoint protocol");
1100 return PROTOCOL_ERROR;
1101 }
1102
1103 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Wpt_Data,
1104 data, len);
1105
1106 if (!GPS_Write_Packet(fd,tra)) {
1107 return gps_errno;
1108 }
1109
1110 if (!GPS_Get_Ack(fd, &tra, &rec)) {
1111 GPS_Error("A100_Send: Pid_Wpt_Data not acknowledged");
1112 return gps_errno;
1113 }
1114 }
1115
1116 GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Wpt);
1117 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt,
1118 data,2);
1119 if (!GPS_Write_Packet(fd,tra)) {
1120 return gps_errno;
1121 }
1122 if (!GPS_Get_Ack(fd, &tra, &rec)) {
1123 GPS_Error("Waypoint complete data not acknowledged");
1124 return gps_errno;
1125 }
1126
1127 if (!GPS_Device_Off(fd)) {
1128 return gps_errno;
1129 }
1130
1131 return 1;
1132 }
1133
1134
1135 /*
1136 * Get the list of waypoint categories from the receiver.
1137 */
GPS_A101_Get(const char * port)1138 int32 GPS_A101_Get(const char* port)
1139 {
1140 static UC data[2];
1141 gpsdevh* fd;
1142 GPS_PPacket tra;
1143 GPS_PPacket rec;
1144 int32 n;
1145 int32 i;
1146
1147
1148 if (!GPS_Device_On(port,&fd)) {
1149 return gps_errno;
1150 }
1151
1152 GPS_Util_Put_Short(data,
1153 COMMAND_ID[gps_device_command].Cmnd_Transfer_Wpt_Cats);
1154 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
1155 data,2);
1156
1157 if (!GPS_Write_Packet(fd,tra)) {
1158 GPS_Error("A101_Get: Cannot write packet");
1159 return FRAMING_ERROR;
1160 }
1161
1162 if (!GPS_Get_Ack(fd, &tra, &rec)) {
1163 GPS_Error("A101_Get: No acknowledge");
1164 return FRAMING_ERROR;
1165 }
1166
1167 if (!GPS_Packet_Read(fd, &rec)) {
1168 return gps_errno;
1169 }
1170 GPS_Send_Ack(fd, &tra, &rec);
1171
1172 n = GPS_Util_Get_Short(rec.data);
1173 for (i = 0; i < n; ++i) {
1174 if (!GPS_Packet_Read(fd, &rec)) {
1175 return gps_errno;
1176 }
1177 if (!GPS_Send_Ack(fd, &tra, &rec)) {
1178 return gps_errno;
1179 }
1180 switch (gps_category_type) {
1181 case pD120:
1182 GPS_D120_Get(i,(char*) rec.data);
1183 break;
1184 }
1185 }
1186 if (!GPS_Packet_Read(fd, &rec)) {
1187 return gps_errno;
1188 }
1189 if (!GPS_Send_Ack(fd, &tra, &rec)) {
1190 return gps_errno;
1191 }
1192
1193 if (rec.type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) {
1194 GPS_Error("A101_Get: Error transferring waypoints. Expected %d completion code. Got %d. %d of %d received", LINK_ID[gps_link_type].Pid_Xfer_Cmplt, rec.type, i, n);
1195 return FRAMING_ERROR;
1196 }
1197
1198
1199 if (!GPS_Device_Off(fd)) {
1200 return gps_errno;
1201 }
1202
1203 return 1;
1204
1205 }
1206
1207 /* @funcstatic GPS_D100_Get *********************************************
1208 **
1209 ** Get waypoint data
1210 **
1211 ** @param [w] way [GPS_PWay *] waypoint array
1212 ** @param [r] s [UC *] packet data
1213 **
1214 ** @return [void]
1215 ************************************************************************/
GPS_D100_Get(GPS_PWay * way,UC * s)1216 static void GPS_D100_Get(GPS_PWay* way, UC* s)
1217 {
1218 UC* p;
1219 int32 i;
1220
1221 p=s;
1222
1223 (*way)->prot = 100;
1224 for (i=0; i<6; ++i) {
1225 (*way)->ident[i] = *p++;
1226 }
1227
1228 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1229 p+=sizeof(int32);
1230
1231 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1232 p+=sizeof(int32);
1233
1234 p+=sizeof(int32);
1235
1236 for (i=0; i<40; ++i) {
1237 (*way)->cmnt[i] = *p++;
1238 }
1239
1240 return;
1241 }
1242
1243
1244
1245 /* @funcstatic GPS_D101_Get *********************************************
1246 **
1247 ** Get waypoint data
1248 **
1249 ** @param [w] way [GPS_PWay *] waypoint array
1250 ** @param [r] s [UC *] packet data
1251 **
1252 ** @return [void]
1253 ************************************************************************/
GPS_D101_Get(GPS_PWay * way,UC * s)1254 static void GPS_D101_Get(GPS_PWay* way, UC* s)
1255 {
1256 UC* p;
1257 int32 i;
1258
1259 p=s;
1260
1261 (*way)->prot = 101;
1262 for (i=0; i<6; ++i) {
1263 (*way)->ident[i] = *p++;
1264 }
1265
1266 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1267 p+=sizeof(int32);
1268
1269 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1270 p+=sizeof(int32);
1271
1272 p+=sizeof(int32);
1273
1274 for (i=0; i<40; ++i) {
1275 (*way)->cmnt[i] = *p++;
1276 }
1277
1278 (*way)->dst = GPS_Util_Get_Float(p);
1279 p+=sizeof(float);
1280
1281 (*way)->smbl = *p;
1282
1283 return;
1284 }
1285
1286
1287
1288 /* @funcstatic GPS_D102_Get ********************************************
1289 **
1290 ** Get waypoint data
1291 **
1292 ** @param [w] way [GPS_PWay *] waypoint array
1293 ** @param [r] s [UC *] packet data
1294 **
1295 ** @return [void]
1296 ************************************************************************/
GPS_D102_Get(GPS_PWay * way,UC * s)1297 static void GPS_D102_Get(GPS_PWay* way, UC* s)
1298 {
1299 UC* p;
1300 int32 i;
1301
1302 p=s;
1303
1304 (*way)->prot = 102;
1305 for (i=0; i<6; ++i) {
1306 (*way)->ident[i] = *p++;
1307 }
1308
1309 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1310 p+=sizeof(int32);
1311
1312 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1313 p+=sizeof(int32);
1314
1315 p+=sizeof(int32);
1316
1317 for (i=0; i<40; ++i) {
1318 (*way)->cmnt[i] = *p++;
1319 }
1320
1321 (*way)->dst = GPS_Util_Get_Float(p);
1322 p+=sizeof(float);
1323
1324 (*way)->smbl = GPS_Util_Get_Short(p);
1325
1326
1327 return;
1328 }
1329
1330
1331
1332 /* @funcstatic GPS_D103_Get *********************************************
1333 **
1334 ** Get waypoint data
1335 **
1336 ** @param [w] way [GPS_PWay *] waypoint array
1337 ** @param [r] s [UC *] packet data
1338 **
1339 ** @return [void]
1340 ************************************************************************/
GPS_D103_Get(GPS_PWay * way,UC * s)1341 static void GPS_D103_Get(GPS_PWay* way, UC* s)
1342 {
1343 UC* p;
1344 int32 i;
1345
1346 p=s;
1347
1348 (*way)->prot = 103;
1349 for (i=0; i<6; ++i) {
1350 (*way)->ident[i] = *p++;
1351 }
1352
1353 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1354 p+=sizeof(int32);
1355
1356 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1357 p+=sizeof(int32);
1358
1359 p+=sizeof(int32);
1360
1361 for (i=0; i<40; ++i) {
1362 (*way)->cmnt[i] = *p++;
1363 }
1364
1365 (*way)->smbl = *p++;
1366 (*way)->dspl = *p;
1367
1368
1369 return;
1370 }
1371
1372
1373
1374 /* @funcstatic GPS_D104_Get ********************************************
1375 **
1376 ** Get waypoint data
1377 **
1378 ** @param [w] way [GPS_PWay *] waypoint array
1379 ** @param [r] s [UC *] packet data
1380 **
1381 ** @return [void]
1382 ************************************************************************/
GPS_D104_Get(GPS_PWay * way,UC * s)1383 static void GPS_D104_Get(GPS_PWay* way, UC* s)
1384 {
1385 UC* p;
1386 int32 i;
1387
1388 p=s;
1389
1390 (*way)->prot = 104;
1391 for (i=0; i<6; ++i) {
1392 (*way)->ident[i] = *p++;
1393 }
1394
1395 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1396 p+=sizeof(int32);
1397
1398 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1399 p+=sizeof(int32);
1400
1401 p+=sizeof(int32);
1402
1403 for (i=0; i<40; ++i) {
1404 (*way)->cmnt[i] = *p++;
1405 }
1406
1407 (*way)->dst = GPS_Util_Get_Float(p);
1408 p+=sizeof(float);
1409
1410 (*way)->smbl = GPS_Util_Get_Short(p);
1411 p+=sizeof(int16);
1412
1413 (*way)->dspl = *p;
1414
1415 return;
1416 }
1417
1418
1419
1420 /* @funcstatic GPS_D105_Get ********************************************
1421 **
1422 ** Get waypoint data
1423 **
1424 ** @param [w] way [GPS_PWay *] waypoint array
1425 ** @param [r] s [UC *] packet data
1426 **
1427 ** @return [void]
1428 ************************************************************************/
GPS_D105_Get(GPS_PWay * way,UC * s)1429 static void GPS_D105_Get(GPS_PWay* way, UC* s)
1430 {
1431 UC* p;
1432 UC* q;
1433
1434 p=s;
1435
1436 (*way)->prot = 105;
1437
1438 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1439 p+=sizeof(int32);
1440
1441 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1442 p+=sizeof(int32);
1443
1444 (*way)->smbl = GPS_Util_Get_Short(p);
1445 p+=sizeof(int16);
1446
1447 q = (UC*)(*way)->wpt_ident;
1448 while ((*q++ = *p++));
1449
1450 return;
1451 }
1452
1453
1454
1455 /* @funcstatic GPS_D106_Get ********************************************
1456 **
1457 ** Get waypoint data
1458 **
1459 ** @param [w] way [GPS_PWay *] waypoint array
1460 ** @param [r] s [UC *] packet data
1461 **
1462 ** @return [void]
1463 ************************************************************************/
GPS_D106_Get(GPS_PWay * way,UC * s)1464 void GPS_D106_Get(GPS_PWay* way, UC* s)
1465 {
1466 UC* p;
1467 UC* q;
1468 int32 i;
1469
1470 p=s;
1471
1472 (*way)->prot = 106;
1473
1474 (*way)->wpt_class = *p++;
1475
1476 for (i=0; i<13; ++i) {
1477 (*way)->subclass[i] = *p++;
1478 }
1479
1480 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1481 p+=sizeof(int32);
1482
1483 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1484 p+=sizeof(int32);
1485
1486 (*way)->smbl = GPS_Util_Get_Short(p);
1487 p+=sizeof(int16);
1488
1489 q = (UC*)(*way)->wpt_ident;
1490 while ((*q++ = *p++));
1491 q = (UC*)(*way)->lnk_ident;
1492 while ((*q++ = *p++));
1493
1494 return;
1495 }
1496
1497
1498
1499 /* @funcstatic GPS_D107_Get ********************************************
1500 **
1501 ** Get waypoint data
1502 **
1503 ** @param [w] way [GPS_PWay *] waypoint array
1504 ** @param [r] s [UC *] packet data
1505 **
1506 ** @return [void]
1507 ************************************************************************/
GPS_D107_Get(GPS_PWay * way,UC * s)1508 static void GPS_D107_Get(GPS_PWay* way, UC* s)
1509 {
1510 UC* p;
1511 int32 i;
1512
1513 p=s;
1514
1515 (*way)->prot = 107;
1516 for (i=0; i<6; ++i) {
1517 (*way)->ident[i] = *p++;
1518 }
1519
1520 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1521 p+=sizeof(int32);
1522
1523 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1524 p+=sizeof(int32);
1525
1526 p+=sizeof(int32);
1527
1528 for (i=0; i<40; ++i) {
1529 (*way)->cmnt[i] = *p++;
1530 }
1531
1532 (*way)->smbl = *p++;
1533 (*way)->dspl = *p++;
1534
1535 (*way)->dst = GPS_Util_Get_Float(p);
1536 p+=sizeof(float);
1537
1538 (*way)->colour = *p++;
1539
1540 return;
1541 }
1542
1543
1544
1545 /* @funcstatic GPS_D108_Get ********************************************
1546 **
1547 ** Get waypoint data
1548 **
1549 ** @param [w] way [GPS_PWay *] waypoint array
1550 ** @param [r] s [UC *] packet data
1551 **
1552 ** @return [void]
1553 ************************************************************************/
GPS_D108_Get(GPS_PWay * way,UC * s)1554 static void GPS_D108_Get(GPS_PWay* way, UC* s)
1555 {
1556 UC* p;
1557 UC* q;
1558
1559 int32 i;
1560
1561 p=s;
1562
1563 (*way)->prot = 108;
1564
1565 (*way)->wpt_class = *p++;
1566 (*way)->colour = *p++;
1567 (*way)->dspl = *p++;
1568 (*way)->attr = *p++;
1569 (*way)->smbl = GPS_Util_Get_Short(p);
1570 p+=sizeof(int16);
1571 for (i=0; i<18; ++i) {
1572 (*way)->subclass[i] = *p++;
1573 }
1574
1575 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1576 p+=sizeof(int32);
1577
1578 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1579 p+=sizeof(int32);
1580
1581 (*way)->alt = GPS_Util_Get_Float(p);
1582 p+=sizeof(float);
1583 (*way)->dpth = GPS_Util_Get_Float(p);
1584 p+=sizeof(float);
1585 (*way)->dst = GPS_Util_Get_Float(p);
1586 p+=sizeof(float);
1587
1588 for (i=0; i<2; ++i) {
1589 (*way)->state[i] = *p++;
1590 }
1591 for (i=0; i<2; ++i) {
1592 (*way)->cc[i] = *p++;
1593 }
1594
1595 q = (UC*)(*way)->ident;
1596 while ((*q++ = *p++));
1597
1598 q = (UC*)(*way)->cmnt;
1599 while ((*q++ = *p++));
1600
1601 q = (UC*)(*way)->facility;
1602 while ((*q++ = *p++));
1603
1604 q = (UC*)(*way)->city;
1605 while ((*q++ = *p++));
1606
1607 q = (UC*)(*way)->addr;
1608 while ((*q++ = *p++));
1609
1610 q = (UC*)(*way)->cross_road;
1611 while ((*q++ = *p++));
1612
1613 return;
1614 }
1615
1616 /* @funcstatic GPS_D109_Get ********************************************
1617 **
1618 ** Get waypoint data
1619 **
1620 ** @param [w] way [GPS_PWay *] waypoint array
1621 ** @param [r] s [UC *] packet data
1622 **
1623 ** @return [void]
1624 ** Quest uses D110's which are just like D109's but with the addition
1625 ** of temp, time, and wpt_cat stuck between ete and ident. Rather than
1626 ** duplicating the function, we just handle this at runtime.
1627 ************************************************************************/
GPS_D109_Get(GPS_PWay * way,UC * s,int protoid)1628 static void GPS_D109_Get(GPS_PWay* way, UC* s, int protoid)
1629 {
1630 UC* p;
1631 UC* q;
1632
1633 int32 i;
1634
1635 p=s;
1636
1637 (*way)->prot = protoid;
1638
1639 p++; /* data packet type */
1640 (*way)->wpt_class = *p++;
1641 (*way)->colour = *p & 0x1f;
1642 (*way)->dspl = (*p++ >> 5) & 3;
1643 (*way)->attr = *p++;
1644 (*way)->smbl = GPS_Util_Get_Short(p);
1645 p+=sizeof(int16);
1646 for (i=0; i<18; ++i) {
1647 (*way)->subclass[i] = *p++;
1648 }
1649
1650 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1651 p+=sizeof(int32);
1652
1653 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1654 p+=sizeof(int32);
1655
1656 (*way)->alt = GPS_Util_Get_Float(p);
1657 p+=sizeof(float);
1658 (*way)->dpth = GPS_Util_Get_Float(p);
1659 p+=sizeof(float);
1660 (*way)->dst = GPS_Util_Get_Float(p);
1661 p+=sizeof(float);
1662
1663 for (i=0; i<2; ++i) {
1664 (*way)->state[i] = *p++;
1665 }
1666 for (i=0; i<2; ++i) {
1667 (*way)->cc[i] = *p++;
1668 }
1669
1670 p += 4; /* Skip over "outbound link ete in seconds */
1671 if (protoid == 110) {
1672 float gps_temp;
1673 gps_temp = GPS_Util_Get_Float(p);
1674 p+=4;
1675 if (gps_temp <= 1.0e24) {
1676 (*way)->temperature_populated = 1;
1677 (*way)->temperature = gps_temp;
1678 }
1679
1680 uint32 gps_time = GPS_Util_Get_Uint(p);
1681 p+=4;
1682 /* The spec says that 0xffffffff is unknown, but the 60CSX with
1683 * firmware 2.5.0 writes zero.
1684 */
1685 if (gps_time != 0xffffffff && gps_time != 0) {
1686 (*way)->time_populated = 1;
1687 (*way)->time = GPS_Math_Gtime_To_Utime(gps_time);
1688 }
1689 (*way)->category = GPS_Util_Get_Short(p);
1690 p += 2;
1691 }
1692
1693 q = (UC*)(*way)->ident;
1694 while ((*q++ = *p++));
1695
1696 q = (UC*)(*way)->cmnt;
1697 while ((*q++ = *p++));
1698
1699 q = (UC*)(*way)->facility;
1700 while ((*q++ = *p++));
1701
1702 q = (UC*)(*way)->city;
1703 while ((*q++ = *p++));
1704
1705 q = (UC*)(*way)->addr;
1706 while ((*q++ = *p++));
1707
1708 q = (UC*)(*way)->cross_road;
1709 while ((*q++ = *p++));
1710
1711 return;
1712 }
1713
1714
1715 /* @funcstatic GPS_D150_Get ********************************************
1716 **
1717 ** Get waypoint data
1718 **
1719 ** @param [w] way [GPS_PWay *] waypoint array
1720 ** @param [r] s [UC *] packet data
1721 **
1722 ** @return [void]
1723 ************************************************************************/
GPS_D150_Get(GPS_PWay * way,UC * s)1724 static void GPS_D150_Get(GPS_PWay* way, UC* s)
1725 {
1726 UC* p;
1727 int32 i;
1728
1729 p=s;
1730
1731 (*way)->prot = 150;
1732 for (i=0; i<6; ++i) {
1733 (*way)->ident[i] = *p++;
1734 }
1735 for (i=0; i<2; ++i) {
1736 (*way)->cc[i] = *p++;
1737 }
1738 (*way)->wpt_class = *p++;
1739
1740 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1741 p+=sizeof(int32);
1742
1743 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1744 p+=sizeof(int32);
1745
1746 (*way)->alt = GPS_Util_Get_Short(p);
1747 p+=sizeof(int16);
1748
1749 for (i=0; i<24; ++i) {
1750 (*way)->city[i] = *p++;
1751 }
1752 for (i=0; i<2; ++i) {
1753 (*way)->state[i] = *p++;
1754 }
1755 for (i=0; i<30; ++i) {
1756 (*way)->name[i] = *p++;
1757 }
1758 for (i=0; i<40; ++i) {
1759 (*way)->cmnt[i] = *p++;
1760 }
1761
1762 return;
1763 }
1764
1765
1766
1767 /* @funcstatic GPS_D151_Get *********************************************
1768 **
1769 ** Get waypoint data
1770 **
1771 ** @param [w] way [GPS_PWay *] waypoint array
1772 ** @param [r] s [UC *] packet data
1773 **
1774 ** @return [void]
1775 ************************************************************************/
GPS_D151_Get(GPS_PWay * way,UC * s)1776 static void GPS_D151_Get(GPS_PWay* way, UC* s)
1777 {
1778 UC* p;
1779 int32 i;
1780
1781 p=s;
1782
1783 (*way)->prot = 151;
1784 for (i=0; i<6; ++i) {
1785 (*way)->ident[i] = *p++;
1786 }
1787
1788 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1789 p+=sizeof(int32);
1790
1791 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1792 p+=sizeof(int32);
1793
1794 p+=sizeof(int32);
1795
1796 for (i=0; i<40; ++i) {
1797 (*way)->cmnt[i] = *p++;
1798 }
1799
1800 (*way)->dst = GPS_Util_Get_Float(p);
1801 p+=sizeof(float);
1802
1803 for (i=0; i<30; ++i) {
1804 (*way)->name[i] = *p++;
1805 }
1806 for (i=0; i<24; ++i) {
1807 (*way)->city[i] = *p++;
1808 }
1809 for (i=0; i<2; ++i) {
1810 (*way)->state[i] = *p++;
1811 }
1812
1813 (*way)->alt = GPS_Util_Get_Short(p);
1814 p+=sizeof(int16);
1815
1816 for (i=0; i<2; ++i) {
1817 (*way)->cc[i] = *p++;
1818 }
1819
1820 ++p;
1821
1822 (*way)->wpt_class = *p;
1823
1824 return;
1825 }
1826
1827
1828
1829 /* @funcstatic GPS_D152_Get ********************************************
1830 **
1831 ** Get waypoint data
1832 **
1833 ** @param [w] way [GPS_PWay *] waypoint array
1834 ** @param [r] s [UC *] packet data
1835 **
1836 ** @return [void]
1837 ************************************************************************/
GPS_D152_Get(GPS_PWay * way,UC * s)1838 static void GPS_D152_Get(GPS_PWay* way, UC* s)
1839 {
1840 UC* p;
1841 int32 i;
1842
1843 p=s;
1844
1845 (*way)->prot = 152;
1846 for (i=0; i<6; ++i) {
1847 (*way)->ident[i] = *p++;
1848 }
1849
1850 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1851 p+=sizeof(int32);
1852
1853 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1854 p+=sizeof(int32);
1855
1856 p+=sizeof(int32);
1857
1858 for (i=0; i<40; ++i) {
1859 (*way)->cmnt[i] = *p++;
1860 }
1861
1862 (*way)->dst = GPS_Util_Get_Float(p);
1863 p+=sizeof(float);
1864
1865 for (i=0; i<30; ++i) {
1866 (*way)->name[i] = *p++;
1867 }
1868 for (i=0; i<24; ++i) {
1869 (*way)->city[i] = *p++;
1870 }
1871 for (i=0; i<2; ++i) {
1872 (*way)->state[i] = *p++;
1873 }
1874
1875 (*way)->alt = GPS_Util_Get_Short(p);
1876 p+=sizeof(int16);
1877
1878 for (i=0; i<2; ++i) {
1879 (*way)->cc[i] = *p++;
1880 }
1881
1882 ++p;
1883
1884 (*way)->wpt_class = *p;
1885
1886 return;
1887 }
1888
1889
1890 /* @funcstatic GPS_D154_Get ********************************************
1891 **
1892 ** Get waypoint data
1893 **
1894 ** @param [w] way [GPS_PWay *] waypoint array
1895 ** @param [r] s [UC *] packet data
1896 **
1897 ** @return [void]
1898 ************************************************************************/
GPS_D154_Get(GPS_PWay * way,UC * s)1899 static void GPS_D154_Get(GPS_PWay* way, UC* s)
1900 {
1901 UC* p;
1902 int32 i;
1903
1904 p=s;
1905
1906 (*way)->prot = 154;
1907 for (i=0; i<6; ++i) {
1908 (*way)->ident[i] = *p++;
1909 }
1910
1911 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1912 p+=sizeof(int32);
1913
1914 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1915 p+=sizeof(int32);
1916
1917 p+=sizeof(int32);
1918
1919 for (i=0; i<40; ++i) {
1920 (*way)->cmnt[i] = *p++;
1921 }
1922
1923 (*way)->dst = GPS_Util_Get_Float(p);
1924 p+=sizeof(float);
1925
1926 for (i=0; i<30; ++i) {
1927 (*way)->name[i] = *p++;
1928 }
1929 for (i=0; i<24; ++i) {
1930 (*way)->city[i] = *p++;
1931 }
1932 for (i=0; i<2; ++i) {
1933 (*way)->state[i] = *p++;
1934 }
1935
1936 (*way)->alt = GPS_Util_Get_Short(p);
1937 p+=sizeof(int16);
1938
1939 for (i=0; i<2; ++i) {
1940 (*way)->cc[i] = *p++;
1941 }
1942
1943 ++p;
1944
1945 (*way)->wpt_class = *p++;
1946
1947 (*way)->smbl = GPS_Util_Get_Short(p);
1948
1949 return;
1950 }
1951
1952
1953 /* @funcstatic GPS_D155_Get *********************************************
1954 **
1955 ** Get waypoint data
1956 **
1957 ** @param [w] way [GPS_PWay *] waypoint array
1958 ** @param [r] s [UC *] packet data
1959 **
1960 ** @return [void]
1961 ************************************************************************/
GPS_D155_Get(GPS_PWay * way,UC * s)1962 static void GPS_D155_Get(GPS_PWay* way, UC* s)
1963 {
1964 UC* p;
1965 int32 i;
1966
1967 p=s;
1968
1969 (*way)->prot = 155;
1970 for (i=0; i<6; ++i) {
1971 (*way)->ident[i] = *p++;
1972 }
1973
1974 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1975 p+=sizeof(int32);
1976
1977 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
1978 p+=sizeof(int32);
1979
1980 p+=sizeof(int32);
1981
1982 for (i=0; i<40; ++i) {
1983 (*way)->cmnt[i] = *p++;
1984 }
1985
1986 (*way)->dst = GPS_Util_Get_Float(p);
1987 p+=sizeof(float);
1988
1989 for (i=0; i<30; ++i) {
1990 (*way)->name[i] = *p++;
1991 }
1992 for (i=0; i<24; ++i) {
1993 (*way)->city[i] = *p++;
1994 }
1995 for (i=0; i<2; ++i) {
1996 (*way)->state[i] = *p++;
1997 }
1998
1999 (*way)->alt = GPS_Util_Get_Short(p);
2000 p+=sizeof(int16);
2001
2002 for (i=0; i<2; ++i) {
2003 (*way)->cc[i] = *p++;
2004 }
2005
2006 ++p;
2007
2008 (*way)->wpt_class = *p++;
2009
2010 (*way)->smbl = GPS_Util_Get_Short(p);
2011 p+=sizeof(int16);
2012
2013 (*way)->dspl = *p;
2014
2015 return;
2016 }
2017
2018 /*
2019 * We'll cheat for now. We know there are no more than 16 categories
2020 * as of this writing for no data type exposes more than 16 bits in the
2021 * bitmask of categories.
2022 */
2023 char gps_categories[16][17];
2024 /*
2025 * Read descriptor s into category number N;
2026 */
2027 static
GPS_D120_Get(int cat_num,char * s)2028 void GPS_D120_Get(int cat_num, char* s)
2029 {
2030 /* we're guaranteed to have no more than 16 chars plus a
2031 * null terminator.
2032 *
2033 * If the unit returned no string, the user has not configured one,
2034 * so mimic the behaviour of the 276/296.
2035 */
2036
2037 if (*s) {
2038 strncpy(gps_categories[cat_num], s, sizeof(gps_categories[0]));
2039 } else {
2040 snprintf(gps_categories[cat_num], sizeof(gps_categories[0]),
2041 "Category %d", cat_num+1);
2042 }
2043 }
2044
2045
2046 /* @funcstatic GPS_D100_Send *******************************************
2047 **
2048 ** Form waypoint data string
2049 **
2050 ** @param [w] data [UC *] string to write to
2051 ** @param [r] way [GPS_PWay] waypoint data
2052 ** @param [w] len [int32 *] packet length
2053 **
2054 ** @return [void]
2055 ************************************************************************/
GPS_D100_Send(UC * data,GPS_PWay way,int32 * len)2056 static void GPS_D100_Send(UC* data, GPS_PWay way, int32* len)
2057 {
2058 UC* p;
2059
2060 p = data;
2061
2062 copy_char_array(&p, way->ident, 6, UpperYes);
2063 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
2064 p+=sizeof(int32);
2065 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
2066 p+=sizeof(int32);
2067 GPS_Util_Put_Uint(p,0);
2068 p+=sizeof(int32);
2069 copy_char_array(&p, way->cmnt, 40, UpperYes);
2070
2071 *len = 58;
2072
2073 return;
2074 }
2075
2076
2077 /* @funcstatic GPS_D101_Send ********************************************
2078 **
2079 ** Form waypoint data string
2080 **
2081 ** @param [w] data [UC *] string to write to
2082 ** @param [r] way [GPS_PWay] waypoint data
2083 ** @param [w] len [int32 *] packet length
2084 **
2085 ** @return [void]
2086 ************************************************************************/
GPS_D101_Send(UC * data,GPS_PWay way,int32 * len)2087 static void GPS_D101_Send(UC* data, GPS_PWay way, int32* len)
2088 {
2089 UC* p;
2090
2091 p = data;
2092
2093 copy_char_array(&p, way->ident, 6, UpperYes);
2094 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
2095 p+=sizeof(int32);
2096 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
2097 p+=sizeof(int32);
2098 GPS_Util_Put_Uint(p,0);
2099 p+=sizeof(int32);
2100 copy_char_array(&p, way->cmnt, 40, UpperYes);
2101
2102
2103 GPS_Util_Put_Float(p,way->dst);
2104 p+= sizeof(float);
2105
2106 *p = way->smbl;
2107
2108 *len = 63;
2109
2110 return;
2111 }
2112
2113
2114 /* @funcstatic GPS_D102_Send ********************************************
2115 **
2116 ** Form waypoint data string
2117 **
2118 ** @param [w] data [UC *] string to write to
2119 ** @param [r] way [GPS_PWay] waypoint data
2120 ** @param [w] len [int32 *] packet length
2121 **
2122 ** @return [void]
2123 ************************************************************************/
GPS_D102_Send(UC * data,GPS_PWay way,int32 * len)2124 static void GPS_D102_Send(UC* data, GPS_PWay way, int32* len)
2125 {
2126 UC* p;
2127
2128 p = data;
2129
2130 copy_char_array(&p, way->ident, 6, UpperYes);
2131 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
2132 p+=sizeof(int32);
2133 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
2134 p+=sizeof(int32);
2135 GPS_Util_Put_Uint(p,0);
2136 p+=sizeof(int32);
2137 copy_char_array(&p, way->cmnt, 40, UpperYes);
2138
2139 GPS_Util_Put_Float(p,way->dst);
2140 p+= sizeof(float);
2141
2142 GPS_Util_Put_Short(p,(US) way->smbl);
2143
2144 *len = 64;
2145
2146 return;
2147 }
2148
2149
2150 /* @funcstatic GPS_D103_Send *******************************************
2151 **
2152 ** Form waypoint data string
2153 **
2154 ** @param [w] data [UC *] string to write to
2155 ** @param [r] way [GPS_PWay] waypoint data
2156 ** @param [w] len [int32 *] packet length
2157 **
2158 ** @return [void]
2159 ************************************************************************/
GPS_D103_Send(UC * data,GPS_PWay way,int32 * len)2160 static void GPS_D103_Send(UC* data, GPS_PWay way, int32* len)
2161 {
2162 UC* p;
2163
2164 p = data;
2165
2166 copy_char_array(&p, way->ident, 6, UpperYes);
2167
2168 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
2169 p+=sizeof(int32);
2170 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
2171 p+=sizeof(int32);
2172 GPS_Util_Put_Uint(p,0);
2173 p+=sizeof(int32);
2174 copy_char_array(&p, way->cmnt, 40, UpperYes);
2175
2176 *p++ = (UC) way->smbl;
2177 *p = (UC) way->dspl;
2178
2179 *len = 60;
2180
2181 return;
2182 }
2183
2184
2185 /* @funcstatic GPS_D104_Send ********************************************
2186 **
2187 ** Form waypoint data string
2188 **
2189 ** @param [w] data [UC *] string to write to
2190 ** @param [r] way [GPS_PWay] waypoint data
2191 ** @param [w] len [int32 *] packet length
2192 **
2193 ** @return [void]
2194 ************************************************************************/
GPS_D104_Send(UC * data,GPS_PWay way,int32 * len)2195 static void GPS_D104_Send(UC* data, GPS_PWay way, int32* len)
2196 {
2197 UC* p;
2198
2199 p = data;
2200
2201 copy_char_array(&p, way->ident, 6, UpperYes);
2202 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
2203 p+=sizeof(int32);
2204 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
2205 p+=sizeof(int32);
2206 GPS_Util_Put_Uint(p,0);
2207 p+=sizeof(int32);
2208 /* byonke confirms that sending lower case comment data to a III+
2209 * results in the comment being truncated there. So we uppercase
2210 * the entire comment.
2211 */
2212 copy_char_array(&p, way->cmnt, 40, UpperYes);
2213
2214 GPS_Util_Put_Float(p,way->dst);
2215 p+= sizeof(float);
2216
2217 GPS_Util_Put_Short(p, (int16) way->smbl);
2218 p+=sizeof(int16);
2219
2220 *p = 3; /* display symbol with waypoint name */
2221
2222 *len = 65;
2223
2224 return;
2225 }
2226
2227
2228 /* @funcstatic GPS_D105_Send *******************************************
2229 **
2230 ** Form waypoint data string
2231 **
2232 ** @param [w] data [UC *] string to write to
2233 ** @param [r] way [GPS_PWay] waypoint data
2234 ** @param [w] len [int32 *] packet length
2235 **
2236 ** @return [void]
2237 ************************************************************************/
GPS_D105_Send(UC * data,GPS_PWay way,int32 * len)2238 static void GPS_D105_Send(UC* data, GPS_PWay way, int32* len)
2239 {
2240 UC* p;
2241 UC* q;
2242
2243 p = data;
2244
2245 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
2246 p+=sizeof(int32);
2247 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
2248 p+=sizeof(int32);
2249
2250 GPS_Util_Put_Short(p, (int16) way->smbl);
2251 p+=sizeof(int16);
2252
2253 q = (UC*) way->wpt_ident;
2254 while ((*p++ = *q++));
2255
2256
2257 *len = p-data;
2258
2259 return;
2260 }
2261
2262
2263 /* @funcstatic GPS_D106_Send ********************************************
2264 **
2265 ** Form waypoint data string
2266 **
2267 ** @param [w] data [UC *] string to write to
2268 ** @param [r] way [GPS_PWay] waypoint data
2269 ** @param [w] len [int32 *] packet length
2270 **
2271 ** @return [void]
2272 ************************************************************************/
GPS_D106_Send(UC * data,GPS_PWay way,int32 * len)2273 static void GPS_D106_Send(UC* data, GPS_PWay way, int32* len)
2274 {
2275 UC* p;
2276 UC* q;
2277 int32 i;
2278
2279 p = data;
2280
2281 *p++ = way->wpt_class;
2282 for (i=0; i<13; ++i) {
2283 *p++ = way->subclass[i];
2284 }
2285 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
2286 p+=sizeof(int32);
2287 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
2288 p+=sizeof(int32);
2289
2290 GPS_Util_Put_Short(p, (int16) way->smbl);
2291 p+=sizeof(int16);
2292
2293 q = (UC*) way->wpt_ident;
2294 while ((*p++ = *q++));
2295 q = (UC*) way->lnk_ident;
2296 while ((*p++ = *q++));
2297
2298 *len = p-data;
2299
2300 return;
2301 }
2302
2303
2304 /* @funcstatic GPS_D107_Send ********************************************
2305 **
2306 ** Form waypoint data string
2307 **
2308 ** @param [w] data [UC *] string to write to
2309 ** @param [r] way [GPS_PWay] waypoint data
2310 ** @param [w] len [int32 *] packet length
2311 **
2312 ** @return [void]
2313 ************************************************************************/
GPS_D107_Send(UC * data,GPS_PWay way,int32 * len)2314 static void GPS_D107_Send(UC* data, GPS_PWay way, int32* len)
2315 {
2316 UC* p;
2317
2318 p = data;
2319
2320 copy_char_array(&p, way->ident, 6, UpperYes);
2321 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
2322 p+=sizeof(int32);
2323 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
2324 p+=sizeof(int32);
2325 GPS_Util_Put_Uint(p,0);
2326 p+=sizeof(int32);
2327 copy_char_array(&p, way->cmnt, 40, UpperYes);
2328
2329 *p++ = way->smbl;
2330 *p++ = way->dspl;
2331
2332 GPS_Util_Put_Float(p,way->dst);
2333 p+= sizeof(float);
2334
2335 *p = way->colour;
2336
2337 *len = 65;
2338
2339 return;
2340 }
2341
2342
2343
2344 /* @funcstatic GPS_D108_Send ********************************************
2345 **
2346 ** Form waypoint data string
2347 **
2348 ** @param [w] data [UC *] string to write to
2349 ** @param [r] way [GPS_PWay] waypoint data
2350 ** @param [w] len [int32 *] packet length
2351 **
2352 ** @return [void]
2353 ************************************************************************/
GPS_D108_Send(UC * data,GPS_PWay way,int32 * len)2354 static void GPS_D108_Send(UC* data, GPS_PWay way, int32* len)
2355 {
2356 UC* p;
2357 UC* q;
2358
2359 int32 i;
2360
2361 p = data;
2362
2363 *p++ = way->wpt_class;
2364 *p++ = way->colour;
2365 *p++ = way->dspl;
2366 *p++ = 0x60;
2367 GPS_Util_Put_Short(p,(US) way->smbl);
2368 p+=sizeof(int16);
2369 for (i=0; i<18; ++i) {
2370 *p++ = way->subclass[i];
2371 }
2372 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
2373 p+=sizeof(int32);
2374 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
2375 p+=sizeof(int32);
2376
2377 if (way->alt_is_unknown) {
2378 GPS_Util_Put_Float(p,(const float) 1.0e25);
2379 } else {
2380 GPS_Util_Put_Float(p,way->alt);
2381 }
2382 p+=sizeof(float);
2383 GPS_Util_Put_Float(p,way->dpth);
2384 p+=sizeof(float);
2385 GPS_Util_Put_Float(p,way->dst);
2386 p+=sizeof(float);
2387
2388 for (i=0; i<2; ++i) {
2389 *p++ = way->state[i];
2390 }
2391 for (i=0; i<2; ++i) {
2392 *p++ = way->cc[i];
2393 }
2394
2395
2396 q = (UC*) way->ident;
2397 i = XMIN(51, sizeof(way->ident));
2398 while ((*p++ = *q++) && i--);
2399 q = (UC*) way->cmnt;
2400 i = XMIN(51, sizeof(way->cmnt));
2401 while ((*p++ = *q++) && i--);
2402 q = (UC*) way->facility;
2403 i = XMIN(31, sizeof(way->facility));
2404 while ((*p++ = *q++) && i--);
2405 q = (UC*) way->city;
2406 i = XMIN(25, sizeof(way->city));
2407 while ((*p++ = *q++) && i--);
2408 q = (UC*) way->addr;
2409 i = XMIN(51, sizeof(way->addr));
2410 while ((*p++ = *q++) && i--);
2411 q = (UC*) way->cross_road;
2412 i = XMIN(51, sizeof(way->cross_road));
2413 while ((*p++ = *q++) && i--);
2414
2415 *len = p-data;
2416
2417 return;
2418 }
2419
2420
2421 /* @funcstatic GPS_D109_Send ********************************************
2422 **
2423 ** Form waypoint data string
2424 **
2425 ** @param [w] data [UC *] string to write to
2426 ** @param [r] way [GPS_PWay] waypoint data
2427 ** @param [w] len [int32 *] packet length
2428 **
2429 ** @return [void]
2430 ** D109's and D110's are so similar, we handle them with the same code.
2431 ************************************************************************/
GPS_D109_Send(UC * data,GPS_PWay way,int32 * len,int protoid)2432 static void GPS_D109_Send(UC* data, GPS_PWay way, int32* len, int protoid)
2433 {
2434 UC* p;
2435 UC* q;
2436
2437 int32 i;
2438
2439 p = data;
2440
2441 *p++ = 1; /* data packet type; must be 1 for D109 and D110 */
2442 *p++ = way->wpt_class;
2443
2444 *p++ = ((way->dspl & 3) << 5) | 0x1f; /* colour & display */
2445
2446 if (protoid == 109) { /* attr */
2447 *p++ = 0x70;
2448 } else if (protoid == 110) {
2449 *p++ = 0x80;
2450 } else {
2451 GPS_Warning("Unknown protoid in GPS_D109_Send.");
2452 }
2453 GPS_Util_Put_Short(p,(US) way->smbl);
2454 p+=sizeof(int16);
2455 for (i=0; i<18; ++i) {
2456 *p++ = way->subclass[i];
2457 }
2458 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
2459 p+=sizeof(int32);
2460 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
2461 p+=sizeof(int32);
2462 if (way->alt_is_unknown) {
2463 GPS_Util_Put_Float(p,(const float) 1.0e25);
2464 } else {
2465 GPS_Util_Put_Float(p,way->alt);
2466 }
2467 p+=sizeof(float);
2468 GPS_Util_Put_Float(p,way->dpth);
2469 p+=sizeof(float);
2470 GPS_Util_Put_Float(p,way->dst);
2471 p+=sizeof(float);
2472
2473 for (i=0; i<2; ++i) {
2474 *p++ = way->state[i];
2475 }
2476 for (i=0; i<2; ++i) {
2477 *p++ = way->cc[i];
2478 }
2479 for (i=0; i<4; ++i) {
2480 *p++ = 0xff; /* D109 silliness for ETE */
2481 }
2482 if (protoid == 110) {
2483 float temp = 1.0e25f;
2484
2485 GPS_Util_Put_Float(p, temp);
2486 p += 4;
2487
2488 if (way->time_populated) {
2489 GPS_Util_Put_Uint(p,GPS_Math_Utime_To_Gtime(way->time));
2490 p+=sizeof(uint32);
2491 } else {
2492 for (i=0; i<4; ++i) {
2493 *p++ = 0xff; /* unknown time*/
2494 }
2495 }
2496
2497 GPS_Util_Put_Short(p, (US) way->category); /* D110 category */
2498 p += 2;
2499 }
2500
2501 q = (UC*) way->ident;
2502 i = XMIN(51, sizeof(way->ident));
2503 while ((*p++ = *q++) && i--);
2504 q = (UC*) way->cmnt;
2505 i = XMIN(51, sizeof(way->cmnt));
2506 while ((*p++ = *q++) && i--);
2507 q = (UC*) way->facility;
2508 i = XMIN(31, sizeof(way->facility));
2509 while ((*p++ = *q++) && i--);
2510 q = (UC*) way->city;
2511 i = XMIN(25, sizeof(way->city));
2512 while ((*p++ = *q++) && i--);
2513 q = (UC*) way->addr;
2514 i = XMIN(51, sizeof(way->addr));
2515 while ((*p++ = *q++) && i--);
2516 q = (UC*) way->cross_road;
2517 i = XMIN(51, sizeof(way->cross_road));
2518 while ((*p++ = *q++) && i--);
2519 *len = p-data;
2520 return;
2521 }
2522
2523
2524 /* @funcstatic GPS_D150_Send ********************************************
2525 **
2526 ** Form waypoint data string
2527 **
2528 ** @param [w] data [UC *] string to write to
2529 ** @param [r] way [GPS_PWay] waypoint data
2530 ** @param [w] len [int32 *] packet length
2531 **
2532 ** @return [void]
2533 ************************************************************************/
GPS_D150_Send(UC * data,GPS_PWay way,int32 * len)2534 static void GPS_D150_Send(UC* data, GPS_PWay way, int32* len)
2535 {
2536 UC* p;
2537 int32 i;
2538
2539 p = data;
2540
2541 copy_char_array(&p, way->ident, 6, UpperYes);
2542 for (i=0; i<2; ++i) {
2543 *p++ = way->cc[i];
2544 }
2545
2546 if (way->wpt_class == 7) {
2547 way->wpt_class = 0;
2548 }
2549 *p++ = way->wpt_class;
2550
2551 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
2552 p+=sizeof(int32);
2553 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
2554 p+=sizeof(int32);
2555
2556 GPS_Util_Put_Short(p,(US) way->alt);
2557 p+=sizeof(int16);
2558
2559 copy_char_array(&p, way->city, 24, UpperYes);
2560 copy_char_array(&p, way->state, 2, UpperYes);
2561 copy_char_array(&p, way->name, 30, UpperYes);
2562 copy_char_array(&p, way->cmnt, 40, UpperYes);
2563
2564 *len = 115;
2565
2566 return;
2567 }
2568
2569
2570 /* @funcstatic GPS_D151_Send ********************************************
2571 **
2572 ** Form waypoint data string
2573 **
2574 ** @param [w] data [UC *] string to write to
2575 ** @param [r] way [GPS_PWay] waypoint data
2576 ** @param [w] len [int32 *] packet length
2577 **
2578 ** @return [void]
2579 ************************************************************************/
GPS_D151_Send(UC * data,GPS_PWay way,int32 * len)2580 static void GPS_D151_Send(UC* data, GPS_PWay way, int32* len)
2581 {
2582 UC* p;
2583 int32 i;
2584
2585 p = data;
2586
2587 copy_char_array(&p, way->ident, 6, UpperYes);
2588
2589 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
2590 p+=sizeof(int32);
2591 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
2592 p+=sizeof(int32);
2593 GPS_Util_Put_Uint(p,0);
2594 p+=sizeof(int32);
2595 copy_char_array(&p, way->cmnt, 40, UpperYes);
2596 GPS_Util_Put_Float(p,way->dst);
2597 p+=sizeof(float);
2598
2599 copy_char_array(&p, way->name, 30, UpperYes);
2600 copy_char_array(&p, way->city, 24, UpperYes);
2601 copy_char_array(&p, way->state, 2, UpperYes);
2602
2603 GPS_Util_Put_Short(p,(US) way->alt);
2604 p+=sizeof(int16);
2605
2606 for (i=0; i<2; ++i) {
2607 *p++ = way->cc[i];
2608 }
2609 *p++ = 0;
2610
2611 if (way->wpt_class == 3) {
2612 way->wpt_class = 0;
2613 }
2614 *p = way->wpt_class;
2615
2616 *len = 124;
2617
2618 return;
2619 }
2620
2621
2622
2623 /* @funcstatic GPS_D152_Send ********************************************
2624 **
2625 ** Form waypoint data string
2626 **
2627 ** @param [w] data [UC *] string to write to
2628 ** @param [r] way [GPS_PWay] waypoint data
2629 ** @param [w] len [int32 *] packet length
2630 **
2631 ** @return [void]
2632 ************************************************************************/
GPS_D152_Send(UC * data,GPS_PWay way,int32 * len)2633 static void GPS_D152_Send(UC* data, GPS_PWay way, int32* len)
2634 {
2635 UC* p;
2636 int32 i;
2637
2638 p = data;
2639
2640 copy_char_array(&p, way->ident, 6, UpperYes);
2641
2642 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
2643 p+=sizeof(int32);
2644 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
2645 p+=sizeof(int32);
2646 GPS_Util_Put_Uint(p,0);
2647 p+=sizeof(int32);
2648 copy_char_array(&p, way->cmnt, 40, UpperYes);
2649 GPS_Util_Put_Float(p,way->dst);
2650 p+=sizeof(float);
2651
2652 copy_char_array(&p, way->name, 30, UpperYes);
2653 copy_char_array(&p, way->city, 24, UpperYes);
2654 copy_char_array(&p, way->state, 2, UpperYes);
2655
2656 GPS_Util_Put_Short(p,(US) way->alt);
2657 p+=sizeof(int16);
2658
2659 for (i=0; i<2; ++i) {
2660 *p++ = way->cc[i];
2661 }
2662 *p++ = 0;
2663
2664 if (way->wpt_class == 5) {
2665 way->wpt_class = 0;
2666 }
2667 *p = way->wpt_class;
2668
2669 *len = 124;
2670
2671 return;
2672 }
2673
2674
2675 /* @funcstatic GPS_D154_Send *******************************************
2676 **
2677 ** Form waypoint data string
2678 **
2679 ** @param [w] data [UC *] string to write to
2680 ** @param [r] way [GPS_PWay] waypoint data
2681 ** @param [w] len [int32 *] packet length
2682 **
2683 ** @return [void]
2684 ************************************************************************/
GPS_D154_Send(UC * data,GPS_PWay way,int32 * len)2685 static void GPS_D154_Send(UC* data, GPS_PWay way, int32* len)
2686 {
2687 UC* p;
2688 int32 i;
2689
2690 p = data;
2691
2692 copy_char_array(&p, way->ident, 6, UpperYes);
2693
2694 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
2695 p+=sizeof(int32);
2696 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
2697 p+=sizeof(int32);
2698 GPS_Util_Put_Uint(p,0);
2699 p+=sizeof(int32);
2700 copy_char_array(&p, way->cmnt, 40, UpperYes);
2701
2702 GPS_Util_Put_Float(p,way->dst);
2703 p+=sizeof(float);
2704
2705 copy_char_array(&p, way->name, 30, UpperYes);
2706 copy_char_array(&p, way->city, 24, UpperYes);
2707 copy_char_array(&p, way->state, 2, UpperYes);
2708
2709 GPS_Util_Put_Short(p,(US) way->alt);
2710 p+=sizeof(int16);
2711
2712 for (i=0; i<2; ++i) {
2713 *p++ = way->cc[i];
2714 }
2715 *p++ = 0;
2716
2717 if (way->wpt_class == 9) {
2718 way->wpt_class = 0;
2719 }
2720 *p++ = way->wpt_class;
2721
2722 GPS_Util_Put_Short(p,(int16)way->smbl);
2723
2724 *len = 126;
2725
2726 return;
2727 }
2728
2729
2730
2731 /* @funcstatic GPS_D155_Send *******************************************
2732 **
2733 ** Form waypoint data string
2734 **
2735 ** @param [w] data [UC *] string to write to
2736 ** @param [r] way [GPS_PWay] waypoint data
2737 ** @param [w] len [int32 *] packet length
2738 **
2739 ** @return [void]
2740 ************************************************************************/
GPS_D155_Send(UC * data,GPS_PWay way,int32 * len)2741 static void GPS_D155_Send(UC* data, GPS_PWay way, int32* len)
2742 {
2743 UC* p;
2744
2745 p = data;
2746
2747 copy_char_array(&p, way->ident, 6, UpperYes);
2748
2749 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
2750 p+=sizeof(int32);
2751 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
2752 p+=sizeof(int32);
2753 GPS_Util_Put_Uint(p,0);
2754 p+=sizeof(int32);
2755 copy_char_array(&p, way->cmnt, 40, UpperYes);
2756 GPS_Util_Put_Float(p,way->dst);
2757 p+=sizeof(float);
2758
2759 copy_char_array(&p, way->name, 30, UpperYes);
2760 copy_char_array(&p, way->city, 24, UpperYes);
2761 copy_char_array(&p, way->state, 2, UpperYes);
2762
2763 GPS_Util_Put_Short(p,(US) way->alt);
2764 p+=sizeof(int16);
2765
2766 copy_char_array(&p, way->cc, 2, UpperYes);
2767 *p++ = 0;
2768
2769 /* Ignore wpt_class; our D155 points are always user type which is "4". */
2770 *p++ = 4;
2771
2772 GPS_Util_Put_Short(p,(int16)way->smbl);
2773 p+=sizeof(int16);
2774
2775 *p = way->dspl;
2776
2777 *len = 127;
2778
2779 return;
2780 }
2781
2782
2783
2784 /* @func GPS_A200_Get ******************************************************
2785 **
2786 ** Get route data from GPS
2787 **
2788 ** @param [r] port [const char *] serial port
2789 ** @param [w] way [GPS_PWay **] waypoint array
2790 **
2791 ** @return [int32] number of waypoint entries
2792 ************************************************************************/
GPS_A200_Get(const char * port,GPS_PWay ** way)2793 int32 GPS_A200_Get(const char* port, GPS_PWay** way)
2794 {
2795 static UC data[2];
2796 gpsdevh* fd;
2797 GPS_PPacket tra;
2798 GPS_PPacket rec;
2799 int32 n;
2800 int32 i;
2801
2802
2803 if (!GPS_Device_On(port,&fd)) {
2804 return gps_errno;
2805 }
2806
2807 GPS_Util_Put_Short(data,
2808 COMMAND_ID[gps_device_command].Cmnd_Transfer_Rte);
2809 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
2810 data,2);
2811 if (!GPS_Write_Packet(fd,tra)) {
2812 return gps_errno;
2813 }
2814 if (!GPS_Get_Ack(fd, &tra, &rec)) {
2815 return gps_errno;
2816 }
2817
2818 if (!GPS_Packet_Read(fd, &rec)) {
2819 return gps_errno;
2820 }
2821 if (!GPS_Send_Ack(fd, &tra, &rec)) {
2822 return gps_errno;
2823 }
2824
2825 n = GPS_Util_Get_Short(rec.data);
2826
2827 if (n)
2828 if (!((*way)=(GPS_PWay*)malloc(n*sizeof(GPS_PWay)))) {
2829 GPS_Error("A200_Get: Insufficient memory");
2830 return MEMORY_ERROR;
2831 }
2832
2833
2834 for (i=0; i<n; ++i) {
2835 if (!((*way)[i]=GPS_Way_New())) {
2836 return MEMORY_ERROR;
2837 }
2838
2839 if (!GPS_Packet_Read(fd, &rec)) {
2840 return gps_errno;
2841 }
2842 if (!GPS_Send_Ack(fd, &tra, &rec)) {
2843 return gps_errno;
2844 }
2845
2846 if (rec.type == LINK_ID[gps_link_type].Pid_Rte_Hdr) {
2847 switch (gps_rte_hdr_type) {
2848 case pD200:
2849 GPS_D200_Get(&((*way)[i]),rec.data);
2850 break;
2851 case pD201:
2852 GPS_D201_Get(&((*way)[i]),rec.data);
2853 break;
2854 case pD202:
2855 GPS_D202_Get(&((*way)[i]),rec.data);
2856 break;
2857 default:
2858 GPS_Error("A200_GET: Unknown route protocol");
2859 return PROTOCOL_ERROR;
2860 }
2861 continue;
2862 }
2863
2864 if (rec.type != LINK_ID[gps_link_type].Pid_Rte_Wpt_Data) {
2865 GPS_Error("A200_GET: Non Pid_rte_Wpt_Data");
2866 return FRAMING_ERROR;
2867 }
2868
2869 (*way)[i]->isrte = 0;
2870 (*way)[i]->islink = 0;
2871
2872 switch (gps_rte_type) {
2873 case pD100:
2874 GPS_D100_Get(&((*way)[i]),rec.data);
2875 break;
2876 case pD101:
2877 GPS_D101_Get(&((*way)[i]),rec.data);
2878 break;
2879 case pD102:
2880 GPS_D102_Get(&((*way)[i]),rec.data);
2881 break;
2882 case pD103:
2883 GPS_D103_Get(&((*way)[i]),rec.data);
2884 break;
2885 case pD104:
2886 GPS_D104_Get(&((*way)[i]),rec.data);
2887 break;
2888 case pD105:
2889 GPS_D105_Get(&((*way)[i]),rec.data);
2890 break;
2891 case pD106:
2892 GPS_D106_Get(&((*way)[i]),rec.data);
2893 break;
2894 case pD107:
2895 GPS_D107_Get(&((*way)[i]),rec.data);
2896 break;
2897 case pD108:
2898 GPS_D108_Get(&((*way)[i]),rec.data);
2899 break;
2900 case pD109:
2901 GPS_D109_Get(&((*way)[i]),rec.data,109);
2902 break;
2903 case pD110:
2904 GPS_D109_Get(&((*way)[i]),rec.data,110);
2905 break;
2906 case pD150:
2907 GPS_D150_Get(&((*way)[i]),rec.data);
2908 break;
2909 case pD151:
2910 GPS_D151_Get(&((*way)[i]),rec.data);
2911 break;
2912 case pD152:
2913 GPS_D152_Get(&((*way)[i]),rec.data);
2914 break;
2915 case pD154:
2916 GPS_D154_Get(&((*way)[i]),rec.data);
2917 break;
2918 case pD155:
2919 GPS_D155_Get(&((*way)[i]),rec.data);
2920 break;
2921 default:
2922 GPS_Error("A200_GET: Unknown route protocol");
2923 return PROTOCOL_ERROR;
2924 }
2925 (*way)[i-1]->prot = (*way)[i]->prot;
2926 }
2927
2928 if (!GPS_Packet_Read(fd, &rec)) {
2929 return gps_errno;
2930 }
2931
2932 if (!GPS_Send_Ack(fd, &tra, &rec)) {
2933 return gps_errno;
2934 }
2935
2936 if (rec.type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) {
2937 GPS_Error("A200_GET: Error transferring routes");
2938 return FRAMING_ERROR;
2939 }
2940
2941 if (i != n) {
2942 GPS_Error("A200_GET: Route entry number mismatch");
2943 return FRAMING_ERROR;
2944 }
2945
2946
2947 if (!GPS_Device_Off(fd)) {
2948 return gps_errno;
2949 }
2950
2951 return n;
2952 }
2953
2954
2955
2956 /* @func GPS_A201_Get ******************************************************
2957 **
2958 ** Get route data from GPS
2959 **
2960 ** @param [r] port [const char *] serial port
2961 ** @param [w] way [GPS_PWay **] waypoint array
2962 **
2963 ** @return [int32] number of waypoint entries
2964 ************************************************************************/
GPS_A201_Get(const char * port,GPS_PWay ** way)2965 int32 GPS_A201_Get(const char* port, GPS_PWay** way)
2966 {
2967 static UC data[2];
2968 gpsdevh* fd;
2969 GPS_PPacket tra;
2970 GPS_PPacket rec;
2971 int32 n;
2972 int32 i;
2973
2974
2975 if (!GPS_Device_On(port,&fd)) {
2976 return gps_errno;
2977 }
2978
2979 GPS_Util_Put_Short(data,
2980 COMMAND_ID[gps_device_command].Cmnd_Transfer_Rte);
2981 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
2982 data,2);
2983 if (!GPS_Write_Packet(fd,tra)) {
2984 return gps_errno;
2985 }
2986 if (!GPS_Get_Ack(fd, &tra, &rec)) {
2987 return gps_errno;
2988 }
2989
2990 if (!GPS_Packet_Read(fd, &rec)) {
2991 return gps_errno;
2992 }
2993 if (!GPS_Send_Ack(fd, &tra, &rec)) {
2994 return gps_errno;
2995 }
2996
2997 n = GPS_Util_Get_Short(rec.data);
2998
2999 if (n)
3000 if (!((*way)=(GPS_PWay*)malloc(n*sizeof(GPS_PWay)))) {
3001 GPS_Error("A201_Get: Insufficient memory");
3002 return MEMORY_ERROR;
3003 }
3004
3005
3006 for (i=0; i<n; ++i) {
3007 if (!((*way)[i]=GPS_Way_New())) {
3008 return MEMORY_ERROR;
3009 }
3010
3011 if (!GPS_Packet_Read(fd, &rec)) {
3012 return gps_errno;
3013 }
3014 if (!GPS_Send_Ack(fd, &tra, &rec)) {
3015 return gps_errno;
3016 }
3017
3018 if (rec.type == LINK_ID[gps_link_type].Pid_Rte_Hdr) {
3019 switch (gps_rte_hdr_type) {
3020 case pD200:
3021 GPS_D200_Get(&((*way)[i]),rec.data);
3022 break;
3023 case pD201:
3024 GPS_D201_Get(&((*way)[i]),rec.data);
3025 break;
3026 case pD202:
3027 GPS_D202_Get(&((*way)[i]),rec.data);
3028 break;
3029 default:
3030 GPS_Error("A201_GET: Unknown route protocol");
3031 return PROTOCOL_ERROR;
3032 }
3033 (*way)[i]->islink = 0;
3034 continue;
3035 }
3036
3037
3038 if (rec.type == LINK_ID[gps_link_type].Pid_Rte_Link_Data) {
3039 switch (gps_rte_link_type) {
3040 case pD210:
3041 GPS_D210_Get(&((*way)[i]),rec.data);
3042 break;
3043 default:
3044 GPS_Error("A201_GET: Unknown route protocol");
3045 return PROTOCOL_ERROR;
3046 }
3047 (*way)[i]->isrte = 0;
3048 (*way)[i]->islink = 1;
3049 continue;
3050 }
3051
3052 if (rec.type != LINK_ID[gps_link_type].Pid_Rte_Wpt_Data) {
3053 GPS_Error("A200_GET: Non Pid_rte_Wpt_Data");
3054 return FRAMING_ERROR;
3055 }
3056
3057 (*way)[i]->isrte = 0;
3058 (*way)[i]->islink = 0;
3059
3060 switch (gps_rte_type) {
3061 case pD100:
3062 GPS_D100_Get(&((*way)[i]),rec.data);
3063 break;
3064 case pD101:
3065 GPS_D101_Get(&((*way)[i]),rec.data);
3066 break;
3067 case pD102:
3068 GPS_D102_Get(&((*way)[i]),rec.data);
3069 break;
3070 case pD103:
3071 GPS_D103_Get(&((*way)[i]),rec.data);
3072 break;
3073 case pD104:
3074 GPS_D104_Get(&((*way)[i]),rec.data);
3075 break;
3076 case pD105:
3077 GPS_D105_Get(&((*way)[i]),rec.data);
3078 break;
3079 case pD106:
3080 GPS_D106_Get(&((*way)[i]),rec.data);
3081 break;
3082 case pD107:
3083 GPS_D107_Get(&((*way)[i]),rec.data);
3084 break;
3085 case pD108:
3086 GPS_D108_Get(&((*way)[i]),rec.data);
3087 break;
3088 case pD109:
3089 GPS_D109_Get(&((*way)[i]),rec.data,109);
3090 break;
3091 case pD110:
3092 GPS_D109_Get(&((*way)[i]),rec.data,110);
3093 break;
3094 case pD150:
3095 GPS_D150_Get(&((*way)[i]),rec.data);
3096 break;
3097 case pD151:
3098 GPS_D151_Get(&((*way)[i]),rec.data);
3099 break;
3100 case pD152:
3101 GPS_D152_Get(&((*way)[i]),rec.data);
3102 break;
3103 case pD154:
3104 GPS_D154_Get(&((*way)[i]),rec.data);
3105 break;
3106 case pD155:
3107 GPS_D155_Get(&((*way)[i]),rec.data);
3108 break;
3109 default:
3110 GPS_Error("A200_GET: Unknown route protocol");
3111 return PROTOCOL_ERROR;
3112 }
3113 (*way)[i-1]->prot = (*way)[i]->prot;
3114 }
3115
3116 if (!GPS_Packet_Read(fd, &rec)) {
3117 return gps_errno;
3118 }
3119
3120 if (!GPS_Send_Ack(fd, &tra, &rec)) {
3121 return gps_errno;
3122 }
3123
3124 if (rec.type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) {
3125 GPS_Error("A200_GET: Error transferring routes");
3126 return FRAMING_ERROR;
3127 }
3128
3129 if (i != n) {
3130 GPS_Error("A200_GET: Route entry number mismatch");
3131 return FRAMING_ERROR;
3132 }
3133
3134
3135 if (!GPS_Device_Off(fd)) {
3136 return gps_errno;
3137 }
3138
3139 return n;
3140 }
3141
3142
3143
3144 /* @func GPS_A200_Send **************************************************
3145 **
3146 ** Send routes to GPS
3147 **
3148 ** @param [r] port [const char *] serial port
3149 ** @param [r] trk [GPS_PWay *] waypoint array
3150 ** @param [r] n [int32] number of waypoint entries
3151 **
3152 ** @return [int32] success
3153 ************************************************************************/
GPS_A200_Send(const char * port,GPS_PWay * way,int32 n)3154 int32 GPS_A200_Send(const char* port, GPS_PWay* way, int32 n)
3155 {
3156 UC data[GPS_ARB_LEN];
3157 gpsdevh* fd;
3158 GPS_PPacket tra;
3159 GPS_PPacket rec;
3160 int32 i;
3161 int32 len;
3162 US method;
3163
3164 if (!GPS_Device_On(port,&fd)) {
3165 return gps_errno;
3166 }
3167
3168 GPS_Util_Put_Short(data,(US) n);
3169 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records,
3170 data,2);
3171 if (!GPS_Write_Packet(fd,tra)) {
3172 return gps_errno;
3173 }
3174 if (!GPS_Get_Ack(fd, &tra, &rec)) {
3175 GPS_Error("A200_Send: Route start data not acknowledged");
3176 return FRAMING_ERROR;
3177 }
3178
3179
3180 for (i=0; i<n; ++i) {
3181 if (way[i]->isrte) {
3182 method = LINK_ID[gps_link_type].Pid_Rte_Hdr;
3183
3184 switch (gps_rte_hdr_type) {
3185 case pD200:
3186 GPS_D200_Send(data,way[i],&len);
3187 break;
3188 case pD201:
3189 GPS_D201_Send(data,way[i],&len);
3190 break;
3191 case pD202:
3192 GPS_D202_Send(data,way[i],&len);
3193 break;
3194 default:
3195 GPS_Error("A200_Send: Unknown route protocol");
3196 return PROTOCOL_ERROR;
3197 }
3198 } else {
3199 method = LINK_ID[gps_link_type].Pid_Rte_Wpt_Data;
3200
3201 switch (gps_rte_type) {
3202 case pD100:
3203 GPS_D100_Send(data,way[i],&len);
3204 break;
3205 case pD101:
3206 GPS_D101_Send(data,way[i],&len);
3207 break;
3208 case pD102:
3209 GPS_D102_Send(data,way[i],&len);
3210 break;
3211 case pD103:
3212 GPS_D103_Send(data,way[i],&len);
3213 break;
3214 case pD104:
3215 GPS_D104_Send(data,way[i],&len);
3216 break;
3217 case pD105:
3218 GPS_D105_Send(data,way[i],&len);
3219 break;
3220 case pD106:
3221 GPS_D106_Send(data,way[i],&len);
3222 break;
3223 case pD107:
3224 GPS_D107_Send(data,way[i],&len);
3225 break;
3226 case pD108:
3227 GPS_D108_Send(data,way[i],&len);
3228 break;
3229 case pD150:
3230 GPS_D150_Send(data,way[i],&len);
3231 break;
3232 case pD151:
3233 GPS_D151_Send(data,way[i],&len);
3234 break;
3235 case pD152:
3236 GPS_D152_Send(data,way[i],&len);
3237 break;
3238 case pD154:
3239 GPS_D154_Send(data,way[i],&len);
3240 break;
3241 case pD155:
3242 GPS_D155_Send(data,way[i],&len);
3243 break;
3244 default:
3245 GPS_Error("A200_Send: Unknown route protocol");
3246 return PROTOCOL_ERROR;
3247 }
3248 }
3249
3250
3251 GPS_Make_Packet(&tra, method, data, len);
3252
3253 if (!GPS_Write_Packet(fd,tra)) {
3254 return gps_errno;
3255 }
3256
3257 if (!GPS_Get_Ack(fd, &tra, &rec)) {
3258 GPS_Error("A200_Send: Route packet not acknowledged");
3259 return FRAMING_ERROR;
3260 }
3261 }
3262
3263 GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Wpt);
3264 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt,
3265 data,2);
3266 if (!GPS_Write_Packet(fd,tra)) {
3267 return gps_errno;
3268 }
3269 if (!GPS_Get_Ack(fd, &tra, &rec)) {
3270 GPS_Error("A200_Send: Route complete data not acknowledged");
3271 return FRAMING_ERROR;
3272 }
3273
3274 if (!GPS_Device_Off(fd)) {
3275 return gps_errno;
3276 }
3277
3278 return 1;
3279 }
3280
3281
3282
3283 /* @func GPS_A201_Send **************************************************
3284 **
3285 ** Send routes to GPS
3286 **
3287 ** @param [r] port [const char *] serial port
3288 ** @param [r] trk [GPS_PWay *] waypoint array
3289 ** @param [r] n [int32] number of waypoint entries
3290 **
3291 ** @return [int32] success
3292 ************************************************************************/
GPS_A201_Send(const char * port,GPS_PWay * way,int32 n)3293 int32 GPS_A201_Send(const char* port, GPS_PWay* way, int32 n)
3294 {
3295 UC data[GPS_ARB_LEN];
3296 gpsdevh* fd;
3297 GPS_PPacket tra;
3298 GPS_PPacket rec;
3299 int32 i;
3300 int32 len;
3301 US method;
3302
3303 if (!GPS_Device_On(port,&fd)) {
3304 return gps_errno;
3305 }
3306
3307 GPS_Util_Put_Short(data,(US) n);
3308 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records,
3309 data,2);
3310 if (!GPS_Write_Packet(fd,tra)) {
3311 return gps_errno;
3312 }
3313 if (!GPS_Get_Ack(fd, &tra, &rec)) {
3314 GPS_Error("A200_Send: Route start data not acknowledged");
3315 return FRAMING_ERROR;
3316 }
3317
3318
3319 for (i=0; i<n; ++i) {
3320 if (way[i]->isrte) {
3321 method = LINK_ID[gps_link_type].Pid_Rte_Hdr;
3322
3323 switch (gps_rte_hdr_type) {
3324 case pD200:
3325 GPS_D200_Send(data,way[i],&len);
3326 break;
3327 case pD201:
3328 GPS_D201_Send(data,way[i],&len);
3329 break;
3330 case pD202:
3331 GPS_D202_Send(data,way[i],&len);
3332 break;
3333 default:
3334 GPS_Error("A200_Send: Unknown route protocol");
3335 return PROTOCOL_ERROR;
3336 }
3337 } else if (way[i]->islink) {
3338 method = LINK_ID[gps_link_type].Pid_Rte_Link_Data;
3339
3340 switch (gps_rte_link_type) {
3341 case pD210:
3342 GPS_D210_Send(data,way[i],&len);
3343 break;
3344 default:
3345 GPS_Error("A201_Send: Unknown route protocol");
3346 return PROTOCOL_ERROR;
3347 }
3348 } else {
3349 method = LINK_ID[gps_link_type].Pid_Rte_Wpt_Data;
3350
3351 switch (gps_rte_type) {
3352 case pD100:
3353 GPS_D100_Send(data,way[i],&len);
3354 break;
3355 case pD101:
3356 GPS_D101_Send(data,way[i],&len);
3357 break;
3358 case pD102:
3359 GPS_D102_Send(data,way[i],&len);
3360 break;
3361 case pD103:
3362 GPS_D103_Send(data,way[i],&len);
3363 break;
3364 case pD104:
3365 GPS_D104_Send(data,way[i],&len);
3366 break;
3367 case pD105:
3368 GPS_D105_Send(data,way[i],&len);
3369 break;
3370 case pD106:
3371 GPS_D106_Send(data,way[i],&len);
3372 break;
3373 case pD107:
3374 GPS_D107_Send(data,way[i],&len);
3375 break;
3376 case pD108:
3377 GPS_D108_Send(data,way[i],&len);
3378 break;
3379 case pD109:
3380 GPS_D109_Send(data,way[i],&len, 109);
3381 break;
3382 case pD110:
3383 GPS_D109_Send(data,way[i],&len, 110);
3384 break;
3385 case pD150:
3386 GPS_D150_Send(data,way[i],&len);
3387 break;
3388 case pD151:
3389 GPS_D151_Send(data,way[i],&len);
3390 break;
3391 case pD152:
3392 GPS_D152_Send(data,way[i],&len);
3393 break;
3394 case pD154:
3395 GPS_D154_Send(data,way[i],&len);
3396 break;
3397 case pD155:
3398 GPS_D155_Send(data,way[i],&len);
3399 break;
3400 default:
3401 GPS_Error("A200_Send: Unknown route protocol");
3402 return PROTOCOL_ERROR;
3403 }
3404 }
3405
3406
3407 GPS_Make_Packet(&tra, method, data, len);
3408
3409 if (!GPS_Write_Packet(fd,tra)) {
3410 return gps_errno;
3411 }
3412
3413 if (!GPS_Get_Ack(fd, &tra, &rec)) {
3414 GPS_Error("A200_Send: Route packet not acknowledged");
3415 return FRAMING_ERROR;
3416 }
3417 }
3418
3419 GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Wpt);
3420 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt,
3421 data,2);
3422 if (!GPS_Write_Packet(fd,tra)) {
3423 return gps_errno;
3424 }
3425 if (!GPS_Get_Ack(fd, &tra, &rec)) {
3426 GPS_Error("A200_Send: Route complete data not acknowledged");
3427 return FRAMING_ERROR;
3428 }
3429
3430 if (!GPS_Device_Off(fd)) {
3431 return gps_errno;
3432 }
3433
3434 return 1;
3435 }
3436
3437
3438
3439
3440
3441 /* @funcstatic GPS_D200_Get ********************************************
3442 **
3443 ** Get route header data
3444 **
3445 ** @param [w] way [GPS_PWay *] waypoint array
3446 ** @param [r] s [UC *] packet data
3447 **
3448 ** @return [void]
3449 ************************************************************************/
GPS_D200_Get(GPS_PWay * way,const UC * s)3450 static void GPS_D200_Get(GPS_PWay* way, const UC* s)
3451 {
3452 (*way)->rte_prot = 200;
3453 (*way)->rte_num = *s;
3454 (*way)->isrte = 1;
3455
3456 return;
3457 }
3458
3459
3460
3461 /* @funcstatic GPS_D201_Get *******************************************
3462 **
3463 ** Get route header data
3464 **
3465 ** @param [w] way [GPS_PWay *] waypoint array
3466 ** @param [r] s [UC *] packet data
3467 **
3468 ** @return [void]
3469 ************************************************************************/
GPS_D201_Get(GPS_PWay * way,UC * s)3470 static void GPS_D201_Get(GPS_PWay* way, UC* s)
3471 {
3472 UC* p;
3473 int32 i;
3474
3475 p=s;
3476
3477 (*way)->rte_prot = 201;
3478 (*way)->rte_num = *p++;
3479 (*way)->isrte = 1;
3480 for (i=0; i<20; ++i) {
3481 (*way)->rte_cmnt[i] = *p++;
3482 }
3483
3484 return;
3485 }
3486
3487
3488
3489 /* @funcstatic GPS_D202_Get ********************************************
3490 **
3491 ** Get route header data
3492 **
3493 ** @param [w] way [GPS_PWay *] waypoint array
3494 ** @param [r] s [UC *] packet data
3495 **
3496 ** @return [void]
3497 ************************************************************************/
GPS_D202_Get(GPS_PWay * way,UC * s)3498 static void GPS_D202_Get(GPS_PWay* way, UC* s)
3499 {
3500 UC* p;
3501 UC* q;
3502
3503 p=s;
3504
3505 (*way)->rte_prot = 202;
3506 #if 0
3507 /* D202 has only a null terminated string for rte_ident */
3508 (*way)->rte_num = *p++;
3509 #endif
3510 (*way)->isrte = 1;
3511 q = (UC*)(*way)->rte_ident;
3512 while ((*q++=*p++));
3513
3514 return;
3515 }
3516
3517
3518
3519 /* @funcstatic GPS_D210_Get ********************************************
3520 **
3521 ** Get route link data
3522 **
3523 ** @param [w] way [GPS_PWay *] waypoint array
3524 ** @param [r] s [UC *] packet data
3525 **
3526 ** @return [void]
3527 ************************************************************************/
GPS_D210_Get(GPS_PWay * way,UC * s)3528 static void GPS_D210_Get(GPS_PWay* way, UC* s)
3529 {
3530 UC* p;
3531 UC* q;
3532 int32 i;
3533
3534 p=s;
3535
3536 (*way)->rte_link_class = GPS_Util_Get_Short(p);
3537 p+=sizeof(int16);
3538 for (i=0; i<18; ++i) {
3539 (*way)->rte_link_subclass[i] = *p++;
3540 }
3541 q = (UC*)(*way)->rte_link_ident;
3542 while ((*q++=*p++));
3543
3544 return;
3545 }
3546
3547
3548
3549 /* @funcstatic GPS_D200_Send *******************************************
3550 **
3551 ** Form route header data string
3552 **
3553 ** @param [w] data [UC *] string to write to
3554 ** @param [r] way [GPS_PWay] waypoint data
3555 ** @param [w] len [int32 *] packet length
3556 **
3557 ** @return [void]
3558 ************************************************************************/
GPS_D200_Send(UC * data,GPS_PWay way,int32 * len)3559 static void GPS_D200_Send(UC* data, GPS_PWay way, int32* len)
3560 {
3561
3562 *data = way->rte_num;
3563 *len = 1;
3564
3565 return;
3566 }
3567
3568
3569
3570 /* @funcstatic GPS_D201_Send *******************************************
3571 **
3572 ** Form route header data string
3573 **
3574 ** @param [w] data [UC *] string to write to
3575 ** @param [r] way [GPS_PWay] waypoint data
3576 ** @param [w] len [int32 *] packet length
3577 **
3578 ** @return [void]
3579 ************************************************************************/
GPS_D201_Send(UC * data,GPS_PWay way,int32 * len)3580 static void GPS_D201_Send(UC* data, GPS_PWay way, int32* len)
3581 {
3582 UC* p;
3583
3584 p = data;
3585
3586 *p++ = way->rte_num;
3587 copy_char_array(&p, way->rte_cmnt, 20, UpperYes);
3588 *len = 21;
3589
3590 return;
3591 }
3592
3593
3594
3595 /* @funcstatic GPS_D202_Send ********************************************
3596 **
3597 ** Form route header data string
3598 **
3599 ** @param [w] data [UC *] string to write to
3600 ** @param [r] way [GPS_PWay] waypoint data
3601 ** @param [w] len [int32 *] packet length
3602 **
3603 ** @return [void]
3604 ************************************************************************/
GPS_D202_Send(UC * data,GPS_PWay way,int32 * len)3605 static void GPS_D202_Send(UC* data, GPS_PWay way, int32* len)
3606 {
3607 UC* p;
3608 UC* q;
3609
3610 p = data;
3611 q = (UC*) way->rte_ident;
3612
3613 while ((*p++ = *q++));
3614
3615 *len = p-data;
3616
3617 return;
3618 }
3619
3620
3621
3622 /* @funcstatic GPS_D210_Send ********************************************
3623 **
3624 ** Form route link data string
3625 **
3626 ** @param [w] data [UC *] string to write to
3627 ** @param [r] way [GPS_PWay] waypoint data
3628 ** @param [w] len [int32 *] packet length
3629 **
3630 ** @return [void]
3631 ************************************************************************/
GPS_D210_Send(UC * data,GPS_PWay way,int32 * len)3632 static void GPS_D210_Send(UC* data, GPS_PWay way, int32* len)
3633 {
3634 UC* p;
3635 UC* q;
3636 int32 i;
3637
3638 p = data;
3639
3640 GPS_Util_Put_Short(p,(US) way->rte_link_class);
3641 p+=sizeof(int16);
3642 for (i=0; i<18; ++i) {
3643 *p++ = way->rte_link_subclass[i];
3644 }
3645
3646 q = (UC*) way->rte_link_ident;
3647 while ((*p++ = *q++));
3648
3649 *len = p-data;
3650
3651 return;
3652 }
3653
3654
3655
3656 /* @func GPS_A300_Get ******************************************************
3657 **
3658 ** Get track data from GPS
3659 **
3660 ** @param [r] port [const char *] serial port
3661 ** @param [w] trk [GPS_PTrack **] track array
3662 **
3663 ** @return [int32] number of track entries
3664 ************************************************************************/
GPS_A300_Get(const char * port,GPS_PTrack ** trk,pcb_fn)3665 int32 GPS_A300_Get(const char* port , GPS_PTrack** trk, pcb_fn)
3666 {
3667 static UC data[2];
3668 gpsdevh* fd;
3669 GPS_PPacket tra;
3670 GPS_PPacket rec;
3671 int32 n;
3672 int32 i;
3673 int32 ret;
3674
3675
3676 if (gps_trk_transfer == -1) {
3677 return GPS_UNSUPPORTED;
3678 }
3679
3680 /* Only those GPS' with L001 can send track data */
3681 if (!LINK_ID[gps_link_type].Pid_Trk_Data) {
3682 GPS_Warning("A300 protocol unsupported");
3683 return GPS_UNSUPPORTED;
3684 }
3685
3686 if (!GPS_Device_On(port, &fd)) {
3687 return gps_errno;
3688 }
3689
3690 GPS_Util_Put_Short(data,
3691 COMMAND_ID[gps_device_command].Cmnd_Transfer_Trk);
3692 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
3693 data,2);
3694 if (!GPS_Write_Packet(fd,tra)) {
3695 return gps_errno;
3696 }
3697 if (!GPS_Get_Ack(fd, &tra, &rec)) {
3698 return gps_errno;
3699 }
3700 if (!GPS_Packet_Read(fd, &rec)) {
3701 return gps_errno;
3702 }
3703 if (!GPS_Send_Ack(fd, &tra, &rec)) {
3704 return gps_errno;
3705 }
3706
3707
3708 n = GPS_Util_Get_Short(rec.data);
3709
3710 if (n)
3711 if (!((*trk)=(GPS_PTrack*)malloc(n*sizeof(GPS_PTrack)))) {
3712 GPS_Error("A300_Get: Insufficient memory");
3713 return MEMORY_ERROR;
3714 }
3715 for (i=0; i<n; ++i)
3716 if (!((*trk)[i]=GPS_Track_New())) {
3717 return MEMORY_ERROR;
3718 }
3719
3720 switch (gps_trk_type) {
3721 case pD300:
3722 ret = GPS_D300_Get(*trk,n,fd);
3723 if (ret<0) {
3724 return ret;
3725 }
3726 break;
3727 default:
3728 GPS_Error("A300_GET: Unknown track protocol");
3729 return PROTOCOL_ERROR;
3730 }
3731
3732 if (ret != n) {
3733 GPS_Error("A300_GET: Track entry number mismatch");
3734 return FRAMING_ERROR;
3735 }
3736
3737 if (!GPS_Device_Off(fd)) {
3738 return gps_errno;
3739 }
3740
3741 return ret;
3742 }
3743
3744 /*
3745 * This is to get around a problem with the x305 sporting units.
3746 * The unit will not "finalize" a track unless the operator manually
3747 * does it from the pushbutton panel OR until the device has gone through
3748 * a 'get runs' cycle. Garmin's Training Center, of course, does this
3749 * because it actually uses that data. Here we just go through the
3750 * mechanics of building and sending the requests and then throwing away
3751 * all the data in order to finalize that.
3752 *
3753 * Hopefully, this won't be needed forever.
3754 */
3755 int
drain_run_cmd(gpsdevh * fd)3756 drain_run_cmd(gpsdevh* fd)
3757 {
3758 GPS_PPacket tra;
3759 GPS_PPacket rec;
3760 static UC data[2];
3761
3762 GPS_Util_Put_Short(data,
3763 COMMAND_ID[gps_device_command].Cmnd_Transfer_Runs);
3764 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
3765 data,2);
3766
3767 if (!GPS_Write_Packet(fd,tra)) {
3768 return gps_errno;
3769 }
3770 if (!GPS_Get_Ack(fd, &tra, &rec)) {
3771 return gps_errno;
3772 }
3773
3774 for (;;) {
3775 if (!GPS_Packet_Read(fd, &rec)) {
3776 return gps_errno;
3777 }
3778 if (!GPS_Send_Ack(fd, &tra, &rec)) {
3779 return gps_errno;
3780 }
3781 if (rec.type == LINK_ID[gps_link_type].Pid_Xfer_Cmplt) {
3782 break;
3783 }
3784 }
3785
3786 return 0;
3787 }
3788
3789 /* @func GPS_A301_Get ******************************************************
3790 **
3791 ** Get track data from GPS (A301/A302)
3792 **
3793 ** @param [r] port [const char *] serial port
3794 ** @param [w] trk [GPS_PTrack **] track array
3795 ** @param [r] protoid [int] protocol ID (301 or 302)
3796 **
3797 ** @return [int32] number of track entries
3798 ************************************************************************/
GPS_A301_Get(const char * port,GPS_PTrack ** trk,pcb_fn cb,int protoid)3799 int32 GPS_A301_Get(const char* port, GPS_PTrack** trk, pcb_fn cb, int protoid)
3800 {
3801 static UC data[2];
3802 gpsdevh* fd;
3803 GPS_PPacket tra;
3804 GPS_PPacket rec;
3805 int32 n;
3806 int32 i;
3807 US Pid_Trk_Data, Pid_Trk_Hdr, Cmnd_Transfer_Trk;
3808 int32 trk_type, trk_hdr_type;
3809
3810 if (gps_trk_transfer == -1) {
3811 return GPS_UNSUPPORTED;
3812 }
3813
3814 /* A301 and A302 are similar except for all these protocol IDs */
3815 switch (protoid) {
3816 case 301:
3817 Pid_Trk_Data = LINK_ID[gps_link_type].Pid_Trk_Data;
3818 Pid_Trk_Hdr = LINK_ID[gps_link_type].Pid_Trk_Hdr;
3819 Cmnd_Transfer_Trk = COMMAND_ID[gps_device_command].Cmnd_Transfer_Trk;
3820 trk_type = gps_trk_type;
3821 trk_hdr_type = gps_trk_hdr_type;
3822 break;
3823 case 302:
3824 Pid_Trk_Data = LINK_ID[gps_link_type].Pid_Course_Trk_Data;
3825 Pid_Trk_Hdr = LINK_ID[gps_link_type].Pid_Course_Trk_Hdr;
3826 Cmnd_Transfer_Trk = COMMAND_ID[gps_device_command].Cmnd_Transfer_Course_Tracks;
3827 trk_type = gps_run_crs_trk_type;
3828 trk_hdr_type = gps_run_crs_trk_hdr_type;
3829 break;
3830 default:
3831 GPS_Error("A301_Get: Bad protocol ID %d", protoid);
3832 return GPS_UNSUPPORTED;
3833 }
3834
3835 /* Only those GPS' with L001 can send track data */
3836 if (!Pid_Trk_Data) {
3837 GPS_Warning("A301 protocol unsupported");
3838 return GPS_UNSUPPORTED;
3839 }
3840
3841 if (!GPS_Device_On(port, &fd)) {
3842 return gps_errno;
3843 }
3844
3845 if (protoid == 301 && trk_type == pD304 && gps_run_transfer != -1) {
3846 drain_run_cmd(fd);
3847 }
3848
3849 GPS_Util_Put_Short(data, Cmnd_Transfer_Trk);
3850 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
3851 data,2);
3852 if (!GPS_Write_Packet(fd,tra)) {
3853 return gps_errno;
3854 }
3855 if (!GPS_Get_Ack(fd, &tra, &rec)) {
3856 return gps_errno;
3857 }
3858 if (!GPS_Packet_Read(fd, &rec)) {
3859 return gps_errno;
3860 }
3861 if (!GPS_Send_Ack(fd, &tra, &rec)) {
3862 return gps_errno;
3863 }
3864
3865
3866 n = GPS_Util_Get_Short(rec.data);
3867
3868 if (n)
3869 if (!((*trk)=(GPS_PTrack*)malloc(n*sizeof(GPS_PTrack)))) {
3870 GPS_Error("A301_Get: Insufficient memory");
3871 return MEMORY_ERROR;
3872 }
3873 for (i=0; i<n; ++i)
3874 if (!((*trk)[i]=GPS_Track_New())) {
3875 return MEMORY_ERROR;
3876 }
3877
3878 for (i=0; i<n; ++i) {
3879 if (!GPS_Packet_Read(fd, &rec)) {
3880 return gps_errno;
3881 }
3882 if (!GPS_Send_Ack(fd, &tra, &rec)) {
3883 return gps_errno;
3884 }
3885 if (rec.type == Pid_Trk_Hdr) {
3886 switch (trk_hdr_type) {
3887 case pD310:
3888 case pD312:
3889 GPS_D310_Get(&((*trk)[i]),rec.data);
3890 break;
3891 case pD311:
3892 GPS_D311_Get(&((*trk)[i]),rec.data);
3893 break;
3894 default:
3895 GPS_Error("A301_Get: Unknown track protocol");
3896 return PROTOCOL_ERROR;
3897 }
3898 (*trk)[i]->ishdr = 1;
3899 continue;
3900 }
3901
3902 if (rec.type != Pid_Trk_Data) {
3903 GPS_Error("A301_Get: Non-Pid_Trk_Data");
3904 return FRAMING_ERROR;
3905 }
3906
3907 (*trk)[i]->ishdr = 0;
3908
3909 switch (trk_type) {
3910 case pD300:
3911 GPS_D300b_Get(&((*trk)[i]),rec.data);
3912 break;
3913 case pD301:
3914 GPS_D301b_Get(&((*trk)[i]),rec.data);
3915 break;
3916 case pD302:
3917 GPS_D302b_Get(&((*trk)[i]),rec.data);
3918 break;
3919 case pD303:
3920 case pD304:
3921 GPS_D303b_Get(&((*trk)[i]),rec.data);
3922 /* Fitness devices don't send track segment markers, so we have
3923 * to create them ourselves. We do so at the beginning of the
3924 * track or if the device signals a pause by sending two
3925 * invalid points in a row.
3926 */
3927 if (i>0) {
3928 if ((*trk)[i-1]->ishdr ||
3929 (Is_Trackpoint_Invalid((*trk)[i-1]) &&
3930 Is_Trackpoint_Invalid((*trk)[i]))) {
3931 (*trk)[i]->tnew = 1;
3932 }
3933 }
3934 break;
3935 default:
3936 GPS_Error("A301_GET: Unknown track protocol");
3937 return PROTOCOL_ERROR;
3938 }
3939 /* Cheat and don't _really_ pass the trkpt back */
3940 if (cb) {
3941 cb(n, nullptr);
3942 }
3943 }
3944
3945 if (!GPS_Packet_Read(fd, &rec)) {
3946 return gps_errno;
3947 }
3948 if (!GPS_Send_Ack(fd, &tra, &rec)) {
3949 return gps_errno;
3950 }
3951 if (rec.type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) {
3952 GPS_Error("A301_Get: Error transferring tracks");
3953 return FRAMING_ERROR;
3954 }
3955
3956 if (i != n) {
3957 GPS_Error("A301_GET: Track entry number mismatch");
3958 return FRAMING_ERROR;
3959 }
3960
3961 if (!GPS_Device_Off(fd)) {
3962 return gps_errno;
3963 }
3964
3965 return n;
3966 }
3967
3968
3969 /* @func GPS_A300_Send **************************************************
3970 **
3971 ** Send track log to GPS
3972 **
3973 ** @param [r] port [const char *] serial port
3974 ** @param [r] trk [GPS_PTrack *] track array
3975 ** @param [r] n [int32] number of track entries
3976 **
3977 ** @return [int32] success
3978 ************************************************************************/
GPS_A300_Send(const char * port,GPS_PTrack * trk,int32 n)3979 int32 GPS_A300_Send(const char* port, GPS_PTrack* trk, int32 n)
3980 {
3981 UC data[GPS_ARB_LEN];
3982 gpsdevh* fd;
3983 GPS_PPacket tra;
3984 GPS_PPacket rec;
3985 int32 i;
3986 int32 len;
3987
3988 if (gps_trk_transfer == -1) {
3989 return GPS_UNSUPPORTED;
3990 }
3991
3992 /* Only those GPS' with L001 can send track data */
3993 if (!LINK_ID[gps_link_type].Pid_Trk_Data) {
3994 GPS_Warning("A300 protocol unsupported");
3995 return GPS_UNSUPPORTED;
3996 }
3997
3998 if (!GPS_Device_On(port, &fd)) {
3999 return gps_errno;
4000 }
4001
4002 GPS_Util_Put_Short(data,(US) n);
4003 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records,
4004 data,2);
4005 if (!GPS_Write_Packet(fd,tra)) {
4006 return gps_errno;
4007 }
4008 if (!GPS_Get_Ack(fd, &tra, &rec)) {
4009 GPS_Error("A300_Send: Track start data not acknowledged");
4010 return FRAMING_ERROR;
4011 }
4012
4013 for (i=0; i<n; ++i) {
4014 switch (gps_trk_type) {
4015 case pD300:
4016 GPS_D300_Send(data,trk[i],&len);
4017 break;
4018 default:
4019 GPS_Error("A300_Send: Unknown track protocol");
4020 return PROTOCOL_ERROR;
4021 }
4022
4023 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Trk_Data,
4024 data, len);
4025
4026 if (!GPS_Write_Packet(fd,tra)) {
4027 return gps_errno;
4028 }
4029
4030 if (!GPS_Get_Ack(fd, &tra, &rec)) {
4031 GPS_Error("A300_Send: Pid_Trk_Data not acknowledged");
4032 return FRAMING_ERROR;
4033 }
4034 }
4035
4036 GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Trk);
4037 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt,
4038 data,2);
4039 if (!GPS_Write_Packet(fd,tra)) {
4040 return gps_errno;
4041 }
4042 if (!GPS_Get_Ack(fd, &tra, &rec)) {
4043 GPS_Error("A300_Send: Track complete data not acknowledged");
4044 return FRAMING_ERROR;
4045 }
4046
4047 if (!GPS_Device_Off(fd)) {
4048 return gps_errno;
4049 }
4050
4051 return 1;
4052 }
4053
4054
4055
4056 /* @func GPS_A301_Send **************************************************
4057 **
4058 ** Send track log to GPS (A301/A302). Note that in case of A302, track
4059 ** log transfer is part of the course transfer sequence, so we must not
4060 ** call GPS_Device_On/Off() but expect to get a usable gpsdevh from our
4061 ** caller.
4062 **
4063 ** @param [r] port [const char *] serial port
4064 ** @param [r] trk [GPS_PTrack *] track array
4065 ** @param [r] n [int32] number of track entries
4066 ** @param [r] protoid [int] protocol ID (301 or 302)
4067 ** @param [r] fd [gpsdevh *] pointer to communication port (for A302 only)
4068 **
4069 ** @return [int32] success
4070 ************************************************************************/
GPS_A301_Send(const char * port,GPS_PTrack * trk,int32 n,int protoid,gpsdevh * fd)4071 int32 GPS_A301_Send(const char* port, GPS_PTrack* trk, int32 n, int protoid,
4072 gpsdevh* fd)
4073 {
4074 UC data[GPS_ARB_LEN];
4075 GPS_PPacket tra;
4076 GPS_PPacket rec;
4077 int32 i;
4078 int32 len;
4079 US method;
4080 US Pid_Trk_Data, Pid_Trk_Hdr, Cmnd_Transfer_Trk;
4081 int32 trk_type, trk_hdr_type;
4082
4083 if (gps_trk_transfer == -1) {
4084 return GPS_UNSUPPORTED;
4085 }
4086
4087 /* A301 and A302 are similar except for all these protocol IDs */
4088 switch (protoid) {
4089 case 301:
4090 Pid_Trk_Data = LINK_ID[gps_link_type].Pid_Trk_Data;
4091 Pid_Trk_Hdr = LINK_ID[gps_link_type].Pid_Trk_Hdr;
4092 Cmnd_Transfer_Trk = COMMAND_ID[gps_device_command].Cmnd_Transfer_Trk;
4093 trk_type = gps_trk_type;
4094 trk_hdr_type = gps_trk_hdr_type;
4095 break;
4096 case 302:
4097 Pid_Trk_Data = LINK_ID[gps_link_type].Pid_Course_Trk_Data;
4098 Pid_Trk_Hdr = LINK_ID[gps_link_type].Pid_Course_Trk_Hdr;
4099 Cmnd_Transfer_Trk = COMMAND_ID[gps_device_command].Cmnd_Transfer_Course_Tracks;
4100 trk_type = gps_run_crs_trk_type;
4101 trk_hdr_type = gps_run_crs_trk_hdr_type;
4102 break;
4103 default:
4104 GPS_Error("A301_Send: Bad protocol ID %d", protoid);
4105 return GPS_UNSUPPORTED;
4106 }
4107
4108 /* Only those GPS' with L001 can send track data */
4109 if (!Pid_Trk_Data) {
4110 GPS_Warning("A301 protocol unsupported");
4111 return GPS_UNSUPPORTED;
4112 }
4113
4114 if (protoid != 302 && !GPS_Device_On(port, &fd)) {
4115 return gps_errno;
4116 }
4117
4118 GPS_Util_Put_Short(data,(US) n);
4119 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records,
4120 data,2);
4121 if (!GPS_Write_Packet(fd,tra)) {
4122 return gps_errno;
4123 }
4124 if (!GPS_Get_Ack(fd, &tra, &rec)) {
4125 GPS_Error("A301_Send: Track start data not acknowledged");
4126 return FRAMING_ERROR;
4127 }
4128
4129
4130 for (i=0; i<n; ++i) {
4131 if (trk[i]->ishdr) {
4132 method = Pid_Trk_Hdr;
4133
4134 switch (trk_hdr_type) {
4135 case pD310:
4136 case pD312:
4137 GPS_D310_Send(data,trk[i],&len);
4138 break;
4139 case pD311:
4140 GPS_D311_Send(data,trk[i],&len);
4141 break;
4142 default:
4143 GPS_Error("A301_Send: Unknown track protocol %d", trk_hdr_type);
4144 return PROTOCOL_ERROR;
4145 }
4146 } else {
4147 method = Pid_Trk_Data;
4148
4149 switch (trk_type) {
4150 case pD300:
4151 GPS_D300_Send(data,trk[i],&len);
4152 break;
4153 case pD301:
4154 GPS_D301_Send(data,trk[i],&len,301);
4155 break;
4156 case pD302:
4157 GPS_D301_Send(data,trk[i],&len,302);
4158 break;
4159 case pD303:
4160 case pD304:
4161 GPS_D303_Send(data,trk[i],&len,(trk_type==pD303) ? 303 : 304);
4162 break;
4163 default:
4164 GPS_Error("A301_Send: Unknown track protocol");
4165 return PROTOCOL_ERROR;
4166 }
4167 }
4168
4169 GPS_Make_Packet(&tra, method, data, len);
4170
4171 if (!GPS_Write_Packet(fd,tra)) {
4172 return gps_errno;
4173 }
4174
4175 if (!GPS_Get_Ack(fd, &tra, &rec)) {
4176 GPS_Error("A301_Send: Track packet not acknowledged");
4177 return FRAMING_ERROR;
4178 }
4179 }
4180
4181 GPS_Util_Put_Short(data, Cmnd_Transfer_Trk);
4182 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt,
4183 data,2);
4184 if (!GPS_Write_Packet(fd,tra)) {
4185 return gps_errno;
4186 }
4187 if (!GPS_Get_Ack(fd, &tra, &rec)) {
4188 GPS_Error("A301_Send: Track complete data not acknowledged");
4189 return FRAMING_ERROR;
4190 }
4191
4192 if (protoid != 302 && !GPS_Device_Off(fd)) {
4193 return gps_errno;
4194 }
4195
4196 return 1;
4197 }
4198
4199
4200 /* @func GPS_D300_Get ******************************************************
4201 **
4202 ** Get track data
4203 **
4204 ** @param [w] trk [GPS_PTrack *] track array
4205 ** @param [r] entries [int32] number of packets to receive
4206 ** @param [r] fd [int32] file descriptor
4207 **
4208 ** @return [int32] number of entries read
4209 ************************************************************************/
GPS_D300_Get(GPS_PTrack * trk,int32 entries,gpsdevh * fd)4210 int32 GPS_D300_Get(GPS_PTrack* trk, int32 entries, gpsdevh* fd)
4211 {
4212 GPS_PPacket tra;
4213 GPS_PPacket rec;
4214 int32 i;
4215
4216
4217 for (i=0; i<entries; ++i) {
4218 if (!GPS_Packet_Read(fd, &rec)) {
4219 return gps_errno;
4220 }
4221 if (!GPS_Send_Ack(fd, &tra, &rec)) {
4222 return gps_errno;
4223 }
4224
4225 GPS_A300_Translate(rec.data, &trk[i]);
4226 }
4227
4228
4229 if (!GPS_Packet_Read(fd, &rec)) {
4230 return gps_errno;
4231 }
4232 if (!GPS_Send_Ack(fd, &tra, &rec)) {
4233 return gps_errno;
4234 }
4235
4236
4237 if (rec.type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) {
4238 GPS_Error("D300_GET: Error transferring track log");
4239 return FRAMING_ERROR;
4240 }
4241
4242 return i;
4243 }
4244
4245
4246
4247 /* @func GPS_D300b_Get ******************************************************
4248 **
4249 ** Get track data (A301 protocol)
4250 **
4251 ** @param [w] trk [GPS_PTrack *] track
4252 ** @param [r] data [UC *] packet data
4253 **
4254 ** @return [void]
4255 ************************************************************************/
GPS_D300b_Get(GPS_PTrack * trk,UC * data)4256 void GPS_D300b_Get(GPS_PTrack* trk, UC* data)
4257 {
4258
4259 GPS_A300_Translate(data, trk);
4260 return;
4261 }
4262
4263
4264
4265 /* @func GPS_D301b_Get ******************************************************
4266 **
4267 ** Get track data (A301 protocol)
4268 **
4269 ** @param [w] trk [GPS_PTrack *] track
4270 ** @param [r] data [UC *] packet data
4271 **
4272 ** @return [void]
4273 ************************************************************************/
GPS_D301b_Get(GPS_PTrack * trk,UC * data)4274 void GPS_D301b_Get(GPS_PTrack* trk, UC* data)
4275 {
4276 UC* p;
4277 uint32 t;
4278
4279 p=data;
4280
4281 (*trk)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
4282 p+=sizeof(int32);
4283
4284 (*trk)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
4285 p+=sizeof(int32);
4286
4287 t = GPS_Util_Get_Uint(p);
4288 if (!t || t==0x7fffffff || t==0xffffffff) {
4289 (*trk)->Time=0;
4290 } else {
4291 (*trk)->Time = GPS_Math_Gtime_To_Utime((time_t)t);
4292 }
4293 p+=sizeof(uint32);
4294
4295 (*trk)->alt = GPS_Util_Get_Float(p);
4296 p+=sizeof(float);
4297
4298 (*trk)->dpth = GPS_Util_Get_Float(p);
4299 p+=sizeof(float);
4300
4301 (*trk)->tnew = *p;
4302
4303 return;
4304 }
4305
4306 /* @func GPS_D302b_Get ******************************************************
4307 **
4308 ** Get track data (A301 protocol)
4309 **
4310 ** @param [w] trk [GPS_PTrack *] track
4311 ** @param [r] data [UC *] packet data
4312 **
4313 ** @return [void]
4314 ************************************************************************/
GPS_D302b_Get(GPS_PTrack * trk,UC * data)4315 void GPS_D302b_Get(GPS_PTrack* trk, UC* data)
4316 {
4317 UC* p;
4318 uint32 t;
4319 double gps_temp;
4320
4321 p=data;
4322
4323 (*trk)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
4324 p+=sizeof(int32);
4325
4326 (*trk)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
4327 p+=sizeof(int32);
4328
4329 t = GPS_Util_Get_Uint(p);
4330 if (!t || t==0x7fffffff || t==0xffffffff) {
4331 (*trk)->Time=0;
4332 } else {
4333 (*trk)->Time = GPS_Math_Gtime_To_Utime((time_t)t);
4334 }
4335 p+=sizeof(uint32);
4336
4337 (*trk)->alt = GPS_Util_Get_Float(p);
4338 p+=sizeof(float);
4339
4340 (*trk)->dpth = GPS_Util_Get_Float(p);
4341 p+=sizeof(float);
4342
4343 /* The only difference between 302 and 301 is the presence of temp
4344 * in the middle. Nice planning, eh?
4345 */
4346 gps_temp = GPS_Util_Get_Float(p);
4347 if (gps_temp <= 1.0e24) {
4348 (*trk)->temperature_populated = 1;
4349 (*trk)->temperature = gps_temp;
4350 }
4351
4352 p+=sizeof(float);
4353
4354 (*trk)->tnew = *p;
4355
4356 return;
4357 }
4358
4359
4360 /* @func GPS_D303b_Get ******************************************************
4361 **
4362 ** Get track data (A302 protocol) -- used in Forerunner 301
4363 **
4364 ** @param [w] trk [GPS_PTrack *] track
4365 ** @param [r] data [UC *] packet data
4366 **
4367 ** @return [void]
4368 ************************************************************************/
GPS_D303b_Get(GPS_PTrack * trk,UC * data)4369 void GPS_D303b_Get(GPS_PTrack* trk, UC* data)
4370 {
4371 UC* p;
4372 uint32 t;
4373 uint32 raw_lat, raw_lon;
4374 int lat_undefined, lon_undefined;
4375 p=data;
4376
4377 /* Latitude and longitude are sometimes invalid (0x7fffffff or
4378 * maybe 0xffffffff?) I guess this makes sense if the device is
4379 * reporting heart rate and time anyway. I presume that latitude
4380 * and longitude are defined or left undefined together?
4381 */
4382 raw_lat = GPS_Util_Get_Int(p);
4383 lat_undefined = !raw_lat || raw_lat==0x7fffffff || raw_lat==0xffffffff;
4384 if (lat_undefined) {
4385 (*trk)->lat=0;
4386 } else {
4387 (*trk)->lat = GPS_Math_Semi_To_Deg(raw_lat);
4388 }
4389 p+=sizeof(int32);
4390
4391 raw_lon = GPS_Util_Get_Int(p);
4392 lon_undefined = !raw_lon || raw_lon==0x7fffffff || raw_lon==0xffffffff;
4393 if (lon_undefined) {
4394 (*trk)->lon=0;
4395 } else {
4396 (*trk)->lon = GPS_Math_Semi_To_Deg(raw_lon);
4397 }
4398 p+=sizeof(int32);
4399
4400 /*
4401 * Let the caller decide if it wants to toss trackpionts with only
4402 * heart rate and/or time data.
4403 */
4404 if (lat_undefined || lon_undefined) {
4405 (*trk)->no_latlon = 1;
4406 }
4407
4408 if (lat_undefined != lon_undefined) {
4409 GPS_Warning("GPS_D303b_Get: assumption (lat_undefined == lon_undefined) violated");
4410 }
4411
4412 t = GPS_Util_Get_Uint(p);
4413
4414 if (!t || t==0x7fffffff || t==0xffffffff) {
4415 (*trk)->Time=0;
4416 } else {
4417 (*trk)->Time = GPS_Math_Gtime_To_Utime((time_t)t);
4418 }
4419 p+=sizeof(uint32);
4420
4421 (*trk)->alt = GPS_Util_Get_Float(p);
4422 p+=sizeof(float);
4423
4424 /* Heartrate is reported as 0 if there is no signal from
4425 * a heartrate monitor.
4426 * 303 and 304 are identical until now.
4427 */
4428 switch (gps_trk_type) {
4429 case pD304:
4430 (*trk)->distance = GPS_Util_Get_Float(p);
4431 (*trk)->distance_populated = ((*trk)->distance <= 1e24);
4432 p+=sizeof(float); /* A float indicating number of meters travelled. */
4433
4434 (*trk)->heartrate = (*p++);
4435 /* crank cadence, RPM, 0xff if invalid. */
4436 if (*p != 0xff) {
4437 (*trk)->cadence = (*p);
4438 }
4439 p++;
4440
4441 (*trk)->wsensor_pres = (*p++);
4442
4443 break;
4444 case pD303:
4445 (*trk)->heartrate = *p++;
4446 break;
4447 }
4448
4449 return;
4450 }
4451
4452
4453 /* @func GPS_D310_Get ******************************************************
4454 **
4455 ** Get track header data (A301 protocol)
4456 **
4457 ** @param [w] trk [GPS_PTrack *] track
4458 ** @param [r] s [UC *] packet data
4459 **
4460 ** @return [void]
4461 ************************************************************************/
GPS_D310_Get(GPS_PTrack * trk,UC * s)4462 void GPS_D310_Get(GPS_PTrack* trk, UC* s)
4463 {
4464 UC* p;
4465 UC* q;
4466
4467 p=s;
4468
4469 (*trk)->dspl = *p++;
4470 (*trk)->colour = *p++;
4471
4472 q = (UC*)(*trk)->trk_ident;
4473
4474 while ((*q++ = *p++));
4475
4476 return;
4477 }
4478
4479 /* @func GPS_D311_Get ******************************************************
4480 **
4481 ** Get track header data (A301 protocol)
4482 **
4483 ** @param [w] trk [GPS_PTrack *] track
4484 ** @param [r] s [UC *] packet data
4485 **
4486 ** @return [void]
4487 ************************************************************************/
GPS_D311_Get(GPS_PTrack * trk,UC * s)4488 void GPS_D311_Get(GPS_PTrack* trk, UC* s)
4489 {
4490 short identifier;
4491
4492 /* Forerunner */
4493 identifier = GPS_Util_Get_Short(s);
4494 sprintf((*trk)->trk_ident, "%d", identifier);
4495
4496 return;
4497 }
4498
4499
4500 /* @func GPS_D300_Send **************************************************
4501 **
4502 ** Form track data string
4503 **
4504 ** @param [w] data [UC *] string to write to
4505 ** @param [r] trk [GPS_PTrack] track data
4506 ** @param [w] len [int32 *] packet length
4507 **
4508 ** @return [void]
4509 ************************************************************************/
GPS_D300_Send(UC * data,GPS_PTrack trk,int32 * len)4510 void GPS_D300_Send(UC* data, GPS_PTrack trk, int32* len)
4511 {
4512 UC* p;
4513
4514 p = data;
4515 GPS_A300_Encode(p,trk);
4516
4517 *len = 13;
4518
4519 return;
4520 }
4521
4522
4523
4524 /* @func GPS_D301_Send **************************************************
4525 **
4526 ** Form track data string (D301 or D302, depending on type value)
4527 **
4528 ** @param [w] data [UC *] string to write to
4529 ** @param [r] trk [GPS_PTrack] track data
4530 ** @param [w] len [int32 *] packet length
4531 ** @param [r] type [int] track point type (301 or 302)
4532 **
4533 ** @return [void]
4534 ************************************************************************/
GPS_D301_Send(UC * data,GPS_PTrack trk,int32 * len,int type)4535 void GPS_D301_Send(UC* data, GPS_PTrack trk, int32* len, int type)
4536 {
4537 UC* p;
4538
4539 p = data;
4540 GPS_A300_Encode(p,trk);
4541 p = data+12;
4542
4543 GPS_Util_Put_Float(p,trk->alt);
4544 p+=sizeof(float);
4545 GPS_Util_Put_Float(p,trk->dpth);
4546 p+=sizeof(float);
4547
4548 if (type == 302) {
4549 /* Temperature */
4550 GPS_Util_Put_Float(p, 1.0e25f);
4551 p+=sizeof(float);
4552 }
4553
4554 *p = trk->tnew;
4555 p+=sizeof(UC);
4556
4557 *len = p-data;
4558
4559 return;
4560 }
4561
4562
4563 /* @func GPS_D303_Send **************************************************
4564 **
4565 ** Form track data string (D303/D304)
4566 **
4567 ** @param [w] data [UC *] string to write to
4568 ** @param [r] trk [GPS_PTrack] track data
4569 ** @param [w] len [int32 *] packet length
4570 ** @param [r] protoid [int] track point type (303 or 304)
4571 **
4572 ** @return [void]
4573 ************************************************************************/
GPS_D303_Send(UC * data,GPS_PTrack trk,int32 * len,int protoid)4574 void GPS_D303_Send(UC* data, GPS_PTrack trk, int32* len, int protoid)
4575 {
4576 UC* p;
4577
4578 p = data;
4579 GPS_A300_Encode(p,trk);
4580 p = data+12;
4581
4582 GPS_Util_Put_Float(p,trk->alt);
4583 p+=sizeof(float);
4584
4585 if (protoid == 304) {
4586 GPS_Util_Put_Float(p, trk->distance_populated ? trk->distance : 1e25);
4587 p+=sizeof(float);
4588 }
4589
4590 *p = trk->heartrate;
4591 p+=sizeof(char);
4592
4593 if (protoid == 304) {
4594 *p = trk->cadence > 0 ? trk->cadence : 0xff;
4595 p+=sizeof(char);
4596
4597 *p = trk->wsensor_pres;
4598 p+=sizeof(char);
4599 }
4600
4601 *len = p-data;
4602 }
4603
4604 /* @func GPS_D311_Send **************************************************
4605 **
4606 ** Form track header data string
4607 **
4608 ** @param [w] data [UC *] string to write to
4609 ** @param [r] trk [GPS_PTrack] track data
4610 ** @param [w] len [int32 *] length of data
4611 **
4612 ** @return [void]
4613 ************************************************************************/
GPS_D311_Send(UC * data,GPS_PTrack trk,int32 * len)4614 void GPS_D311_Send(UC* data, GPS_PTrack trk, int32* len)
4615 {
4616 UC* p;
4617
4618 p = data;
4619 GPS_Util_Put_Short(p,strtoul(trk->trk_ident, nullptr, 0));
4620 p += 2;
4621 *len = p-data;
4622 }
4623
4624 /* @func GPS_D310_Send **************************************************
4625 **
4626 ** Form track header data string
4627 **
4628 ** @param [w] data [UC *] string to write to
4629 ** @param [r] trk [GPS_PTrack] track data
4630 ** @param [w] len [int32 *] length of data
4631 **
4632 ** @return [void]
4633 ************************************************************************/
GPS_D310_Send(UC * data,GPS_PTrack trk,int32 * len)4634 void GPS_D310_Send(UC* data, GPS_PTrack trk, int32* len)
4635 {
4636 UC* p;
4637 UC* q;
4638
4639 p = data;
4640
4641 *p++ = trk->dspl;
4642 *p++ = trk->colour;
4643
4644 q = (UC*) trk->trk_ident;
4645 while ((*p++ = *q++));
4646
4647 *len = p-data;
4648 }
4649
4650
4651 /* @funcstatic GPS_A300_Translate ***************************************
4652 **
4653 ** Translate track packet to track structure
4654 **
4655 ** @param [r] s [const UC *] track packet data
4656 ** @param [w] trk [GPS_PTrack *] track entry pointer
4657 **
4658 ** @return [void]
4659 ************************************************************************/
GPS_A300_Translate(UC * s,GPS_PTrack * trk)4660 static void GPS_A300_Translate(UC* s, GPS_PTrack* trk)
4661 {
4662 UC* p;
4663 uint32 t;
4664
4665 p=s;
4666
4667 (*trk)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
4668 p+=sizeof(int32);
4669
4670 (*trk)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
4671 p+=sizeof(int32);
4672
4673 t = GPS_Util_Get_Uint(p);
4674 if (!t || t==0x7fffffff || t==0xffffffff) {
4675 (*trk)->Time=0;
4676 } else {
4677 (*trk)->Time = GPS_Math_Gtime_To_Utime((time_t)t);
4678 }
4679 p+=sizeof(uint32);
4680
4681 (*trk)->tnew = *p;
4682 }
4683
4684
4685
4686 /* @funcstatic GPS_A300_Encode ***************************************
4687 **
4688 ** Encode track structure to track packet
4689 **
4690 ** @param [w] s [UC *] string to write to
4691 ** @param [r] trk [GPS_PTrack] track entry
4692 **
4693 ** @return [void]
4694 ************************************************************************/
GPS_A300_Encode(UC * s,GPS_PTrack trk)4695 static void GPS_A300_Encode(UC* s, GPS_PTrack trk)
4696 {
4697 UC* p;
4698
4699 p=s;
4700
4701 /* Note: lat/lon == 0x7fffffff is only valid for D303/D304, but our
4702 * caller shouldn't set no_latlon unless one of these protocols actually
4703 * is in use */
4704 GPS_Util_Put_Int(p,trk->no_latlon ? 0x7fffffff : GPS_Math_Deg_To_Semi(trk->lat));
4705 p+=sizeof(int32);
4706
4707 GPS_Util_Put_Int(p,trk->no_latlon ? 0x7fffffff : GPS_Math_Deg_To_Semi(trk->lon));
4708 p+=sizeof(int32);
4709
4710 GPS_Util_Put_Uint(p,GPS_Math_Utime_To_Gtime(trk->Time));
4711 p+=sizeof(uint32);
4712
4713 *p = (UC) trk->tnew;
4714 }
4715
4716
4717
4718 /* @func GPS_A400_Get **************************************************
4719 **
4720 ** Get proximity waypoint data from GPS
4721 **
4722 ** @param [r] port [const char *] serial port
4723 ** @param [w] way [GPS_PWay **] waypoint array
4724 **
4725 ** @return [int32] number of waypoint entries
4726 ************************************************************************/
GPS_A400_Get(const char * port,GPS_PWay ** way)4727 int32 GPS_A400_Get(const char* port, GPS_PWay** way)
4728 {
4729 static UC data[2];
4730 gpsdevh* fd;
4731 GPS_PPacket tra;
4732 GPS_PPacket rec;
4733 int32 n;
4734 int32 i;
4735
4736 if (gps_prx_waypt_transfer == -1) {
4737 return GPS_UNSUPPORTED;
4738 }
4739
4740
4741 if (!GPS_Device_On(port, &fd)) {
4742 return gps_errno;
4743 }
4744
4745
4746 GPS_Util_Put_Short(data,
4747 COMMAND_ID[gps_device_command].Cmnd_Transfer_Prx);
4748 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
4749 data,2);
4750 if (!GPS_Write_Packet(fd,tra)) {
4751 return gps_errno;
4752 }
4753 if (!GPS_Get_Ack(fd, &tra, &rec)) {
4754 return gps_errno;
4755 }
4756
4757 if (!GPS_Device_Chars_Ready(fd)) {
4758 GPS_Warning("A400 (ppx) protocol not supported");
4759
4760 if (!GPS_Device_Off(fd)) {
4761 return gps_errno;
4762 }
4763
4764 return GPS_UNSUPPORTED;
4765 }
4766
4767 if (!GPS_Packet_Read(fd, &rec)) {
4768 return gps_errno;
4769 }
4770
4771 if (!GPS_Send_Ack(fd, &tra, &rec)) {
4772 return gps_errno;
4773 }
4774
4775 n = GPS_Util_Get_Short(rec.data);
4776
4777 if (n)
4778 if (!((*way)=(GPS_PWay*)malloc(n*sizeof(GPS_PWay)))) {
4779 GPS_Error("A400_Get: Insufficient memory");
4780 return MEMORY_ERROR;
4781 }
4782
4783
4784 for (i=0; i<n; ++i) {
4785 if (!((*way)[i]=GPS_Way_New())) {
4786 return MEMORY_ERROR;
4787 }
4788
4789 if (!GPS_Packet_Read(fd, &rec)) {
4790 return gps_errno;
4791 }
4792 if (!GPS_Send_Ack(fd, &tra, &rec)) {
4793 return gps_errno;
4794 }
4795
4796
4797 switch (gps_prx_waypt_type) {
4798 case pD400:
4799 GPS_D400_Get(&((*way)[i]),rec.data);
4800 break;
4801 case pD101:
4802 GPS_D101_Get(&((*way)[i]),rec.data);
4803 break;
4804 case pD102:
4805 GPS_D102_Get(&((*way)[i]),rec.data);
4806 break;
4807 case pD403:
4808 GPS_D403_Get(&((*way)[i]),rec.data);
4809 break;
4810 case pD104:
4811 GPS_D104_Get(&((*way)[i]),rec.data);
4812 break;
4813 case pD105:
4814 GPS_D105_Get(&((*way)[i]),rec.data);
4815 break;
4816 case pD106:
4817 GPS_D106_Get(&((*way)[i]),rec.data);
4818 break;
4819 case pD107:
4820 GPS_D107_Get(&((*way)[i]),rec.data);
4821 break;
4822 case pD108:
4823 GPS_D108_Get(&((*way)[i]),rec.data);
4824 break;
4825 case pD109:
4826 GPS_D109_Get(&((*way)[i]),rec.data,109);
4827 break;
4828 case pD110:
4829 GPS_D109_Get(&((*way)[i]),rec.data,110);
4830 break;
4831 case pD450:
4832 GPS_D450_Get(&((*way)[i]),rec.data);
4833 break;
4834 case pD151:
4835 GPS_D151_Get(&((*way)[i]),rec.data);
4836 break;
4837 case pD152:
4838 GPS_D152_Get(&((*way)[i]),rec.data);
4839 break;
4840 case pD154:
4841 GPS_D154_Get(&((*way)[i]),rec.data);
4842 break;
4843 case pD155:
4844 GPS_D155_Get(&((*way)[i]),rec.data);
4845 break;
4846 default:
4847 GPS_Error("A400_GET: Unknown prx waypoint protocol");
4848 return PROTOCOL_ERROR;
4849 }
4850 }
4851
4852 if (!GPS_Packet_Read(fd, &rec)) {
4853 return gps_errno;
4854 }
4855 if (!GPS_Send_Ack(fd, &tra, &rec)) {
4856 return gps_errno;
4857 }
4858
4859 if (rec.type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) {
4860 GPS_Error("A400_GET: Error transferring prx waypoints");
4861 return FRAMING_ERROR;
4862 }
4863
4864 if (i != n) {
4865 GPS_Error("A400_GET: Prx waypoint entry number mismatch");
4866 return FRAMING_ERROR;
4867 }
4868
4869 if (!GPS_Device_Off(fd)) {
4870 return gps_errno;
4871 }
4872
4873 return n;
4874 }
4875
4876
4877
4878 /* @func GPS_A400_Send **************************************************
4879 **
4880 ** Send proximity waypoints to GPS
4881 **
4882 ** @param [r] port [const char *] serial port
4883 ** @param [r] trk [GPS_PWay *] waypoint array
4884 ** @param [r] n [int32] number of waypoint entries
4885 **
4886 ** @return [int32] success
4887 ************************************************************************/
GPS_A400_Send(const char * port,GPS_PWay * way,int32 n)4888 int32 GPS_A400_Send(const char* port, GPS_PWay* way, int32 n)
4889 {
4890 UC data[GPS_ARB_LEN];
4891 gpsdevh* fd;
4892 GPS_PPacket tra;
4893 GPS_PPacket rec;
4894 int32 i;
4895 int32 len;
4896
4897 if (gps_prx_waypt_transfer == -1) {
4898 return GPS_UNSUPPORTED;
4899 }
4900
4901 if (!GPS_Device_On(port, &fd)) {
4902 return gps_errno;
4903 }
4904
4905
4906 GPS_Util_Put_Short(data,(US) n);
4907 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records,
4908 data,2);
4909 if (!GPS_Write_Packet(fd,tra)) {
4910 return gps_errno;
4911 }
4912 if (!GPS_Get_Ack(fd, &tra, &rec)) {
4913 GPS_Error("A400_Send: Prx start data not acknowledged");
4914 return FRAMING_ERROR;
4915 }
4916
4917
4918 for (i=0; i<n; ++i) {
4919 switch (gps_prx_waypt_type) {
4920 case pD400:
4921 GPS_D400_Send(data,way[i],&len);
4922 break;
4923 case pD101:
4924 GPS_D101_Send(data,way[i],&len);
4925 break;
4926 case pD102:
4927 GPS_D102_Send(data,way[i],&len);
4928 break;
4929 case pD403:
4930 GPS_D403_Send(data,way[i],&len);
4931 break;
4932 case pD104:
4933 GPS_D104_Send(data,way[i],&len);
4934 break;
4935 case pD105:
4936 GPS_D105_Send(data,way[i],&len);
4937 break;
4938 case pD106:
4939 GPS_D106_Send(data,way[i],&len);
4940 break;
4941 case pD107:
4942 GPS_D107_Send(data,way[i],&len);
4943 break;
4944 case pD108:
4945 GPS_D108_Send(data,way[i],&len);
4946 break;
4947 case pD450:
4948 GPS_D450_Send(data,way[i],&len);
4949 break;
4950 case pD151:
4951 GPS_D151_Send(data,way[i],&len);
4952 break;
4953 case pD152:
4954 GPS_D152_Send(data,way[i],&len);
4955 break;
4956 case pD154:
4957 GPS_D154_Send(data,way[i],&len);
4958 break;
4959 case pD155:
4960 GPS_D155_Send(data,way[i],&len);
4961 break;
4962 default:
4963 GPS_Error("A400_Send: Unknown prx waypoint protocol");
4964 return PROTOCOL_ERROR;
4965 }
4966
4967 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Prx_Wpt_Data,
4968 data, len);
4969
4970 if (!GPS_Write_Packet(fd,tra)) {
4971 return gps_errno;
4972 }
4973
4974 if (!GPS_Get_Ack(fd, &tra, &rec)) {
4975 GPS_Error("A400_Send: Pid_Prx_Wpt_Data not acknowledged");
4976 return FRAMING_ERROR;
4977 }
4978 }
4979
4980 GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Prx);
4981 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt,
4982 data,2);
4983 if (!GPS_Write_Packet(fd,tra)) {
4984 return gps_errno;
4985 }
4986 if (!GPS_Get_Ack(fd, &tra, &rec)) {
4987 GPS_Error("A400_Send: Prx waypoint complete data not acknowledged");
4988 return FRAMING_ERROR;
4989 }
4990
4991 if (!GPS_Device_Off(fd)) {
4992 return gps_errno;
4993 }
4994
4995 return 1;
4996 }
4997
4998
4999
5000 /* @funcstatic GPS_D400_Get ********************************************
5001 **
5002 ** Get proximity waypoint data
5003 **
5004 ** @param [w] way [GPS_PWay *] waypoint array
5005 ** @param [r] s [UC *] packet data
5006 **
5007 ** @return [void]
5008 ************************************************************************/
GPS_D400_Get(GPS_PWay * way,UC * s)5009 static void GPS_D400_Get(GPS_PWay* way, UC* s)
5010 {
5011 UC* p;
5012 int32 i;
5013
5014 p=s;
5015
5016 (*way)->prot = 400;
5017 for (i=0; i<6; ++i) {
5018 (*way)->ident[i] = *p++;
5019 }
5020
5021 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
5022 p+=sizeof(int32);
5023
5024 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
5025 p+=sizeof(int32);
5026
5027 p+=sizeof(int32);
5028
5029 for (i=0; i<40; ++i) {
5030 (*way)->cmnt[i] = *p++;
5031 }
5032
5033 (*way)->dst=GPS_Util_Get_Float(p);
5034 }
5035
5036
5037 /* @funcstatic GPS_D403_Get ********************************************
5038 **
5039 ** Get proximity waypoint data
5040 **
5041 ** @param [w] way [GPS_PWay *] waypoint array
5042 ** @param [r] s [UC *] packet data
5043 **
5044 ** @return [void]
5045 ************************************************************************/
GPS_D403_Get(GPS_PWay * way,UC * s)5046 static void GPS_D403_Get(GPS_PWay* way, UC* s)
5047 {
5048 UC* p;
5049 int32 i;
5050
5051 p=s;
5052
5053 (*way)->prot = 403;
5054 for (i=0; i<6; ++i) {
5055 (*way)->ident[i] = *p++;
5056 }
5057
5058 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
5059 p+=sizeof(int32);
5060
5061 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
5062 p+=sizeof(int32);
5063
5064 p+=sizeof(int32);
5065
5066 for (i=0; i<40; ++i) {
5067 (*way)->cmnt[i] = *p++;
5068 }
5069
5070 (*way)->smbl = *p++;
5071 (*way)->dspl = *p++;
5072
5073 (*way)->dst=GPS_Util_Get_Float(p);
5074 }
5075
5076
5077 /* @funcstatic GPS_D450_Get ********************************************
5078 **
5079 ** Get proximity waypoint data
5080 **
5081 ** @param [w] way [GPS_PWay *] waypoint array
5082 ** @param [r] s [UC *] packet data
5083 **
5084 ** @return [void]
5085 ************************************************************************/
GPS_D450_Get(GPS_PWay * way,UC * s)5086 static void GPS_D450_Get(GPS_PWay* way, UC* s)
5087 {
5088 UC* p;
5089 int32 i;
5090
5091 p=s;
5092
5093 (*way)->prot = 450;
5094
5095 (*way)->idx = GPS_Util_Get_Short(p);
5096 p+=sizeof(int16);
5097
5098 for (i=0; i<6; ++i) {
5099 (*way)->ident[i] = *p++;
5100 }
5101 for (i=0; i<2; ++i) {
5102 (*way)->cc[i] = *p++;
5103 }
5104 (*way)->wpt_class = *p++;
5105
5106 (*way)->lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
5107 p+=sizeof(int32);
5108
5109 (*way)->lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
5110 p+=sizeof(int32);
5111
5112 (*way)->alt = GPS_Util_Get_Short(p);
5113 p+=sizeof(int16);
5114
5115 for (i=0; i<24; ++i) {
5116 (*way)->city[i] = *p++;
5117 }
5118 for (i=0; i<2; ++i) {
5119 (*way)->state[i] = *p++;
5120 }
5121 for (i=0; i<30; ++i) {
5122 (*way)->name[i] = *p++;
5123 }
5124 for (i=0; i<40; ++i) {
5125 (*way)->cmnt[i] = *p++;
5126 }
5127
5128 (*way)->dst=GPS_Util_Get_Float(p);
5129 }
5130
5131
5132 /* @funcstatic GPS_D400_Send ********************************************
5133 **
5134 ** Form proximity waypoint data string
5135 **
5136 ** @param [w] data [UC *] string to write to
5137 ** @param [r] way [GPS_PWay] waypoint data
5138 ** @param [w] len [int32 *] packet length
5139 **
5140 ** @return [void]
5141 ************************************************************************/
GPS_D400_Send(UC * data,GPS_PWay way,int32 * len)5142 static void GPS_D400_Send(UC* data, GPS_PWay way, int32* len)
5143 {
5144 UC* p;
5145 int32 i;
5146
5147 p = data;
5148
5149 for (i=0; i<6; ++i) {
5150 *p++ = way->ident[i];
5151 }
5152 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
5153 p+=sizeof(int32);
5154 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
5155 p+=sizeof(int32);
5156 GPS_Util_Put_Uint(p,0);
5157 p+=sizeof(int32);
5158 for (i=0; i<40; ++i) {
5159 *p++ = way->cmnt[i];
5160 }
5161
5162 GPS_Util_Put_Float(p,way->dst);
5163
5164 *len = 62;
5165 }
5166
5167
5168 /* @funcstatic GPS_D403_Send *******************************************
5169 **
5170 ** Form proximity waypoint data string
5171 **
5172 ** @param [w] data [UC *] string to write to
5173 ** @param [r] way [GPS_PWay] waypoint data
5174 ** @param [w] len [int32 *] packet length
5175 **
5176 ** @return [void]
5177 ************************************************************************/
GPS_D403_Send(UC * data,GPS_PWay way,int32 * len)5178 static void GPS_D403_Send(UC* data, GPS_PWay way, int32* len)
5179 {
5180 UC* p;
5181 int32 i;
5182
5183 p = data;
5184
5185 for (i=0; i<6; ++i) {
5186 *p++ = way->ident[i];
5187 }
5188 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
5189 p+=sizeof(int32);
5190 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
5191 p+=sizeof(int32);
5192 GPS_Util_Put_Uint(p,0);
5193 p+=sizeof(int32);
5194 for (i=0; i<40; ++i) {
5195 *p++ = way->cmnt[i];
5196 }
5197
5198 *p++ = way->smbl;
5199 *p = way->dspl;
5200
5201 GPS_Util_Put_Float(p,way->dst);
5202
5203 *len = 64;
5204 }
5205
5206
5207 /* @funcstatic GPS_D450_Send *******************************************
5208 **
5209 ** Form proximity waypoint data string
5210 **
5211 ** @param [w] data [UC *] string to write to
5212 ** @param [r] way [GPS_PWay] waypoint data
5213 ** @param [w] len [int32 *] packet length
5214 **
5215 ** @return [void]
5216 ************************************************************************/
GPS_D450_Send(UC * data,GPS_PWay way,int32 * len)5217 static void GPS_D450_Send(UC* data, GPS_PWay way, int32* len)
5218 {
5219 UC* p;
5220 int32 i;
5221
5222 p = data;
5223
5224 GPS_Util_Put_Short(p,(US) way->idx);
5225 p+=sizeof(int16);
5226
5227 for (i=0; i<6; ++i) {
5228 *p++ = way->ident[i];
5229 }
5230 for (i=0; i<2; ++i) {
5231 *p++ = way->cc[i];
5232 }
5233 *p++ = way->wpt_class;
5234
5235 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lat));
5236 p+=sizeof(int32);
5237 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(way->lon));
5238 p+=sizeof(int32);
5239
5240 GPS_Util_Put_Short(p,(US) way->alt);
5241 p+=sizeof(int16);
5242
5243 for (i=0; i<24; ++i) {
5244 *p++ = way->city[i];
5245 }
5246 for (i=0; i<2; ++i) {
5247 *p++ = way->state[i];
5248 }
5249 for (i=0; i<30; ++i) {
5250 *p++ = way->name[i];
5251 }
5252 for (i=0; i<40; ++i) {
5253 *p++ = way->cmnt[i];
5254 }
5255
5256 GPS_Util_Put_Float(p,way->dst);
5257
5258
5259 *len = 121;
5260 }
5261
5262
5263
5264 /* @func GPS_A500_Get ******************************************************
5265 **
5266 ** Get almanac from GPS
5267 **
5268 ** @param [r] port [const char *] serial port
5269 ** @param [w] alm [GPS_PAlmanac **] almanac array
5270 **
5271 ** @return [int32] number of almanac entries
5272 ************************************************************************/
GPS_A500_Get(const char * port,GPS_PAlmanac ** alm)5273 int32 GPS_A500_Get(const char* port, GPS_PAlmanac** alm)
5274 {
5275 static UC data[2];
5276 gpsdevh* fd;
5277 GPS_PPacket trapkt;
5278 GPS_PPacket recpkt;
5279 int32 i, n;
5280
5281 if (gps_almanac_transfer == -1) {
5282 return GPS_UNSUPPORTED;
5283 }
5284
5285 if (!GPS_Device_On(port, &fd)) {
5286 return gps_errno;
5287 }
5288
5289 GPS_Util_Put_Short(data,
5290 COMMAND_ID[gps_device_command].Cmnd_Transfer_Alm);
5291 GPS_Make_Packet(&trapkt, LINK_ID[gps_link_type].Pid_Command_Data,
5292 data,2);
5293 if (!GPS_Write_Packet(fd,trapkt)) {
5294 return gps_errno;
5295 }
5296 if (!GPS_Get_Ack(fd, &trapkt, &recpkt)) {
5297 return gps_errno;
5298 }
5299 if (!GPS_Packet_Read(fd, &recpkt)) {
5300 return gps_errno;
5301 }
5302 if (!GPS_Send_Ack(fd, &trapkt, &recpkt)) {
5303 return gps_errno;
5304 }
5305
5306 n = GPS_Util_Get_Short(recpkt.data);
5307
5308 if (n)
5309 if (!((*alm)=(GPS_PAlmanac*)malloc(n*sizeof(GPS_PAlmanac)))) {
5310 GPS_Error("A500_Get: Insufficient memory");
5311 return MEMORY_ERROR;
5312 }
5313
5314 for (i=0; i<n; ++i) {
5315 if (!((*alm)[i]=GPS_Almanac_New())) {
5316 return MEMORY_ERROR;
5317 }
5318 if (!GPS_Packet_Read(fd, &recpkt)) {
5319 return gps_errno;
5320 }
5321
5322 if (!GPS_Send_Ack(fd, &trapkt, &recpkt)) {
5323 return gps_errno;
5324 }
5325
5326 switch (gps_almanac_type) {
5327 case pD500:
5328 GPS_A500_Translate(recpkt.data, &((*alm)[i]));
5329 break;
5330 case pD501:
5331 GPS_A500_Translate(recpkt.data, &((*alm)[i]));
5332 (*alm)[i]->hlth=recpkt.data[42];
5333 break;
5334 case pD550:
5335 (*alm)[i]->svid = recpkt.data[0];
5336 GPS_A500_Translate(recpkt.data+1, &((*alm)[i]));
5337 break;
5338 case pD551:
5339 (*alm)[i]->svid = recpkt.data[0];
5340 GPS_A500_Translate(recpkt.data+1, &((*alm)[i]));
5341 (*alm)[i]->hlth = recpkt.data[43];
5342 break;
5343 default:
5344 GPS_Error("A500_GET: Unknown almanac protocol");
5345 return PROTOCOL_ERROR;
5346 }
5347 /* Cheat and don't _really_ pass the trkpt back */
5348 /* cb(n, NULL);*/
5349 }
5350
5351 if (!GPS_Packet_Read(fd, &recpkt)) {
5352 return gps_errno;
5353 }
5354 if (!GPS_Send_Ack(fd, &trapkt, &recpkt)) {
5355 return gps_errno;
5356 }
5357 if (recpkt.type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) {
5358 GPS_Error("A500_Get: Error transferring almanac");
5359 return FRAMING_ERROR;
5360 }
5361
5362 if (i != n) {
5363 GPS_Error("A500_GET: Almanac entry number mismatch");
5364 return FRAMING_ERROR;
5365 }
5366
5367 if (!GPS_Device_Off(fd)) {
5368 return gps_errno;
5369 }
5370
5371 return n;
5372 }
5373
5374
5375 /* @func GPS_A500_Send **************************************************
5376 **
5377 ** Send almanac to GPS
5378 **
5379 ** @param [r] port [const char *] serial port
5380 ** @param [r] alm [GPS_PAlmanac *] almanac array
5381 ** @param [r] n [int32] number of almanac entries
5382 **
5383 ** @return [int32] success
5384 ************************************************************************/
GPS_A500_Send(const char * port,GPS_PAlmanac * alm,int32 n)5385 int32 GPS_A500_Send(const char* port, GPS_PAlmanac* alm, int32 n)
5386 {
5387 UC data[GPS_ARB_LEN];
5388 gpsdevh* fd;
5389 GPS_PPacket tra;
5390 GPS_PPacket rec;
5391 int32 i;
5392 int32 len;
5393 int32 timesent;
5394 int32 posnsent;
5395 int32 ret;
5396
5397 if (!GPS_Device_On(port, &fd)) {
5398 return gps_errno;
5399 }
5400
5401
5402 GPS_Util_Put_Short(data,(US) n);
5403 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records,
5404 data,2);
5405 if (!GPS_Write_Packet(fd,tra)) {
5406 return gps_errno;
5407 }
5408 if (!GPS_Get_Ack(fd, &tra, &rec)) {
5409 GPS_Error("A500_Send: Almanac start data not acknowledged");
5410 return FRAMING_ERROR;
5411 }
5412
5413
5414 for (i=0; i<n; ++i) {
5415 switch (gps_almanac_type) {
5416 case pD500:
5417 if (n!=32) {
5418 GPS_Error("A500_Send: SATELLITES: n!=32 specified");
5419 return PROTOCOL_ERROR;
5420 }
5421 GPS_D500_Send(data,alm[i]);
5422 len = 42;
5423 break;
5424 case pD501:
5425 if (n!=32) {
5426 GPS_Error("A500_Send: SATELLITES: n!=32 specified");
5427 return PROTOCOL_ERROR;
5428 }
5429 GPS_D501_Send(data,alm[i]);
5430 len = 43;
5431 break;
5432 case pD550:
5433 GPS_D550_Send(data,alm[i]);
5434 len = 43;
5435 break;
5436 case pD551:
5437 GPS_D551_Send(data,alm[i]);
5438 len = 44;
5439 break;
5440 default:
5441 GPS_Error("A500_Send: Unknown almanac protocol");
5442 return 0;
5443 }
5444
5445 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Almanac_Data,
5446 data, len);
5447
5448 if (!GPS_Write_Packet(fd,tra)) {
5449 return gps_errno;
5450 }
5451
5452 if (!GPS_Get_Ack(fd, &tra, &rec)) {
5453 GPS_Error("A500_Send: Almanac Pid_Almanac_Data not acknowledged");
5454 return FRAMING_ERROR;
5455 }
5456 }
5457
5458 GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Alm);
5459 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt,
5460 data,2);
5461 if (!GPS_Write_Packet(fd,tra)) {
5462 return gps_errno;
5463 }
5464 if (!GPS_Get_Ack(fd, &tra, &rec)) {
5465 GPS_Error("A500_Send: Almanac complete data not acknowledged");
5466 return FRAMING_ERROR;
5467 }
5468
5469 timesent=posnsent=0;
5470
5471 /*
5472 * Allow GPS a little while to decide whether it wants to ask for
5473 * the time. Note that the time sent is held in gps_save_time
5474 * global
5475 */
5476 if (GPS_Device_Wait(fd)) {
5477 if (!GPS_Packet_Read(fd, &rec)) {
5478 return gps_errno;
5479 }
5480
5481 if (!GPS_Send_Ack(fd, &tra, &rec)) {
5482 return gps_errno;
5483 }
5484
5485 if (rec.type == LINK_ID[gps_link_type].Pid_Command_Data &&
5486 GPS_Util_Get_Short(rec.data) == COMMAND_ID[gps_device_command].
5487 Cmnd_Transfer_Time) {
5488 GPS_User("INFO: GPS time request. Sending....");
5489 ret = GPS_Rqst_Send_Time(fd,gps_save_time);
5490 if (ret < 0) {
5491 return ret;
5492 }
5493 timesent=1;
5494 }
5495 }
5496
5497
5498
5499 /*
5500 * Allow GPS a little while to decide whether it wants to ask for
5501 * the position. Note that the posn sent is held in gps_save_lat
5502 * and gps_save_lon global!
5503 */
5504 if (GPS_Device_Wait(fd)) {
5505 if (!GPS_Packet_Read(fd, &rec)) {
5506 return gps_errno;
5507 }
5508
5509 if (!GPS_Send_Ack(fd, &tra, &rec)) {
5510 return gps_errno;
5511 }
5512
5513 if (rec.type == LINK_ID[gps_link_type].Pid_Command_Data &&
5514 GPS_Util_Get_Short(rec.data) == COMMAND_ID[gps_device_command].
5515 Cmnd_Transfer_Posn) {
5516 GPS_User("INFO: GPS position request. Sending....");
5517 ret = GPS_Rqst_Send_Position(fd,gps_save_lat,gps_save_lon);
5518 if (ret < 0) {
5519 return ret;
5520 }
5521 posnsent=1;
5522 }
5523 }
5524
5525 if (!timesent) {
5526 ret = GPS_Rqst_Send_Time(fd,gps_save_time);
5527 if (ret < 0) {
5528 return ret;
5529 }
5530 }
5531
5532
5533 if (!posnsent) {
5534 ret = GPS_Rqst_Send_Position(fd,gps_save_lat,gps_save_lon);
5535 if (ret < 0) {
5536 return ret;
5537 }
5538 }
5539
5540
5541 if (!GPS_Device_Off(fd)) {
5542 return gps_errno;
5543 }
5544
5545 return 1;
5546 }
5547
5548 /* @funcstatic GPS_A500_Translate ***************************************
5549 **
5550 ** Translate almanac packet to almanac structure
5551 **
5552 ** @param [r] s [const UC *] almanac packet data
5553 ** @param [w] alm [GPS_PAlmanac *] almanac entry pointer
5554 **
5555 ** @return [void]
5556 ************************************************************************/
GPS_A500_Translate(UC * s,GPS_PAlmanac * alm)5557 static void GPS_A500_Translate(UC* s, GPS_PAlmanac* alm)
5558 {
5559 UC* p;
5560
5561 p=s;
5562
5563 (*alm)->wn = GPS_Util_Get_Short(p);
5564 p+=sizeof(int16);
5565
5566 (*alm)->toa = GPS_Util_Get_Float(p);
5567 p+=sizeof(float);
5568
5569 (*alm)->af0 = GPS_Util_Get_Float(p);
5570 p+=sizeof(float);
5571
5572 (*alm)->af1 = GPS_Util_Get_Float(p);
5573 p+=sizeof(float);
5574
5575 (*alm)->e = GPS_Util_Get_Float(p);
5576 p+=sizeof(float);
5577
5578 (*alm)->sqrta = GPS_Util_Get_Float(p);
5579 p+=sizeof(float);
5580
5581 (*alm)->m0 = GPS_Util_Get_Float(p);
5582 p+=sizeof(float);
5583
5584 (*alm)->w = GPS_Util_Get_Float(p);
5585 p+=sizeof(float);
5586
5587 (*alm)->omg0 = GPS_Util_Get_Float(p);
5588 p+=sizeof(float);
5589
5590 (*alm)->odot = GPS_Util_Get_Float(p);
5591 p+=sizeof(float);
5592
5593 (*alm)->i = GPS_Util_Get_Float(p);
5594 p+=sizeof(float);
5595 }
5596
5597
5598 /* @funcstatic GPS_D500_Send *******************************************
5599 **
5600 ** Form almanac data string
5601 **
5602 ** @param [w] data [UC *] string to write to
5603 ** @param [r] alm [GPS_PAlmanac] almanac data
5604 **
5605 ** @return [void]
5606 ************************************************************************/
GPS_D500_Send(UC * data,GPS_PAlmanac alm)5607 static void GPS_D500_Send(UC* data, GPS_PAlmanac alm)
5608 {
5609 UC* p;
5610
5611 p = data;
5612 GPS_A500_Encode(p,alm);
5613 }
5614
5615
5616
5617 /* @funcstatic GPS_D501_Send ********************************************
5618 **
5619 ** Form almanac data string
5620 **
5621 ** @param [w] data [UC *] string to write to
5622 ** @param [r] alm [GPS_PAlmanac] almanac data
5623 **
5624 ** @return [void]
5625 ************************************************************************/
GPS_D501_Send(UC * data,GPS_PAlmanac alm)5626 static void GPS_D501_Send(UC* data, GPS_PAlmanac alm)
5627 {
5628 UC* p;
5629
5630 p=data;
5631 p[42] = alm->hlth;
5632 GPS_A500_Encode(p,alm);
5633 }
5634
5635
5636
5637 /* @funcstatic GPS_D550_Send ********************************************
5638 **
5639 ** Form almanac data string
5640 **
5641 ** @param [w] data [UC *] string to write to
5642 ** @param [r] alm [GPS_PAlmanac] almanac data
5643 **
5644 ** @return [void]
5645 ************************************************************************/
GPS_D550_Send(UC * data,GPS_PAlmanac alm)5646 static void GPS_D550_Send(UC* data, GPS_PAlmanac alm)
5647 {
5648 UC* p;
5649
5650 p = data;
5651 *p = alm->svid;
5652 GPS_A500_Encode(p+1,alm);
5653 }
5654
5655
5656
5657 /* @funcstatic GPS_D551_Send ********************************************
5658 **
5659 ** Form almanac data string
5660 **
5661 ** @param [w] data [UC *] string to write to
5662 ** @param [r] alm [GPS_PAlmanac] almanac data
5663 **
5664 ** @return [void]
5665 ************************************************************************/
GPS_D551_Send(UC * data,GPS_PAlmanac alm)5666 static void GPS_D551_Send(UC* data, GPS_PAlmanac alm)
5667 {
5668 UC* p;
5669
5670 p = data;
5671 *p = alm->svid;
5672 GPS_A500_Encode(p+1,alm);
5673 p[43] = alm->hlth;
5674 }
5675
5676
5677
5678 /* @funcstatic GPS_A500_Encode ***************************************
5679 **
5680 ** Encode almanac structure to almanac packet
5681 **
5682 ** @param [w] s [UC *] string to write to
5683 ** @param [r] alm [GPS_PAlmanac] almanac entry
5684 **
5685 ** @return [void]
5686 ************************************************************************/
GPS_A500_Encode(UC * s,GPS_PAlmanac alm)5687 static void GPS_A500_Encode(UC* s, GPS_PAlmanac alm)
5688 {
5689 UC* p;
5690
5691 p=s;
5692
5693 GPS_Util_Put_Short(p,alm->wn);
5694 p+=sizeof(int16);
5695
5696 GPS_Util_Put_Float(p,alm->toa);
5697 p+=sizeof(float);
5698
5699 GPS_Util_Put_Float(p,alm->af0);
5700 p+=sizeof(float);
5701
5702 GPS_Util_Put_Float(p,alm->af1);
5703 p+=sizeof(float);
5704
5705 GPS_Util_Put_Float(p,alm->e);
5706 p+=sizeof(float);
5707
5708 GPS_Util_Put_Float(p,alm->sqrta);
5709 p+=sizeof(float);
5710
5711 GPS_Util_Put_Float(p,alm->m0);
5712 p+=sizeof(float);
5713
5714 GPS_Util_Put_Float(p,alm->w);
5715 p+=sizeof(float);
5716
5717 GPS_Util_Put_Float(p,alm->omg0);
5718 p+=sizeof(float);
5719
5720 GPS_Util_Put_Float(p,alm->odot);
5721 p+=sizeof(float);
5722
5723 GPS_Util_Put_Float(p,alm->i);
5724 }
5725
5726
5727 /* @func GPS_A600_Get ******************************************************
5728 **
5729 ** Get time from GPS
5730 **
5731 ** @param [r] port [const char *] serial port
5732 **
5733 ** @return [time_t] GPS time as unix system time, -ve if error
5734 ************************************************************************/
GPS_A600_Get(const char * port)5735 time_t GPS_A600_Get(const char* port)
5736 {
5737 static UC data[2];
5738 gpsdevh* fd;
5739 GPS_PPacket tra;
5740 GPS_PPacket rec;
5741 time_t ret;
5742
5743 if (!GPS_Device_On(port, &fd)) {
5744 return gps_errno;
5745 }
5746
5747 GPS_Util_Put_Short(data,
5748 COMMAND_ID[gps_device_command].Cmnd_Transfer_Time);
5749 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
5750 data,2);
5751 if (!GPS_Write_Packet(fd,tra)) {
5752 return gps_errno;
5753 }
5754 if (!GPS_Get_Ack(fd, &tra, &rec)) {
5755 return gps_errno;
5756 }
5757
5758 if (!GPS_Packet_Read(fd, &rec)) {
5759 return gps_errno;
5760 }
5761 if (!GPS_Send_Ack(fd, &tra, &rec)) {
5762 return gps_errno;
5763 }
5764
5765 switch (gps_date_time_type) {
5766 case pD600:
5767 ret = GPS_D600_Get(rec);
5768 break;
5769 default:
5770 GPS_Error("A600_Get: Unknown data/time protocol");
5771 return PROTOCOL_ERROR;
5772 }
5773
5774 if (!GPS_Device_Off(fd)) {
5775 return gps_errno;
5776 }
5777
5778 return ret;
5779 }
5780
5781
5782
5783
5784
5785 /* @func GPS_A600_Send **************************************************
5786 **
5787 ** Send time to GPS
5788 **
5789 ** @param [r] port [const char *] serial port
5790 ** @param [r] Time [time_t] unix-style time
5791 **
5792 ** @return [int32] success
5793 ************************************************************************/
GPS_A600_Send(const char * port,time_t Time)5794 int32 GPS_A600_Send(const char* port, time_t Time)
5795 {
5796 gpsdevh* fd;
5797 GPS_PPacket tra;
5798 GPS_PPacket rec;
5799 int32 posnsent=0;
5800 int32 ret=0;
5801
5802 if (!GPS_Device_On(port, &fd)) {
5803 return gps_errno;
5804 }
5805
5806 switch (gps_date_time_type) {
5807 case pD600:
5808 GPS_D600_Send(tra,Time);
5809 break;
5810 default:
5811 GPS_Error("A600_Send: Unknown data/time protocol");
5812 return PROTOCOL_ERROR;
5813 }
5814
5815 if (!GPS_Write_Packet(fd,tra)) {
5816 return gps_error;
5817 }
5818 if (!GPS_Get_Ack(fd, &tra, &rec)) {
5819 return gps_error;
5820 }
5821
5822
5823 /*
5824 * Allow GPS a little while to decide whether it wants to ask for
5825 * the position. Note that the posn sent is held in gps_save_lat
5826 * and gps_save_lon globals!
5827 */
5828 if (GPS_Device_Wait(fd)) {
5829 if (!GPS_Packet_Read(fd, &rec)) {
5830 return gps_errno;
5831 }
5832
5833 if (!GPS_Send_Ack(fd, &tra, &rec)) {
5834 return gps_errno;
5835 }
5836
5837 if (rec.type == LINK_ID[gps_link_type].Pid_Command_Data &&
5838 GPS_Util_Get_Short(rec.data) == COMMAND_ID[gps_device_command].
5839 Cmnd_Transfer_Posn) {
5840 GPS_User("INFO: GPS position request. Sending....");
5841 ret = GPS_Rqst_Send_Position(fd,gps_save_lat,gps_save_lon);
5842 if (ret < 0) {
5843 return ret;
5844 }
5845 posnsent=1;
5846 }
5847 }
5848
5849
5850 if (!posnsent) {
5851 ret = GPS_Rqst_Send_Position(fd,gps_save_lat,gps_save_lon);
5852 if (ret < 0) {
5853 return ret;
5854 }
5855 }
5856
5857
5858 if (!GPS_Device_Off(fd)) {
5859 return gps_errno;
5860 }
5861
5862 return 1;
5863 }
5864
5865
5866
5867
5868
5869 /* @func GPS_D600_Get ******************************************************
5870 **
5871 ** Convert date/time packet to ints
5872 **
5873 ** @param [r] packet [GPS_PPacket] packet
5874 **
5875 ** @return [time_t] gps time as unix system time
5876 ************************************************************************/
GPS_D600_Get(GPS_PPacket & packet)5877 time_t GPS_D600_Get(GPS_PPacket& packet)
5878 {
5879 UC* p;
5880 static struct tm ts;
5881
5882 p = packet.data;
5883
5884 ts.tm_mon = *p++ - 1;
5885 ts.tm_mday = *p++;
5886 ts.tm_year = (int32) GPS_Util_Get_Short(p) - 1900;
5887 p+=2;
5888 ts.tm_hour = (int32) GPS_Util_Get_Short(p);
5889 p+=2;
5890 ts.tm_min = *p++;
5891 ts.tm_sec = *p++;
5892
5893 return mktime(&ts);
5894 }
5895
5896
5897 /* @func GPS_D600_Send ******************************************************
5898 **
5899 ** make a time packet for sending to the GPS
5900 **
5901 ** @param [w] packet [GPS_PPacket *] packet
5902 ** @param [r] Time [time_t] unix-style time
5903 **
5904 ** @return [void]
5905 ************************************************************************/
GPS_D600_Send(GPS_PPacket & packet,time_t Time)5906 void GPS_D600_Send(GPS_PPacket& packet, time_t Time)
5907 {
5908 UC data[10];
5909 UC* p;
5910 struct tm* ts;
5911
5912 p = data;
5913
5914 ts = localtime(&Time);
5915 *p++ = ts->tm_mon+1;
5916 *p++ = ts->tm_mday;
5917
5918 GPS_Util_Put_Short(p,(US)(ts->tm_year+1900));
5919 p+=2;
5920 GPS_Util_Put_Short(p,(US) ts->tm_hour);
5921 p+=2;
5922
5923 *p++ = ts->tm_min;
5924 *p = ts->tm_sec;
5925
5926 GPS_Make_Packet(&packet, LINK_ID[gps_link_type].Pid_Date_Time_Data,
5927 data,8);
5928 }
5929
5930
5931
5932
5933 /* @func GPS_A700_Get ******************************************************
5934 **
5935 ** Get position from GPS
5936 **
5937 ** @param [r] port [const char *] serial port
5938 ** @param [w] lat [double *] latitude (deg)
5939 ** @param [w] lon [double *] longitude (deg)
5940 **
5941 ** @return [int32] success
5942 ************************************************************************/
GPS_A700_Get(const char * port,double * lat,double * lon)5943 int32 GPS_A700_Get(const char* port, double* lat, double* lon)
5944 {
5945 static UC data[2];
5946 gpsdevh* fd;
5947 GPS_PPacket tra;
5948 GPS_PPacket rec;
5949
5950 if (!GPS_Device_On(port, &fd)) {
5951 return gps_errno;
5952 }
5953
5954
5955 GPS_Util_Put_Short(data,
5956 COMMAND_ID[gps_device_command].Cmnd_Transfer_Posn);
5957 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
5958 data,2);
5959 if (!GPS_Write_Packet(fd,tra)) {
5960 return gps_errno;
5961 }
5962 if (!GPS_Get_Ack(fd, &tra, &rec)) {
5963 return gps_errno;
5964 }
5965
5966 if (!GPS_Packet_Read(fd, &rec)) {
5967 return gps_errno;
5968 }
5969 if (!GPS_Send_Ack(fd, &tra, &rec)) {
5970 return gps_errno;
5971 }
5972
5973 switch (gps_position_type) {
5974 case pD700:
5975 GPS_D700_Get(rec, lat, lon);
5976 break;
5977 default:
5978 GPS_Error("A700_Get: Unknown position protocol");
5979 return PROTOCOL_ERROR;
5980 }
5981
5982 if (!GPS_Device_Off(fd)) {
5983 return gps_errno;
5984 }
5985
5986 return 1;
5987 }
5988
5989
5990
5991 /* @func GPS_A700_Send ******************************************************
5992 **
5993 ** Send position to GPS
5994 **
5995 ** @param [r] port [const char *] serial port
5996 ** @param [r] lat [double] latitude (deg)
5997 ** @param [r] lon [double] longitude (deg)
5998 **
5999 ** @return [int32] success
6000 ************************************************************************/
GPS_A700_Send(const char * port,double lat,double lon)6001 int32 GPS_A700_Send(const char* port, double lat, double lon)
6002 {
6003 gpsdevh* fd;
6004 GPS_PPacket tra;
6005 GPS_PPacket rec;
6006
6007 if (!GPS_Device_On(port, &fd)) {
6008 return gps_errno;
6009 }
6010
6011
6012 switch (gps_position_type) {
6013 case pD700:
6014 GPS_D700_Send(tra,lat,lon);
6015 break;
6016 default:
6017 GPS_Error("A700_Send: Unknown position protocol");
6018 return PROTOCOL_ERROR;
6019 }
6020
6021 if (!GPS_Write_Packet(fd,tra)) {
6022 return 0;
6023 }
6024 if (!GPS_Get_Ack(fd, &tra, &rec)) {
6025 return 0;
6026 }
6027
6028
6029 if (!GPS_Device_Off(fd)) {
6030 return gps_errno;
6031 }
6032
6033 return 1;
6034 }
6035
6036
6037
6038 /* @func GPS_D700_Get ******************************************************
6039 **
6040 ** Convert position packet to lat/long in degrees
6041 **
6042 ** @param [r] packet [GPS_PPacket] packet
6043 ** @param [w] lat [double *] latitude (deg)
6044 ** @param [w] lon [double *] longitude (deg)
6045 **
6046 ** @return [void]
6047 ************************************************************************/
GPS_D700_Get(GPS_PPacket & packet,double * lat,double * lon)6048 void GPS_D700_Get(GPS_PPacket& packet, double* lat, double* lon)
6049 {
6050 UC* p;
6051 double t;
6052
6053 p = packet.data;
6054
6055 t = GPS_Util_Get_Double(p);
6056 *lat = GPS_Math_Rad_To_Deg(t);
6057
6058 p += sizeof(double);
6059
6060 t = GPS_Util_Get_Double(p);
6061 *lon = GPS_Math_Rad_To_Deg(t);
6062 }
6063
6064
6065 /* @func GPS_D700_Send ******************************************************
6066 **
6067 ** make a position packet for sending to the GPS
6068 **
6069 ** @param [w] packet [GPS_PPacket *] packet
6070 ** @param [r] lat [double] latitude (deg)
6071 ** @param [r] lon [double] longitude (deg)
6072 **
6073 ** @return [void]
6074 ************************************************************************/
GPS_D700_Send(GPS_PPacket & packet,double lat,double lon)6075 void GPS_D700_Send(GPS_PPacket& packet, double lat, double lon)
6076 {
6077 UC data[16];
6078 UC* p;
6079
6080 lat = GPS_Math_Deg_To_Rad(lat);
6081 lon = GPS_Math_Deg_To_Rad(lon);
6082
6083 p = data;
6084
6085 GPS_Util_Put_Double(p,lat);
6086 p+=sizeof(double);
6087 GPS_Util_Put_Double(p,lon);
6088
6089 GPS_Make_Packet(&packet, LINK_ID[gps_link_type].Pid_Position_Data,
6090 data,16);
6091 }
6092
6093
6094
6095 /* @func GPS_A800_On ******************************************************
6096 **
6097 ** Turn on GPS PVT
6098 **
6099 ** @param [r] port [const char *] serial port
6100 ** @param [w] fd [int32 *] file descriptor
6101 **
6102 ** @return [int32] success
6103 ************************************************************************/
GPS_A800_On(const char * port,gpsdevh ** fd)6104 int32 GPS_A800_On(const char* port, gpsdevh** fd)
6105 {
6106 static UC data[2];
6107 GPS_PPacket tra;
6108 GPS_PPacket rec;
6109
6110 if (!GPS_Device_On(port, fd)) {
6111 return gps_errno;
6112 }
6113
6114
6115 GPS_Util_Put_Short(data,
6116 COMMAND_ID[gps_device_command].Cmnd_Start_Pvt_Data);
6117 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
6118 data,2);
6119 if (!GPS_Write_Packet(*fd,tra)) {
6120 return gps_errno;
6121 }
6122 if (!GPS_Get_Ack(*fd, &tra, &rec)) {
6123 GPS_Error("A800_on: Pvt start data not acknowledged");
6124 return FRAMING_ERROR;
6125 }
6126
6127 return 1;
6128 }
6129
6130
6131
6132 /* @func GPS_A800_Off ******************************************************
6133 **
6134 ** Turn off GPS PVT
6135 **
6136 ** @param [r] port [const char *] port
6137 ** @param [w] fd [int32 *] file descriptor
6138 **
6139 ** @return [int32] success
6140 ************************************************************************/
GPS_A800_Off(const char *,gpsdevh ** fd)6141 int32 GPS_A800_Off(const char*, gpsdevh** fd)
6142 {
6143 static UC data[2];
6144 GPS_PPacket tra;
6145 GPS_PPacket rec;
6146
6147
6148 GPS_Util_Put_Short(data,
6149 COMMAND_ID[gps_device_command].Cmnd_Stop_Pvt_Data);
6150 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Command_Data,
6151 data,2);
6152 if (!GPS_Write_Packet(*fd,tra)) {
6153 return gps_errno;
6154 }
6155 if (!GPS_Get_Ack(*fd, &tra, &rec)) {
6156 GPS_Error("A800_Off: Not acknowledged");
6157 return FRAMING_ERROR;
6158 }
6159
6160
6161 // if(!GPS_Device_Off(*fd))
6162 // return gps_errno;
6163
6164 return 1;
6165 }
6166
6167
6168 /* @func GPS_A800_Get **************************************************
6169 **
6170 ** make a position packet for sending to the GPS
6171 **
6172 ** @param [r] fd [int32 *] file descriptor
6173 ** @param [w] packet [GPS_PPvt_Data *] packet
6174 **
6175 ** @return [int32] success
6176 ************************************************************************/
GPS_A800_Get(gpsdevh ** fd,GPS_PPvt_Data * packet)6177 int32 GPS_A800_Get(gpsdevh** fd, GPS_PPvt_Data* packet)
6178 {
6179 GPS_PPacket tra;
6180 GPS_PPacket rec;
6181
6182
6183 if (!GPS_Packet_Read(*fd, &rec)) {
6184 return gps_errno;
6185 }
6186
6187 if (!GPS_Send_Ack(*fd, &tra, &rec)) {
6188 return gps_errno;
6189 }
6190
6191 if (rec.type != LINK_ID[gps_link_type].Pid_Pvt_Data) {
6192 return 0;
6193 }
6194
6195 switch (gps_pvt_type) {
6196 case pD800:
6197 GPS_D800_Get(rec,packet);
6198 break;
6199 default:
6200 GPS_Error("A800_GET: Unknown pvt protocol");
6201 return PROTOCOL_ERROR;
6202 }
6203
6204 return 1;
6205 }
6206
6207
6208
6209 /* @func GPS_D800_Get ******************************************************
6210 **
6211 ** Convert packet to pvt structure
6212 **
6213 ** @param [r] packet [GPS_PPacket] packet
6214 ** @param [w] pvt [GPS_PPvt_Data *] pvt structure
6215 **
6216 ** @return [void]
6217 ************************************************************************/
GPS_D800_Get(GPS_PPacket & packet,GPS_PPvt_Data * pvt)6218 void GPS_D800_Get(GPS_PPacket& packet, GPS_PPvt_Data* pvt)
6219 {
6220 UC* p;
6221
6222 p = packet.data;
6223
6224 (*pvt)->alt = GPS_Util_Get_Float(p);
6225 p+=sizeof(float);
6226
6227 (*pvt)->epe = GPS_Util_Get_Float(p);
6228 p+=sizeof(float);
6229
6230 (*pvt)->eph = GPS_Util_Get_Float(p);
6231 p+=sizeof(float);
6232
6233 (*pvt)->epv = GPS_Util_Get_Float(p);
6234 p+=sizeof(float);
6235
6236 (*pvt)->fix = GPS_Util_Get_Short(p);
6237 p+=sizeof(int16);
6238
6239 (*pvt)->tow = GPS_Util_Get_Double(p);
6240 p+=sizeof(double);
6241
6242 (*pvt)->lat = GPS_Math_Rad_To_Deg(GPS_Util_Get_Double(p));
6243 p+=sizeof(double);
6244
6245 (*pvt)->lon = GPS_Math_Rad_To_Deg(GPS_Util_Get_Double(p));
6246 p+=sizeof(double);
6247
6248 (*pvt)->east = GPS_Util_Get_Float(p);
6249 p+=sizeof(float);
6250
6251 (*pvt)->north = GPS_Util_Get_Float(p);
6252 p+=sizeof(float);
6253
6254 (*pvt)->up = GPS_Util_Get_Float(p);
6255 p+=sizeof(float);
6256
6257 (*pvt)->msl_hght = GPS_Util_Get_Float(p);
6258 p+=sizeof(float);
6259
6260 (*pvt)->leap_scnds = GPS_Util_Get_Short(p);
6261 p+=sizeof(int16);
6262
6263 (*pvt)->wn_days = GPS_Util_Get_Int(p);
6264 }
6265
6266 /* @func GPS_A906_Get ******************************************************
6267 **
6268 ** Get lap data from GPS
6269 **
6270 ** @param [r] port [const char *] serial port
6271 ** @param [w] trk [GPS_PLap **] lap array
6272 **
6273 ** @return [int32] number of lap entries
6274 ************************************************************************/
6275
GPS_A906_Get(const char * port,GPS_PLap ** lap,pcb_fn cb)6276 int32 GPS_A906_Get(const char* port, GPS_PLap** lap, pcb_fn cb)
6277 {
6278 static UC data[2];
6279 gpsdevh* fd;
6280 GPS_PPacket trapkt;
6281 GPS_PPacket recpkt;
6282 int32 i, n;
6283
6284 if (gps_lap_transfer == -1) {
6285 return GPS_UNSUPPORTED;
6286 }
6287
6288 if (!GPS_Device_On(port, &fd)) {
6289 return gps_errno;
6290 }
6291
6292 GPS_Util_Put_Short(data,
6293 COMMAND_ID[gps_device_command].Cmnd_Transfer_Laps);
6294 GPS_Make_Packet(&trapkt, LINK_ID[gps_link_type].Pid_Command_Data,
6295 data,2);
6296 if (!GPS_Write_Packet(fd,trapkt)) {
6297 return gps_errno;
6298 }
6299 if (!GPS_Get_Ack(fd, &trapkt, &recpkt)) {
6300 return gps_errno;
6301 }
6302 if (!GPS_Packet_Read(fd, &recpkt)) {
6303 return gps_errno;
6304 }
6305 if (!GPS_Send_Ack(fd, &trapkt, &recpkt)) {
6306 return gps_errno;
6307 }
6308
6309 n = GPS_Util_Get_Short(recpkt.data);
6310
6311 if (n)
6312 if (!((*lap)=(GPS_PLap*)malloc(n*sizeof(GPS_PLap)))) {
6313 GPS_Error("A906_Get: Insufficient memory");
6314 return MEMORY_ERROR;
6315 }
6316
6317 for (i=0; i<n; ++i) {
6318 if (!((*lap)[i]=GPS_Lap_New())) {
6319 return MEMORY_ERROR;
6320 }
6321 if (!GPS_Packet_Read(fd, &recpkt)) {
6322 return gps_errno;
6323 }
6324
6325 if (!GPS_Send_Ack(fd, &trapkt, &recpkt)) {
6326 return gps_errno;
6327 }
6328
6329 switch (gps_lap_type) {
6330 case pD906:
6331 case pD1001:
6332 case pD1011:
6333 case pD1015:
6334 GPS_D1011b_Get(&((*lap)[i]),recpkt.data);
6335 break;
6336 default:
6337 GPS_Error("A906_Get: Unknown Lap protocol %d\n", gps_lap_type);
6338 return PROTOCOL_ERROR;
6339 }
6340
6341 /* Cheat and don't _really_ pass the trkpt back */
6342 cb(n, nullptr);
6343 }
6344
6345 if (!GPS_Packet_Read(fd, &recpkt)) {
6346 return gps_errno;
6347 }
6348 if (!GPS_Send_Ack(fd, &trapkt, &recpkt)) {
6349 return gps_errno;
6350 }
6351 if (recpkt.type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) {
6352 GPS_Error("A906_Get: Error transferring laps");
6353 return FRAMING_ERROR;
6354 }
6355
6356 if (i != n) {
6357 GPS_Error("A906_GET: Lap entry number mismatch");
6358 return FRAMING_ERROR;
6359 }
6360
6361 if (!GPS_Device_Off(fd)) {
6362 return gps_errno;
6363 }
6364 return n;
6365 }
6366
6367 /* @func GPS_D1011b_Get ******************************************************
6368 **
6369 ** Convert packet D906, D1001, D1011, D1015 to lap structure
6370 **
6371 ** @param [r] packet [GPS_PPacket] packet
6372 ** @param [w] pvt [GPS_PLap *] lap structure
6373 **
6374 ** @return [void]
6375 ************************************************************************/
GPS_D1011b_Get(GPS_PLap * Lap,UC * p)6376 void GPS_D1011b_Get(GPS_PLap* Lap, UC* p)
6377 {
6378 uint32 t;
6379
6380 /* Lap index (not in D906) */
6381 switch (gps_lap_type) {
6382 case pD906:
6383 (*Lap)->index = -1;
6384 break;
6385 case pD1001:
6386 (*Lap)->index = GPS_Util_Get_Uint(p);
6387 p+=sizeof(uint32);
6388 break;
6389 case pD1011:
6390 case pD1015:
6391 (*Lap)->index = GPS_Util_Get_Short(p);
6392 p+=sizeof(uint16);
6393 p+=sizeof(uint16); /*unused*/
6394 break;
6395 default:
6396 break;
6397 }
6398
6399 t = GPS_Util_Get_Uint(p);
6400 (*Lap)->start_time = GPS_Math_Gtime_To_Utime((time_t)t);
6401 p+=sizeof(uint32);
6402
6403 (*Lap)->total_time = GPS_Util_Get_Int(p);
6404 p+=sizeof(int32);
6405
6406 (*Lap)->total_distance = GPS_Util_Get_Float(p);
6407 p+=sizeof(float);
6408 if (gps_lap_type != pD906) {
6409 (*Lap)->max_speed = GPS_Util_Get_Float(p);
6410 p+=sizeof(float);
6411 }
6412
6413 (*Lap)->begin_lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
6414 p+=sizeof(int32);
6415 (*Lap)->begin_lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
6416 p+=sizeof(int32);
6417 (*Lap)->end_lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
6418 p+=sizeof(int32);
6419 (*Lap)->end_lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
6420 p+=sizeof(int32);
6421
6422 (*Lap)->calories = GPS_Util_Get_Short(p);
6423 p+=sizeof(int16);
6424
6425 /* Track index, only in D906*/
6426 if (gps_lap_type == pD906) {
6427 (*Lap)->track_index = *p++;
6428 p++; /*Unused*/
6429
6430 /*Last field, no more to do */
6431 return;
6432 } else {
6433 (*Lap)->track_index = -1;
6434 }
6435
6436 (*Lap)->avg_heart_rate = *p++;
6437 (*Lap)->max_heart_rate = *p++;
6438 (*Lap)->intensity = *p++;
6439
6440 switch (gps_lap_type) {
6441 case pD1001:
6442 /*No more fields */
6443 return;
6444 case pD1011:
6445 case pD1015:
6446 if (*p != 0xff) {
6447 (*Lap)->avg_cadence = *p;
6448 }
6449 p++;
6450 (*Lap)->trigger_method = *p++;
6451 break;
6452 default:
6453 /*pD906 already returned*/
6454 break;
6455 }
6456
6457 if (gps_lap_type==pD1015) {
6458 /*some unknown fields like 04 dc 44 ff ff */
6459 /* (*Lap)->unk1015_1 = *p++; normally 4?
6460 (*Lap)->unk1015_2 = GPS_Util_Get_Short(p);wkt related , ffff otherwise
6461 p+=sizeof(int16);
6462 (*Lap)->unk1015_3 = GPS_Util_Get_Short(p);ffff ?
6463 p+=sizeof(int16);
6464 */
6465 }
6466
6467 }
6468
6469
6470 /* @func GPS_A1006_Get **********************************************
6471 **
6472 ** Get Courses from GPS. According to Garmin protocol specification, this
6473 ** includes getting all course laps, course tracks and course points
6474 ** from the device.
6475 **
6476 ** @param [r] port [const char *] serial port
6477 ** @param [w] crs [GPS_PCourse **] pointer to course array
6478 ** @param [w] lap [GPS_PCourse_Lap **] pointer to course lap array
6479 ** @param [w] trk [GPS_PTrack **] pointer to track array
6480 ** @param [w] lap [GPS_PCourse_Point **] pointer to course point array
6481 ** @param [w] n_lap [int32 **] pointer to number of lap entries
6482 ** @param [w] n_trk [int32 **] pointer to number of track entries
6483 ** @param [w] n_cpt [int32 **] pointer to number of course point entries
6484 **
6485 ** @return [int32] number of course entries
6486 ************************************************************************/
6487
GPS_A1006_Get(const char * port,GPS_PCourse ** crs,pcb_fn cb)6488 int32 GPS_A1006_Get
6489 (const char* port,
6490 GPS_PCourse** crs,
6491 pcb_fn cb)
6492
6493 {
6494 static UC data[2];
6495 gpsdevh* fd;
6496 GPS_PPacket trapkt;
6497 GPS_PPacket recpkt;
6498 int32 i, n;
6499
6500 if (gps_course_transfer == -1) {
6501 return GPS_UNSUPPORTED;
6502 }
6503
6504 if (!GPS_Device_On(port, &fd)) {
6505 return gps_errno;
6506 }
6507
6508 GPS_Util_Put_Short(data,
6509 COMMAND_ID[gps_device_command].Cmnd_Transfer_Courses);
6510 GPS_Make_Packet(&trapkt, LINK_ID[gps_link_type].Pid_Command_Data,
6511 data,2);
6512 if (!GPS_Write_Packet(fd,trapkt)) {
6513 return gps_errno;
6514 }
6515 if (!GPS_Get_Ack(fd, &trapkt, &recpkt)) {
6516 return gps_errno;
6517 }
6518 if (!GPS_Packet_Read(fd, &recpkt)) {
6519 return gps_errno;
6520 }
6521 if (!GPS_Send_Ack(fd, &trapkt, &recpkt)) {
6522 return gps_errno;
6523 }
6524
6525 n = GPS_Util_Get_Short(recpkt.data);
6526
6527
6528 if (n)
6529 if (!((*crs)=(GPS_PCourse*)malloc(n*sizeof(GPS_PCourse)))) {
6530 GPS_Error("A1006_Get: Insufficient memory");
6531 return MEMORY_ERROR;
6532 }
6533
6534 for (i=0; i<n; ++i) {
6535 if (!((*crs)[i]=GPS_Course_New())) {
6536 return MEMORY_ERROR;
6537 }
6538 if (!GPS_Packet_Read(fd, &recpkt)) {
6539 return gps_errno;
6540 }
6541
6542 if (!GPS_Send_Ack(fd, &trapkt, &recpkt)) {
6543 return gps_errno;
6544 }
6545
6546 switch (gps_course_type) {
6547 case pD1006:
6548 GPS_D1006_Get(&((*crs)[i]),recpkt.data);
6549 break;
6550 default:
6551 GPS_Error("A1006_Get: Unknown Course protocol %d\n",
6552 gps_course_type);
6553 return PROTOCOL_ERROR;
6554 }
6555
6556 // Cheat and don't _really_ pass the crs back
6557 if (cb) {
6558 cb(n, nullptr);
6559 }
6560 }
6561
6562 if (!GPS_Packet_Read(fd, &recpkt)) {
6563 return gps_errno;
6564 }
6565 if (!GPS_Send_Ack(fd, &trapkt, &recpkt)) {
6566 return gps_errno;
6567 }
6568 if (recpkt.type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) {
6569 GPS_Error("A1006_Get: Error transferring courses");
6570 return FRAMING_ERROR;
6571 }
6572
6573 if (i != n) {
6574 GPS_Error("A1006_GET: Course entry number mismatch");
6575 return FRAMING_ERROR;
6576 }
6577
6578 if (!GPS_Device_Off(fd)) {
6579 return gps_errno;
6580 }
6581 return n;
6582 }
6583
6584
6585 /* @func GPS_A1006_Send **************************************************
6586 ** Send Courses to GPS.
6587 **
6588 ** Note that different to other GPS_Axxx_Send functions, the device
6589 ** communication is not initialized/ended within the function, since
6590 ** this packet transfer is only part of a series of transfers to the
6591 ** device. Communication init/end has to be handled by the caller.
6592 **
6593 ** @param [r] port [const char *] serial port
6594 ** @param [r] crs [GPS_PCourse *] pointer to Course array
6595 ** @param [r] n_wkt [int32] number of Course entries
6596 ** @param [r] fd [gpsdevh *] pointer to the communication port
6597 **
6598 ** @return [int32] success
6599 ************************************************************************/
GPS_A1006_Send(const char *,GPS_PCourse * crs,int32 n_crs,gpsdevh * fd)6600 int32 GPS_A1006_Send(const char*,
6601 GPS_PCourse* crs,
6602 int32 n_crs,
6603 gpsdevh* fd)
6604 {
6605 UC data[GPS_ARB_LEN];
6606 GPS_PPacket tra;
6607 GPS_PPacket rec;
6608 int32 i;
6609 int32 len;
6610
6611 GPS_Util_Put_Short(data,(US) n_crs);
6612 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records,
6613 data,2);
6614 if (!GPS_Write_Packet(fd,tra)) {
6615 return gps_errno;
6616 }
6617 if (!GPS_Get_Ack(fd, &tra, &rec)) {
6618 GPS_Error("A1006_Send: Course start data not acknowledged");
6619 return FRAMING_ERROR;
6620 }
6621
6622 for (i=0; i<n_crs; ++i) {
6623 switch (gps_course_type) {
6624 case pD1006:
6625 GPS_D1006_Send(data,crs[i],&len);
6626 break;
6627 default:
6628 GPS_Error("A1006_Send: Unknown course type %d\n",
6629 gps_course_type);
6630 return PROTOCOL_ERROR;
6631 }
6632
6633 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Course,
6634 data,(US) len);
6635
6636 if (!GPS_Write_Packet(fd,tra)) {
6637 return gps_errno;
6638 }
6639
6640 if (!GPS_Get_Ack(fd, &tra, &rec)) {
6641 GPS_Error("A1006_Send: Pid_Course not acknowledged");
6642 return gps_errno;
6643 }
6644 }
6645
6646 GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Courses);
6647 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt,
6648 data,2);
6649 if (!GPS_Write_Packet(fd,tra)) {
6650 return gps_errno;
6651 }
6652 if (!GPS_Get_Ack(fd, &tra, &rec)) {
6653 GPS_Error("A1006_Send: Course complete data not acknowledged");
6654 return FRAMING_ERROR;
6655 }
6656
6657 return 1;
6658 }
6659
6660
6661 /* @func GPS_D1006_Get ******************************************************
6662 **
6663 ** Convert packet D1006 to course structure
6664 **
6665 ** @param [w] crs [GPS_PCourse *] Course
6666 ** @param [r] p [UC *] packet data
6667 **
6668 ** @return [void]
6669 ************************************************************************/
GPS_D1006_Get(GPS_PCourse * crs,UC * p)6670 void GPS_D1006_Get(GPS_PCourse* crs, UC* p)
6671 {
6672 int i;
6673 (*crs)->index = GPS_Util_Get_Short(p);
6674 p+=sizeof(uint16);
6675 p+=sizeof(uint16); // unused
6676 for (i=0; i<16; ++i) {
6677 (*crs)->course_name[i] = *p++;
6678 }
6679 (*crs)->track_index = GPS_Util_Get_Short(p);
6680 p+=sizeof(uint16);
6681 }
6682
6683
6684 /* @funcstatic GPS_D1006_Send *******************************************
6685 **
6686 ** Form course data string
6687 **
6688 ** @param [w] data [UC *] string to write to
6689 ** @param [r] crs [GPS_PCourse] course data
6690 ** @param [w] len [int32 *] packet length
6691 **
6692 ** @return [void]
6693 ************************************************************************/
GPS_D1006_Send(UC * data,GPS_PCourse crs,int32 * len)6694 void GPS_D1006_Send(UC* data, GPS_PCourse crs, int32* len)
6695 {
6696 UC* p;
6697 int j;
6698 p = data;
6699
6700 GPS_Util_Put_Short(p, (US) crs->index);
6701 p += 2;
6702
6703 GPS_Util_Put_Uint(p,0);
6704 p+=sizeof(uint16);
6705
6706 for (j=0; j<16; ++j) {
6707 *p++ = crs->course_name[j];
6708 }
6709
6710 GPS_Util_Put_Short(p, (US) crs->track_index);
6711 p += 2;
6712
6713 *len = p-data;
6714 }
6715
6716
6717 /* @func GPS_A1007_Get ******************************************************
6718 **
6719 ** Get course lap data from GPS
6720 **
6721 ** @param [r] port [const char *] serial port
6722 ** @param [w] clp [GPS_PCourse_Lap **] course lap array
6723 **
6724 ** @return [int32] number of lap entries
6725 ************************************************************************/
6726
GPS_A1007_Get(const char * port,GPS_PCourse_Lap ** clp,pcb_fn cb)6727 int32 GPS_A1007_Get(const char* port, GPS_PCourse_Lap** clp, pcb_fn cb)
6728 {
6729 static UC data[2];
6730 gpsdevh* fd;
6731 GPS_PPacket trapkt;
6732 GPS_PPacket recpkt;
6733 int32 i, n;
6734
6735 if (gps_course_lap_transfer == -1) {
6736 return GPS_UNSUPPORTED;
6737 }
6738
6739 if (!GPS_Device_On(port, &fd)) {
6740 return gps_errno;
6741 }
6742
6743 GPS_Util_Put_Short(data,
6744 COMMAND_ID[gps_device_command].Cmnd_Transfer_Course_Laps);
6745 GPS_Make_Packet(&trapkt, LINK_ID[gps_link_type].Pid_Command_Data,
6746 data,2);
6747 if (!GPS_Write_Packet(fd,trapkt)) {
6748 return gps_errno;
6749 }
6750 if (!GPS_Get_Ack(fd, &trapkt, &recpkt)) {
6751 return gps_errno;
6752 }
6753 if (!GPS_Packet_Read(fd, &recpkt)) {
6754 return gps_errno;
6755 }
6756 if (!GPS_Send_Ack(fd, &trapkt, &recpkt)) {
6757 return gps_errno;
6758 }
6759
6760 n = GPS_Util_Get_Short(recpkt.data);
6761
6762
6763 if (n)
6764 if (!((*clp)=(GPS_PCourse_Lap*)malloc(n*sizeof(GPS_PCourse_Lap)))) {
6765 GPS_Error("A1007_Get: Insufficient memory");
6766 return MEMORY_ERROR;
6767 }
6768
6769 for (i=0; i<n; ++i) {
6770 if (!((*clp)[i]=GPS_Course_Lap_New())) {
6771 return MEMORY_ERROR;
6772 }
6773 if (!GPS_Packet_Read(fd, &recpkt)) {
6774 return gps_errno;
6775 }
6776
6777 if (!GPS_Send_Ack(fd, &trapkt, &recpkt)) {
6778 return gps_errno;
6779 }
6780
6781 switch (gps_course_lap_type) {
6782 case pD1007:
6783 GPS_D1007_Get(&((*clp)[i]),recpkt.data);
6784 break;
6785 default:
6786 GPS_Error("A1007_Get: Unknown Course Lap protocol %d\n",
6787 gps_course_lap_type);
6788 return PROTOCOL_ERROR;
6789 }
6790
6791 /* Cheat and don't _really_ pass the trkpt back */
6792 if (cb) {
6793 cb(n, nullptr);
6794 }
6795 }
6796
6797 if (!GPS_Packet_Read(fd, &recpkt)) {
6798 return gps_errno;
6799 }
6800 if (!GPS_Send_Ack(fd, &trapkt, &recpkt)) {
6801 return gps_errno;
6802 }
6803 if (recpkt.type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) {
6804 GPS_Error("A1007_Get: Error transferring course laps");
6805 return FRAMING_ERROR;
6806 }
6807
6808 if (i != n) {
6809 GPS_Error("A1007_GET: Course Lap entry number mismatch");
6810 return FRAMING_ERROR;
6811 }
6812
6813 if (!GPS_Device_Off(fd)) {
6814 return gps_errno;
6815 }
6816 return n;
6817 }
6818
6819
6820 /* @func GPS_A1007_Send **************************************************
6821 ** Send Course Lap to GPS.
6822 **
6823 ** Note that different to other GPS_Axxx_Send functions, the device
6824 ** communication is not initialized/ended within the function, since
6825 ** this packet transfer is only part of a series of transfers to the
6826 ** device. Communication init/end has to be handled by the caller.
6827 **
6828 ** @param [r] port [const char *] serial port
6829 ** @param [r] clp [GPS_PCourse_Lap *] pointer to CourseLap array
6830 ** @param [r] n_clp [int32] number of CourseLap entries
6831 ** @param [r] fd [gpsdevh *] pointer to the communication port
6832 **
6833 ** @return [int32] success
6834 ************************************************************************/
GPS_A1007_Send(const char *,GPS_PCourse_Lap * clp,int32 n_clp,gpsdevh * fd)6835 int32 GPS_A1007_Send(const char*,
6836 GPS_PCourse_Lap* clp,
6837 int32 n_clp,
6838 gpsdevh* fd)
6839 {
6840 UC data[GPS_ARB_LEN];
6841 GPS_PPacket tra;
6842 GPS_PPacket rec;
6843 int32 i;
6844 int32 len;
6845
6846 GPS_Util_Put_Short(data,(US) n_clp);
6847 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records,
6848 data,2);
6849 if (!GPS_Write_Packet(fd,tra)) {
6850 return gps_errno;
6851 }
6852 if (!GPS_Get_Ack(fd, &tra, &rec)) {
6853 GPS_Error("A1007_Send: CourseLap start data not acknowledged");
6854 return FRAMING_ERROR;
6855 }
6856
6857 for (i=0; i<n_clp; ++i) {
6858 switch (gps_course_lap_type) {
6859 case pD1007:
6860 GPS_D1007_Send(data,clp[i],&len);
6861 break;
6862 default:
6863 GPS_Error("A1007_Send: Unknown course_lap type %d\n",
6864 gps_course_lap_type);
6865 return PROTOCOL_ERROR;
6866 }
6867
6868 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Course_Lap,
6869 data,(US) len);
6870
6871 if (!GPS_Write_Packet(fd,tra)) {
6872 return gps_errno;
6873 }
6874
6875 if (!GPS_Get_Ack(fd, &tra, &rec)) {
6876 GPS_Error("A1007_Send: Pid_Course_Lap not acknowledged");
6877 return gps_errno;
6878 }
6879 }
6880
6881 GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Course_Laps);
6882 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt,
6883 data,2);
6884 if (!GPS_Write_Packet(fd,tra)) {
6885 return gps_errno;
6886 }
6887 if (!GPS_Get_Ack(fd, &tra, &rec)) {
6888 GPS_Error("A1007_Send: CourseLap complete data not acknowledged");
6889 return FRAMING_ERROR;
6890 }
6891
6892 return 1;
6893 }
6894
6895
6896 /* @func GPS_D1007_Get ******************************************************
6897 **
6898 ** Convert packet D1007 to course lap structure
6899 **
6900 ** @param [r] packet [GPS_PPacket] packet
6901 ** @param [w] clp [GPS_PCourse_Lap *] course lap structure
6902 **
6903 ** @return [void]
6904 ************************************************************************/
GPS_D1007_Get(GPS_PCourse_Lap * clp,UC * p)6905 void GPS_D1007_Get(GPS_PCourse_Lap* clp, UC* p)
6906 {
6907 (*clp)->course_index = GPS_Util_Get_Short(p);
6908 p+=sizeof(uint16);
6909
6910 (*clp)->lap_index = GPS_Util_Get_Short(p);
6911 p+=sizeof(uint16);
6912
6913 (*clp)->total_time = GPS_Util_Get_Int(p);
6914 p+=sizeof(uint32);
6915
6916 (*clp)->total_dist = GPS_Util_Get_Float(p);
6917 p+=sizeof(float);
6918
6919 (*clp)->begin_lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
6920 p+=sizeof(int32);
6921 (*clp)->begin_lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
6922 p+=sizeof(int32);
6923 (*clp)->end_lat = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
6924 p+=sizeof(int32);
6925 (*clp)->end_lon = GPS_Math_Semi_To_Deg(GPS_Util_Get_Int(p));
6926 p+=sizeof(int32);
6927
6928 (*clp)->avg_heart_rate = *p++;
6929 (*clp)->max_heart_rate = *p++;
6930 (*clp)->intensity = *p++;
6931 if (*p != 0xff) {
6932 (*clp)->avg_cadence = *p;
6933 }
6934 p++;
6935 }
6936
6937
6938 /* @funcstatic GPS_D1007_Send *******************************************
6939 **
6940 ** Form course lap data string
6941 **
6942 ** @param [w] data [UC *] string to write to
6943 ** @param [r] clp [GPS_PCourse_Lap] course lap data
6944 ** @param [w] len [int32 *] packet length
6945 **
6946 ** @return [void]
6947 ************************************************************************/
GPS_D1007_Send(UC * data,GPS_PCourse_Lap clp,int32 * len)6948 void GPS_D1007_Send(UC* data, GPS_PCourse_Lap clp, int32* len)
6949 {
6950 UC* p;
6951 p = data;
6952
6953 GPS_Util_Put_Short(p, (US) clp->course_index);
6954 p += 2;
6955
6956 GPS_Util_Put_Short(p, (US) clp->lap_index);
6957 p += 2;
6958
6959 GPS_Util_Put_Uint(p, clp->total_time);
6960 p+=sizeof(int32);
6961
6962 GPS_Util_Put_Float(p,clp->total_dist);
6963 p+= sizeof(float);
6964
6965 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(clp->begin_lat));
6966 p+=sizeof(int32);
6967 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(clp->begin_lon));
6968 p+=sizeof(int32);
6969
6970 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(clp->end_lat));
6971 p+=sizeof(int32);
6972 GPS_Util_Put_Int(p,GPS_Math_Deg_To_Semi(clp->end_lon));
6973 p+=sizeof(int32);
6974
6975 *p++ = clp->avg_heart_rate;
6976
6977 *p++ = clp->max_heart_rate;
6978
6979 *p++ = clp->intensity;
6980
6981 *p++ = clp->avg_cadence > 0 ? clp->avg_cadence : 0xff;
6982
6983 *len = p-data;
6984 }
6985
6986
6987 /* @func GPS_A1008_Get ******************************************************
6988 **
6989 ** Get course points from GPS
6990 **
6991 ** @param [r] port [const char *] serial port
6992 ** @param [w] cpt [GPS_PCourse_Point **] course point array
6993 **
6994 ** @return [int32] number of course point entries
6995 ************************************************************************/
6996
GPS_A1008_Get(const char * port,GPS_PCourse_Point ** cpt,pcb_fn cb)6997 int32 GPS_A1008_Get(const char* port, GPS_PCourse_Point** cpt, pcb_fn cb)
6998 {
6999 static UC data[2];
7000 gpsdevh* fd;
7001 GPS_PPacket trapkt;
7002 GPS_PPacket recpkt;
7003 int32 i, n;
7004
7005 if (gps_course_point_transfer == -1) {
7006 return GPS_UNSUPPORTED;
7007 }
7008
7009 if (!GPS_Device_On(port, &fd)) {
7010 return gps_errno;
7011 }
7012
7013 GPS_Util_Put_Short(data,
7014 COMMAND_ID[gps_device_command].Cmnd_Transfer_Course_Points);
7015 GPS_Make_Packet(&trapkt, LINK_ID[gps_link_type].Pid_Command_Data,
7016 data,2);
7017 if (!GPS_Write_Packet(fd,trapkt)) {
7018 return gps_errno;
7019 }
7020 if (!GPS_Get_Ack(fd, &trapkt, &recpkt)) {
7021 return gps_errno;
7022 }
7023 if (!GPS_Packet_Read(fd, &recpkt)) {
7024 return gps_errno;
7025 }
7026 if (!GPS_Send_Ack(fd, &trapkt, &recpkt)) {
7027 return gps_errno;
7028 }
7029
7030 n = GPS_Util_Get_Short(recpkt.data);
7031
7032
7033 if (n)
7034 if (!((*cpt)=(GPS_PCourse_Point*)malloc(n*sizeof(GPS_PCourse_Point)))) {
7035 GPS_Error("A1008_Get: Insufficient memory");
7036 return MEMORY_ERROR;
7037 }
7038
7039 for (i=0; i<n; ++i) {
7040 if (!((*cpt)[i]=GPS_Course_Point_New())) {
7041 return MEMORY_ERROR;
7042 }
7043 if (!GPS_Packet_Read(fd, &recpkt)) {
7044 return gps_errno;
7045 }
7046
7047 if (!GPS_Send_Ack(fd, &trapkt, &recpkt)) {
7048 return gps_errno;
7049 }
7050
7051 switch (gps_course_point_type) {
7052 case pD1012:
7053 GPS_D1012_Get(&((*cpt)[i]),recpkt.data);
7054 break;
7055 default:
7056 GPS_Error("A1008_Get: Unknown Course Point protocol %d\n",
7057 gps_course_point_type);
7058 return PROTOCOL_ERROR;
7059 }
7060
7061 /* Cheat and don't _really_ pass the trkpt back */
7062 if (cb) {
7063 cb(n, nullptr);
7064 }
7065 }
7066
7067 if (!GPS_Packet_Read(fd, &recpkt)) {
7068 return gps_errno;
7069 }
7070 if (!GPS_Send_Ack(fd, &trapkt, &recpkt)) {
7071 return gps_errno;
7072 }
7073 if (recpkt.type != LINK_ID[gps_link_type].Pid_Xfer_Cmplt) {
7074 GPS_Error("A1008_Get: Error transferring course points");
7075 return FRAMING_ERROR;
7076 }
7077
7078 if (i != n) {
7079 GPS_Error("A1008_GET: Course Point entry number mismatch");
7080 return FRAMING_ERROR;
7081 }
7082
7083 if (!GPS_Device_Off(fd)) {
7084 return gps_errno;
7085 }
7086 return n;
7087 }
7088
7089
7090 /* @func GPS_A1008_Send **************************************************
7091 ** Send Course Points to GPS.
7092 **
7093 ** Note that different to other GPS_Axxx_Send functions, the device
7094 ** communication is not initialized/ended within the function, since
7095 ** this packet transfer is only part of a series of transfers to the
7096 ** device. Communication init/end has to be handled by the caller.
7097 **
7098 **
7099 ** @param [r] port [const char *] serial port
7100 ** @param [r] cpt [GPS_PCourse_Point *] pointer to CoursePoint array
7101 ** @param [r] n_cpt [int32] number of CoursePoint entries
7102 ** @param [r] fd [gpsdevh *] pointer to the communication port
7103 **
7104 ** @return [int32] success
7105 ************************************************************************/
GPS_A1008_Send(const char *,GPS_PCourse_Point * cpt,int32 n_cpt,gpsdevh * fd)7106 int32 GPS_A1008_Send(const char*,
7107 GPS_PCourse_Point* cpt,
7108 int32 n_cpt,
7109 gpsdevh* fd)
7110 {
7111 UC data[GPS_ARB_LEN];
7112 GPS_PPacket tra;
7113 GPS_PPacket rec;
7114 int32 i;
7115 int32 len;
7116
7117 GPS_Util_Put_Short(data,(US) n_cpt);
7118 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Records,
7119 data,2);
7120 if (!GPS_Write_Packet(fd,tra)) {
7121 return gps_errno;
7122 }
7123 if (!GPS_Get_Ack(fd, &tra, &rec)) {
7124 GPS_Error("GPS_A1008_Send: Coursepoint start data not acknowledged");
7125 return FRAMING_ERROR;
7126 }
7127
7128 for (i=0; i<n_cpt; ++i) {
7129 switch (gps_course_point_type) {
7130 case pD1012:
7131 GPS_D1012_Send(data,cpt[i],&len);
7132 break;
7133 default:
7134 GPS_Error("GPS_A1008_Send: Unknown course_point type %d\n",
7135 gps_course_point_type);
7136 return PROTOCOL_ERROR;
7137 }
7138
7139 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Course_Point,
7140 data,(US) len);
7141
7142 if (!GPS_Write_Packet(fd,tra)) {
7143 return gps_errno;
7144 }
7145
7146 if (!GPS_Get_Ack(fd, &tra, &rec)) {
7147 GPS_Error("A1008_Send: Pid_Course_Point not acknowledged");
7148 return gps_errno;
7149 }
7150 }
7151
7152 GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Transfer_Course_Points);
7153 GPS_Make_Packet(&tra, LINK_ID[gps_link_type].Pid_Xfer_Cmplt,
7154 data,2);
7155 if (!GPS_Write_Packet(fd,tra)) {
7156 return gps_errno;
7157 }
7158 if (!GPS_Get_Ack(fd, &tra, &rec)) {
7159 GPS_Error("A1008_Send: CoursePoint complete data not acknowledged");
7160 return FRAMING_ERROR;
7161 }
7162
7163 return 1;
7164 }
7165
7166
7167 /* @func GPS_D1012_Get ******************************************************
7168 **
7169 ** Convert packet D1012 to course point structure
7170 **
7171 ** @param [w] cpt [GPS_PCourse_Point *] Course Point
7172 ** @param [r] p [UC *] packet data
7173 **
7174 ** @return [void]
7175 ************************************************************************/
GPS_D1012_Get(GPS_PCourse_Point * cpt,UC * p)7176 void GPS_D1012_Get(GPS_PCourse_Point* cpt, UC* p)
7177 {
7178 int i;
7179 uint32 t;
7180
7181 for (i=0; i<11; ++i) {
7182 (*cpt)->name[i] = *p++;
7183 }
7184 p++; //unused
7185 (*cpt)->course_index = GPS_Util_Get_Short(p);
7186 p+=sizeof(uint16);
7187 p+=sizeof(uint16); // unused
7188
7189 t = GPS_Util_Get_Uint(p);
7190 (*cpt)->track_point_time = GPS_Math_Gtime_To_Utime((time_t)t);
7191 p+=sizeof(uint32);
7192
7193 (*cpt)->point_type = *p++;
7194
7195 }
7196
7197
7198 /* @funcstatic GPS_D1012_Send *******************************************
7199 **
7200 ** Form course point data string
7201 **
7202 ** @param [w] data [UC *] string to write to
7203 ** @param [r] cpt [GPS_PCourse_Point] course point data
7204 ** @param [w] len [int32 *] packet length
7205 **
7206 ** @return [void]
7207 ************************************************************************/
GPS_D1012_Send(UC * data,GPS_PCourse_Point cpt,int32 * len)7208 void GPS_D1012_Send(UC* data, GPS_PCourse_Point cpt, int32* len)
7209 {
7210 UC* p;
7211 int j;
7212 p = data;
7213
7214 for (j=0; j<11; ++j) {
7215 *p++ = cpt->name[j];
7216 }
7217
7218 GPS_Util_Put_Uint(p,0);
7219 p++;
7220
7221 GPS_Util_Put_Short(p, (US) cpt->course_index);
7222 p += 2;
7223
7224 GPS_Util_Put_Uint(p,0);
7225 p+=sizeof(uint16);
7226
7227 GPS_Util_Put_Uint(p,GPS_Math_Utime_To_Gtime(cpt->track_point_time));
7228 p+=sizeof(uint32);
7229
7230 *p++ = cpt->point_type;
7231
7232 *len = p-data;
7233 }
7234
7235
7236 /* @func GPS_A1009_Get ******************************************************
7237 **
7238 ** Get course limits from GPS
7239 **
7240 ** @param [r] port [const char *] serial port
7241 ** @param [w] limits [GPS_PCourse_Limits] course limits structure
7242 **
7243 ** @return [int32] success
7244 ************************************************************************/
7245
GPS_A1009_Get(const char * port,GPS_PCourse_Limits limits)7246 int32 GPS_A1009_Get(const char* port, GPS_PCourse_Limits limits)
7247 {
7248 static UC data[2];
7249 gpsdevh* fd;
7250 GPS_PPacket trapkt;
7251 GPS_PPacket recpkt;
7252
7253 if (gps_course_limits_transfer == -1) {
7254 return GPS_UNSUPPORTED;
7255 }
7256
7257 if (!GPS_Device_On(port, &fd)) {
7258 return gps_errno;
7259 }
7260
7261 GPS_Util_Put_Short(data,
7262 COMMAND_ID[gps_device_command].Cmnd_Transfer_Course_Limits);
7263 GPS_Make_Packet(&trapkt, LINK_ID[gps_link_type].Pid_Command_Data,
7264 data,2);
7265 if (!GPS_Write_Packet(fd,trapkt)) {
7266 return gps_errno;
7267 }
7268 if (!GPS_Get_Ack(fd, &trapkt, &recpkt)) {
7269 return gps_errno;
7270 }
7271 if (!GPS_Packet_Read(fd, &recpkt)) {
7272 return gps_errno;
7273 }
7274 if (!GPS_Send_Ack(fd, &trapkt, &recpkt)) {
7275 return gps_errno;
7276 }
7277
7278 switch (gps_course_limits_type) {
7279 case pD1013:
7280 GPS_D1013_Get(limits,recpkt.data);
7281 break;
7282 default:
7283 GPS_Error("A1009_Get: Unknown Course Limits protocol %d\n",
7284 gps_course_limits_type);
7285 return PROTOCOL_ERROR;
7286 }
7287
7288 if (!GPS_Device_Off(fd)) {
7289 return gps_errno;
7290 }
7291 return 1;
7292 }
7293
7294
7295 /* @func GPS_D1013_Get ******************************************************
7296 **
7297 ** Convert packet D1013 to course limits structure
7298 **
7299 ** @param [w] limits [GPS_PCourse_Limits] course limits structure
7300 ** @param [r] p [UC *] packet data
7301 **
7302 ** @return [void]
7303 ************************************************************************/
GPS_D1013_Get(GPS_PCourse_Limits limits,UC * p)7304 void GPS_D1013_Get(GPS_PCourse_Limits limits, UC* p)
7305 {
7306 limits->max_courses = GPS_Util_Get_Uint(p);
7307 p+=sizeof(uint32);
7308
7309 limits->max_course_laps = GPS_Util_Get_Uint(p);
7310 p+=sizeof(uint32);
7311
7312 limits->max_course_pnt = GPS_Util_Get_Uint(p);
7313 p+=sizeof(uint32);
7314
7315 limits->max_course_trk_pnt = GPS_Util_Get_Uint(p);
7316 p+=sizeof(uint32);
7317 }
7318
7319
7320 /*
7321 * It's unfortunate that these aren't constant and therefore switchable,
7322 * but they really are runtime variable. Sigh.
7323 */
7324 const char*
Get_Pkt_Type(US p,US d0,const char ** xinfo)7325 Get_Pkt_Type(US p, US d0, const char** xinfo)
7326 {
7327 *xinfo = nullptr;
7328 #define LT LINK_ID[gps_link_type]
7329 if (p == LT.Pid_Ack_Byte) {
7330 return "ACK";
7331 }
7332 if (p == LT.Pid_Command_Data) {
7333 switch (d0) {
7334 case 0:
7335 *xinfo = "Abort";
7336 break;
7337 case 1:
7338 *xinfo = "Xfer Alm";
7339 break;
7340 case 2:
7341 *xinfo = "Xfer Posn";
7342 break;
7343 case 3:
7344 *xinfo = "Xfer Prx";
7345 break;
7346 case 4:
7347 *xinfo = "Xfer Rte";
7348 break;
7349 case 5:
7350 *xinfo = "Xfer Time";
7351 break;
7352 case 6:
7353 *xinfo = "Xfer Trk";
7354 break;
7355 case 7:
7356 *xinfo = "Xfer Wpt";
7357 break;
7358 case 8:
7359 *xinfo = "Power Down";
7360 break;
7361 case 49:
7362 *xinfo = "Xfer PVT Start";
7363 break;
7364 case 50:
7365 *xinfo = "Xfer PVT Stop";
7366 break;
7367 case 92:
7368 *xinfo = "Flight Records";
7369 break;
7370 case 117:
7371 *xinfo = "Xfer Laps";
7372 break;
7373 case 121:
7374 *xinfo = "Xfer Categories";
7375 break;
7376 case 450:
7377 *xinfo = "Xfer Runs";
7378 break;
7379 case 451:
7380 *xinfo = "Xfer Workouts";
7381 break;
7382 case 452:
7383 *xinfo = "Xfer Wkt Occurrences";
7384 break;
7385 case 453:
7386 *xinfo = "Xfer User Profile ";
7387 break;
7388 case 454:
7389 *xinfo = "Xfer Wkt Limits";
7390 break;
7391 case 561:
7392 *xinfo = "Xfer Courses";
7393 break;
7394 case 562:
7395 *xinfo = "Xfer Course Laps";
7396 break;
7397 case 563:
7398 *xinfo = "Xfer Course Point";
7399 break;
7400 case 564:
7401 *xinfo = "Xfer Course Tracks";
7402 break;
7403 case 565:
7404 *xinfo = "Xfer Course Limits";
7405 break;
7406
7407 default:
7408 *xinfo = "Unknown";
7409 }
7410 return "CMDDAT";
7411 }
7412 if (p == LT.Pid_Protocol_Array) {
7413 return "PRTARR";
7414 }
7415 if (p == LT.Pid_Product_Rqst) {
7416 return "PRDREQ";
7417 }
7418 if (p == LT.Pid_Product_Data) {
7419 return "PRDDAT";
7420 }
7421 if (p == LT.Pid_Ext_Product_Data) {
7422 return "PRDEDA";
7423 }
7424
7425 if (p == LT.Pid_Xfer_Cmplt) {
7426 return "XFRCMP";
7427 }
7428 if (p == LT.Pid_Date_Time_Data) {
7429 return "DATTIM";
7430 }
7431 if (p == LT.Pid_Position_Data) {
7432 return "POS";
7433 }
7434 if (p == LT.Pid_Prx_Wpt_Data) {
7435 return "WPT";
7436 }
7437 if (p == LT.Pid_Nak_Byte) {
7438 return "NAK";
7439 }
7440 if (p == LT.Pid_Records) {
7441 return "RECORD";
7442 }
7443 if (p == LT.Pid_Rte_Hdr) {
7444 return "RTEHDR";
7445 }
7446 if (p == LT.Pid_Rte_Wpt_Data) {
7447 return "RTEWPT";
7448 }
7449 if (p == LT.Pid_Almanac_Data) {
7450 return "RALMAN";
7451 }
7452 if (p == LT.Pid_Trk_Data) {
7453 return "TRKDAT";
7454 }
7455 if (p == LT.Pid_Wpt_Data) {
7456 return "WPTDAT";
7457 }
7458 if (p == LT.Pid_Pvt_Data) {
7459 return "PVTDAT";
7460 }
7461 if (p == LT.Pid_Rte_Link_Data) {
7462 return "LNKDAT";
7463 }
7464 if (p == LT.Pid_Trk_Hdr) {
7465 return "TRKHDR";
7466 }
7467
7468 if (p == LT.Pid_FlightBook_Record) {
7469 return "FLIBOO";
7470 }
7471 if (p == LT.Pid_Lap) {
7472 return "LAPDAT";
7473 }
7474 if (p == LT.Pid_Wpt_Cat) {
7475 return "WPTCAT";
7476 }
7477 if (p == LT.Pid_Run) {
7478 return "RUNDAT";
7479 }
7480 if (p == LT.Pid_Workout) {
7481 return "WKTDAT";
7482 }
7483 if (p == LT.Pid_Workout_Occurrence) {
7484 return "WKTOCC";
7485 }
7486 if (p == LT.Pid_Fitness_User_Profile) {
7487 return "UPROFI";
7488 }
7489 if (p == LT.Pid_Workout_Limits) {
7490 return "WKTLIM";
7491 }
7492 if (p == LT.Pid_Course) {
7493 return "CRSDAT";
7494 }
7495 if (p == LT.Pid_Course_Lap) {
7496 return "CRSLAP";
7497 }
7498 if (p == LT.Pid_Course_Point) {
7499 return "CRSPOI";
7500 }
7501 if (p == LT.Pid_Course_Trk_Hdr) {
7502 return "CRSTHD";
7503 }
7504 if (p == LT.Pid_Course_Trk_Data) {
7505 return "CRSTDA";
7506 }
7507 if (p == LT.Pid_Course_Limits) {
7508 return "CRSLIM";
7509 }
7510 if (p == LT.Pid_Trk2_Hdr) {
7511 return "TRKHD2";
7512 }
7513
7514 if (p == GUSB_REQUEST_BULK) {
7515 return "REQBLK";
7516 }
7517 if (p == GUSB_SESSION_START) {
7518 return "SESREQ";
7519 }
7520 if (p == GUSB_SESSION_ACK) {
7521 return "SESACK";
7522 }
7523
7524 return "UNKNOWN";
7525 }
7526
7527
7528 /* @funcstatic Is_Trackpoint_Invalid ***********************************
7529 **
7530 ** Check if a trackpoint is invalid. Needed for D303/D304 to check for
7531 ** pauses.
7532 **
7533 **
7534 ** @param [r] trk [GPS_PTrack *] track
7535 ** @param [r] n [int32] Index of trackpoint
7536 **
7537 ** @return [UC] 1 if the trackpoint is invalid
7538 ************************************************************************/
Is_Trackpoint_Invalid(GPS_PTrack trk)7539 static UC Is_Trackpoint_Invalid(GPS_PTrack trk)
7540 {
7541 /* FIXME: We should have more *_is_unknown fields instead of
7542 * checking for special values here (e.g. cadence = 0 would be
7543 * perfectly valid, but GPS_D303b_Get() chose to use it to mark
7544 * nonexistent cadence data.
7545 */
7546 return trk->no_latlon && trk->distance > 1e24 &&
7547 !trk->heartrate && !trk->cadence;
7548 }
7549
7550
7551 /* @func GPS_Prepare_Track_For_Device **********************************
7552 **
7553 ** Perform device-specific adjustments on a track before upload.
7554 **
7555 ** @param [r] trk [GPS_PTrack **] track
7556 ** @param [r] n [int32 *] Number of trackpoints
7557 ************************************************************************/
GPS_Prepare_Track_For_Device(GPS_PTrack ** trk,int32 * n)7558 void GPS_Prepare_Track_For_Device(GPS_PTrack** trk, int32* n)
7559 {
7560 int32 i, j;
7561
7562 /* D303/304 marks track segments with two consecutive invalid track
7563 * points instead of the tnew flag. Create them unless we're at the
7564 * beginning of a track or there are already invalid track points
7565 * (because the track was downloaded using D303/304). This needs to be
7566 * done here because it will change the number of track points.
7567 */
7568 if (gps_trk_type == pD303 || gps_trk_type == pD304) {
7569 for (i=0; i<*n; ++i) {
7570 if ((*trk)[i]->tnew && i>0 && !(*trk)[i]->ishdr && !(*trk)[i-1]->ishdr) {
7571 /* Create invalid points based on the data from the point
7572 * marked with tnew and the one before it.
7573 */
7574 for (j=i-1; j<=i; j++) {
7575 if (!Is_Trackpoint_Invalid((*trk)[j])) {
7576 GPS_PTrack trkpt = GPS_Track_New();
7577 *trkpt = *((*trk)[j]);
7578 trkpt->no_latlon = 1;
7579 trkpt->alt = (float) 1e25;
7580 trkpt->distance_populated = 0;
7581 trkpt->heartrate = 0;
7582 trkpt->cadence = 0xff;
7583 *trk = (struct GPS_STrack**) xrealloc(*trk, (*n+1) * sizeof(GPS_PTrack));
7584 memmove(&(*trk)[i+1], &(*trk)[i], (*n-i) * sizeof(GPS_PTrack));
7585 (*trk)[i] = trkpt;
7586 i++;
7587 j++;
7588 (*n)++;
7589 }
7590 }
7591 }
7592 }
7593 }
7594 }
7595
GPS_Set_Baud_Rate(const char * port,int br)7596 int32 GPS_Set_Baud_Rate(const char* port, int br)
7597 {
7598
7599 gpsdevh* fd;
7600
7601 if (!GPS_Device_On(port, &fd)) {
7602 return gps_errno;
7603 }
7604
7605 if (gps_is_usb) return -1; // this feature is serial only
7606 GPS_Serial_Set_Baud_Rate(fd, br);
7607
7608 if (!GPS_Device_Off(fd)) {
7609 return gps_errno;
7610 }
7611
7612 return 0;
7613
7614 }
7615