trunk/src/gps.c

/*
 * Copyright (C) 2009 Till Harbaum <till@harbaum.org>.
 *
 * This file is part of Maep.
 *
 * Maep 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.
 *
 * Maep 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 Maep.  If not, see <http://www.gnu.org/licenses/>.
 */

/* 
 * This version focusses on low power consumption. This means that 
 * a component can register for GPS callbacks for certain events 
 * (e.g. position change, altitude change, track change ...) and it
 * will only get notified if one of these changes. Also it's been 
 * verified that the change in question exceed a certain limit.
 */

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

#ifdef USE_MAEMO
#ifdef ENABLE_GPSBT
#include <gpsbt.h>
#include <gpsmgr.h>
#endif
#include <errno.h>
#else
#ifdef USE_LIBGPS
#include <gps.h>
#include <locale.h>
#endif
#endif

#include "gps.h"

static void gps_unregister_all(gps_state_t *gps_state);

/* limits below which a fix is considered different */
#define TRACK_PRECISION (1.0)           // one degree
#define POS_PRECISION (1.0/60000.0)     // 0.001 minute
#define EPH_PRECISION (2.0)             // 2 meters
#define ALTITUDE_PRECISION (1.0)        // 1 meters

static gboolean 
track_differs(struct gps_t *fix1, struct gps_t *fix2) {
  return( fabsf(fix1->track - fix2->track) >= TRACK_PRECISION);
}

static gboolean 
latlon_differs(struct gps_t *fix1, struct gps_t *fix2) {
  if( fabsf(fix1->latitude - fix2->latitude) >= POS_PRECISION)
    return TRUE;

  if( fabsf(fix1->longitude - fix2->longitude) >= POS_PRECISION)
    return TRUE;

  return FALSE;
}

static gboolean 
eph_differs(struct gps_t *fix1, struct gps_t *fix2) {
  return( fabsf(fix1->eph - fix2->eph) >= EPH_PRECISION);
}

static gboolean 
altitude_differs(struct gps_t *fix1, struct gps_t *fix2) {
  return( fabsf(fix1->altitude - fix2->altitude) >= ALTITUDE_PRECISION);
}

static void gps_cb_func(gpointer data, gpointer user_data) {
  gps_callback_t *callback = (gps_callback_t*)data;
  gps_state_t *gps_state = (gps_state_t*)user_data;

  /* we can save some energy here if we make sure that */
  /* we really only wake up the main app whenever the */
  /* gps state changed in a way that the map needs to */
  /* be updated */
  
  /* for all data check if its status has changed (data is available */
  /* or not) or if the data itself changed */

  /* clear "changed" bits */
  gps_state->set &= ~CHANGED_MASK;

  /* check for changes in position */
  if(callback->mask & LATLON_CHANGED &&
     (((gps_state->set & FIX_LATLON_SET) != 
       (gps_state->last.set & FIX_LATLON_SET)) ||
      ((gps_state->set & FIX_LATLON_SET) && 
       latlon_differs(&gps_state->last.fix, &gps_state->fix)))) 
    gps_state->set |= LATLON_CHANGED;

  /* check for changes in eph */
  if(callback->mask & HERR_CHANGED &&
     (((gps_state->set & FIX_HERR_SET) != 
       (gps_state->last.set & FIX_HERR_SET)) ||
      ((gps_state->set & FIX_HERR_SET) && 
       eph_differs(&gps_state->last.fix, &gps_state->fix))))
    gps_state->set |= HERR_CHANGED;

  /* check for changes in track */
  if(callback->mask & TRACK_CHANGED &&
     (((gps_state->set & FIX_TRACK_SET) != 
       (gps_state->last.set & FIX_TRACK_SET)) ||
      ((gps_state->set & FIX_TRACK_SET) && 
       track_differs(&gps_state->last.fix, &gps_state->fix))))
    gps_state->set |= TRACK_CHANGED;

  /* check for changes in altitude */
  if(callback->mask & ALTITUDE_CHANGED &&
     (((gps_state->set & FIX_ALTITUDE_SET) != 
       (gps_state->last.set & FIX_ALTITUDE_SET)) ||
      ((gps_state->set & FIX_ALTITUDE_SET) && 
       altitude_differs(&gps_state->last.fix, &gps_state->fix))))
    gps_state->set |= ALTITUDE_CHANGED;

  if(gps_state->set & CHANGED_MASK)
    callback->cb(gps_state->set, &gps_state->fix, callback->data);
}

#ifndef ENABLE_LIBLOCATION

static gboolean gps_notify(gpointer data) {
  gps_state_t *gps_state = (gps_state_t*)data;

  if(gps_state->callbacks) {
    g_mutex_lock(gps_state->mutex);

    /* tell all clients */
    g_slist_foreach(gps_state->callbacks, gps_cb_func, gps_state);

    g_mutex_unlock(gps_state->mutex);
  }

  /* remember last state reported */
  gps_state->last.set = gps_state->set;
  gps_state->last.fix = gps_state->fix;
  
  return FALSE; 
}

#ifndef USE_LIBGPS

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

static int gps_connect(gps_state_t *gps_state) {
  GnomeVFSResult vfs_result;

#ifdef USE_MAEMO
  char errstr[256] = "";

  if(!gps_state) {
    printf("No gps state\n");
    return -1;
  }

  /* 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("gps: 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("gps: Error creating connection to GPSD, retrying ...\n");

    retries--;
    sleep(1);
  }

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

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

    retries--;
    sleep(1);
  }

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

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

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

  return 0;
}       

static double parse_double(char *val) {
  if(val[0] == '?') return NAN;
  return g_ascii_strtod(val, NULL);
}

/* unpack a daemon response into a status structure */
static void gps_unpack(char *buf, gps_state_t *gps_state) {
  char *ns, *sp, *tp;
  int j;

  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] == '?') {
            gps_state->fix.latitude = NAN;
            gps_state->fix.longitude = NAN;
            gps_state->fix.altitude = NAN;
            gps_state->fix.track = NAN;
            gps_state->fix.eph = NAN;
          } else {
            char tag[MAXTAGLEN+1], alt[20];
            char eph[20], track[20],speed[20];
            char lat[20], lon[20];
            if(sscanf(sp+2, 
                      "%8s %*s %*s %19s %19s "
                      "%19s %19s %*s %19s %19s %*s "
                      "%*s %*s %*s %*s",
                      tag, lat, lon,
                      alt, eph, track, speed) == 7) {

              gps_state->fix.latitude = parse_double(lat);
              gps_state->fix.longitude = parse_double(lon);
              gps_state->fix.altitude = parse_double(alt);
              gps_state->fix.eph = parse_double(eph);
              gps_state->fix.track = parse_double(track);

              gps_state->set |= FIX_LATLON_SET;

              if (!isnan(gps_state->fix.eph))
                gps_state->set |= FIX_HERR_SET;

              if (!isnan(gps_state->fix.track))
                gps_state->set |= FIX_TRACK_SET;

              if(!isnan(gps_state->fix.altitude))
                gps_state->set |= FIX_ALTITUDE_SET;
            }
          }
          break;

        case 'Y': 
          gps_state->fix.sat_num = 0;

          if (sp[2] != '?') {
            (void)sscanf(sp, "Y=%*s %*s %d ", &gps_state->fix.sat_num);

            /* clear all slots */
            for (j = 0; j < gps_state->fix.sat_num; j++) 
              gps_state->fix.sat_data[j].prn = 
              gps_state->fix.sat_data[j].ss = 
              gps_state->fix.sat_data[j].used = 0;

            for (j = 0; j < gps_state->fix.sat_num; j++) {
              if ((sp != NULL) && ((sp = strchr(sp, ':')) != NULL)) {
                sp++;
                (void)sscanf(sp, "%d %*d %*d %d %d", 
                             &gps_state->fix.sat_data[j].prn,
                             &gps_state->fix.sat_data[j].ss, 
                             &gps_state->fix.sat_data[j].used);
              }
            }
          }
          gps_state->set |= FIX_SATELLITE_SET;
          break;
        }
      }
    }
  }
}

static gpointer gps_thread(gpointer data) {
  gps_state_t *gps_state = (gps_state_t*)data;
  
  GnomeVFSFileSize bytes_read;
  GnomeVFSResult vfs_result;
  char str[512];

  gps_state->set = 0;

  gboolean connected = FALSE;

  int cnt=1000;

  const char *msg_pos = "o\r\n"; /* pos request */
  const char *msg_sat = "y\r\n"; /* sat request */

  while(1) {
    /* just lock and unlock the control mutex. This stops the thread */
    /* while the main process locks this mutex */
    g_mutex_lock(gps_state->control_mutex);
    g_mutex_unlock(gps_state->control_mutex);
    g_mutex_lock(gps_state->global_mutex);
    g_mutex_unlock(gps_state->global_mutex);

    if(!connected) {
      printf("gps: trying to connect\n");

      if(gps_connect(gps_state) < 0)
        sleep(10);
      else 
        connected = TRUE;

    } else {
      const char *msg;
      if(!cnt) msg = msg_sat;
      else     msg = msg_pos;
  
      if(GNOME_VFS_OK == 
         (vfs_result = gnome_vfs_socket_write(gps_state->socket,
                      msg, strlen(msg)+1, &bytes_read, NULL))) {

        /* update every second, wait here to make sure a complete */
        /* reply is received */
        if(cnt <= 1) usleep(500000);
        else         sleep(1);

        if(bytes_read == (strlen(msg)+1)) {
          vfs_result = gnome_vfs_socket_read(gps_state->socket,
             str, sizeof(str)-1, &bytes_read, NULL);

          if(vfs_result == GNOME_VFS_OK) {
            str[bytes_read] = 0; 

            g_mutex_lock(gps_state->mutex);

            /* assume we could't parse anything ... */
            if(!cnt) gps_state->set &= ~FIX_SATELLITE_SET;
            else     gps_state->set &=
               ~(FIX_LATLON_SET|FIX_HERR_SET|FIX_ALTITUDE_SET|FIX_TRACK_SET);
            
            gps_unpack(str, gps_state);

            /* notify applications of state if useful */
            g_idle_add(gps_notify, gps_state); 

            g_mutex_unlock(gps_state->mutex);       
          }
        }

        /* if we have clients requesting sat information return to */
        /* state 0 which will request them. Otherwise loop in states */
        /* 1 - 4 which don't request them */
        if(cnt++ >= 5) {
          if(gps_state->sat_requests) cnt = 0;
          else                        cnt = 1;
        }
      }
    }
  }

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

gps_state_t *gps_init(void) {
  gps_state_t *gps_state = g_new0(gps_state_t, 1);

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

  return gps_state;
}

void gps_release(gps_state_t *gps_state) { 
  gps_unregister_all(gps_state);

#ifdef USE_MAEMO
  gpsbt_stop(&gps_state->context);
#endif

  g_free(gps_state);
}

#else // USE_LIBGPS

static gpointer gps_thread(gpointer data) {
  gps_state_t *gps_state = (gps_state_t*)data;

  /* the following is required for libgps to be able to parse */
  /* the gps messages. Unfortunately this also affect the rest of */
  /* the program and the main thread */
  setlocale(LC_NUMERIC, "C");

  while(1) {
    /* just lock and unlock the control mutex. This stops the thread */
    /* while the main process locks this mutex */
    g_mutex_lock(gps_state->control_mutex);
    g_mutex_unlock(gps_state->control_mutex);

    gps_poll(gps_state->data);

    g_mutex_lock(gps_state->mutex);
    
    /* assume we could't parse anything ... */
    gps_state->set = 0;
    
    if(gps_state->data->fix.mode >= MODE_2D) {
      /* latlon valid */
      gps_state->set |= FIX_LATLON_SET;
      gps_state->fix.latitude = gps_state->data->fix.latitude;
      gps_state->fix.longitude = gps_state->data->fix.longitude;

      gps_state->fix.eph = 
        gps_state->data->fix.epy > gps_state->data->fix.epx ?
        gps_state->data->fix.epy : gps_state->data->fix.epx;
      if(!isnan(gps_state->fix.eph)) gps_state->set |= FIX_HERR_SET;

      gps_state->fix.track = gps_state->data->fix.track;
      if(!isnan(gps_state->fix.track)) gps_state->set |= FIX_TRACK_SET;
    }

    if(gps_state->data->fix.mode >= MODE_3D) {
      /* altitude valid */
      gps_state->set |= FIX_ALTITUDE_SET;
      gps_state->fix.altitude = gps_state->data->fix.altitude;
    } else
      gps_state->fix.altitude = NAN;

    /* notify applications of state if useful */
    g_idle_add(gps_notify, gps_state); 
    
    g_mutex_unlock(gps_state->mutex);       
    
  }

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

gps_state_t *gps_init(void) {
  gps_state_t *gps_state = g_new0(gps_state_t, 1);

  gps_state->mutex = g_mutex_new();
  gps_state->control_mutex = g_mutex_new();

  gps_state->data = gps_open("127.0.0.1", DEFAULT_GPSD_PORT );
  if(!gps_state->data)
    perror("gps_open()");

  (void)gps_stream(gps_state->data, WATCH_ENABLE, NULL);

  gps_state->thread_p = 
    g_thread_create(gps_thread, gps_state, FALSE, NULL);

  return gps_state;
}

void gps_release(gps_state_t *gps_state) { 
  gps_unregister_all(gps_state);
  gps_close(gps_state->data); 
  g_free(gps_state);
}

#endif // USE_LIBGPS

static void gps_background_enable(gps_state_t *gps_state, gboolean enable) {
  printf("GPS: %sable background process\n", enable?"en":"dis");

  /* start and stop gps thread by locking and unlocking the control mutex */
  if(enable) g_mutex_unlock(gps_state->control_mutex);
  else       g_mutex_lock(gps_state->control_mutex);
}

void gps_enable(gps_state_t *gps_state, gboolean enable) {
  /* just ignore request if we are already in that state */
  if(gps_state->stopped == !enable) return;

  if(!enable) g_mutex_lock(gps_state->global_mutex);
  else        g_mutex_unlock(gps_state->global_mutex);

  gps_state->stopped = !enable;
}


#else

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

  gps_state->set = 0;

  if(device->fix->fields & LOCATION_GPS_DEVICE_LATLONG_SET) { 
    gps_state->set |= FIX_LATLON_SET | FIX_HERR_SET;
    gps_state->fix.latitude = device->fix->latitude;
    gps_state->fix.longitude = device->fix->longitude;
    gps_state->fix.eph = device->fix->eph/100.0;  // we want eph in meters
  }

  if(device->fix->fields & LOCATION_GPS_DEVICE_ALTITUDE_SET) {
    gps_state->set |= FIX_ALTITUDE_SET;
    gps_state->fix.altitude = device->fix->altitude;
  } else
    gps_state->fix.altitude = NAN;

  if(device->fix->fields & LOCATION_GPS_DEVICE_TRACK_SET) {
    gps_state->set |= FIX_TRACK_SET;
    gps_state->fix.track = device->fix->track;
  }

  /* tell all clients */
  g_slist_foreach(gps_state->callbacks, gps_cb_func, gps_state);

  /* remember last state reported */
  gps_state->last.set = gps_state->set;
  gps_state->last.fix = gps_state->fix;
}

gps_state_t *gps_init(void) {
  gps_state_t *gps_state = g_new0(gps_state_t, 1);

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

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

  gps_state->control = location_gpsd_control_get_default();

  if(gps_state->control
#if MAEMO_VERSION_MAJOR < 5
     && gps_state->control->can_control
#endif
     ) {

    printf("gps: Having control over GPSD and GPS is to be enabled, starting it\n");
    location_gpsd_control_start(gps_state->control);
  }
  return gps_state;
}

void gps_release(gps_state_t *gps_state) {
  gps_unregister_all(gps_state);

  if(gps_state->control
#if MAEMO_VERSION_MAJOR < 5
     && gps_state->control->can_control
#endif     
     ) {
    printf("gps: Having control over GPSD, stopping it\n");
    location_gpsd_control_stop(gps_state->control);
  }
  
  /* Disconnect signal */
  g_signal_handler_disconnect(gps_state->device, gps_state->idd_changed);
  gps_state->connected = FALSE;
  
  g_free(gps_state);
}

static void gps_control(gps_state_t *gps_state, 
                        gboolean enable0, gboolean enable1) {
  if(!gps_state->connected) {
    /* both "enable" signals have to be true */
    if(enable0 && enable1) {
      printf("GPS: connecting\n");
      gps_state->idd_changed = 
        g_signal_connect(gps_state->device, "changed", 
                         G_CALLBACK(location_changed), gps_state);
      gps_state->connected = TRUE;
    }
  } else {
    /* at least one "enable" is false */
    if(!enable0 || !enable1) {
      printf("GPS: disconnecting\n");
      /* Disconnect signal */
      g_signal_handler_disconnect(gps_state->device, gps_state->idd_changed);
      gps_state->connected = FALSE;
    }
  }
}

/* enable/disable due to all windows being in background */
void gps_background_enable(gps_state_t *gps_state, gboolean enable) {
  if(gps_state->backgrounded == !enable) return;

  gps_state->backgrounded = !enable;
  gps_control(gps_state, gps_state->stopped, gps_state->backgrounded);
}

/* enable/disable on app request */
void gps_enable(gps_state_t *gps_state, gboolean enable) {
  if(gps_state->stopped == !enable) return;

  gps_state->stopped = !enable;
  gps_control(gps_state, gps_state->stopped, gps_state->backgrounded);
}

#endif // USE_LIBLOCATION

static gint compare(gconstpointer a, gconstpointer b) {
  return ((gps_callback_t*)a)->cb != b;
}

void gps_register_callback(gps_state_t *gps_state, int mask, 
                           gps_cb cb, void *data) {
  /* make sure the callback isn't already registered */
  GSList *list = g_slist_find_custom(gps_state->callbacks, cb, compare);
  if(list) {
    printf("GPS register: ignoring duplicate\n"); 
    return;
  }

  gps_callback_t *callback = g_new0(gps_callback_t, 1);

  g_assert(mask & CHANGED_MASK);
  g_assert(!(mask & ~CHANGED_MASK));
  
  callback->mask = mask;
  callback->cb = cb;
  callback->data = data;

#if !defined(ENABLE_LIBLOCATION) && !defined(USE_LIBGPS)
  /* the gpsd solution needs to know the number of clients */
  /* requesting sat information */
  if(mask & SATELLITE_CHANGED) 
    gps_state->sat_requests++;
#endif

  gps_state->callbacks = g_slist_append(gps_state->callbacks, callback);
  
  if(g_slist_length(gps_state->callbacks) == 1)
    gps_background_enable(gps_state, TRUE);
}

void gps_unregister_callback(gps_state_t *gps_state, gps_cb cb) {
  /* find callback in list */
  GSList *list = g_slist_find_custom(gps_state->callbacks, cb, compare);
  g_assert(list);

#if !defined(ENABLE_LIBLOCATION) && !defined(USE_LIBGPS)
  /* the gpsd solution needs to know the number of clients */
  /* requesting sat information */
  gps_callback_t *cb_s = list->data;
  if(cb_s->mask & SATELLITE_CHANGED) 
    gps_state->sat_requests--;
#endif

  /* and de-chain and free it */
  g_free(list->data);
  gps_state->callbacks = g_slist_remove(gps_state->callbacks, list->data);

  if(g_slist_length(gps_state->callbacks) == 0)
    gps_background_enable(gps_state, FALSE);
}

static void gps_unregister_all(gps_state_t *gps_state) {
  g_slist_foreach(gps_state->callbacks, (GFunc)g_free, NULL);
  g_slist_free(gps_state->callbacks);
}
1	/*
2	* Copyright (C) 2009 Till Harbaum <till@harbaum.org>.
3	*
4	* This file is part of Maep.
5	*
6	* Maep 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	* Maep 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 Maep. If not, see <http://www.gnu.org/licenses/>.
18	*/
19
20	/*
21	* This version focusses on low power consumption. This means that
22	* a component can register for GPS callbacks for certain events
23	* (e.g. position change, altitude change, track change ...) and it
24	* will only get notified if one of these changes. Also it's been
25	* verified that the change in question exceed a certain limit.
26	*/
27
28	#include <stdio.h>
29	#include <string.h>
30	#include <math.h>
31	#include <stdlib.h>
32
33	#ifdef USE_MAEMO
34	#ifdef ENABLE_GPSBT
35	#include <gpsbt.h>
36	#include <gpsmgr.h>
37	#endif
38	#include <errno.h>
39	#else
40	#ifdef USE_LIBGPS
41	#include <gps.h>
42	#include <locale.h>
43	#endif
44	#endif
45
46	#include "gps.h"
47
48	static void gps_unregister_all(gps_state_t *gps_state);
49
50	/* limits below which a fix is considered different */
51	#define TRACK_PRECISION (1.0) // one degree
52	#define POS_PRECISION (1.0/60000.0) // 0.001 minute
53	#define EPH_PRECISION (2.0) // 2 meters
54	#define ALTITUDE_PRECISION (1.0) // 1 meters
55
56	static gboolean
57	track_differs(struct gps_t fix1, struct gps_t fix2) {
58	return( fabsf(fix1->track - fix2->track) >= TRACK_PRECISION);
59	}
60
61	static gboolean
62	latlon_differs(struct gps_t fix1, struct gps_t fix2) {
63	if( fabsf(fix1->latitude - fix2->latitude) >= POS_PRECISION)
64	return TRUE;
65
66	if( fabsf(fix1->longitude - fix2->longitude) >= POS_PRECISION)
67	return TRUE;
68
69	return FALSE;
70	}
71
72	static gboolean
73	eph_differs(struct gps_t fix1, struct gps_t fix2) {
74	return( fabsf(fix1->eph - fix2->eph) >= EPH_PRECISION);
75	}
76
77	static gboolean
78	altitude_differs(struct gps_t fix1, struct gps_t fix2) {
79	return( fabsf(fix1->altitude - fix2->altitude) >= ALTITUDE_PRECISION);
80	}
81
82	static void gps_cb_func(gpointer data, gpointer user_data) {
83	gps_callback_t callback = (gps_callback_t)data;
84	gps_state_t gps_state = (gps_state_t)user_data;
85
86	/* we can save some energy here if we make sure that */
87	/* we really only wake up the main app whenever the */
88	/* gps state changed in a way that the map needs to */
89	/* be updated */
90
91	/* for all data check if its status has changed (data is available */
92	/* or not) or if the data itself changed */
93
94	/* clear "changed" bits */
95	gps_state->set &= ~CHANGED_MASK;
96
97	/* check for changes in position */
98	if(callback->mask & LATLON_CHANGED &&
99	(((gps_state->set & FIX_LATLON_SET) !=
100	(gps_state->last.set & FIX_LATLON_SET)) \|\|
101	((gps_state->set & FIX_LATLON_SET) &&
102	latlon_differs(&gps_state->last.fix, &gps_state->fix))))
103	gps_state->set \|= LATLON_CHANGED;
104
105	/* check for changes in eph */
106	if(callback->mask & HERR_CHANGED &&
107	(((gps_state->set & FIX_HERR_SET) !=
108	(gps_state->last.set & FIX_HERR_SET)) \|\|
109	((gps_state->set & FIX_HERR_SET) &&
110	eph_differs(&gps_state->last.fix, &gps_state->fix))))
111	gps_state->set \|= HERR_CHANGED;
112
113	/* check for changes in track */
114	if(callback->mask & TRACK_CHANGED &&
115	(((gps_state->set & FIX_TRACK_SET) !=
116	(gps_state->last.set & FIX_TRACK_SET)) \|\|
117	((gps_state->set & FIX_TRACK_SET) &&
118	track_differs(&gps_state->last.fix, &gps_state->fix))))
119	gps_state->set \|= TRACK_CHANGED;
120
121	/* check for changes in altitude */
122	if(callback->mask & ALTITUDE_CHANGED &&
123	(((gps_state->set & FIX_ALTITUDE_SET) !=
124	(gps_state->last.set & FIX_ALTITUDE_SET)) \|\|
125	((gps_state->set & FIX_ALTITUDE_SET) &&
126	altitude_differs(&gps_state->last.fix, &gps_state->fix))))
127	gps_state->set \|= ALTITUDE_CHANGED;
128
129	if(gps_state->set & CHANGED_MASK)
130	callback->cb(gps_state->set, &gps_state->fix, callback->data);
131	}
132
133	#ifndef ENABLE_LIBLOCATION
134
135	static gboolean gps_notify(gpointer data) {
136	gps_state_t gps_state = (gps_state_t)data;
137
138	if(gps_state->callbacks) {
139	g_mutex_lock(gps_state->mutex);
140
141	/* tell all clients */
142	g_slist_foreach(gps_state->callbacks, gps_cb_func, gps_state);
143
144	g_mutex_unlock(gps_state->mutex);
145	}
146
147	/* remember last state reported */
148	gps_state->last.set = gps_state->set;
149	gps_state->last.fix = gps_state->fix;
150
151	return FALSE;
152	}
153
154	#ifndef USE_LIBGPS
155
156	/* maybe user configurable later on ... */
157	#define GPSD_HOST "127.0.0.1"
158	#define GPSD_PORT 2947
159
160	static int gps_connect(gps_state_t *gps_state) {
161	GnomeVFSResult vfs_result;
162
163	#ifdef USE_MAEMO
164	char errstr[256] = "";
165
166	if(!gps_state) {
167	printf("No gps state\n");
168	return -1;
169	}
170
171	/* We need to start gpsd (via gpsbt) first. */
172	memset(&gps_state->context, 0, sizeof(gpsbt_t));
173	errno = 0;
174
175	if(gpsbt_start(NULL, 0, 0, 0, errstr, sizeof(errstr),
176	0, &gps_state->context) < 0) {
177	printf("gps: Error connecting to GPS receiver: (%d) %s (%s)\n",
178	errno, strerror(errno), errstr);
179	}
180	#endif
181
182	/************ from here down pure gnome/gtk/gpsd ******************/
183
184	/* try to connect to gpsd */
185	/* Create a socket to interact with GPSD. */
186
187	int retries = 5;
188	while(retries &&
189	(GNOME_VFS_OK != (vfs_result = gnome_vfs_inet_connection_create(
190	&gps_state->iconn, GPSD_HOST, GPSD_PORT, NULL)))) {
191	printf("gps: Error creating connection to GPSD, retrying ...\n");
192
193	retries--;
194	sleep(1);
195	}
196
197	if(!retries) {
198	printf("gps: Finally failed ...\n");
199	return -1;
200	}
201
202	retries = 5;
203	while(retries && ((gps_state->socket =
204	gnome_vfs_inet_connection_to_socket(gps_state->iconn)) == NULL)) {
205	printf("gps: Error connecting GPSD socket, retrying ...\n");
206
207	retries--;
208	sleep(1);
209	}
210
211	if(!retries) {
212	printf("gps: Finally failed ...\n");
213	return -1;
214	}
215
216	GTimeVal timeout = { 10, 0 };
217	if(GNOME_VFS_OK != (vfs_result = gnome_vfs_socket_set_timeout(
218	gps_state->socket, &timeout, NULL))) {
219	printf("gps: Error setting GPSD timeout\n");
220	return -1;
221	}
222
223	printf("gps: GPSD connected ...\n");
224
225	return 0;
226	}
227
228	static double parse_double(char *val) {
229	if(val[0] == '?') return NAN;
230	return g_ascii_strtod(val, NULL);
231	}
232
233	/* unpack a daemon response into a status structure */
234	static void gps_unpack(char buf, gps_state_t gps_state) {
235	char ns, sp, *tp;
236	int j;
237
238	for(ns = buf; ns; ns = strstr(ns+1, "GPSD")) {
239	if(strncmp(ns, "GPSD", 4) == 0) {
240	/* the following should execute each time we have a good next sp */
241	for (sp = ns + 5; *sp != '\0'; sp = tp+1) {
242	tp = sp + strcspn(sp, ",\r\n");
243	if (*tp == '\0') tp--;
244	else *tp = '\0';
245
246	switch (*sp) {
247	case 'O':
248	if (sp[2] == '?') {
249	gps_state->fix.latitude = NAN;
250	gps_state->fix.longitude = NAN;
251	gps_state->fix.altitude = NAN;
252	gps_state->fix.track = NAN;
253	gps_state->fix.eph = NAN;
254	} else {
255	char tag[MAXTAGLEN+1], alt[20];
256	char eph[20], track[20],speed[20];
257	char lat[20], lon[20];
258	if(sscanf(sp+2,
259	"%8s %s %s %19s %19s "
260	"%19s %19s %s %19s %19s %s "
261	"%s %s %s %s",
262	tag, lat, lon,
263	alt, eph, track, speed) == 7) {
264
265	gps_state->fix.latitude = parse_double(lat);
266	gps_state->fix.longitude = parse_double(lon);
267	gps_state->fix.altitude = parse_double(alt);
268	gps_state->fix.eph = parse_double(eph);
269	gps_state->fix.track = parse_double(track);
270
271	gps_state->set \|= FIX_LATLON_SET;
272
273	if (!isnan(gps_state->fix.eph))
274	gps_state->set \|= FIX_HERR_SET;
275
276	if (!isnan(gps_state->fix.track))
277	gps_state->set \|= FIX_TRACK_SET;
278
279	if(!isnan(gps_state->fix.altitude))
280	gps_state->set \|= FIX_ALTITUDE_SET;
281	}
282	}
283	break;
284
285	case 'Y':
286	gps_state->fix.sat_num = 0;
287
288	if (sp[2] != '?') {
289	(void)sscanf(sp, "Y=%s %s %d ", &gps_state->fix.sat_num);
290
291	/* clear all slots */
292	for (j = 0; j < gps_state->fix.sat_num; j++)
293	gps_state->fix.sat_data[j].prn =
294	gps_state->fix.sat_data[j].ss =
295	gps_state->fix.sat_data[j].used = 0;
296
297	for (j = 0; j < gps_state->fix.sat_num; j++) {
298	if ((sp != NULL) && ((sp = strchr(sp, ':')) != NULL)) {
299	sp++;
300	(void)sscanf(sp, "%d %d %d %d %d",
301	&gps_state->fix.sat_data[j].prn,
302	&gps_state->fix.sat_data[j].ss,
303	&gps_state->fix.sat_data[j].used);
304	}
305	}
306	}
307	gps_state->set \|= FIX_SATELLITE_SET;
308	break;
309	}
310	}
311	}
312	}
313	}
314
315	static gpointer gps_thread(gpointer data) {
316	gps_state_t gps_state = (gps_state_t)data;
317
318	GnomeVFSFileSize bytes_read;
319	GnomeVFSResult vfs_result;
320	char str[512];
321
322	gps_state->set = 0;
323
324	gboolean connected = FALSE;
325
326	int cnt=1000;
327
328	const char msg_pos = "o\r\n"; / pos request */
329	const char msg_sat = "y\r\n"; / sat request */
330
331	while(1) {
332	/* just lock and unlock the control mutex. This stops the thread */
333	/* while the main process locks this mutex */
334	g_mutex_lock(gps_state->control_mutex);
335	g_mutex_unlock(gps_state->control_mutex);
336	g_mutex_lock(gps_state->global_mutex);
337	g_mutex_unlock(gps_state->global_mutex);
338
339	if(!connected) {
340	printf("gps: trying to connect\n");
341
342	if(gps_connect(gps_state) < 0)
343	sleep(10);
344	else
345	connected = TRUE;
346
347	} else {
348	const char *msg;
349	if(!cnt) msg = msg_sat;
350	else msg = msg_pos;
351
352	if(GNOME_VFS_OK ==
353	(vfs_result = gnome_vfs_socket_write(gps_state->socket,
354	msg, strlen(msg)+1, &bytes_read, NULL))) {
355
356	/* update every second, wait here to make sure a complete */
357	/* reply is received */
358	if(cnt <= 1) usleep(500000);
359	else sleep(1);
360
361	if(bytes_read == (strlen(msg)+1)) {
362	vfs_result = gnome_vfs_socket_read(gps_state->socket,
363	str, sizeof(str)-1, &bytes_read, NULL);
364
365	if(vfs_result == GNOME_VFS_OK) {
366	str[bytes_read] = 0;
367
368	g_mutex_lock(gps_state->mutex);
369
370	/* assume we could't parse anything ... */
371	if(!cnt) gps_state->set &= ~FIX_SATELLITE_SET;
372	else gps_state->set &=
373	~(FIX_LATLON_SET\|FIX_HERR_SET\|FIX_ALTITUDE_SET\|FIX_TRACK_SET);
374
375	gps_unpack(str, gps_state);
376
377	/* notify applications of state if useful */
378	g_idle_add(gps_notify, gps_state);
379
380	g_mutex_unlock(gps_state->mutex);
381	}
382	}
383
384	/* if we have clients requesting sat information return to */
385	/* state 0 which will request them. Otherwise loop in states */
386	/* 1 - 4 which don't request them */
387	if(cnt++ >= 5) {
388	if(gps_state->sat_requests) cnt = 0;
389	else cnt = 1;
390	}
391	}
392	}
393	}
394
395	printf("gps: thread ended???\n");
396	return NULL;
397	}
398
399	gps_state_t *gps_init(void) {
400	gps_state_t *gps_state = g_new0(gps_state_t, 1);
401
402	/* start a new thread to listen to gpsd */
403	gps_state->mutex = g_mutex_new();
404	gps_state->control_mutex = g_mutex_new();
405	gps_state->global_mutex = g_mutex_new();
406	gps_state->thread_p =
407	g_thread_create(gps_thread, gps_state, FALSE, NULL);
408
409	return gps_state;
410	}
411
412	void gps_release(gps_state_t *gps_state) {
413	gps_unregister_all(gps_state);
414
415	#ifdef USE_MAEMO
416	gpsbt_stop(&gps_state->context);
417	#endif
418
419	g_free(gps_state);
420	}
421
422	#else // USE_LIBGPS
423
424	static gpointer gps_thread(gpointer data) {
425	gps_state_t gps_state = (gps_state_t)data;
426
427	/* the following is required for libgps to be able to parse */
428	/* the gps messages. Unfortunately this also affect the rest of */
429	/* the program and the main thread */
430	setlocale(LC_NUMERIC, "C");
431
432	while(1) {
433	/* just lock and unlock the control mutex. This stops the thread */
434	/* while the main process locks this mutex */
435	g_mutex_lock(gps_state->control_mutex);
436	g_mutex_unlock(gps_state->control_mutex);
437
438	gps_poll(gps_state->data);
439
440	g_mutex_lock(gps_state->mutex);
441
442	/* assume we could't parse anything ... */
443	gps_state->set = 0;
444
445	if(gps_state->data->fix.mode >= MODE_2D) {
446	/* latlon valid */
447	gps_state->set \|= FIX_LATLON_SET;
448	gps_state->fix.latitude = gps_state->data->fix.latitude;
449	gps_state->fix.longitude = gps_state->data->fix.longitude;
450
451	gps_state->fix.eph =
452	gps_state->data->fix.epy > gps_state->data->fix.epx ?
453	gps_state->data->fix.epy : gps_state->data->fix.epx;
454	if(!isnan(gps_state->fix.eph)) gps_state->set \|= FIX_HERR_SET;
455
456	gps_state->fix.track = gps_state->data->fix.track;
457	if(!isnan(gps_state->fix.track)) gps_state->set \|= FIX_TRACK_SET;
458	}
459
460	if(gps_state->data->fix.mode >= MODE_3D) {
461	/* altitude valid */
462	gps_state->set \|= FIX_ALTITUDE_SET;
463	gps_state->fix.altitude = gps_state->data->fix.altitude;
464	} else
465	gps_state->fix.altitude = NAN;
466
467	/* notify applications of state if useful */
468	g_idle_add(gps_notify, gps_state);
469
470	g_mutex_unlock(gps_state->mutex);
471
472	}
473
474	printf("gps: thread ended???\n");
475	return NULL;
476	}
477
478	gps_state_t *gps_init(void) {
479	gps_state_t *gps_state = g_new0(gps_state_t, 1);
480
481	gps_state->mutex = g_mutex_new();
482	gps_state->control_mutex = g_mutex_new();
483
484	gps_state->data = gps_open("127.0.0.1", DEFAULT_GPSD_PORT );
485	if(!gps_state->data)
486	perror("gps_open()");
487
488	(void)gps_stream(gps_state->data, WATCH_ENABLE, NULL);
489
490	gps_state->thread_p =
491	g_thread_create(gps_thread, gps_state, FALSE, NULL);
492
493	return gps_state;
494	}
495
496	void gps_release(gps_state_t *gps_state) {
497	gps_unregister_all(gps_state);
498	gps_close(gps_state->data);
499	g_free(gps_state);
500	}
501
502	#endif // USE_LIBGPS
503
504	static void gps_background_enable(gps_state_t *gps_state, gboolean enable) {
505	printf("GPS: %sable background process\n", enable?"en":"dis");
506
507	/* start and stop gps thread by locking and unlocking the control mutex */
508	if(enable) g_mutex_unlock(gps_state->control_mutex);
509	else g_mutex_lock(gps_state->control_mutex);
510	}
511
512	void gps_enable(gps_state_t *gps_state, gboolean enable) {
513	/* just ignore request if we are already in that state */
514	if(gps_state->stopped == !enable) return;
515
516	if(!enable) g_mutex_lock(gps_state->global_mutex);
517	else g_mutex_unlock(gps_state->global_mutex);
518
519	gps_state->stopped = !enable;
520	}
521
522
523	#else
524
525	static void
526	location_changed(LocationGPSDevice device, gps_state_t gps_state) {
527
528	gps_state->set = 0;
529
530	if(device->fix->fields & LOCATION_GPS_DEVICE_LATLONG_SET) {
531	gps_state->set \|= FIX_LATLON_SET \| FIX_HERR_SET;
532	gps_state->fix.latitude = device->fix->latitude;
533	gps_state->fix.longitude = device->fix->longitude;
534	gps_state->fix.eph = device->fix->eph/100.0; // we want eph in meters
535	}
536
537	if(device->fix->fields & LOCATION_GPS_DEVICE_ALTITUDE_SET) {
538	gps_state->set \|= FIX_ALTITUDE_SET;
539	gps_state->fix.altitude = device->fix->altitude;
540	} else
541	gps_state->fix.altitude = NAN;
542
543	if(device->fix->fields & LOCATION_GPS_DEVICE_TRACK_SET) {
544	gps_state->set \|= FIX_TRACK_SET;
545	gps_state->fix.track = device->fix->track;
546	}
547
548	/* tell all clients */
549	g_slist_foreach(gps_state->callbacks, gps_cb_func, gps_state);
550
551	/* remember last state reported */
552	gps_state->last.set = gps_state->set;
553	gps_state->last.fix = gps_state->fix;
554	}
555
556	gps_state_t *gps_init(void) {
557	gps_state_t *gps_state = g_new0(gps_state_t, 1);
558
559	gps_state->device = g_object_new(LOCATION_TYPE_GPS_DEVICE, NULL);
560	if(!gps_state->device) {
561	printf("gps: Unable to connect to liblocation\n");
562	g_free(gps_state);
563	return NULL;
564	}
565
566	gps_state->idd_changed =
567	g_signal_connect(gps_state->device, "changed",
568	G_CALLBACK(location_changed), gps_state);
569	gps_state->connected = TRUE;
570
571	gps_state->control = location_gpsd_control_get_default();
572
573	if(gps_state->control
574	#if MAEMO_VERSION_MAJOR < 5
575	&& gps_state->control->can_control
576	#endif
577	) {
578
579	printf("gps: Having control over GPSD and GPS is to be enabled, starting it\n");
580	location_gpsd_control_start(gps_state->control);
581	}
582	return gps_state;
583	}
584
585	void gps_release(gps_state_t *gps_state) {
586	gps_unregister_all(gps_state);
587
588	if(gps_state->control
589	#if MAEMO_VERSION_MAJOR < 5
590	&& gps_state->control->can_control
591	#endif
592	) {
593	printf("gps: Having control over GPSD, stopping it\n");
594	location_gpsd_control_stop(gps_state->control);
595	}
596
597	/* Disconnect signal */
598	g_signal_handler_disconnect(gps_state->device, gps_state->idd_changed);
599	gps_state->connected = FALSE;
600
601	g_free(gps_state);
602	}
603
604	static void gps_control(gps_state_t *gps_state,
605	gboolean enable0, gboolean enable1) {
606	if(!gps_state->connected) {
607	/* both "enable" signals have to be true */
608	if(enable0 && enable1) {
609	printf("GPS: connecting\n");
610	gps_state->idd_changed =
611	g_signal_connect(gps_state->device, "changed",
612	G_CALLBACK(location_changed), gps_state);
613	gps_state->connected = TRUE;
614	}
615	} else {
616	/* at least one "enable" is false */
617	if(!enable0 \|\| !enable1) {
618	printf("GPS: disconnecting\n");
619	/* Disconnect signal */
620	g_signal_handler_disconnect(gps_state->device, gps_state->idd_changed);
621	gps_state->connected = FALSE;
622	}
623	}
624	}
625
626	/* enable/disable due to all windows being in background */
627	void gps_background_enable(gps_state_t *gps_state, gboolean enable) {
628	if(gps_state->backgrounded == !enable) return;
629
630	gps_state->backgrounded = !enable;
631	gps_control(gps_state, gps_state->stopped, gps_state->backgrounded);
632	}
633
634	/* enable/disable on app request */
635	void gps_enable(gps_state_t *gps_state, gboolean enable) {
636	if(gps_state->stopped == !enable) return;
637
638	gps_state->stopped = !enable;
639	gps_control(gps_state, gps_state->stopped, gps_state->backgrounded);
640	}
641
642	#endif // USE_LIBLOCATION
643
644	static gint compare(gconstpointer a, gconstpointer b) {
645	return ((gps_callback_t*)a)->cb != b;
646	}
647
648	void gps_register_callback(gps_state_t *gps_state, int mask,
649	gps_cb cb, void *data) {
650	/* make sure the callback isn't already registered */
651	GSList *list = g_slist_find_custom(gps_state->callbacks, cb, compare);
652	if(list) {
653	printf("GPS register: ignoring duplicate\n");
654	return;
655	}
656
657	gps_callback_t *callback = g_new0(gps_callback_t, 1);
658
659	g_assert(mask & CHANGED_MASK);
660	g_assert(!(mask & ~CHANGED_MASK));
661
662	callback->mask = mask;
663	callback->cb = cb;
664	callback->data = data;
665
666	#if !defined(ENABLE_LIBLOCATION) && !defined(USE_LIBGPS)
667	/* the gpsd solution needs to know the number of clients */
668	/* requesting sat information */
669	if(mask & SATELLITE_CHANGED)
670	gps_state->sat_requests++;
671	#endif
672
673	gps_state->callbacks = g_slist_append(gps_state->callbacks, callback);
674
675	if(g_slist_length(gps_state->callbacks) == 1)
676	gps_background_enable(gps_state, TRUE);
677	}
678
679	void gps_unregister_callback(gps_state_t *gps_state, gps_cb cb) {
680	/* find callback in list */
681	GSList *list = g_slist_find_custom(gps_state->callbacks, cb, compare);
682	g_assert(list);
683
684	#if !defined(ENABLE_LIBLOCATION) && !defined(USE_LIBGPS)
685	/* the gpsd solution needs to know the number of clients */
686	/* requesting sat information */
687	gps_callback_t *cb_s = list->data;
688	if(cb_s->mask & SATELLITE_CHANGED)
689	gps_state->sat_requests--;
690	#endif
691
692	/* and de-chain and free it */
693	g_free(list->data);
694	gps_state->callbacks = g_slist_remove(gps_state->callbacks, list->data);
695
696	if(g_slist_length(gps_state->callbacks) == 0)
697	gps_background_enable(gps_state, FALSE);
698	}
699
700	static void gps_unregister_all(gps_state_t *gps_state) {
701	g_slist_foreach(gps_state->callbacks, (GFunc)g_free, NULL);
702	g_slist_free(gps_state->callbacks);
703	}