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