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