Contents of /trunk/src/gps.c

Parent Directory Parent Directory | Revision Log Revision Log

Revision 7 - (show annotations)
Thu Jun 25 15:24:24 2009 UTC (14 years, 10 months ago) by harbaum
File MIME type: text/plain
File size: 15563 byte(s) 
Translations
1 /*
2 * Copyright (C) 2008 Till Harbaum <till@harbaum.org>.
3 *
4 * This file is part of GPXView.
5 *
6 * GPXView 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 * GPXView 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 GPXView. 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 "gpxview.h"
25
26
27 #ifdef USE_MAEMO
28 #ifdef ENABLE_GPSBT
29 #include <gpsbt.h>
30 #include <gpsmgr.h>
31 #endif
32 #include <errno.h>
33 #endif
34
35 #ifndef ENABLE_LIBLOCATION
36
37 /* maybe user configurable later on ... */
38 #define GPSD_HOST "127.0.0.1"
39 #define GPSD_PORT 2947
40
41 gps_sat_t *gps_get_sats(appdata_t *appdata) {
42 gps_sat_t *retval = NULL;
43
44 g_mutex_lock(appdata->gps_state->mutex);
45 if(appdata->gps_state->gpsdata.set & SATELLITE_SET) {
46 int i;
47 retval = malloc(sizeof(gps_sat_t));
48 retval->num = appdata->gps_state->gpsdata.satellites;
49
50 retval->PRN = malloc(sizeof(int)*retval->num);
51 retval->used = malloc(sizeof(int)*retval->num);
52 retval->ss = malloc(sizeof(int)*retval->num);
53
54 if(retval->num) {
55 for(i=0;i<retval->num;i++) {
56 retval->PRN[i] = appdata->gps_state->gpsdata.PRN[i];
57 retval->ss[i] = appdata->gps_state->gpsdata.ss[i];
58 retval->used[i] = appdata->gps_state->gpsdata.used[i];
59 }
60 }
61 }
62
63 g_mutex_unlock(appdata->gps_state->mutex);
64
65 return retval;
66 }
67
68 pos_t *gps_get_pos(appdata_t *appdata) {
69 static pos_t retval;
70
71 retval.lat = NAN;
72
73 g_mutex_lock(appdata->gps_state->mutex);
74 if(appdata->gps_state->gpsdata.set & STATUS_SET)
75 if(appdata->gps_state->gpsdata.status != STATUS_NO_FIX)
76 if(appdata->gps_state->gpsdata.set & LATLON_SET)
77 retval = appdata->gps_state->gpsdata.fix.pos;
78
79 g_mutex_unlock(appdata->gps_state->mutex);
80
81 if(isnan(retval.lat))
82 return NULL;
83
84 return &retval;
85 }
86
87 float gps_get_heading(appdata_t *appdata) {
88 float retval = NAN;
89
90 g_mutex_lock(appdata->gps_state->mutex);
91 if(appdata->gps_state->gpsdata.set & STATUS_SET)
92 if(appdata->gps_state->gpsdata.status != STATUS_NO_FIX)
93 if(appdata->gps_state->gpsdata.set & TRACK_SET)
94 retval = appdata->gps_state->gpsdata.fix.track;
95
96 g_mutex_unlock(appdata->gps_state->mutex);
97 return retval;
98 }
99
100 float gps_get_epe(appdata_t *appdata) {
101 float retval = NAN;
102
103 g_mutex_lock(appdata->gps_state->mutex);
104 if(appdata->gps_state->gpsdata.set & STATUS_SET)
105 if(appdata->gps_state->gpsdata.status != STATUS_NO_FIX)
106 retval = appdata->gps_state->gpsdata.fix.eph;
107
108 g_mutex_unlock(appdata->gps_state->mutex);
109 return retval;
110 }
111
112 static int gps_connect(gps_state_t *gps_state) {
113 GnomeVFSResult vfs_result;
114 #ifdef USE_MAEMO
115 char errstr[256] = "";
116
117 /* We need to start gpsd (via gpsbt) first. */
118 memset(&gps_state->context, 0, sizeof(gpsbt_t));
119 errno = 0;
120
121 if(gpsbt_start(NULL, 0, 0, 0, errstr, sizeof(errstr),
122 0, &gps_state->context) < 0) {
123 printf("Error connecting to GPS receiver: (%d) %s (%s)\n",
124 errno, strerror(errno), errstr);
125 }
126 #endif
127
128 /************** from here down pure gnome/gtk/gpsd ********************/
129
130 /* try to connect to gpsd */
131 /* Create a socket to interact with GPSD. */
132
133 int retries = 5;
134 while(retries &&
135 (GNOME_VFS_OK != (vfs_result = gnome_vfs_inet_connection_create(
136 &gps_state->iconn, GPSD_HOST, GPSD_PORT, NULL)))) {
137 printf("Error creating connection to GPSD, retrying ...\n");
138
139 retries--;
140 sleep(1);
141 }
142
143 if(!retries) {
144 printf("Finally failed ...\n");
145 return -1;
146 }
147
148 retries = 5;
149 while(retries && ((gps_state->socket =
150 gnome_vfs_inet_connection_to_socket(gps_state->iconn)) == NULL)) {
151 printf("Error creating connecting GPSD socket, retrying ...\n");
152
153 retries--;
154 sleep(1);
155 }
156
157 if(!retries) {
158 printf("Finally failed ...\n");
159 return -1;
160 }
161
162 GTimeVal timeout = { 10, 0 };
163 if(GNOME_VFS_OK != (vfs_result = gnome_vfs_socket_set_timeout(
164 gps_state->socket, &timeout, NULL))) {
165 printf("Error setting GPSD timeout\n");
166 return -1;
167 }
168
169 printf("GPSD connected ...\n");
170
171 return 0;
172 }
173
174 void gps_clear_fix(struct gps_fix_t *fixp) {
175 fixp->mode = MODE_NOT_SEEN;
176 fixp->pos.lat = fixp->pos.lon = NAN;
177 fixp->track = NAN;
178 fixp->speed = NAN;
179 fixp->climb = NAN;
180 fixp->altitude = NAN;
181 fixp->ept = NAN;
182 fixp->eph = NAN;
183 fixp->epv = NAN;
184 fixp->epd = NAN;
185 fixp->eps = NAN;
186 fixp->epc = NAN;
187 }
188
189 /* unpack a daemon response into a status structure */
190 static void gps_unpack(char *buf, struct gps_data_t *gpsdata) {
191 char *ns, *sp, *tp;
192
193 for(ns = buf; ns; ns = strstr(ns+1, "GPSD")) {
194 if(strncmp(ns, "GPSD", 4) == 0) {
195 /* the following should execute each time we have a good next sp */
196 for (sp = ns + 5; *sp != '\0'; sp = tp+1) {
197 tp = sp + strcspn(sp, ",\r\n");
198 if (*tp == '\0') tp--;
199 else *tp = '\0';
200
201 switch (*sp) {
202 case 'A':
203 if (sp[2] == '?') {
204 gpsdata->fix.altitude = NAN;
205 } else {
206 (void)sscanf(sp, "A=%lf", &gpsdata->fix.altitude);
207 gpsdata->set |= ALTITUDE_SET;
208 }
209 break;
210 /* B - baudrate isn't supported by gpxview */
211 /* C - cycle isn't supported by gpxview */
212 /* D - utc time isn't supported by gpxview */
213 case 'E':
214 gpsdata->epe = gpsdata->fix.eph = gpsdata->fix.epv = NAN;
215 /* epe should always be present if eph or epv is */
216 if (sp[2] != '?') {
217 char epe[20], eph[20], epv[20];
218 (void)sscanf(sp, "E=%s %s %s", epe, eph, epv);
219 #define DEFAULT(val) (val[0] == '?') ? NAN : g_ascii_strtod(val, NULL)
220 gpsdata->epe = DEFAULT(epe);
221 gpsdata->fix.eph = DEFAULT(eph);
222 gpsdata->fix.epv = DEFAULT(epv);
223 #undef DEFAULT
224 }
225 break;
226 /* F - device name isn't supported by gpxview */
227 /* I - gps id isn't supported by gpxview */
228 /* K - known devices list isn't supported by gpxview */
229 case 'M':
230 if (sp[2] == '?') {
231 gpsdata->fix.mode = MODE_NOT_SEEN;
232 } else {
233 gpsdata->fix.mode = atoi(sp+2);
234 gpsdata->set |= MODE_SET;
235 }
236 break;
237 /* N - driver mode reporting isn't supported by gpxview */
238 case 'O':
239 if (sp[2] == '?') {
240 gpsdata->set =
241 (gpsdata->set & SATELLITE_SET) | // fix for below
242 MODE_SET | STATUS_SET; // this clears sat info??
243 gpsdata->status = STATUS_NO_FIX;
244 gps_clear_fix(&gpsdata->fix);
245 } else {
246 struct gps_fix_t nf;
247 char tag[MAXTAGLEN+1], alt[20];
248 char eph[20], epv[20], track[20],speed[20], climb[20];
249 char epd[20], eps[20], epc[20], mode[2];
250 char timestr[20], ept[20], lat[20], lon[20];
251 int st = sscanf(sp+2,
252 "%8s %19s %19s %19s %19s %19s %19s %19s %19s %19s %19s %19s %19s %19s %1s",
253 tag, timestr, ept, lat, lon,
254 alt, eph, epv, track, speed, climb,
255 epd, eps, epc, mode);
256 if (st >= 14) {
257 #define DEFAULT(val) (val[0] == '?') ? NAN : g_ascii_strtod(val, NULL)
258 nf.pos.lat = DEFAULT(lat);
259 nf.pos.lon = DEFAULT(lon);
260 nf.ept = DEFAULT(ept);
261 nf.altitude = DEFAULT(alt);
262 nf.eph = DEFAULT(eph);
263 nf.epv = DEFAULT(epv);
264 nf.track = DEFAULT(track);
265 nf.speed = DEFAULT(speed);
266 nf.climb = DEFAULT(climb);
267 nf.epd = DEFAULT(epd);
268 nf.eps = DEFAULT(eps);
269 nf.epc = DEFAULT(epc);
270 #undef DEFAULT
271 if (st >= 15)
272 nf.mode = (mode[0] == '?') ? MODE_NOT_SEEN : atoi(mode);
273 else
274 nf.mode = (alt[0] == '?') ? MODE_2D : MODE_3D;
275 if (alt[0] != '?')
276 gpsdata->set |= ALTITUDE_SET | CLIMB_SET;
277 if (isnan(nf.eph)==0)
278 gpsdata->set |= HERR_SET;
279 if (isnan(nf.epv)==0)
280 gpsdata->set |= VERR_SET;
281 if (isnan(nf.track)==0)
282 gpsdata->set |= TRACK_SET | SPEED_SET;
283 if (isnan(nf.eps)==0)
284 gpsdata->set |= SPEEDERR_SET;
285 if (isnan(nf.epc)==0)
286 gpsdata->set |= CLIMBERR_SET;
287 gpsdata->fix = nf;
288 gpsdata->set |= TIME_SET|TIMERR_SET|LATLON_SET|MODE_SET;
289 gpsdata->status = STATUS_FIX;
290 gpsdata->set |= STATUS_SET;
291 }
292 }
293 break;
294 case 'P':
295 if (sp[2] == '?') {
296 gpsdata->fix.pos.lat = NAN;
297 gpsdata->fix.pos.lon = NAN;
298 } else {
299 char lat[20], lon[20];
300 (void)sscanf(sp, "P=%19s %19s", lat, lon);
301 gpsdata->fix.pos.lat = g_ascii_strtod(lat, NULL);
302 gpsdata->fix.pos.lon = g_ascii_strtod(lon, NULL);
303 gpsdata->set |= LATLON_SET;
304 }
305 break;
306 case 'Q':
307 if (sp[2] == '?') {
308 gpsdata->satellites_used = 0;
309 gpsdata->pdop = 0;
310 gpsdata->hdop = 0;
311 gpsdata->vdop = 0;
312 } else {
313 (void)sscanf(sp, "Q=%d %lf %lf %lf %lf %lf",
314 &gpsdata->satellites_used,
315 &gpsdata->pdop,
316 &gpsdata->hdop,
317 &gpsdata->vdop,
318 &gpsdata->tdop,
319 &gpsdata->gdop);
320 gpsdata->set |= HDOP_SET | VDOP_SET | PDOP_SET;
321 }
322 break;
323 case 'S':
324 if (sp[2] == '?') {
325 gpsdata->status = -1;
326 } else {
327 gpsdata->status = atoi(sp+2);
328 gpsdata->set |= STATUS_SET;
329 }
330 break;
331 case 'T':
332 if (sp[2] == '?') {
333 gpsdata->fix.track = NAN;
334 } else {
335 (void)sscanf(sp, "T=%lf", &gpsdata->fix.track);
336 gpsdata->set |= TRACK_SET;
337 }
338 break;
339 case 'U':
340 if (sp[2] == '?') {
341 gpsdata->fix.climb = NAN;
342 } else {
343 (void)sscanf(sp, "U=%lf", &gpsdata->fix.climb);
344 gpsdata->set |= CLIMB_SET;
345 }
346 break;
347 case 'V':
348 if (sp[2] == '?') {
349 gpsdata->fix.speed = NAN;
350 } else {
351 (void)sscanf(sp, "V=%lf", &gpsdata->fix.speed);
352 /* V reply is in kt, fix.speed is in metres/sec */
353 gpsdata->fix.speed = gpsdata->fix.speed / MPS_TO_KNOTS;
354 gpsdata->set |= SPEED_SET;
355 }
356 break;
357 case 'X':
358 if (sp[2] == '?')
359 gpsdata->online = -1;
360 else {
361 (void)sscanf(sp, "X=%lf", &gpsdata->online);
362 gpsdata->set |= ONLINE_SET;
363 }
364 break;
365 case 'Y':
366 if (sp[2] == '?') {
367 gpsdata->satellites = 0;
368 } else {
369 int j, i1, i2, i3, i4, i5;
370 int PRN[MAXCHANNELS];
371 int elevation[MAXCHANNELS], azimuth[MAXCHANNELS];
372 int ss[MAXCHANNELS], used[MAXCHANNELS];
373 char tag[MAXTAGLEN+1], timestamp[21];
374
375 (void)sscanf(sp, "Y=%8s %20s %d ",
376 tag, timestamp, &gpsdata->satellites);
377 for (j = 0; j < gpsdata->satellites; j++) {
378 PRN[j]=elevation[j]=azimuth[j]=ss[j]=used[j]=0;
379 }
380 // printf("sats = %d\n", gpsdata->satellites);
381 for (j = 0, gpsdata->satellites_used = 0; j < gpsdata->satellites; j++) {
382 if ((sp != NULL) && ((sp = strchr(sp, ':')) != NULL)) {
383 sp++;
384 (void)sscanf(sp, "%d %d %d %d %d", &i1, &i2, &i3, &i4, &i5);
385 PRN[j] = i1;
386 elevation[j] = i2; azimuth[j] = i3;
387 ss[j] = i4; used[j] = i5;
388 if (i5 == 1)
389 gpsdata->satellites_used++;
390 }
391 }
392 memcpy(gpsdata->PRN, PRN, sizeof(PRN));
393 memcpy(gpsdata->elevation, elevation, sizeof(elevation));
394 memcpy(gpsdata->azimuth, azimuth,sizeof(azimuth));
395 memcpy(gpsdata->ss, ss, sizeof(ss));
396 memcpy(gpsdata->used, used, sizeof(used));
397 /*@ +compdef @*/
398 }
399 gpsdata->set |= SATELLITE_SET;
400 break;
401 /* Z and $ - profiling isn't supported by gpxview */
402 }
403 }
404 }
405 }
406 }
407
408 gpointer gps_thread(gpointer data) {
409 GnomeVFSFileSize bytes_read;
410 GnomeVFSResult vfs_result;
411 char str[512];
412 appdata_t *appdata = (appdata_t*)data;
413 int cnt=1000;
414
415 const char *msg_pos = "o\r\n"; /* pos request */
416 const char *msg_sat = "y\r\n"; /* sat request */
417
418 appdata->gps_state->gpsdata.set = 0;
419
420 gboolean connected = FALSE;
421
422 while(1) {
423 if(appdata->use_gps) {
424 if(!connected) {
425 printf("trying to connect\n");
426
427 if(gps_connect(appdata->gps_state) < 0)
428 sleep(10);
429 else
430 connected = TRUE;
431 } else {
432 const char *msg;
433 if(!cnt) msg = msg_sat;
434 else msg = msg_pos;
435
436 if(GNOME_VFS_OK ==
437 (vfs_result = gnome_vfs_socket_write(appdata->gps_state->socket,
438 msg, strlen(msg)+1, &bytes_read, NULL))) {
439
440 /* update every second, wait here to make sure a complete */
441 /* reply is received */
442 if(cnt <= 1) usleep(500000);
443 else sleep(1);
444
445 if(bytes_read == (strlen(msg)+1)) {
446 vfs_result = gnome_vfs_socket_read(appdata->gps_state->socket,
447 str, sizeof(str)-1, &bytes_read, NULL);
448 if(vfs_result == GNOME_VFS_OK) {
449 str[bytes_read] = 0;
450
451 // printf("msg: %s (%d)\n", str, strlen(str));
452
453 g_mutex_lock(appdata->gps_state->mutex);
454
455 if(!cnt) appdata->gps_state->gpsdata.set &= ~SATELLITE_SET;
456 else appdata->gps_state->gpsdata.set &=
457 ~(TIME_SET|TIMERR_SET|LATLON_SET|MODE_SET|STATUS_SET);
458
459 gps_unpack(str, &appdata->gps_state->gpsdata);
460 g_mutex_unlock(appdata->gps_state->mutex);
461 }
462 }
463 }
464 if(cnt++ >= 5) cnt = 0;
465 }
466 } else {
467 if(connected) {
468 printf("stopping GPS connection due to user request\n");
469 gnome_vfs_inet_connection_destroy(appdata->gps_state->iconn, NULL);
470
471 #ifdef USE_MAEMO
472 gpsbt_stop(&appdata->gps_state->context);
473 #endif
474 connected = FALSE;
475 } else
476 sleep(1);
477 }
478 }
479
480 printf("GPS thread ended???\n");
481 return NULL;
482 }
483
484 void gps_init(appdata_t *appdata) {
485 appdata->gps_state = malloc(sizeof(gps_state_t));
486 memset(appdata->gps_state, 0, sizeof(gps_state_t));
487
488 /* start a new thread to listen to gpsd */
489 appdata->gps_state->mutex = g_mutex_new();
490 appdata->gps_state->thread_p =
491 g_thread_create(gps_thread, appdata, FALSE, NULL);
492 }
493
494 void gps_release(appdata_t *appdata) {
495 #ifdef USE_MAEMO
496 gpsbt_stop(&appdata->gps_state->context);
497 #endif
498 free(appdata->gps_state);
499 }
500
501 #else
502
503 #warning "liblocation interface not completely implemented"
504
505 static void
506 location_changed(LocationGPSDevice *device, gps_state_t *gps_state) {
507
508 gps_state->fix =
509 (device->fix->fields & LOCATION_GPS_DEVICE_LATLONG_SET);
510
511 if(gps_state->fix) {
512 gps_state->latitude = device->fix->latitude;
513 gps_state->longitude = device->fix->longitude;
514 gps_state->heading = device->fix->track;
515 gps_state->epe = device->fix->eph;
516
517
518 }
519 }
520
521 void gps_init(appdata_t *appdata) {
522 gps_state_t *gps_state = appdata->gps_state = g_new0(gps_state_t, 1);
523
524 printf("GPS init: Using liblocation\n");
525
526 gps_state->device = g_object_new(LOCATION_TYPE_GPS_DEVICE, NULL);
527 if(!gps_state->device) {
528 printf("Unable to connect to liblocation\n");
529 return;
530 }
531
532 gps_state->idd_changed =
533 g_signal_connect(gps_state->device, "changed",
534 G_CALLBACK(location_changed), gps_state);
535
536 }
537
538 void gps_release(appdata_t *appdata) {
539 gps_state_t *gps_state = appdata->gps_state;
540
541 if(!gps_state->device) return;
542
543 /* Disconnect signal */
544 g_signal_handler_disconnect(gps_state->device, gps_state->idd_changed);
545
546 g_free(appdata->gps_state);
547 appdata->gps_state = NULL;
548 }
549
550 pos_t *gps_get_pos(appdata_t *appdata) {
551 static pos_t pos;
552
553 if(!appdata->use_gps)
554 return NULL;
555
556 gps_state_t *gps_state = appdata->gps_state;
557
558 if(!gps_state->fix)
559 return NULL;
560
561 pos.lat = gps_state->latitude;
562 pos.lon = gps_state->longitude;
563
564 return &pos;
565 }
566
567 float gps_get_heading(appdata_t *appdata) {
568 gps_state_t *gps_state = appdata->gps_state;
569
570 if(!gps_state->fix)
571 return NAN;
572
573 return gps_state->heading;
574 }
575
576 float gps_get_epe(appdata_t *appdata) {
577 gps_state_t *gps_state = appdata->gps_state;
578
579 if(!gps_state->fix)
580 return NAN;
581
582 return gps_state->epe;
583 }
584
585 gps_sat_t *gps_get_sats(appdata_t *appdata) { return NULL; }
586
587
588 #endif