Contents of /trunk/src/gps.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 186 - (hide annotations)
Fri Jul 3 14:07:27 2009 UTC (14 years, 11 months ago) by harbaum
File MIME type: text/plain
File size: 9423 byte(s)
Wizard tests
1 harbaum 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 harbaum 118 #ifdef ENABLE_LIBLOCATION
27    
28     #include <location/location-gps-device.h>
29    
30 harbaum 156 gboolean gps_get_pos(appdata_t *appdata, pos_t *pos, float *alt) {
31 harbaum 144 if(!appdata->gps_enabled)
32 harbaum 156 return FALSE;
33 harbaum 144
34 harbaum 118 gps_state_t *gps_state = appdata->gps_state;
35    
36     if(!gps_state->fix)
37 harbaum 156 return FALSE;
38 harbaum 118
39 harbaum 156 if(pos) {
40     pos->lat = gps_state->latitude;
41     pos->lon = gps_state->longitude;
42     }
43 harbaum 118
44 harbaum 156 if(alt)
45     *alt = gps_state->altitude;
46    
47     return TRUE;
48 harbaum 118 }
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 harbaum 156
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 harbaum 118 }
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 harbaum 124 #if MAEMO_VERSION_MAJOR < 5
83 harbaum 118 gps_state->control = location_gpsd_control_get_default();
84 harbaum 144
85 harbaum 186 if(gps_state->control && gps_state->control->can_control) {
86 harbaum 144 printf("Having control over GPSD and GPS is to be enabled, starting it\n");
87 harbaum 118 location_gpsd_control_start(gps_state->control);
88     }
89 harbaum 124 #endif
90 harbaum 118 }
91    
92     void gps_release(appdata_t *appdata) {
93     gps_state_t *gps_state = appdata->gps_state;
94    
95     if(!gps_state->device) return;
96    
97 harbaum 124 #if MAEMO_VERSION_MAJOR < 5
98 harbaum 144 if(gps_state->control && gps_state->control->can_control) {
99 harbaum 118 printf("Having control over GPSD, stopping it\n");
100     location_gpsd_control_stop(gps_state->control);
101     }
102 harbaum 124 #endif
103 harbaum 118
104 harbaum 144 /* Disconnect signal */
105 harbaum 118 g_signal_handler_disconnect(gps_state->device, gps_state->idd_changed);
106    
107     g_free(appdata->gps_state);
108     appdata->gps_state = NULL;
109     }
110    
111 harbaum 144 void gps_enable(appdata_t *appdata, gboolean enable) {
112     appdata->gps_enabled = enable;
113     }
114    
115 harbaum 118 #else // ENABLE_LIBLOCATION
116    
117 harbaum 112 #ifdef ENABLE_GPSBT
118 harbaum 1 #include <gpsbt.h>
119     #include <gpsmgr.h>
120     #include <errno.h>
121     #endif
122    
123     /* maybe user configurable later on ... */
124     #define GPSD_HOST "127.0.0.1"
125     #define GPSD_PORT 2947
126    
127 harbaum 156 gboolean gps_get_pos(appdata_t *appdata, pos_t *pos, float *alt) {
128     if(pos) pos->lat = NAN;
129 harbaum 1
130     g_mutex_lock(appdata->gps_state->mutex);
131 harbaum 156 if(appdata->gps_state->gpsdata.set & STATUS_SET) {
132     if(appdata->gps_state->gpsdata.status != STATUS_NO_FIX) {
133 harbaum 1 if(appdata->gps_state->gpsdata.set & LATLON_SET)
134 harbaum 156 *pos = appdata->gps_state->gpsdata.fix.pos;
135     if(appdata->gps_state->gpsdata.set & ALTITUDE_SET)
136     *alt = appdata->gps_state->gpsdata.fix.alt;
137     }
138     }
139    
140 harbaum 1 g_mutex_unlock(appdata->gps_state->mutex);
141 harbaum 156
142     return(!isnan(pos->lat));
143 harbaum 1 }
144    
145     static int gps_connect(gps_state_t *gps_state) {
146     GnomeVFSResult vfs_result;
147 harbaum 112 #ifdef ENABLE_GPSBT
148 harbaum 1 char errstr[256] = "";
149    
150     /* We need to start gpsd (via gpsbt) first. */
151     memset(&gps_state->context, 0, sizeof(gpsbt_t));
152     errno = 0;
153    
154     if(gpsbt_start(NULL, 0, 0, 0, errstr, sizeof(errstr),
155     0, &gps_state->context) < 0) {
156     printf("Error connecting to GPS receiver: (%d) %s (%s)\n",
157     errno, strerror(errno), errstr);
158     }
159     #endif
160    
161     /************** from here down pure gnome/gtk/gpsd ********************/
162    
163     /* try to connect to gpsd */
164     /* Create a socket to interact with GPSD. */
165    
166 harbaum 156 printf("GPSD: trying to connect to %s %d\n", GPSD_HOST, GPSD_PORT);
167    
168 harbaum 1 int retries = 5;
169     while(retries &&
170     (GNOME_VFS_OK != (vfs_result = gnome_vfs_inet_connection_create(
171     &gps_state->iconn, GPSD_HOST, GPSD_PORT, NULL)))) {
172     printf("Error creating connection to GPSD, retrying ...\n");
173    
174     retries--;
175     sleep(1);
176     }
177    
178     if(!retries) {
179     printf("Finally failed ...\n");
180     return -1;
181     }
182    
183     retries = 5;
184     while(retries && ((gps_state->socket =
185     gnome_vfs_inet_connection_to_socket(gps_state->iconn)) == NULL)) {
186     printf("Error creating connecting GPSD socket, retrying ...\n");
187    
188     retries--;
189     sleep(1);
190     }
191    
192     if(!retries) {
193     printf("Finally failed ...\n");
194     gnome_vfs_inet_connection_destroy(gps_state->iconn, NULL);
195     return -1;
196     }
197    
198     GTimeVal timeout = { 10, 0 };
199     if(GNOME_VFS_OK != (vfs_result = gnome_vfs_socket_set_timeout(
200     gps_state->socket, &timeout, NULL))) {
201     printf("Error setting GPSD timeout\n");
202     gnome_vfs_inet_connection_destroy(gps_state->iconn, NULL);
203     return -1;
204     }
205    
206     printf("GPSD connected ...\n");
207    
208     return 0;
209     }
210    
211     void gps_clear_fix(struct gps_fix_t *fixp) {
212     fixp->mode = MODE_NOT_SEEN;
213     fixp->pos.lat = fixp->pos.lon = NAN;
214 harbaum 156 fixp->alt = NAN;
215 harbaum 1 fixp->eph = NAN;
216     }
217    
218     /* unpack a daemon response into a status structure */
219     static void gps_unpack(char *buf, struct gps_data_t *gpsdata) {
220     char *ns, *sp, *tp;
221    
222     for(ns = buf; ns; ns = strstr(ns+1, "GPSD")) {
223     if(strncmp(ns, "GPSD", 4) == 0) {
224     /* the following should execute each time we have a good next sp */
225     for (sp = ns + 5; *sp != '\0'; sp = tp+1) {
226     tp = sp + strcspn(sp, ",\r\n");
227     if (*tp == '\0') tp--;
228     else *tp = '\0';
229    
230     switch (*sp) {
231     case 'O':
232     if (sp[2] == '?') {
233     gpsdata->set =
234     (gpsdata->set & SATELLITE_SET) | // fix for below
235     MODE_SET | STATUS_SET; // this clears sat info??
236     gpsdata->status = STATUS_NO_FIX;
237     gps_clear_fix(&gpsdata->fix);
238     } else {
239     struct gps_fix_t nf;
240     char tag[MAXTAGLEN+1], alt[20], eph[20], lat[20], lon[20], mode[2];
241     int st = sscanf(sp+2,
242     "%8s %*s %*s %19s %19s "
243     "%19s %19s %*s %*s %*s %*s "
244     "%*s %*s %*s %1s",
245     tag, lat, lon,
246     alt, eph,
247     mode);
248     if (st >= 5) {
249     #define DEFAULT(val) (val[0] == '?') ? NAN : g_ascii_strtod(val, NULL)
250     nf.pos.lat = DEFAULT(lat);
251     nf.pos.lon = DEFAULT(lon);
252     nf.eph = DEFAULT(eph);
253 harbaum 156 nf.alt = DEFAULT(alt);
254 harbaum 1 #undef DEFAULT
255     if (st >= 6)
256     nf.mode = (mode[0] == '?') ? MODE_NOT_SEEN : atoi(mode);
257     else
258     nf.mode = (alt[0] == '?') ? MODE_2D : MODE_3D;
259     gpsdata->fix = nf;
260     gpsdata->set |= LATLON_SET|MODE_SET;
261     gpsdata->status = STATUS_FIX;
262     gpsdata->set |= STATUS_SET;
263 harbaum 156
264     if(alt[0] != '?')
265     gpsdata->set |= ALTITUDE_SET;
266 harbaum 1 }
267     }
268     break;
269     }
270     }
271     }
272     }
273     }
274    
275 harbaum 144 void gps_enable(appdata_t *appdata, gboolean enable) {
276     appdata->gps_enabled = enable;
277     }
278    
279 harbaum 1 gpointer gps_thread(gpointer data) {
280     GnomeVFSFileSize bytes_read;
281     GnomeVFSResult vfs_result;
282     char str[512];
283     appdata_t *appdata = (appdata_t*)data;
284    
285     const char *msg = "o\r\n"; /* pos request */
286    
287     appdata->gps_state->gpsdata.set = 0;
288    
289     gboolean connected = FALSE;
290    
291     while(1) {
292     if(appdata->gps_enabled) {
293     if(!connected) {
294     printf("trying to connect\n");
295    
296     if(gps_connect(appdata->gps_state) < 0)
297     sleep(10);
298     else
299     connected = TRUE;
300     } else {
301     if(GNOME_VFS_OK ==
302     (vfs_result = gnome_vfs_socket_write(appdata->gps_state->socket,
303     msg, strlen(msg)+1, &bytes_read, NULL))) {
304    
305     /* update every second, wait here to make sure a complete */
306     /* reply is received */
307     sleep(1);
308    
309     if(bytes_read == (strlen(msg)+1)) {
310     vfs_result = gnome_vfs_socket_read(appdata->gps_state->socket,
311     str, sizeof(str)-1, &bytes_read, NULL);
312     if(vfs_result == GNOME_VFS_OK) {
313     str[bytes_read] = 0;
314    
315     printf("msg: %s (%d)\n", str, strlen(str));
316    
317     g_mutex_lock(appdata->gps_state->mutex);
318    
319     appdata->gps_state->gpsdata.set &=
320     ~(LATLON_SET|MODE_SET|STATUS_SET);
321    
322     gps_unpack(str, &appdata->gps_state->gpsdata);
323     g_mutex_unlock(appdata->gps_state->mutex);
324     }
325     }
326     }
327     }
328     } else {
329     if(connected) {
330     printf("stopping GPS connection due to user request\n");
331     gnome_vfs_inet_connection_destroy(appdata->gps_state->iconn, NULL);
332    
333 harbaum 112 #ifdef ENABLE_GPSBT
334 harbaum 1 gpsbt_stop(&appdata->gps_state->context);
335     #endif
336     connected = FALSE;
337     } else
338     sleep(1);
339     }
340     }
341    
342     printf("GPS thread ended???\n");
343     return NULL;
344     }
345    
346     void gps_init(appdata_t *appdata) {
347     appdata->gps_state = g_new0(gps_state_t, 1);
348    
349 harbaum 118 printf("GPS init: Using gpsd\n");
350    
351 harbaum 1 /* start a new thread to listen to gpsd */
352     appdata->gps_state->mutex = g_mutex_new();
353     appdata->gps_state->thread_p =
354     g_thread_create(gps_thread, appdata, FALSE, NULL);
355     }
356    
357     void gps_release(appdata_t *appdata) {
358 harbaum 112 #ifdef ENABLE_GPSBT
359 harbaum 1 gpsbt_stop(&appdata->gps_state->context);
360     #endif
361     g_free(appdata->gps_state);
362 harbaum 118 appdata->gps_state = NULL;
363 harbaum 1 }
364 harbaum 118
365     #endif // ENABLE_LIBLOCATION