1 /********************************************************************
2 ** @source JEEPS command 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) 2005, 2006 Robert Lipe
8 ** @modified Copyright (C) 2007 Achim Schumacher
9 ** @modified Copyright (C) 2010 Martin Buck
10 ** @@
11 **
12 ** This library is free software; you can redistribute it and/or
13 ** modify it under the terms of the GNU Library General Public
14 ** License as published by the Free Software Foundation; either
15 ** version 2 of the License, or (at your option) any later version.
16 **
17 ** This library is distributed in the hope that it will be useful,
18 ** but WITHOUT ANY WARRANTY; without even the implied warranty of
19 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
20 ** Library General Public License for more details.
21 **
22 ** You should have received a copy of the GNU Library General Public
23 ** License along with this library; if not, write to the
24 ** Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
25 ** Boston, MA 02110-1301, USA.
26 ********************************************************************/
27 #include "gps.h"
28 #include <cfloat>
29 #include <cstdio>
30 #include <cstdlib>
31
32
33 /* @func GPS_Command_Off ***********************************************
34 **
35 ** Turn off power on GPS
36 **
37 ** @return [int32] success
38 ************************************************************************/
39
GPS_Command_Off(const char * port)40 int32 GPS_Command_Off(const char* port)
41 {
42 static UC data[2];
43 gpsdevh* fd;
44 GPS_PPacket tra;
45 GPS_PPacket rec;
46
47 GPS_Util_Little();
48
49 if (!GPS_Device_On(port, &fd)) {
50 return gps_errno;
51 }
52
53 GPS_Util_Put_Short(data,COMMAND_ID[gps_device_command].Cmnd_Turn_Off_Pwr);
54
55 /* robertl - LINK_ID isn't set yet. Hardcode it to Garmin spec value */
56 GPS_Make_Packet(&tra, 10, /* LINK_ID[gps_link_type].Pid_Command_Data, */
57 data,2);
58 if (!GPS_Write_Packet(fd,tra)) {
59 return gps_errno;
60 }
61
62 if (!GPS_Device_Chars_Ready(fd)) {
63 if (!GPS_Get_Ack(fd, &tra, &rec)) {
64 return gps_errno;
65 }
66 GPS_User("Power off command acknowledged");
67 }
68
69 if (!GPS_Device_Off(fd)) {
70 return gps_errno;
71 }
72
73 return 1;
74 }
75
76
77 /* @func GPS_Command_Get_Waypoint ***************************************
78 **
79 ** Get waypoint from GPS
80 **
81 ** @param [r] port [const char *] serial port
82 ** @param [w] way [GPS_PWay **] pointer to waypoint array
83 **
84 ** @return [int32] number of waypoint entries
85 ************************************************************************/
86
GPS_Command_Get_Waypoint(const char * port,GPS_PWay ** way,pcb_fn cb)87 int32 GPS_Command_Get_Waypoint(const char* port, GPS_PWay** way, pcb_fn cb)
88 {
89 int32 ret=0;
90
91 /*
92 * It's a bit tacky to do this up front without ticking the
93 * progress meter, but this come in pretty quickly...
94 */
95 if (gps_category_transfer) {
96 ret = GPS_A101_Get(port);
97 if (!ret) {
98 fatal("blah");
99 return PROTOCOL_ERROR;
100 }
101
102 }
103
104 switch (gps_waypt_transfer) {
105 case pA100:
106 ret = GPS_A100_Get(port,way, cb);
107 break;
108 default:
109 GPS_Error("Get_Waypoint: Unknown waypoint protocol");
110 return PROTOCOL_ERROR;
111 }
112
113 return ret;
114 }
115
116
117
118 /* @func GPS_Command_Send_Waypoint ******************************************
119 **
120 ** Send waypoints to GPS
121 **
122 ** @param [r] port [const char *] serial port
123 ** @param [r] way [GPS_PWay *] waypoint array
124 ** @param [r] n [int32] number of waypoint entries
125 **
126 ** @return [int32] success
127 ************************************************************************/
128
GPS_Command_Send_Waypoint(const char * port,GPS_PWay * way,int32 n,int (* cb)(struct GPS_SWay **))129 int32 GPS_Command_Send_Waypoint(const char* port, GPS_PWay* way, int32 n, int (*cb)(struct GPS_SWay**))
130 {
131 int32 ret=0;
132
133 switch (gps_waypt_transfer) {
134 case pA100:
135 ret = GPS_A100_Send(port, way, n, cb);
136 break;
137 default:
138 GPS_Error("Send_Waypoint: Unknown waypoint protocol");
139 return PROTOCOL_ERROR;
140 }
141
142 return ret;
143 }
144
145
146 /* @func GPS_Command_Get_Route **************************************
147 **
148 ** Get Route(s) from GPS
149 **
150 ** @param [r] port [const char *] serial port
151 ** @param [w] way [GPS_PWay **] pointer to waypoint array
152 **
153 ** @return [int32] number of waypoint entries
154 ************************************************************************/
155
GPS_Command_Get_Route(const char * port,GPS_PWay ** way)156 int32 GPS_Command_Get_Route(const char* port, GPS_PWay** way)
157 {
158 int32 ret=0;
159
160 switch (gps_route_transfer) {
161 case pA200:
162 ret = GPS_A200_Get(port,way);
163 break;
164 case pA201:
165 ret = GPS_A201_Get(port,way);
166 break;
167 default:
168 GPS_Error("Get_Route: Unknown route protocol");
169 return PROTOCOL_ERROR;
170 }
171
172 return ret;
173 }
174
175
176
177 /* @func GPS_Command_Send_Route ****************************************
178 **
179 ** Send route(s) to GPS
180 **
181 ** @param [r] port [const char *] serial port
182 ** @param [r] way [GPS_PWay *] waypoint array
183 ** @param [r] n [int32] number of waypoint entries
184 **
185 ** @return [int32] success
186 ************************************************************************/
187
GPS_Command_Send_Route(const char * port,GPS_PWay * way,int32 n)188 int32 GPS_Command_Send_Route(const char* port, GPS_PWay* way, int32 n)
189 {
190 int32 ret=0;
191
192
193 switch (gps_route_transfer) {
194 case pA200:
195 ret = GPS_A200_Send(port, way, n);
196 break;
197 case pA201:
198 ret = GPS_A201_Send(port, way, n);
199 break;
200 default:
201 GPS_Error("Send_Route: Unknown route protocol");
202 return PROTOCOL_ERROR;
203 }
204
205 return ret;
206 }
207
208
209 /* @func GPS_Command_Get_Track ***************************************
210 **
211 ** Get track log from GPS
212 **
213 ** @param [r] port [const char *] serial port
214 ** @param [w] trk [GPS_PTrack **] pointer to track array
215 **
216 ** @return [int32] number of track entries
217 ************************************************************************/
218
GPS_Command_Get_Track(const char * port,GPS_PTrack ** trk,pcb_fn cb)219 int32 GPS_Command_Get_Track(const char* port, GPS_PTrack** trk, pcb_fn cb)
220 {
221 int32 ret=0;
222
223 if (gps_trk_transfer == -1) {
224 return GPS_UNSUPPORTED;
225 }
226
227 switch (gps_trk_transfer) {
228 case pA300:
229 ret = GPS_A300_Get(port,trk,cb);
230 break;
231 case pA301:
232 case pA302:
233 ret = GPS_A301_Get(port,trk,cb,301);
234 break;
235 default:
236 GPS_Error("Get_Track: Unknown track protocol %d\n", gps_trk_transfer);
237 return PROTOCOL_ERROR;
238 }
239
240 return ret;
241 }
242
243
244
245 /* @func GPS_Command_Send_Track ******************************************
246 **
247 ** Send track log to GPS
248 **
249 ** @param [r] port [const char *] serial port
250 ** @param [r] trk [GPS_PTrack *] track array
251 ** @param [r] n [int32] number of track entries
252 **
253 ** @return [int32] success
254 ************************************************************************/
255
GPS_Command_Send_Track(const char * port,GPS_PTrack * trk,int32 n,int eraset)256 int32 GPS_Command_Send_Track(const char* port, GPS_PTrack* trk, int32 n, int eraset)
257 {
258 int32 ret=0;
259
260 if (gps_trk_transfer == -1) {
261 return GPS_UNSUPPORTED;
262 }
263
264 switch (gps_trk_transfer) {
265 case pA300:
266 ret = GPS_A300_Send(port, trk, n);
267 break;
268 case pA301:
269 ret = GPS_A301_Send(port, trk, n, 301, nullptr);
270 break;
271 case pA302:
272 /* Units with A302 don't support track upload, so we convert the
273 * track to a course on the fly and send that instead
274 */
275 ret = GPS_Command_Send_Track_As_Course(port, trk, n, nullptr, 0, eraset);
276 break;
277 default:
278 GPS_Error("Send_Track: Unknown track protocol %d.", gps_trk_transfer);
279 break;
280 }
281
282 return ret;
283 }
284
285
286 /* @func GPS_Command_Get_Proximity **************************************
287 **
288 ** Get proximitywaypoint from GPS
289 **
290 ** @param [r] port [const char *] serial port
291 ** @param [w] way [GPS_PWay **] pointer to waypoint array
292 **
293 ** @return [int32] number of waypoint entries
294 ************************************************************************/
295
GPS_Command_Get_Proximity(const char * port,GPS_PWay ** way)296 int32 GPS_Command_Get_Proximity(const char* port, GPS_PWay** way)
297 {
298 int32 ret=0;
299
300 if (gps_prx_waypt_transfer == -1) {
301 return GPS_UNSUPPORTED;
302 }
303
304 switch (gps_prx_waypt_transfer) {
305 case pA400:
306 ret = GPS_A400_Get(port,way);
307 break;
308 default:
309 GPS_Error("Get_Proximity: Unknown proximity protocol");
310 return PROTOCOL_ERROR;
311 }
312
313 return ret;
314 }
315
316
317
318 /* @func GPS_Command_Send_Proximity ******************************************
319 **
320 ** Send proximity waypoints to GPS
321 **
322 ** @param [r] port [const char *] serial port
323 ** @param [r] way [GPS_PWay *] waypoint array
324 ** @param [r] n [int32] number of waypoint entries
325 **
326 ** @return [int32] success
327 ************************************************************************/
328
GPS_Command_Send_Proximity(const char * port,GPS_PWay * way,int32 n)329 int32 GPS_Command_Send_Proximity(const char* port, GPS_PWay* way, int32 n)
330 {
331 int32 ret=0;
332
333
334 if (gps_prx_waypt_transfer == -1) {
335 return GPS_UNSUPPORTED;
336 }
337
338
339 switch (gps_prx_waypt_transfer) {
340 case pA400:
341 ret = GPS_A400_Send(port, way, n);
342 break;
343 default:
344 GPS_Error("Send_Proximity: Unknown proximity protocol");
345 break;
346 }
347
348 return ret;
349 }
350
351
352
353 /* @func GPS_Command_Get_Almanac ***************************************
354 **
355 ** Get almanac from GPS
356 **
357 ** @param [r] port [const char *] serial port
358 ** @param [w] alm [GPS_PAlmanac **] pointer to almanac array
359 **
360 ** @return [int32] number of almanac entries
361 ************************************************************************/
362
GPS_Command_Get_Almanac(const char * port,GPS_PAlmanac ** alm)363 int32 GPS_Command_Get_Almanac(const char* port, GPS_PAlmanac** alm)
364 {
365 int32 ret=0;
366
367 if (gps_almanac_transfer == -1) {
368 return GPS_UNSUPPORTED;
369 }
370
371 switch (gps_almanac_transfer) {
372 case pA500:
373 ret = GPS_A500_Get(port,alm);
374 break;
375 default:
376 GPS_Error("Get_Almanac: Unknown almanac protocol");
377 return PROTOCOL_ERROR;
378 }
379
380 return ret;
381 }
382
383
384
385 /* @func GPS_Command_Send_Almanac ******************************************
386 **
387 ** Send almanac to GPS
388 **
389 ** @param [r] port [const char *] serial port
390 ** @param [r] alm [GPS_PAlmanac *] almanac array
391 ** @param [r] n [int32] number of almanac entries
392 **
393 ** @return [int32] success
394 ************************************************************************/
395
GPS_Command_Send_Almanac(const char * port,GPS_PAlmanac * alm,int32 n)396 int32 GPS_Command_Send_Almanac(const char* port, GPS_PAlmanac* alm, int32 n)
397 {
398 int32 ret=0;
399
400 if (gps_almanac_transfer == -1) {
401 return GPS_UNSUPPORTED;
402 }
403
404 switch (gps_almanac_transfer) {
405 case pA500:
406 ret = GPS_A500_Send(port, alm, n);
407 break;
408 default:
409 GPS_Error("Send_Almanac: Unknown almanac protocol");
410 break;
411 }
412
413 return ret;
414 }
415
416
417
418 /* @func GPS_Command_Get_Time ******************************************
419 **
420 ** Get time from GPS
421 **
422 ** @param [r] port [const char *] serial port
423 **
424 ** @return [time_t] unix-style time
425 ************************************************************************/
426
GPS_Command_Get_Time(const char * port)427 time_t GPS_Command_Get_Time(const char* port)
428 {
429 time_t ret=0;
430
431 switch (gps_date_time_transfer) {
432 case pA600:
433 ret = GPS_A600_Get(port);
434 break;
435 /*
436 * If the unit doesn't support it (i.e. a C320 in charging mode),
437 * but don't treat as error; return as zero.
438 */
439 case -1:
440 return 0;
441 default:
442 GPS_Error("Get_Time: Unknown date/time protocol");
443 return PROTOCOL_ERROR;
444 }
445
446 return ret;
447 }
448
449
450
451 /* @func GPS_Command_Send_Time ******************************************
452 **
453 ** Set GPS time
454 **
455 ** @param [r] port [const char *] serial port
456 ** @param [r] Time [time_t] unix-style time
457 **
458 ** @return [int32] true if OK
459 ************************************************************************/
460
GPS_Command_Send_Time(const char * port,time_t Time)461 int32 GPS_Command_Send_Time(const char* port, time_t Time)
462 {
463 time_t ret=0;
464
465 switch (gps_date_time_transfer) {
466 case pA600:
467 ret = GPS_A600_Send(port, Time);
468 break;
469 default:
470 GPS_Error("Send_Time: Unknown date/time protocol");
471 return PROTOCOL_ERROR;
472 }
473
474 return ret;
475 }
476
477
478
479
480 /* @func GPS_Command_Get_Position ***************************************
481 **
482 ** Get position from GPS
483 **
484 ** @param [r] port [const char *] serial port
485 ** @param [w] lat [double *] latitude (deg)
486 ** @param [w] lon [double *] longitude (deg)
487 **
488 ** @return [int32] success
489 ************************************************************************/
490
GPS_Command_Get_Position(const char * port,double * lat,double * lon)491 int32 GPS_Command_Get_Position(const char* port, double* lat, double* lon)
492 {
493 int32 ret=0;
494
495 switch (gps_position_transfer) {
496 case pA700:
497 ret = GPS_A700_Get(port,lat,lon);
498 break;
499 /*
500 * If the unit doesn't support it (i.e. a C320 in charging mode),
501 * zero lat/lon, but don't treat as error.
502 */
503 case -1:
504 *lat = *lon = 0.0;
505 break;
506 default:
507 GPS_Error("Get_Position: Unknown position protocol");
508 return PROTOCOL_ERROR;
509 }
510
511 return ret;
512 }
513
514
515
516 /* @func GPS_Command_Send_Position ******************************************
517 **
518 ** Set GPS position
519 **
520 ** @param [r] port [const char *] serial port
521 ** @param [r] lat [double] latitude (deg)
522 ** @param [r] lon [double] longitude (deg)
523 **
524 ** @return [int32] success
525 ************************************************************************/
526
GPS_Command_Send_Position(const char * port,double lat,double lon)527 int32 GPS_Command_Send_Position(const char* port, double lat, double lon)
528 {
529 int32 ret=0;
530
531 switch (gps_position_transfer) {
532 case pA700:
533 ret = GPS_A700_Send(port, lat, lon);
534 break;
535 default:
536 GPS_Error("Send_Position: Unknown position protocol");
537 return PROTOCOL_ERROR;
538 }
539
540 return ret;
541 }
542
543
544 /* @func GPS_Command_Pvt_On ********************************************
545 **
546 ** Instruct GPS to start sending Pvt data every second
547 **
548 ** @param [r] port [const char *] serial port
549 ** @param [w] fd [int32 *] file descriptor
550 **
551 ** @return [int32] success if supported and GPS starts sending
552 ************************************************************************/
553
GPS_Command_Pvt_On(const char * port,gpsdevh ** fd)554 int32 GPS_Command_Pvt_On(const char* port, gpsdevh** fd)
555 {
556 int32 ret=0;
557
558
559 if (gps_pvt_transfer == -1) {
560 return GPS_UNSUPPORTED;
561 }
562
563
564 switch (gps_pvt_transfer) {
565 case pA800:
566 ret = GPS_A800_On(port,fd);
567 break;
568 default:
569 GPS_Error("Pvt_On: Unknown position protocol");
570 return PROTOCOL_ERROR;
571 }
572
573
574 return ret;
575 }
576
577
578
579 /* @func GPS_Command_Pvt_Off ********************************************
580 **
581 ** Instruct GPS to stop sending Pvt data every second
582 **
583 ** @param [r] port [const char *] serial port
584 ** @param [w] fd [int32 *] file descriptor
585 **
586 ** @return [int32] success
587 ************************************************************************/
588
GPS_Command_Pvt_Off(const char * port,gpsdevh ** fd)589 int32 GPS_Command_Pvt_Off(const char* port, gpsdevh** fd)
590 {
591 int32 ret=0;
592
593
594 if (gps_pvt_transfer == -1) {
595 return GPS_UNSUPPORTED;
596 }
597
598 switch (gps_pvt_transfer) {
599 case pA800:
600 ret = GPS_A800_Off(port,fd);
601 break;
602 default:
603 GPS_Error("Pvt_Off: Unknown position protocol");
604 return PROTOCOL_ERROR;
605 }
606
607 return ret;
608 }
609
610
611
612 /* @func GPS_Command_Pvt_Get ********************************************
613 **
614 ** Get a single PVT info entry
615 **
616 ** @param [w] fd [int32 *] file descriptor
617 ** @param [w] pvt [GPS_PPvt_Data *] pvt data structure to fill
618 **
619 ** @return [int32] success
620 ************************************************************************/
621
GPS_Command_Pvt_Get(gpsdevh ** fd,GPS_PPvt_Data * pvt)622 int32 GPS_Command_Pvt_Get(gpsdevh** fd, GPS_PPvt_Data* pvt)
623 {
624 int32 ret=0;
625
626 if (gps_pvt_transfer == -1) {
627 return GPS_UNSUPPORTED;
628 }
629
630 (*pvt)->fix = 0;
631
632 switch (gps_pvt_transfer) {
633 case pA800:
634 ret = GPS_A800_Get(fd,pvt);
635 break;
636 default:
637 GPS_Error("Pvt_Get: Unknown position protocol");
638 return PROTOCOL_ERROR;
639 }
640
641 return ret;
642 }
643
644 /* @func GPS_Command_Get_Lap ***************************************
645 **
646 ** Get lap from GPS
647 **
648 ** @param [r] port [const char *] serial port
649 ** @param [w] lap [GPS_PLap **] pointer to lap array
650 **
651 ** @return [int32] number of lap entries
652 ************************************************************************/
653
GPS_Command_Get_Lap(const char * port,GPS_PLap ** lap,pcb_fn cb)654 int32 GPS_Command_Get_Lap(const char* port, GPS_PLap** lap, pcb_fn cb)
655 {
656 int32 ret=0;
657
658 if (gps_lap_transfer == -1) {
659 return GPS_UNSUPPORTED;
660 }
661
662 switch (gps_lap_transfer) {
663 case pA906:
664 ret = GPS_A906_Get(port,lap, cb);
665 break;
666 default:
667 GPS_Error("Get_Lap: Unknown lap protocol");
668 return PROTOCOL_ERROR;
669 }
670
671 return ret;
672 }
673
674 /* @func GPS_Command_Get_Course ***************************************
675 **
676 ** Get Courses from GPS. According to Garmin protocol specification, this
677 ** includes getting all course laps, course tracks and course points
678 ** from the device.
679 **
680 ** @param [r] port [const char *] serial port
681 ** @param [w] crs [GPS_PCourse **] pointer to course array
682 ** @param [w] clp [GPS_PCourse_Lap **] pointer to course lap array
683 ** @param [w] trk [GPS_PTrack **] pointer to track array
684 ** @param [w] cpt [GPS_PCourse_Point **] pointer to course point array
685 ** @param [w] n_clp [int32 **] pointer to number of lap entries
686 ** @param [w] n_trk [int32 **] pointer to number of track entries
687 ** @param [w] n_cpt [int32 **] pointer to number of course point entries
688 **
689 ** @return [int32] number of course entries
690 ************************************************************************/
GPS_Command_Get_Course(const char * port,GPS_PCourse ** crs,GPS_PCourse_Lap ** clp,GPS_PTrack ** trk,GPS_PCourse_Point ** cpt,int32 * n_clp,int32 * n_trk,int32 * n_cpt,pcb_fn cb)691 int32 GPS_Command_Get_Course
692 (const char* port,
693 GPS_PCourse** crs,
694 GPS_PCourse_Lap** clp,
695 GPS_PTrack** trk,
696 GPS_PCourse_Point** cpt,
697 int32* n_clp,
698 int32* n_trk,
699 int32* n_cpt,
700 pcb_fn cb)
701 {
702 int32 ret=0;
703
704 if (gps_course_transfer == -1) {
705 return GPS_UNSUPPORTED;
706 }
707
708 switch (gps_course_transfer) {
709 case pA1006:
710 ret = GPS_A1006_Get(port,crs,cb);
711 break;
712 default:
713 GPS_Error("Get_Course: Unknown course protocol");
714 return PROTOCOL_ERROR;
715 }
716
717 switch (gps_course_lap_transfer) {
718 case pA1007:
719 *n_clp = GPS_A1007_Get(port,clp, nullptr);
720 break;
721 default:
722 GPS_Error("Get_Course: Unknown course lap protocol");
723 return PROTOCOL_ERROR;
724 }
725
726 switch (gps_course_trk_transfer) {
727 case pA1012:
728 GPS_Error("Get_Course: Not implemented track protocol %d\n",
729 gps_trk_transfer);
730 break;
731 case pA302:
732 *n_trk = GPS_A301_Get(port,trk,cb,302);
733 break;
734 default:
735 GPS_Error("Get_Course: Unknown course track protocol %d\n",
736 gps_trk_transfer);
737 return PROTOCOL_ERROR;
738 }
739
740 switch (gps_course_point_transfer) {
741 case pA1008:
742 *n_cpt = GPS_A1008_Get(port,cpt, nullptr);
743 break;
744 default:
745 GPS_Error("Get_Course: Unknown course point protocol");
746 return PROTOCOL_ERROR;
747 }
748
749 return ret;
750 }
751
752
753 /* @func GPS_Command_Send_Course ***************************************
754 **
755 ** Send Courses to GPS. According to Garmin protocol specification, this
756 ** includes sending all course laps, course tracks and course points
757 ** to the device.
758 **
759 ** @param [r] port [const char *] serial port
760 ** @param [w] crs [GPS_PCourse **] course array
761 ** @param [w] clp [GPS_PCourse_Lap *] course lap array
762 ** @param [w] trk [GPS_PTrack *] track array
763 ** @param [w] cpt [GPS_PCourse_Point *] course point array
764 ** @param [w] n_crs [int32] number of course entries
765 ** @param [w] n_clp [int32] number of lap entries
766 ** @param [w] n_trk [int32] number of track entries
767 ** @param [w] n_cpt [int32] number of course point entries
768 **
769 ** @return [int32] Success
770 ************************************************************************/
GPS_Command_Send_Course(const char * port,GPS_PCourse * crs,GPS_PCourse_Lap * clp,GPS_PTrack * trk,GPS_PCourse_Point * cpt,int32 n_crs,int32 n_clp,int32 n_trk,int32 n_cpt)771 int32 GPS_Command_Send_Course
772 (const char* port,
773 GPS_PCourse* crs,
774 GPS_PCourse_Lap* clp,
775 GPS_PTrack* trk,
776 GPS_PCourse_Point* cpt,
777 int32 n_crs,
778 int32 n_clp,
779 int32 n_trk,
780 int32 n_cpt)
781 {
782 gpsdevh* fd;
783 GPS_OCourse_Limits limits;
784 int32 ret;
785 int32 ret_crs=0;
786 int32 ret_clp=0;
787 int32 ret_trk=0;
788 int32 ret_cpt=0;
789
790 if (gps_course_transfer == -1 || gps_course_limits_transfer == -1) {
791 return GPS_UNSUPPORTED;
792 }
793
794 /* Check course limits to make sure we're not exceeding the device's
795 * capacity.
796 */
797 switch (gps_course_limits_transfer) {
798 case pA1009:
799 ret = GPS_A1009_Get(port,&limits);
800 Q_UNUSED(ret);
801 break;
802 default:
803 GPS_Error("Send_Course: Unknown course limitsprotocol");
804 return PROTOCOL_ERROR;
805 }
806
807 if (n_crs > limits.max_courses
808 || n_clp > limits.max_course_laps
809 || n_trk > limits.max_course_trk_pnt
810 || n_cpt > limits.max_course_pnt) {
811 GPS_Error("Course upload would exceed device capacity:");
812 GPS_Error("# of courses: %d, max: %d", n_crs, limits.max_courses);
813 GPS_Error("# of laps: %d, max: %d", n_clp, limits.max_course_laps);
814 GPS_Error("# of track points: %d, max: %d", n_trk, limits.max_course_trk_pnt);
815 GPS_Error("# of course points: %d, max: %d", n_cpt, limits.max_course_pnt);
816 return GPS_UNSUPPORTED;
817 }
818
819 /* Initialize device communication:
820 * In contrast to other transfer protocols, this has to be handled here;
821 * shutting off communication in between the different parts
822 * could lead to data corruption on the device because all the courses
823 * and their associated lap and track data have to be sent in one
824 * transaction.
825 */
826 if (!GPS_Device_On(port,&fd)) {
827 return gps_errno;
828 }
829
830 switch (gps_course_transfer) {
831 case pA1006:
832 ret_crs = GPS_A1006_Send(port,crs,n_crs,fd);
833 break;
834 default:
835 GPS_Error("Send_Course: Unknown course protocol");
836 return PROTOCOL_ERROR;
837 }
838
839 switch (gps_course_lap_transfer) {
840 case pA1007:
841 ret_clp = GPS_A1007_Send(port,clp,n_clp,fd);
842 break;
843 default:
844 GPS_Error("Send_Course: Unknown course lap protocol");
845 return PROTOCOL_ERROR;
846 }
847
848 switch (gps_course_trk_transfer) {
849 case pA1012:
850 GPS_Error("Send_Course: Not implemented track protocol %d\n",
851 gps_trk_transfer);
852 break;
853 case pA302:
854 ret_trk = GPS_A301_Send(port,trk,n_trk,302,fd);
855 break;
856 default:
857 GPS_Error("Send_Course: Unknown course track protocol %d\n",
858 gps_trk_transfer);
859 return PROTOCOL_ERROR;
860 }
861
862 switch (gps_course_point_transfer) {
863 case pA1008:
864 ret_cpt = GPS_A1008_Send(port,cpt,n_cpt,fd);
865 break;
866 default:
867 GPS_Error("Send_Course: Unknown course point protocol");
868 return PROTOCOL_ERROR;
869 }
870
871 if (!GPS_Device_Off(fd)) {
872 return gps_errno;
873 }
874
875
876 return ret_crs * ret_clp * ret_trk * ret_cpt;
877 }
878
879
880 /* @funcstatic Unique_Course_Index *************************************
881 **
882 ** Choose a course index that's not yet used by another course.
883 **
884 ** @param [r] crs [GPS_PCourse **] course array
885 ** @param [r] n_crs [int32] number of course entries
886 **
887 ** @return [uint32] course index
888 ************************************************************************/
Unique_Course_Index(GPS_PCourse * crs,int n_crs)889 static uint32 Unique_Course_Index(GPS_PCourse* crs, int n_crs)
890 {
891 uint32 idx;
892 int i;
893
894 for (idx=0; ; idx++) {
895 for (i=0; i<n_crs; i++)
896 if (crs[i]->index==idx) {
897 break; /* Already have this index */
898 }
899 if (i>=n_crs) {
900 return idx; /* Found unused index */
901 }
902 }
903 }
904
905
906 /* @funcstatic Unique_Track_Index ***************************************
907 **
908 ** Choose a track index that's not yet used by another track referenced
909 ** by the courses.
910 **
911 ** @param [r] crs [GPS_PCourse **] course array
912 ** @param [r] n_crs [int32] number of course entries
913 **
914 ** @return [uint32] track index
915 ************************************************************************/
Unique_Track_Index(GPS_PCourse * crs,int n_crs)916 static uint32 Unique_Track_Index(GPS_PCourse* crs, int n_crs)
917 {
918 uint32 idx;
919 int i;
920
921 for (idx=0; ; idx++) {
922 for (i=0; i<n_crs; i++)
923 if (crs[i]->track_index==idx) {
924 break; /* Already have this index */
925 }
926 if (i>=n_crs) {
927 return idx; /* Found unused index */
928 }
929 }
930 }
931
932
933 /* @funcstatic Calculate_Course_Lap_Data *******************************
934 **
935 ** Calculate lap data totals from individual track points. Also
936 ** generates time stamps for track points if they don't have
937 ** time stamps yet (using an arbitrary speed of 10 km/h which is
938 ** currently hardcoded. This is required so that course points can
939 ** refer to track points and identify them uniquely.
940 **
941 ** @param [w] clp [GPS_PCourse_Lap] course lap to be calculated
942 ** @param [r] ctk [GPS_PTrack *] track array to calculate lap from
943 ** @param [r] ctk_start [int] start index of lap in track array
944 ** @param [r] ctk_end [int] end index of lap in track array
945 **
946 ** @return [void]
947 ************************************************************************/
948 static void
Calculate_Course_Lap_Data(GPS_PCourse_Lap clp,GPS_PTrack * ctk,int ctk_start,int ctk_end)949 Calculate_Course_Lap_Data(GPS_PCourse_Lap clp, GPS_PTrack* ctk,
950 int ctk_start, int ctk_end)
951 {
952 int i;
953 double heartrate_sum = 0, cadence_sum = 0;
954 int heartrate_sum_time = 0, cadence_sum_time = 0;
955 double time_synth_speed = 10.0 * 1000 / 3600; /* speed in m/s */
956
957 if (ctk_start && ctk_end && !ctk[ctk_start]->Time) {
958 ctk[ctk_start]->Time = GPS_Time_Now();
959 } else {
960 time_synth_speed = 0;
961 }
962
963 clp->total_dist = 0;
964 clp->avg_heart_rate = 0;
965 clp->max_heart_rate = 0;
966 clp->intensity = 0;
967 clp->avg_cadence = 0xff;
968 for (i=ctk_start; i <= ctk_end; i++) {
969 if (ctk[i]->heartrate && ctk[i]->heartrate > clp->max_heart_rate) {
970 clp->max_heart_rate = ctk[i]->heartrate;
971 }
972 if (i < ctk_end) {
973 double dist = 0;
974 int seg_time;
975
976 if (!ctk[i]->no_latlon && !ctk[i+1]->no_latlon)
977 dist = gcgeodist(ctk[i]->lat, ctk[i]->lon,
978 ctk[i+1]->lat, ctk[i+1]->lon);
979 clp->total_dist += dist;
980
981 if (time_synth_speed) {
982 ctk[i+1]->Time = ctk[i]->Time + (dist / time_synth_speed + 0.5);
983 }
984
985 seg_time = ctk[i+1]->Time - ctk[i]->Time;
986
987 if (ctk[i]->heartrate) {
988 heartrate_sum += ctk[i]->heartrate * seg_time;
989 heartrate_sum_time += seg_time;
990 }
991 if (ctk[i]->cadence) {
992 cadence_sum += ctk[i]->cadence * seg_time;
993 cadence_sum_time += seg_time;
994 }
995 }
996 }
997
998 clp->total_time = 0;
999 clp->begin_lat = 0x7fffffff;
1000 clp->begin_lon = 0x7fffffff;
1001 clp->end_lat = 0x7fffffff;
1002 clp->end_lon = 0x7fffffff;
1003 if (ctk_start && ctk_end) {
1004 clp->total_time = (ctk[ctk_end]->Time - ctk[ctk_start]->Time) * 100;
1005 if (!ctk[ctk_start]->no_latlon && !ctk[ctk_end]->no_latlon) {
1006 clp->begin_lat = ctk[ctk_start]->lat;
1007 clp->begin_lon = ctk[ctk_start]->lon;
1008 clp->end_lat = ctk[ctk_end]->lat;
1009 clp->end_lon = ctk[ctk_end]->lon;
1010 }
1011 }
1012 if (heartrate_sum_time) {
1013 clp->avg_heart_rate = heartrate_sum / heartrate_sum_time;
1014 }
1015 if (cadence_sum_time) {
1016 clp->avg_cadence = cadence_sum / cadence_sum_time;
1017 }
1018 }
1019
1020
1021 /* @funcstatic Course_Garbage_Collect **********************************
1022 **
1023 ** Remove duplicate courses, then remove unreferenced laps, tracks and
1024 ** course points from arrays.
1025 **
1026 ** @param [w] crs [GPS_PCourse *] course array
1027 ** @param [w] n_crs [int *] number of course entries
1028 ** @param [w] clp [GPS_PCourse_Lap *] course lap array
1029 ** @param [w] n_clp [int *] number of lap entries
1030 ** @param [w] ctk [GPS_PTrack *] track array
1031 ** @param [w] n_ctk [int *] number of track entries
1032 ** @param [w] cpt [GPS_PCourse_Point *] course point array
1033 ** @param [w] n_cpt [int *] number of course point entries
1034 **
1035 ** @return [void]
1036 ************************************************************************/
1037 static void
Course_Garbage_Collect(GPS_PCourse * crs,int * n_crs,GPS_PCourse_Lap * clp,int * n_clp,GPS_PTrack * ctk,int * n_ctk,GPS_PCourse_Point * cpt,int * n_cpt)1038 Course_Garbage_Collect(GPS_PCourse* crs, int* n_crs,
1039 GPS_PCourse_Lap* clp, int* n_clp,
1040 GPS_PTrack* ctk, int* n_ctk,
1041 GPS_PCourse_Point* cpt, int* n_cpt)
1042 {
1043 int i, j;
1044
1045 /* Remove courses with duplicate names, keeping newest.
1046 * This is actually pretty important: Sending two courses with the same
1047 * name to the device will result in internal data corruption on the
1048 * device (e.g. "inventing" laps with weird course IDs that nobody ever
1049 * transferred to it, that change with every upload of unrelated data
1050 * and that can't be deleted except with a master reset by holding the
1051 * Mode button during power on).
1052 */
1053 restart_courses:
1054 for (i=*n_crs-1; i>0; i--) {
1055 for (j=i-1; j>=0; j--) {
1056 if (!strcmp(crs[i]->course_name, crs[j]->course_name)) {
1057 /* Remove course */
1058 GPS_Course_Del(&crs[j]);
1059 memmove(&crs[j], &crs[j+1], (*n_crs-j-1)*sizeof(*crs));
1060 (*n_crs)--;
1061 goto restart_courses;
1062 }
1063 }
1064 }
1065
1066 /* Remove unreferenced laps */
1067 restart_laps:
1068 for (i=0; i<*n_clp; i++) {
1069 for (j=0; j<*n_crs; j++)
1070 if (crs[j]->index == clp[i]->course_index) {
1071 break;
1072 }
1073 if (j>=*n_crs) {
1074 /* Remove lap */
1075 GPS_Course_Lap_Del(&clp[i]);
1076 memmove(&clp[i], &clp[i+1], (*n_clp-i-1)*sizeof(*clp));
1077 (*n_clp)--;
1078 goto restart_laps;
1079 }
1080 }
1081
1082 /* Remove unreferenced tracks */
1083 restart_tracks:
1084 for (i=0; i<*n_ctk; i++) {
1085 uint32 trk_idx;
1086
1087 if (!ctk[i]->ishdr) {
1088 continue;
1089 }
1090 trk_idx = strtoul(ctk[i]->trk_ident, nullptr, 0);
1091 for (j=0; j<*n_crs; j++)
1092 if (crs[j]->track_index == trk_idx) {
1093 break;
1094 }
1095 if (j>=*n_crs) {
1096 /* Remove track */
1097 for (j=i; j<*n_ctk; j++) {
1098 if (j!=i && ctk[j]->ishdr) {
1099 break;
1100 }
1101 GPS_Track_Del(&ctk[j]);
1102 }
1103 memmove(&ctk[i], &ctk[j], (*n_ctk-j)*sizeof(*ctk));
1104 *(n_ctk) -= j-i;
1105 goto restart_tracks;
1106 }
1107 }
1108
1109 /* Remove unreferenced/duplicate course points */
1110 restart_course_points:
1111 for (i=0; i<*n_cpt; i++) {
1112 /* Check for unreferenced point */
1113 for (j=0; j<*n_crs; j++)
1114 if (crs[j]->index == cpt[i]->course_index) {
1115 break;
1116 }
1117 if (j<*n_crs) {
1118 /* Check for duplicate point */
1119 for (j=0; j<i; j++)
1120 if (cpt[i]->course_index == cpt[j]->course_index &&
1121 cpt[i]->track_point_time == cpt[j]->track_point_time) {
1122 break;
1123 }
1124 if (j>=i) {
1125 continue; /* Referenced & unique */
1126 }
1127 }
1128 /* Remove course point */
1129 GPS_Course_Point_Del(&cpt[i]);
1130 memmove(&cpt[i], &cpt[i+1], (*n_cpt-i-1)*sizeof(*cpt));
1131 (*n_cpt)--;
1132 goto restart_course_points;
1133 }
1134 }
1135
1136
1137 /* @func GPS_Command_Send_Track_As_Course ******************************
1138 **
1139 ** Convert track log to course, then send to GPS. Since sending a course
1140 ** to the device will erase all existing courses regardless of their
1141 ** name or index, we first have to download all courses, merge the new
1142 ** one and then send all courses at once.
1143 **
1144 ** @param [r] port [const char *] serial port
1145 ** @param [r] trk [GPS_PTrack *] track array
1146 ** @param [r] n_trk [int32] number of track entries
1147 ** @param [r] wpt [GPS_PWay *] waypoint array
1148 ** @param [r] n_wpt [int32] number of waypoint entries
1149 **
1150 ** @return [int32] success
1151 ************************************************************************/
1152
GPS_Command_Send_Track_As_Course(const char * port,GPS_PTrack * trk,int32 n_trk,GPS_PWay * wpt,int32 n_wpt,int eraset)1153 int32 GPS_Command_Send_Track_As_Course(const char* port, GPS_PTrack* trk, int32 n_trk,
1154 GPS_PWay* wpt, int32 n_wpt, int eraset)
1155 {
1156 GPS_PCourse* crs = nullptr;
1157 GPS_PCourse_Lap* clp = nullptr;
1158 GPS_PTrack* ctk = nullptr;
1159 GPS_PCourse_Point* cpt = nullptr;
1160 int n_crs, n_clp=0, n_ctk=0, n_cpt=0;
1161 int i, j, trk_end, new_crs, first_new_ctk;
1162 int32 ret;
1163
1164 /* Read existing courses from device */
1165 if (eraset) {
1166 n_crs = 0;
1167 } else {
1168 n_crs = GPS_Command_Get_Course(port, &crs, &clp, &ctk, &cpt, &n_clp, &n_ctk, &n_cpt, nullptr);
1169 if (n_crs < 0) {
1170 return n_crs;
1171 }
1172 }
1173
1174
1175 /* Create new course+lap+track points for each track */
1176 new_crs = n_crs;
1177 for (i=0; i<n_trk; i++) {
1178 if (!trk[i]->ishdr) {
1179 continue;
1180 }
1181
1182 /* Find end of track */
1183 for (trk_end=i; trk_end<n_trk-1; trk_end++)
1184 if (trk[trk_end+1]->ishdr) {
1185 break;
1186 }
1187 if (trk_end==i) {
1188 continue; /* Skip empty track */
1189 }
1190
1191 /* Create & append course */
1192 crs = (struct GPS_SCourse**)xrealloc(crs, (n_crs+1) * sizeof(GPS_PCourse));
1193 crs[n_crs] = GPS_Course_New();
1194 if (!crs[n_crs]) {
1195 if (clp) xfree(clp);
1196 return MEMORY_ERROR;
1197 }
1198
1199 crs[n_crs]->index = Unique_Course_Index(crs, n_crs);
1200 strncpy(crs[n_crs]->course_name, trk[i]->trk_ident,
1201 sizeof(crs[n_crs]->course_name)-1);
1202
1203 crs[n_crs]->track_index = Unique_Track_Index(crs, n_crs);
1204
1205 /* Create & append new lap */
1206 clp = (struct GPS_SCourse_Lap**) xrealloc(clp, (n_clp+1) * sizeof(GPS_PCourse_Lap));
1207 clp[n_clp] = GPS_Course_Lap_New();
1208 if (!clp[n_clp]) {
1209 if (clp) xfree(clp);
1210 return MEMORY_ERROR;
1211 }
1212
1213 clp[n_clp]->course_index = crs[n_crs]->index; /* Index of associated course */
1214 clp[n_clp]->lap_index = 0; /* Lap index, unique per course */
1215 Calculate_Course_Lap_Data(clp[n_clp], trk, i+1, trk_end);
1216 n_crs++;
1217 n_clp++;
1218 }
1219
1220 /* Append new track points */
1221 ctk = (struct GPS_STrack**) xrealloc(ctk, (n_ctk+n_trk) * sizeof(GPS_PTrack));
1222 first_new_ctk = n_ctk;
1223 for (i=0; i<n_trk; i++) {
1224 if (trk[i]->ishdr && (i>=n_trk || trk[i+1]->ishdr)) {
1225 continue;
1226 }
1227
1228 ctk[n_ctk] = GPS_Track_New();
1229 if (!ctk[n_ctk]) {
1230 if (clp) xfree(clp);
1231 return MEMORY_ERROR;
1232 }
1233 *ctk[n_ctk] = *trk[i];
1234
1235 if (trk[i]->ishdr) {
1236 /* Index of new track, must match the track index in associated course */
1237 memset(ctk[n_ctk]->trk_ident, 0, sizeof(ctk[n_ctk]->trk_ident));
1238 sprintf(ctk[n_ctk]->trk_ident, "%u", crs[new_crs]->track_index);
1239 new_crs++;
1240 }
1241 n_ctk++;
1242 }
1243
1244 /* Convert waypoints to course points by searching closest track point &
1245 * append
1246 */
1247 cpt = (struct GPS_SCourse_Point**) xrealloc(cpt, (n_cpt+n_wpt) * sizeof(GPS_PCourse_Point));
1248 for (i=0; i<n_wpt; i++) {
1249 double dist, min_dist = DBL_MAX;
1250 uint32 min_dist_idx = 0, trk_idx = 0, min_dist_trk_idx = 0;
1251
1252 /* Find closest track point */
1253 for (j=first_new_ctk; j<n_ctk; j++) {
1254 if (ctk[j]->ishdr) {
1255 trk_idx = strtoul(ctk[j]->trk_ident, nullptr, 0);
1256 continue;
1257 }
1258 if (wpt[i] == nullptr || ctk[j] == nullptr) {
1259 fatal("Internal error in GPS_Command_Send_Track_As_Course");
1260 }
1261 dist = gcgeodist(wpt[i]->lat, wpt[i]->lon, ctk[j]->lat, ctk[j]->lon);
1262 if (dist < min_dist) {
1263 min_dist = dist;
1264 min_dist_idx = j;
1265 min_dist_trk_idx = trk_idx;
1266 }
1267 }
1268
1269 if (wpt[i] == nullptr) {
1270 fatal("Internal error in GPS_Command_Send_Track_As_Course: no wpt");
1271 }
1272 cpt[i+n_cpt] = GPS_Course_Point_New();
1273 strncpy(cpt[i+n_cpt]->name, wpt[i]->cmnt,
1274 sizeof(cpt[i+n_cpt]->name) - 1);
1275 for (j=0; j<n_crs; j++)
1276 if (crs[j]->track_index == min_dist_trk_idx) {
1277 cpt[i+n_cpt]->course_index = crs[j]->index;
1278 break;
1279 }
1280 cpt[i+n_cpt]->track_point_time = ctk[min_dist_idx]->Time;
1281 cpt[i+n_cpt]->point_type = 0;
1282 }
1283 n_cpt += n_wpt;
1284
1285 /* Remove course data that's no longer needed */
1286 Course_Garbage_Collect(crs, &n_crs, clp, &n_clp, ctk, &n_ctk, cpt, &n_cpt);
1287
1288 /* Finally send courses including new ones to device */
1289 ret = GPS_Command_Send_Course(port, crs, clp, ctk, cpt,
1290 n_crs, n_clp, n_ctk, n_cpt);
1291
1292 for (i=0; i<n_crs; i++) {
1293 GPS_Course_Del(&crs[i]);
1294 }
1295 free(crs);
1296
1297 for (i=0; i<n_clp; i++) {
1298 GPS_Course_Lap_Del(&clp[i]);
1299 }
1300 free(clp);
1301
1302 for (i=0; i<n_ctk; i++) {
1303 GPS_Track_Del(&ctk[i]);
1304 }
1305 free(ctk);
1306
1307 for (i=0; i<n_cpt; i++) {
1308 GPS_Course_Point_Del(&cpt[i]);
1309 }
1310 free(cpt);
1311
1312 return ret;
1313 }
1314
1315 /*Stubs for unimplemented stuff*/
GPS_Command_Get_Workout(const char *,void **,int (*)(int,struct GPS_SWay **))1316 int32 GPS_Command_Get_Workout(const char* /* port */, void** /* lap */, int (* /* cb */)(int, struct GPS_SWay**))
1317 {
1318 return 0;
1319 }
1320
GPS_Command_Get_Fitness_User_Profile(const char *,void **,int (*)(int,struct GPS_SWay **))1321 int32 GPS_Command_Get_Fitness_User_Profile(const char* /* port */, void** /* lap */, int (* /* cb */)(int, struct GPS_SWay**))
1322 {
1323 return 0;
1324 }
1325
GPS_Command_Get_Workout_Limits(const char *,void **,int (*)(int,struct GPS_SWay **))1326 int32 GPS_Command_Get_Workout_Limits(const char* /* port */, void** /* lap */, int (* /* cb */)(int, struct GPS_SWay**))
1327 {
1328 return 0;
1329 }
1330
GPS_Command_Get_Course_Limits(const char *,void **,int (*)(int,struct GPS_SWay **))1331 int32 GPS_Command_Get_Course_Limits(const char* /* port */, void** /* lap */, int (* /* cb */)(int, struct GPS_SWay**))
1332 {
1333 return 0;
1334 }
1335