Contents of /trunk/src/gps.c

Parent Directory Parent Directory | Revision Log Revision Log


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