1/*
2Copyright (c) 2018, Raspberry Pi (Trading) Ltd.
3All rights reserved.
4
5Redistribution and use in source and binary forms, with or without
6modification, are permitted provided that the following conditions are met:
7 * Redistributions of source code must retain the above copyright
8 notice, this list of conditions and the following disclaimer.
9 * Redistributions in binary form must reproduce the above copyright
10 notice, this list of conditions and the following disclaimer in the
11 documentation and/or other materials provided with the distribution.
12 * Neither the name of the copyright holder nor the
13 names of its contributors may be used to endorse or promote products
14 derived from this software without specific prior written permission.
15
16THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26*/
27
28#include <stdio.h>
29#include <stdlib.h>
30#include <ctype.h>
31#include <memory.h>
32#include <errno.h>
33#include <math.h>
34
35#include "RaspiGPS.h"
36
37typedef struct
38{
39 pthread_mutex_t gps_cache_mutex;
40 gpsd_info gpsd;
41 struct gps_data_t gpsdata_cache;
42 time_t last_valid_time;
43 pthread_t gps_reader_thread;
44 int terminated;
45 int gps_reader_thread_ok;
46} GPS_READER_DATA;
47
48static GPS_READER_DATA gps_reader_data;
49
50#define GPS_CACHE_EXPIRY 5 // in seconds
51
52struct gps_data_t *raspi_gps_lock()
53{
54 pthread_mutex_lock(&gps_reader_data.gps_cache_mutex);
55 return &gps_reader_data.gpsdata_cache;
56}
57
58void raspi_gps_unlock()
59{
60 pthread_mutex_unlock(&gps_reader_data.gps_cache_mutex);
61}
62
63static void *gps_reader_process(void *gps_reader_data_ptr)
64{
65 while (!gps_reader_data.terminated)
66 {
67 int ret = 0;
68 int gps_valid = 0;
69
70 gps_reader_data.gpsd.gpsdata.set = 0;
71 gps_reader_data.gpsd.gpsdata.fix.mode = 0;
72 if (connect_gpsd(&gps_reader_data.gpsd) < 0 ||
73 (ret = read_gps_data_once(&gps_reader_data.gpsd)) < 0 )
74 break;
75
76 if (ret > 0 && gps_reader_data.gpsd.gpsdata.online)
77 {
78 if (gps_reader_data.gpsd.gpsdata.fix.mode >= MODE_2D)
79 {
80 // we have GPS fix, copy fresh data to cache
81 gps_valid = 1;
82 time(&gps_reader_data.last_valid_time);
83 pthread_mutex_lock(&gps_reader_data.gps_cache_mutex);
84 memcpy(&gps_reader_data.gpsdata_cache, &gps_reader_data.gpsd.gpsdata,
85 sizeof(struct gps_data_t));
86 pthread_mutex_unlock(&gps_reader_data.gps_cache_mutex);
87 }
88 }
89 if (!gps_valid)
90 {
91 time_t now;
92 time(&now);
93 if (now - gps_reader_data.last_valid_time > GPS_CACHE_EXPIRY)
94 {
95 // our cache is stale, clear it
96 pthread_mutex_lock(&gps_reader_data.gps_cache_mutex);
97 gps_reader_data.gpsdata_cache.online = gps_reader_data.gpsd.gpsdata.online;
98 gps_reader_data.gpsdata_cache.set = 0;
99 gps_reader_data.gpsdata_cache.fix.mode = 0;
100 pthread_mutex_unlock(&gps_reader_data.gps_cache_mutex);
101 }
102 // we lost GPS fix, copy GPS time to cache if available
103 if (gps_reader_data.gpsd.gpsdata.set & TIME_SET)
104 {
105 pthread_mutex_lock(&gps_reader_data.gps_cache_mutex);
106 gps_reader_data.gpsdata_cache.set |= TIME_SET;
107 gps_reader_data.gpsdata_cache.fix.time = gps_reader_data.gpsd.gpsdata.fix.time;
108 pthread_mutex_unlock(&gps_reader_data.gps_cache_mutex);
109 }
110 }
111 }
112 return NULL;
113}
114
115void raspi_gps_shutdown(int verbose)
116{
117 gps_reader_data.terminated = 1;
118
119 if (gps_reader_data.gps_reader_thread_ok)
120 {
121 if (verbose)
122 fprintf(stderr, "Waiting for GPS reader thread to terminate\n");
123
124 pthread_join(gps_reader_data.gps_reader_thread, NULL);
125 }
126 if (verbose && gps_reader_data.gpsd.gpsd_connected)
127 fprintf(stderr, "Closing gpsd connection\n\n");
128
129 disconnect_gpsd(&gps_reader_data.gpsd);
130
131 libgps_unload(&gps_reader_data.gpsd);
132
133 pthread_mutex_destroy(&gps_reader_data.gps_cache_mutex);
134}
135
136int raspi_gps_setup(int verbose)
137{
138 memset(&gps_reader_data, 0, sizeof(gps_reader_data));
139
140 pthread_mutex_init(&gps_reader_data.gps_cache_mutex, NULL);
141
142 gpsd_init(&gps_reader_data.gpsd);
143 if (libgps_load(&gps_reader_data.gpsd))
144 {
145 pthread_mutex_destroy(&gps_reader_data.gps_cache_mutex);
146 fprintf(stderr, "Unable to load the libGPS library");
147 return -1;
148 }
149 if (verbose)
150 fprintf(stderr, "Connecting to gpsd @ %s:%s\n",
151 gps_reader_data.gpsd.server, gps_reader_data.gpsd.port);
152
153 if (connect_gpsd(&gps_reader_data.gpsd))
154 {
155 fprintf(stderr, "no gpsd running or network error: %d, %s\n",
156 errno, gps_reader_data.gpsd.gps_errstr(errno));
157
158 libgps_unload(&gps_reader_data.gpsd);
159
160 pthread_mutex_destroy(&gps_reader_data.gps_cache_mutex);
161
162 return -1;
163 }
164 if (verbose)
165 fprintf(stderr, "Waiting for GPS time\n");
166
167 if (wait_gps_time(&gps_reader_data.gpsd, 2))
168 {
169 if (verbose)
170 fprintf(stderr, "Warning: GPS time not available\n");
171 }
172 if (verbose)
173 fprintf(stderr, "Creating GPS reader thread\n");
174
175 if (pthread_create(&gps_reader_data.gps_reader_thread, NULL,
176 gps_reader_process, &gps_reader_data))
177 {
178 fprintf(stderr, "Error creating GPS reader thread\n");
179 gps_reader_data.terminated = 1;
180 raspi_gps_shutdown(verbose);
181 return -1;
182 }
183
184 gps_reader_data.gps_reader_thread_ok = 1;
185 return 0;
186}
187
188char *raspi_gps_location_string()
189{
190 char lat[24] = {"n/a"};
191 char lon[24] = {"n/a"};
192 char alt[24] = {"Altitude n/a"};
193 char speed[24] = {"Speed n/a"};
194 char track[24] = {"Track n/a"};
195 char datetime[32] = {"Time n/a"};
196 char *text;
197
198 struct gps_data_t *gpsdata = raspi_gps_lock();
199
200 if (gpsdata->set & TIME_SET)
201 {
202 time_t rawtime;
203 struct tm *timeinfo;
204 rawtime = gpsdata->fix.time;
205 timeinfo = localtime(&rawtime);
206 strftime(datetime, sizeof(datetime), "%Y:%m:%d %H:%M:%S", timeinfo);
207 }
208
209 if (gpsdata->online && gpsdata->fix.mode >= MODE_2D)
210 {
211 if (gpsdata->set & LATLON_SET)
212 {
213 if (!isnan(gpsdata->fix.latitude))
214 snprintf(lat, sizeof(lat), "%.6lf", gpsdata->fix.latitude);
215
216 if (!isnan(gpsdata->fix.longitude))
217 snprintf(lon, sizeof(lon), "%.6lf", gpsdata->fix.longitude);
218 }
219
220 if ((gpsdata->set & ALTITUDE_SET) && (gpsdata->fix.mode >= MODE_3D) &&
221 (!isnan(gpsdata->fix.altitude)))
222 {
223 snprintf(alt, sizeof(alt), "Altitude=%.2fm", gpsdata->fix.altitude);
224 }
225
226 if ((gpsdata->set & SPEED_SET) && !isnan(gpsdata->fix.speed))
227 {
228 snprintf(speed, sizeof(speed), "Speed=%.2fkph", gpsdata->fix.speed*MPS_TO_KPH);
229 }
230
231 if ((gpsdata->set & TRACK_SET) && !isnan(gpsdata->fix.track))
232 {
233 snprintf(track, sizeof(track), "Track=%.2f", gpsdata->fix.track);
234 }
235 }
236 raspi_gps_unlock();
237
238 asprintf(&text, "%s Lat %s Long %s %s %s %s", datetime, lat, lon, alt, speed, track);
239
240 return text;
241}
242