Contents of /trunk/src/gps.c

Parent Directory Parent Directory | Revision Log Revision Log


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