Contents of /trunk/src/gps.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 156 - (show annotations)
Wed Apr 1 12:47:35 2009 UTC (15 years, 2 months ago) by harbaum
File MIME type: text/plain
File size: 9815 byte(s)
Track handling redone
1 /*
2 * Copyright (C) 2008 Till Harbaum <till@harbaum.org>.
3 *
4 * This file is part of OSM2Go.
5 *
6 * OSM2Go is free software: you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation, either version 3 of the License, or
9 * (at your option) any later version.
10 *
11 * OSM2Go is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with OSM2Go. If not, see <http://www.gnu.org/licenses/>.
18 */
19
20 #include <stdio.h>
21 #include <string.h>
22 #include <math.h>
23
24 #include "appdata.h"
25
26 #ifdef ENABLE_LIBLOCATION
27
28 #include <location/location-gps-device.h>
29
30 gboolean gps_get_pos(appdata_t *appdata, pos_t *pos, float *alt) {
31 if(!appdata->gps_enabled)
32 return FALSE;
33
34 gps_state_t *gps_state = appdata->gps_state;
35
36 if(!gps_state->fix)
37 return FALSE;
38
39 if(pos) {
40 pos->lat = gps_state->latitude;
41 pos->lon = gps_state->longitude;
42 }
43
44 if(alt)
45 *alt = gps_state->altitude;
46
47 return TRUE;
48 }
49
50 static void
51 location_changed(LocationGPSDevice *device, gps_state_t *gps_state) {
52
53 gps_state->fix =
54 (device->fix->fields & LOCATION_GPS_DEVICE_LATLONG_SET);
55
56 if(gps_state->fix) {
57 gps_state->latitude = device->fix->latitude;
58 gps_state->longitude = device->fix->longitude;
59 }
60
61 if(device->fix->fields & LOCATION_GPS_DEVICE_ALTITUDE_SET)
62 gps_state->altitude = device->fix->altitude;
63 else
64 gps_state->altitude = NAN;
65 }
66
67 void gps_init(appdata_t *appdata) {
68 gps_state_t *gps_state = appdata->gps_state = g_new0(gps_state_t, 1);
69
70 printf("GPS init: Using liblocation\n");
71
72 gps_state->device = g_object_new(LOCATION_TYPE_GPS_DEVICE, NULL);
73 if(!gps_state->device) {
74 printf("Unable to connect to liblocation\n");
75 return;
76 }
77
78 gps_state->idd_changed =
79 g_signal_connect(gps_state->device, "changed",
80 G_CALLBACK(location_changed), gps_state);
81
82 #if MAEMO_VERSION_MAJOR < 5
83 gps_state->control = location_gpsd_control_get_default();
84
85 if(appdata->gps_enabled && gps_state->control &&
86 gps_state->control->can_control) {
87 printf("Having control over GPSD and GPS is to be enabled, starting it\n");
88 location_gpsd_control_start(gps_state->control);
89 }
90 #endif
91 }
92
93 void gps_release(appdata_t *appdata) {
94 gps_state_t *gps_state = appdata->gps_state;
95
96 if(!gps_state->device) return;
97
98 #if MAEMO_VERSION_MAJOR < 5
99 if(gps_state->control && gps_state->control->can_control) {
100 printf("Having control over GPSD, stopping it\n");
101 location_gpsd_control_stop(gps_state->control);
102 }
103 #endif
104
105 /* Disconnect signal */
106 g_signal_handler_disconnect(gps_state->device, gps_state->idd_changed);
107
108 g_free(appdata->gps_state);
109 appdata->gps_state = NULL;
110 }
111
112 void gps_enable(appdata_t *appdata, gboolean enable) {
113 appdata->gps_enabled = enable;
114
115 #if MAEMO_VERSION_MAJOR < 5
116 gps_state_t *gps_state = appdata->gps_state;
117 if(gps_state->control && gps_state->control->can_control) {
118 printf("Having control over GPSD, %sing it\n",
119 enable?"start":"stop");
120
121 if(enable)
122 location_gpsd_control_start(gps_state->control);
123 else
124 location_gpsd_control_stop(gps_state->control);
125 }
126 #endif
127 }
128
129 #else // ENABLE_LIBLOCATION
130
131 #ifdef ENABLE_GPSBT
132 #include <gpsbt.h>
133 #include <gpsmgr.h>
134 #include <errno.h>
135 #endif
136
137 /* maybe user configurable later on ... */
138 #define GPSD_HOST "127.0.0.1"
139 #define GPSD_PORT 2947
140
141 gboolean gps_get_pos(appdata_t *appdata, pos_t *pos, float *alt) {
142 if(pos) pos->lat = NAN;
143
144 g_mutex_lock(appdata->gps_state->mutex);
145 if(appdata->gps_state->gpsdata.set & STATUS_SET) {
146 if(appdata->gps_state->gpsdata.status != STATUS_NO_FIX) {
147 if(appdata->gps_state->gpsdata.set & LATLON_SET)
148 *pos = appdata->gps_state->gpsdata.fix.pos;
149 if(appdata->gps_state->gpsdata.set & ALTITUDE_SET)
150 *alt = appdata->gps_state->gpsdata.fix.alt;
151 }
152 }
153
154 g_mutex_unlock(appdata->gps_state->mutex);
155
156 return(!isnan(pos->lat));
157 }
158
159 static int gps_connect(gps_state_t *gps_state) {
160 GnomeVFSResult vfs_result;
161 #ifdef ENABLE_GPSBT
162 char errstr[256] = "";
163
164 /* We need to start gpsd (via gpsbt) first. */
165 memset(&gps_state->context, 0, sizeof(gpsbt_t));
166 errno = 0;
167
168 if(gpsbt_start(NULL, 0, 0, 0, errstr, sizeof(errstr),
169 0, &gps_state->context) < 0) {
170 printf("Error connecting to GPS receiver: (%d) %s (%s)\n",
171 errno, strerror(errno), errstr);
172 }
173 #endif
174
175 /************** from here down pure gnome/gtk/gpsd ********************/
176
177 /* try to connect to gpsd */
178 /* Create a socket to interact with GPSD. */
179
180 printf("GPSD: trying to connect to %s %d\n", GPSD_HOST, GPSD_PORT);
181
182 int retries = 5;
183 while(retries &&
184 (GNOME_VFS_OK != (vfs_result = gnome_vfs_inet_connection_create(
185 &gps_state->iconn, GPSD_HOST, GPSD_PORT, NULL)))) {
186 printf("Error creating connection to GPSD, retrying ...\n");
187
188 retries--;
189 sleep(1);
190 }
191
192 if(!retries) {
193 printf("Finally failed ...\n");
194 return -1;
195 }
196
197 retries = 5;
198 while(retries && ((gps_state->socket =
199 gnome_vfs_inet_connection_to_socket(gps_state->iconn)) == NULL)) {
200 printf("Error creating connecting GPSD socket, retrying ...\n");
201
202 retries--;
203 sleep(1);
204 }
205
206 if(!retries) {
207 printf("Finally failed ...\n");
208 gnome_vfs_inet_connection_destroy(gps_state->iconn, NULL);
209 return -1;
210 }
211
212 GTimeVal timeout = { 10, 0 };
213 if(GNOME_VFS_OK != (vfs_result = gnome_vfs_socket_set_timeout(
214 gps_state->socket, &timeout, NULL))) {
215 printf("Error setting GPSD timeout\n");
216 gnome_vfs_inet_connection_destroy(gps_state->iconn, NULL);
217 return -1;
218 }
219
220 printf("GPSD connected ...\n");
221
222 return 0;
223 }
224
225 void gps_clear_fix(struct gps_fix_t *fixp) {
226 fixp->mode = MODE_NOT_SEEN;
227 fixp->pos.lat = fixp->pos.lon = NAN;
228 fixp->alt = NAN;
229 fixp->eph = NAN;
230 }
231
232 /* unpack a daemon response into a status structure */
233 static void gps_unpack(char *buf, struct gps_data_t *gpsdata) {
234 char *ns, *sp, *tp;
235
236 for(ns = buf; ns; ns = strstr(ns+1, "GPSD")) {
237 if(strncmp(ns, "GPSD", 4) == 0) {
238 /* the following should execute each time we have a good next sp */
239 for (sp = ns + 5; *sp != '\0'; sp = tp+1) {
240 tp = sp + strcspn(sp, ",\r\n");
241 if (*tp == '\0') tp--;
242 else *tp = '\0';
243
244 switch (*sp) {
245 case 'O':
246 if (sp[2] == '?') {
247 gpsdata->set =
248 (gpsdata->set & SATELLITE_SET) | // fix for below
249 MODE_SET | STATUS_SET; // this clears sat info??
250 gpsdata->status = STATUS_NO_FIX;
251 gps_clear_fix(&gpsdata->fix);
252 } else {
253 struct gps_fix_t nf;
254 char tag[MAXTAGLEN+1], alt[20], eph[20], lat[20], lon[20], mode[2];
255 int st = sscanf(sp+2,
256 "%8s %*s %*s %19s %19s "
257 "%19s %19s %*s %*s %*s %*s "
258 "%*s %*s %*s %1s",
259 tag, lat, lon,
260 alt, eph,
261 mode);
262 if (st >= 5) {
263 #define DEFAULT(val) (val[0] == '?') ? NAN : g_ascii_strtod(val, NULL)
264 nf.pos.lat = DEFAULT(lat);
265 nf.pos.lon = DEFAULT(lon);
266 nf.eph = DEFAULT(eph);
267 nf.alt = DEFAULT(alt);
268 #undef DEFAULT
269 if (st >= 6)
270 nf.mode = (mode[0] == '?') ? MODE_NOT_SEEN : atoi(mode);
271 else
272 nf.mode = (alt[0] == '?') ? MODE_2D : MODE_3D;
273 gpsdata->fix = nf;
274 gpsdata->set |= LATLON_SET|MODE_SET;
275 gpsdata->status = STATUS_FIX;
276 gpsdata->set |= STATUS_SET;
277
278 if(alt[0] != '?')
279 gpsdata->set |= ALTITUDE_SET;
280 }
281 }
282 break;
283 }
284 }
285 }
286 }
287 }
288
289 void gps_enable(appdata_t *appdata, gboolean enable) {
290 appdata->gps_enabled = enable;
291 }
292
293 gpointer gps_thread(gpointer data) {
294 GnomeVFSFileSize bytes_read;
295 GnomeVFSResult vfs_result;
296 char str[512];
297 appdata_t *appdata = (appdata_t*)data;
298
299 const char *msg = "o\r\n"; /* pos request */
300
301 appdata->gps_state->gpsdata.set = 0;
302
303 gboolean connected = FALSE;
304
305 while(1) {
306 if(appdata->gps_enabled) {
307 if(!connected) {
308 printf("trying to connect\n");
309
310 if(gps_connect(appdata->gps_state) < 0)
311 sleep(10);
312 else
313 connected = TRUE;
314 } else {
315 if(GNOME_VFS_OK ==
316 (vfs_result = gnome_vfs_socket_write(appdata->gps_state->socket,
317 msg, strlen(msg)+1, &bytes_read, NULL))) {
318
319 /* update every second, wait here to make sure a complete */
320 /* reply is received */
321 sleep(1);
322
323 if(bytes_read == (strlen(msg)+1)) {
324 vfs_result = gnome_vfs_socket_read(appdata->gps_state->socket,
325 str, sizeof(str)-1, &bytes_read, NULL);
326 if(vfs_result == GNOME_VFS_OK) {
327 str[bytes_read] = 0;
328
329 printf("msg: %s (%d)\n", str, strlen(str));
330
331 g_mutex_lock(appdata->gps_state->mutex);
332
333 appdata->gps_state->gpsdata.set &=
334 ~(LATLON_SET|MODE_SET|STATUS_SET);
335
336 gps_unpack(str, &appdata->gps_state->gpsdata);
337 g_mutex_unlock(appdata->gps_state->mutex);
338 }
339 }
340 }
341 }
342 } else {
343 if(connected) {
344 printf("stopping GPS connection due to user request\n");
345 gnome_vfs_inet_connection_destroy(appdata->gps_state->iconn, NULL);
346
347 #ifdef ENABLE_GPSBT
348 gpsbt_stop(&appdata->gps_state->context);
349 #endif
350 connected = FALSE;
351 } else
352 sleep(1);
353 }
354 }
355
356 printf("GPS thread ended???\n");
357 return NULL;
358 }
359
360 void gps_init(appdata_t *appdata) {
361 appdata->gps_state = g_new0(gps_state_t, 1);
362
363 printf("GPS init: Using gpsd\n");
364
365 /* start a new thread to listen to gpsd */
366 appdata->gps_state->mutex = g_mutex_new();
367 appdata->gps_state->thread_p =
368 g_thread_create(gps_thread, appdata, FALSE, NULL);
369 }
370
371 void gps_release(appdata_t *appdata) {
372 #ifdef ENABLE_GPSBT
373 gpsbt_stop(&appdata->gps_state->context);
374 #endif
375 g_free(appdata->gps_state);
376 appdata->gps_state = NULL;
377 }
378
379 #endif // ENABLE_LIBLOCATION