maemo/src/gps.c

/*
 * Copyright (C) 2008 Till Harbaum <till@harbaum.org>.
 *
 * This file is part of OSM2Go.
 *
 * OSM2Go is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * OSM2Go is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with OSM2Go.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdio.h>
#include <string.h>
#include <math.h>

#include "appdata.h"

#ifdef ENABLE_LIBLOCATION

#include <location/location-gps-device.h>

pos_t *gps_get_pos(appdata_t *appdata) {
  gps_state_t *gps_state = appdata->gps_state;
  static pos_t retval;

  if(!gps_state->fix)
    return NULL;

  retval.lat = gps_state->latitude;
  retval.lon = gps_state->longitude;

  return &retval;
}

static void
location_changed(LocationGPSDevice *device, gps_state_t *gps_state) {

  gps_state->fix = 
    (device->fix->fields & LOCATION_GPS_DEVICE_LATLONG_SET);
  
  if(gps_state->fix) {
    gps_state->latitude = device->fix->latitude;
    gps_state->longitude = device->fix->longitude;
  }
}

void gps_init(appdata_t *appdata) {
  gps_state_t *gps_state = appdata->gps_state = g_new0(gps_state_t, 1);

  printf("GPS init: Using liblocation\n");

  gps_state->device = g_object_new(LOCATION_TYPE_GPS_DEVICE, NULL);  
  if(!gps_state->device) {
    printf("Unable to connect to liblocation\n");
    return;
  }

  gps_state->idd_changed = 
    g_signal_connect(gps_state->device, "changed", 
                     G_CALLBACK(location_changed), gps_state);

#if MAEMO_VERSION_MAJOR < 5
  gps_state->control = location_gpsd_control_get_default();
  if(gps_state->control->can_control) {
    printf("Having control over GPSD, starting it\n");
    location_gpsd_control_start(gps_state->control);
  }
#endif
}

void gps_release(appdata_t *appdata) { 
  gps_state_t *gps_state = appdata->gps_state;

  if(!gps_state->device) return;

#if MAEMO_VERSION_MAJOR < 5
  /* Disconnect signal */
  if(gps_state->control->can_control) {
    printf("Having control over GPSD, stopping it\n");
    location_gpsd_control_stop(gps_state->control);
  }
#endif

  g_signal_handler_disconnect(gps_state->device, gps_state->idd_changed);

  g_free(appdata->gps_state);
  appdata->gps_state = NULL;
}

#else  // ENABLE_LIBLOCATION

#ifdef ENABLE_GPSBT
#include <gpsbt.h>
#include <gpsmgr.h>
#include <errno.h>
#endif

/* maybe user configurable later on ... */
#define GPSD_HOST "127.0.0.1"
#define GPSD_PORT 2947

pos_t *gps_get_pos(appdata_t *appdata) {
  static pos_t retval;

  retval.lat = NAN;

  g_mutex_lock(appdata->gps_state->mutex);
  if(appdata->gps_state->gpsdata.set & STATUS_SET)
    if(appdata->gps_state->gpsdata.status != STATUS_NO_FIX)
      if(appdata->gps_state->gpsdata.set & LATLON_SET) 
        retval = appdata->gps_state->gpsdata.fix.pos;

  g_mutex_unlock(appdata->gps_state->mutex);

  if(isnan(retval.lat))
    return NULL;

  return &retval;
}

static int gps_connect(gps_state_t *gps_state) {
  GnomeVFSResult vfs_result;
#ifdef ENABLE_GPSBT
  char errstr[256] = "";

  /* We need to start gpsd (via gpsbt) first. */
  memset(&gps_state->context, 0, sizeof(gpsbt_t));
  errno = 0;

  if(gpsbt_start(NULL, 0, 0, 0, errstr, sizeof(errstr),
                 0, &gps_state->context) < 0) {
    printf("Error connecting to GPS receiver: (%d) %s (%s)\n",
           errno, strerror(errno), errstr);
  }
#endif

  /************** from here down pure gnome/gtk/gpsd ********************/

  /* try to connect to gpsd */
  /* Create a socket to interact with GPSD. */

  int retries = 5;
  while(retries && 
        (GNOME_VFS_OK != (vfs_result = gnome_vfs_inet_connection_create(
                &gps_state->iconn, GPSD_HOST, GPSD_PORT, NULL)))) {
    printf("Error creating connection to GPSD, retrying ...\n");

    retries--;
    sleep(1);
  }

  if(!retries) {
    printf("Finally failed ...\n");
    return -1;
  }

  retries = 5;
  while(retries && ((gps_state->socket = 
     gnome_vfs_inet_connection_to_socket(gps_state->iconn)) == NULL)) {
    printf("Error creating connecting GPSD socket, retrying ...\n");

    retries--;
    sleep(1);
  }

  if(!retries) {
    printf("Finally failed ...\n");
    gnome_vfs_inet_connection_destroy(gps_state->iconn, NULL);
    return -1;
  }

  GTimeVal timeout = { 10, 0 };
  if(GNOME_VFS_OK != (vfs_result = gnome_vfs_socket_set_timeout(
        gps_state->socket, &timeout, NULL))) {
    printf("Error setting GPSD timeout\n");
    gnome_vfs_inet_connection_destroy(gps_state->iconn, NULL);
    return -1;
  } 

  printf("GPSD connected ...\n");

  return 0;
}       

void gps_clear_fix(struct gps_fix_t *fixp) {
  fixp->mode = MODE_NOT_SEEN;
  fixp->pos.lat = fixp->pos.lon = NAN;
  fixp->eph = NAN;
}

/* unpack a daemon response into a status structure */
static void gps_unpack(char *buf, struct gps_data_t *gpsdata) {
  char *ns, *sp, *tp;

  for(ns = buf; ns; ns = strstr(ns+1, "GPSD")) {
    if(strncmp(ns, "GPSD", 4) == 0) {
      /* the following should execute each time we have a good next sp */
      for (sp = ns + 5; *sp != '\0'; sp = tp+1) {
        tp = sp + strcspn(sp, ",\r\n");
        if (*tp == '\0') tp--;
        else *tp = '\0';
        
        switch (*sp) {
        case 'O':
          if (sp[2] == '?') {
            gpsdata->set = 
              (gpsdata->set & SATELLITE_SET) | // fix for below
              MODE_SET | STATUS_SET;  // this clears sat info??
            gpsdata->status = STATUS_NO_FIX;
            gps_clear_fix(&gpsdata->fix);
          } else {
            struct gps_fix_t nf;
            char tag[MAXTAGLEN+1], alt[20], eph[20], lat[20], lon[20], mode[2];
            int st = sscanf(sp+2, 
                            "%8s %*s %*s %19s %19s "
                            "%19s %19s %*s %*s %*s %*s "
                            "%*s %*s %*s %1s",
                            tag, lat, lon,
                            alt, eph,
                            mode);
            if (st >= 5) {
#define DEFAULT(val) (val[0] == '?') ? NAN : g_ascii_strtod(val, NULL)
              nf.pos.lat = DEFAULT(lat);
              nf.pos.lon = DEFAULT(lon);
              nf.eph = DEFAULT(eph);
#undef DEFAULT
              if (st >= 6)
                nf.mode = (mode[0] == '?') ? MODE_NOT_SEEN : atoi(mode);
              else
                nf.mode = (alt[0] == '?') ? MODE_2D : MODE_3D;
              gpsdata->fix = nf;
              gpsdata->set |= LATLON_SET|MODE_SET;
              gpsdata->status = STATUS_FIX;
              gpsdata->set |= STATUS_SET;
            }
          }
          break;
        }
      }
    }
  }
}

gpointer gps_thread(gpointer data) {
  GnomeVFSFileSize bytes_read;
  GnomeVFSResult vfs_result;
  char str[512];
  appdata_t *appdata = (appdata_t*)data;

  const char *msg = "o\r\n";   /* pos request */

  appdata->gps_state->gpsdata.set = 0;

  gboolean connected = FALSE;

  while(1) {
    if(appdata->gps_enabled) {
      if(!connected) {
        printf("trying to connect\n");

        if(gps_connect(appdata->gps_state) < 0)
          sleep(10);
        else 
          connected = TRUE;
      } else {
        if(GNOME_VFS_OK == 
           (vfs_result = gnome_vfs_socket_write(appdata->gps_state->socket,
                      msg, strlen(msg)+1, &bytes_read, NULL))) {

          /* update every second, wait here to make sure a complete */
          /* reply is received */
          sleep(1);
          
          if(bytes_read == (strlen(msg)+1)) {
            vfs_result = gnome_vfs_socket_read(appdata->gps_state->socket,
                               str, sizeof(str)-1, &bytes_read, NULL);
            if(vfs_result == GNOME_VFS_OK) {
              str[bytes_read] = 0; 
            
              printf("msg: %s (%d)\n", str, strlen(str));
              
              g_mutex_lock(appdata->gps_state->mutex);
              
              appdata->gps_state->gpsdata.set &= 
                ~(LATLON_SET|MODE_SET|STATUS_SET);
            
              gps_unpack(str, &appdata->gps_state->gpsdata);
              g_mutex_unlock(appdata->gps_state->mutex);
            }
          }
        }
      }
    } else {
      if(connected) {
        printf("stopping GPS connection due to user request\n");
        gnome_vfs_inet_connection_destroy(appdata->gps_state->iconn, NULL);

#ifdef ENABLE_GPSBT
        gpsbt_stop(&appdata->gps_state->context);
#endif
        connected = FALSE;
      } else
        sleep(1);
    }
  }

  printf("GPS thread ended???\n");
  return NULL;
}

void gps_init(appdata_t *appdata) {
  appdata->gps_state = g_new0(gps_state_t, 1);

  printf("GPS init: Using gpsd\n");

  /* start a new thread to listen to gpsd */
  appdata->gps_state->mutex = g_mutex_new();
  appdata->gps_state->thread_p = 
    g_thread_create(gps_thread, appdata, FALSE, NULL);
}

void gps_release(appdata_t *appdata) { 
#ifdef ENABLE_GPSBT
  gpsbt_stop(&appdata->gps_state->context);
#endif
  g_free(appdata->gps_state);
  appdata->gps_state = NULL;
}

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