trunk/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_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);

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