trunk/src/gps.c

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

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

#include "gpxview.h"


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

static void gps_unregister_all(appdata_t *appdata);

#ifndef ENABLE_LIBLOCATION

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

gps_sat_t *gps_get_sats(appdata_t *appdata) {
  gps_sat_t *retval = NULL;

  g_mutex_lock(appdata->gps_state->mutex);
  if(appdata->gps_state->gpsdata.set & SATELLITE_SET) {
    int i;
    retval = malloc(sizeof(gps_sat_t));
    retval->num = appdata->gps_state->gpsdata.satellites;

    retval->PRN  = malloc(sizeof(int)*retval->num);
    retval->used = malloc(sizeof(int)*retval->num);
    retval->ss   = malloc(sizeof(int)*retval->num);

    if(retval->num) {
      for(i=0;i<retval->num;i++) {
        retval->PRN[i]  = appdata->gps_state->gpsdata.PRN[i];
        retval->ss[i]   = appdata->gps_state->gpsdata.ss[i];
        retval->used[i] = appdata->gps_state->gpsdata.used[i];
      }
    }
  }

  g_mutex_unlock(appdata->gps_state->mutex);

  return retval;
}

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;
}

float gps_get_heading(appdata_t *appdata) {
  float retval = 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 & TRACK_SET) 
        retval = appdata->gps_state->gpsdata.fix.track;

  g_mutex_unlock(appdata->gps_state->mutex);
  return retval;
}

float gps_get_eph(appdata_t *appdata) {
  float retval = 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) 
      retval = appdata->gps_state->gpsdata.fix.eph;

  g_mutex_unlock(appdata->gps_state->mutex);
  return retval;
}

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 creating 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;
}       

void gps_clear_fix(struct gps_fix_t *fixp) {
  fixp->mode = MODE_NOT_SEEN;
  fixp->pos.lat = fixp->pos.lon = NAN;
  fixp->track = 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) {
          /* A - altitude is not supported */
          /* B - baudrate isn't supported by gpxview */
          /* C - cycle isn't supported by gpxview */
          /* D - utc time isn't supported by gpxview */
        case 'E':
          gpsdata->fix.eph = NAN;
          /* epe should always be present if eph or epv is */
          if (sp[2] != '?') {
            char epe[20], eph[20], epv[20];
            (void)sscanf(sp, "E=%s %s %s", epe, eph, epv);
#define DEFAULT(val) (val[0] == '?') ? NAN : g_ascii_strtod(val, NULL)
            gpsdata->fix.eph = DEFAULT(eph);
#undef DEFAULT
          }
          break;
          /* F - device name isn't supported by gpxview */
          /* I - gps id isn't supported by gpxview */
          /* K - known devices list isn't supported by gpxview */
        case 'M':
          if (sp[2] == '?') {
            gpsdata->fix.mode = MODE_NOT_SEEN;
          } else {
            gpsdata->fix.mode = atoi(sp+2);
            gpsdata->set |= MODE_SET;
          }
          break;
          /* N - driver mode reporting isn't supported by gpxview */
        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];
            char eph[20], epv[20], track[20],speed[20], climb[20];
            char epd[20], eps[20], epc[20], mode[2];
            char timestr[20], ept[20], lat[20], lon[20];
            int st = sscanf(sp+2, 
                            "%8s %19s %19s %19s %19s %19s %19s %19s %19s %19s %19s %19s %19s %19s %1s",
                            tag, timestr, ept, lat, lon,
                            alt, eph, epv, track, speed, climb,
                            epd, eps, epc, mode);
            if (st >= 14) {
#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);
              nf.track = DEFAULT(track);
#undef DEFAULT
              if (st >= 15)
                nf.mode = (mode[0] == '?') ? MODE_NOT_SEEN : atoi(mode);
              else
                nf.mode = (alt[0] == '?') ? MODE_2D : MODE_3D;
              if (isnan(nf.eph)==0)
                gpsdata->set |= HERR_SET;
              if (isnan(nf.track)==0)
                gpsdata->set |= TRACK_SET;
              gpsdata->fix = nf;
              gpsdata->set |= LATLON_SET|MODE_SET;
              gpsdata->status = STATUS_FIX;
              gpsdata->set |= STATUS_SET;
            }
          }
          break;
        case 'P':
          if (sp[2] == '?') {
            gpsdata->fix.pos.lat = NAN;
            gpsdata->fix.pos.lon = NAN;
          } else {
            char lat[20], lon[20];
            (void)sscanf(sp, "P=%19s %19s", lat, lon);
            gpsdata->fix.pos.lat = g_ascii_strtod(lat, NULL);
            gpsdata->fix.pos.lon = g_ascii_strtod(lon, NULL);
            gpsdata->set |= LATLON_SET;
          }
          break;
          /* Q is not supported */
        case 'S':
          if (sp[2] == '?') {
            gpsdata->status = -1;
          } else {
            gpsdata->status = atoi(sp+2);
            gpsdata->set |= STATUS_SET;
          }
          break;
        case 'T':
          if (sp[2] == '?') {
            gpsdata->fix.track = NAN;
          } else {
            (void)sscanf(sp, "T=%f", &gpsdata->fix.track);
            gpsdata->set |= TRACK_SET;
          }
          break;
          /* U - climb is not supported */
          /* V - is not supported */
          /* X - online is not supported */
        case 'Y':
          if (sp[2] == '?') {
            gpsdata->satellites = 0;
          } else {
            int j, i1, i2, i3, i4, i5;
            int PRN[MAXCHANNELS];
            int elevation[MAXCHANNELS], azimuth[MAXCHANNELS];
            int ss[MAXCHANNELS], used[MAXCHANNELS];
            char tag[MAXTAGLEN+1], timestamp[21];
            
            (void)sscanf(sp, "Y=%8s %20s %d ", 
                         tag, timestamp, &gpsdata->satellites);
            for (j = 0; j < gpsdata->satellites; j++) {
              PRN[j]=elevation[j]=azimuth[j]=ss[j]=used[j]=0;
            }
            //      printf("gps: sats = %d\n", gpsdata->satellites);
            for (j = 0, gpsdata->satellites_used = 0; j < gpsdata->satellites; j++) {
              if ((sp != NULL) && ((sp = strchr(sp, ':')) != NULL)) {
                sp++;
                (void)sscanf(sp, "%d %d %d %d %d", &i1, &i2, &i3, &i4, &i5);
                PRN[j] = i1;
                elevation[j] = i2; azimuth[j] = i3;
                ss[j] = i4; used[j] = i5;
                if (i5 == 1)
                  gpsdata->satellites_used++;
              }
            }
            memcpy(gpsdata->PRN, PRN, sizeof(PRN));
            memcpy(gpsdata->elevation, elevation, sizeof(elevation));
            memcpy(gpsdata->azimuth, azimuth,sizeof(azimuth));
                        memcpy(gpsdata->ss, ss, sizeof(ss));
                        memcpy(gpsdata->used, used, sizeof(used));
                        /*@ +compdef @*/
          }
          gpsdata->set |= SATELLITE_SET;
          break;
          /* Z and $ - profiling isn't supported by gpxview */
        }
      }
    }
  }
}

/* call one of the application provided gps callbacks */
static void do_app_cb(gpointer data, gpointer user_data) {
  appdata_t *appdata = (appdata_t*)user_data;
  gps_cb_t *cb = (gps_cb_t*)data;
  cb->cb(appdata->gps_state, cb->data);
}

/* walk though list of all application provided callbacks */
static gboolean gps_idle_cb(gpointer data) {
  appdata_t *appdata = (appdata_t*)data;
  //  printf("gps: idle callback, calling app callbacks\n");

  g_slist_foreach(appdata->gps_state->cb, do_app_cb, appdata);

  return FALSE;
}

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

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

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

  gboolean connected = FALSE;

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

        if(gps_connect(appdata->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(appdata->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(appdata->gps_state->socket,
                               str, sizeof(str)-1, &bytes_read, NULL);
            if(vfs_result == GNOME_VFS_OK) {
              str[bytes_read] = 0; 
            
              //          printf("gps: msg: %s (%d)\n", str, strlen(str));
              
              g_mutex_lock(appdata->gps_state->mutex);
              
              if(!cnt) appdata->gps_state->gpsdata.set &= ~SATELLITE_SET;
              else     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);
              g_idle_add(gps_idle_cb, appdata); 
            }
          }
        }
        if(cnt++ >= 5) cnt = 0;
      }
    } else {
      if(connected) {
        printf("gps: stopping GPS connection due to user request\n");
        gnome_vfs_inet_connection_destroy(appdata->gps_state->iconn, NULL);

#ifdef USE_MAEMO
        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_change_state(appdata_t *appdata) {
}

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

#else

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

  gps_state->fields = device->fix->fields;

  if(gps_state->fields & LOCATION_GPS_DEVICE_LATLONG_SET) { 
    gps_state->fix.pos.lat = device->fix->latitude;
    gps_state->fix.pos.lon = device->fix->longitude;
    gps_state->fix.eph = device->fix->eph/100.0;  // we want eph in meters
  }

  if(gps_state->fields & LOCATION_GPS_DEVICE_TRACK_SET)
    gps_state->fix.track = device->fix->track;
  
  /* update list of sattelites */

  /* free old list */
  if(gps_state->sats.num) {
    g_free(gps_state->sats.PRN);
    g_free(gps_state->sats.used);
    g_free(gps_state->sats.ss);
    gps_state->sats.num = 0;
  }

  /* build new one */
  if(device->satellites_in_view) {
    gps_state->sats.PRN  = g_new0(int, device->satellites_in_view);
    gps_state->sats.used = g_new0(int, device->satellites_in_view);
    gps_state->sats.ss   = g_new0(int, device->satellites_in_view);

    int i;
    for(i=0;i<device->satellites_in_view;i++) {
      LocationGPSDeviceSatellite *sat = 
        g_ptr_array_index(device->satellites, i);

      gps_state->sats.PRN[i]  = sat->prn;
      gps_state->sats.used[i] = sat->in_use;
      gps_state->sats.ss[i]   = sat->signal_strength;
    }

    gps_state->sats.num  = device->satellites_in_view;
  }
}

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("gps: Unable to connect to liblocation\n");
    return;
  }

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

  gps_state->control = location_gpsd_control_get_default();

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

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

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

  /* only do something if gps actually changes state */
  if(( appdata->use_gps &&  gps_state->in_use) ||
     (!appdata->use_gps && !gps_state->in_use))
    return;

  if(gps_state->control
#if MAEMO_VERSION_MAJOR < 5
     && gps_state->control->can_control
#endif
     ) {
    printf("gps: Having control over GPSD and GPS. ");

    if(appdata->use_gps) {
      printf("Starting it!\n");
      location_gpsd_control_start(gps_state->control);
      gps_state->in_use = TRUE;
    } else {
      printf("Stopping it!\n");
      location_gpsd_control_stop(gps_state->control);
      gps_state->in_use = FALSE;
    }
  }
}

void gps_release(appdata_t *appdata) {
  gps_state_t *gps_state = appdata->gps_state;
  gps_unregister_all(appdata);
  
  if(!gps_state->device) return;

  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);
  
  g_free(appdata->gps_state);
  appdata->gps_state = NULL;
}

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

  if(!appdata->use_gps)
    return NULL;

  gps_state_t *gps_state = appdata->gps_state;

  if(!(gps_state->fields & LOCATION_GPS_DEVICE_LATLONG_SET))
    return NULL;

  pos.lat = gps_state->fix.pos.lat;
  pos.lon = gps_state->fix.pos.lon;

  return &pos; 
}

float gps_get_heading(appdata_t *appdata) { 
  gps_state_t *gps_state = appdata->gps_state;
 
  if(!(gps_state->fields & LOCATION_GPS_DEVICE_TRACK_SET))
    return NAN;

  return gps_state->fix.track; 
}

float gps_get_eph(appdata_t *appdata) { 
  gps_state_t *gps_state = appdata->gps_state;
 
  if(!(gps_state->fields & LOCATION_GPS_DEVICE_LATLONG_SET))
    return NAN;

  return gps_state->fix.eph; 
}

gps_sat_t *gps_get_sats(appdata_t *appdata) { 
  gps_sat_t *retval = NULL;
  gps_state_t *gps_state = appdata->gps_state;

  if(gps_state->sats.num) {
    retval = g_new0(gps_sat_t, 1);
    retval->num = gps_state->sats.num;
  
    retval->PRN  = g_memdup(gps_state->sats.PRN, 
                            sizeof(int)*gps_state->sats.num);
    retval->used = g_memdup(gps_state->sats.used, 
                            sizeof(int)*gps_state->sats.num);
    retval->ss   = g_memdup(gps_state->sats.ss, 
                            sizeof(int)*gps_state->sats.num);
  }

  return retval;
}


#endif

void *gps_register_callback(appdata_t *appdata, gps_cb cb, gpointer data) {
  printf("gps: register gps callback\n");

  if(!appdata->gps_state)
    return NULL;

  /* allocate callback info strcuture */
  gps_cb_t *cb_info = g_new0(gps_cb_t, 1);
  cb_info->cb = cb;
  cb_info->data = data;

  /* and insert it into list of callbacks */
  appdata->gps_state->cb = g_slist_append(appdata->gps_state->cb, cb_info);

  return cb_info;
}


void gps_unregister_callback(appdata_t *appdata, void *cb) {
  printf("gps: unregister gps callback\n");

  if(!appdata->gps_state)
    return;

  /* the item must be in the list */
  g_assert(g_slist_find(appdata->gps_state->cb, cb));
    
  g_free(cb);
  appdata->gps_state->cb = g_slist_remove(appdata->gps_state->cb, cb);
}

static void gps_unregister_all(appdata_t *appdata) {
  printf("gps: unregister all callbacks: ");

  while(appdata->gps_state->cb) {
    printf(".");
    g_free(appdata->gps_state->cb->data);
    appdata->gps_state->cb = g_slist_remove(appdata->gps_state->cb,
                            appdata->gps_state->cb->data);
  }
  printf("\n");
}
1	harbaum	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	harbaum	156	static void gps_unregister_all(appdata_t *appdata);
36
37	harbaum	1	#ifndef ENABLE_LIBLOCATION
38
39			/* maybe user configurable later on ... */
40			#define GPSD_HOST "127.0.0.1"
41			#define GPSD_PORT 2947
42
43			gps_sat_t gps_get_sats(appdata_t appdata) {
44			gps_sat_t *retval = NULL;
45
46			g_mutex_lock(appdata->gps_state->mutex);
47			if(appdata->gps_state->gpsdata.set & SATELLITE_SET) {
48			int i;
49			retval = malloc(sizeof(gps_sat_t));
50			retval->num = appdata->gps_state->gpsdata.satellites;
51
52			retval->PRN = malloc(sizeof(int)*retval->num);
53			retval->used = malloc(sizeof(int)*retval->num);
54			retval->ss = malloc(sizeof(int)*retval->num);
55
56			if(retval->num) {
57			for(i=0;i<retval->num;i++) {
58			retval->PRN[i] = appdata->gps_state->gpsdata.PRN[i];
59			retval->ss[i] = appdata->gps_state->gpsdata.ss[i];
60			retval->used[i] = appdata->gps_state->gpsdata.used[i];
61			}
62			}
63			}
64
65			g_mutex_unlock(appdata->gps_state->mutex);
66
67			return retval;
68			}
69
70			pos_t gps_get_pos(appdata_t appdata) {
71			static pos_t retval;
72
73			retval.lat = NAN;
74
75			g_mutex_lock(appdata->gps_state->mutex);
76			if(appdata->gps_state->gpsdata.set & STATUS_SET)
77			if(appdata->gps_state->gpsdata.status != STATUS_NO_FIX)
78			if(appdata->gps_state->gpsdata.set & LATLON_SET)
79			retval = appdata->gps_state->gpsdata.fix.pos;
80
81			g_mutex_unlock(appdata->gps_state->mutex);
82
83			if(isnan(retval.lat))
84			return NULL;
85
86			return &retval;
87			}
88
89			float gps_get_heading(appdata_t *appdata) {
90			float retval = NAN;
91
92			g_mutex_lock(appdata->gps_state->mutex);
93			if(appdata->gps_state->gpsdata.set & STATUS_SET)
94			if(appdata->gps_state->gpsdata.status != STATUS_NO_FIX)
95			if(appdata->gps_state->gpsdata.set & TRACK_SET)
96			retval = appdata->gps_state->gpsdata.fix.track;
97
98			g_mutex_unlock(appdata->gps_state->mutex);
99			return retval;
100			}
101
102	harbaum	53	float gps_get_eph(appdata_t *appdata) {
103	harbaum	1	float retval = NAN;
104
105			g_mutex_lock(appdata->gps_state->mutex);
106			if(appdata->gps_state->gpsdata.set & STATUS_SET)
107			if(appdata->gps_state->gpsdata.status != STATUS_NO_FIX)
108			retval = appdata->gps_state->gpsdata.fix.eph;
109
110			g_mutex_unlock(appdata->gps_state->mutex);
111			return retval;
112			}
113
114			static int gps_connect(gps_state_t *gps_state) {
115			GnomeVFSResult vfs_result;
116			#ifdef USE_MAEMO
117			char errstr[256] = "";
118
119	harbaum	160	if(!gps_state) {
120			printf("No gps state\n");
121			return -1;
122			}
123
124	harbaum	1	/* We need to start gpsd (via gpsbt) first. */
125			memset(&gps_state->context, 0, sizeof(gpsbt_t));
126			errno = 0;
127
128			if(gpsbt_start(NULL, 0, 0, 0, errstr, sizeof(errstr),
129			0, &gps_state->context) < 0) {
130	harbaum	156	printf("gps: Error connecting to GPS receiver: (%d) %s (%s)\n",
131	harbaum	1	errno, strerror(errno), errstr);
132			}
133			#endif
134
135			/************ from here down pure gnome/gtk/gpsd ******************/
136
137			/* try to connect to gpsd */
138			/* Create a socket to interact with GPSD. */
139
140			int retries = 5;
141			while(retries &&
142			(GNOME_VFS_OK != (vfs_result = gnome_vfs_inet_connection_create(
143			&gps_state->iconn, GPSD_HOST, GPSD_PORT, NULL)))) {
144	harbaum	156	printf("gps: Error creating connection to GPSD, retrying ...\n");
145	harbaum	1
146			retries--;
147			sleep(1);
148			}
149
150			if(!retries) {
151	harbaum	156	printf("gps: Finally failed ...\n");
152	harbaum	1	return -1;
153			}
154
155			retries = 5;
156			while(retries && ((gps_state->socket =
157			gnome_vfs_inet_connection_to_socket(gps_state->iconn)) == NULL)) {
158	harbaum	156	printf("gps: Error creating connecting GPSD socket, retrying ...\n");
159	harbaum	1
160			retries--;
161			sleep(1);
162			}
163
164			if(!retries) {
165	harbaum	156	printf("gps: Finally failed ...\n");
166	harbaum	1	return -1;
167			}
168
169			GTimeVal timeout = { 10, 0 };
170			if(GNOME_VFS_OK != (vfs_result = gnome_vfs_socket_set_timeout(
171			gps_state->socket, &timeout, NULL))) {
172	harbaum	156	printf("gps: Error setting GPSD timeout\n");
173	harbaum	1	return -1;
174			}
175
176	harbaum	156	printf("gps: GPSD connected ...\n");
177	harbaum	1
178			return 0;
179			}
180
181			void gps_clear_fix(struct gps_fix_t *fixp) {
182			fixp->mode = MODE_NOT_SEEN;
183			fixp->pos.lat = fixp->pos.lon = NAN;
184			fixp->track = NAN;
185			fixp->eph = NAN;
186			}
187
188			/* unpack a daemon response into a status structure */
189			static void gps_unpack(char buf, struct gps_data_t gpsdata) {
190			char ns, sp, *tp;
191
192			for(ns = buf; ns; ns = strstr(ns+1, "GPSD")) {
193			if(strncmp(ns, "GPSD", 4) == 0) {
194			/* the following should execute each time we have a good next sp */
195			for (sp = ns + 5; *sp != '\0'; sp = tp+1) {
196			tp = sp + strcspn(sp, ",\r\n");
197			if (*tp == '\0') tp--;
198			else *tp = '\0';
199
200			switch (*sp) {
201	harbaum	233	/* A - altitude is not supported */
202	harbaum	1	/* B - baudrate isn't supported by gpxview */
203			/* C - cycle isn't supported by gpxview */
204			/* D - utc time isn't supported by gpxview */
205			case 'E':
206	harbaum	53	gpsdata->fix.eph = NAN;
207	harbaum	1	/* epe should always be present if eph or epv is */
208			if (sp[2] != '?') {
209			char epe[20], eph[20], epv[20];
210			(void)sscanf(sp, "E=%s %s %s", epe, eph, epv);
211			#define DEFAULT(val) (val[0] == '?') ? NAN : g_ascii_strtod(val, NULL)
212			gpsdata->fix.eph = DEFAULT(eph);
213			#undef DEFAULT
214			}
215			break;
216			/* F - device name isn't supported by gpxview */
217			/* I - gps id isn't supported by gpxview */
218			/* K - known devices list isn't supported by gpxview */
219			case 'M':
220			if (sp[2] == '?') {
221			gpsdata->fix.mode = MODE_NOT_SEEN;
222			} else {
223			gpsdata->fix.mode = atoi(sp+2);
224			gpsdata->set \|= MODE_SET;
225			}
226			break;
227			/* N - driver mode reporting isn't supported by gpxview */
228			case 'O':
229			if (sp[2] == '?') {
230			gpsdata->set =
231			(gpsdata->set & SATELLITE_SET) \| // fix for below
232			MODE_SET \| STATUS_SET; // this clears sat info??
233			gpsdata->status = STATUS_NO_FIX;
234			gps_clear_fix(&gpsdata->fix);
235			} else {
236			struct gps_fix_t nf;
237			char tag[MAXTAGLEN+1], alt[20];
238			char eph[20], epv[20], track[20],speed[20], climb[20];
239			char epd[20], eps[20], epc[20], mode[2];
240			char timestr[20], ept[20], lat[20], lon[20];
241			int st = sscanf(sp+2,
242			"%8s %19s %19s %19s %19s %19s %19s %19s %19s %19s %19s %19s %19s %19s %1s",
243			tag, timestr, ept, lat, lon,
244			alt, eph, epv, track, speed, climb,
245			epd, eps, epc, mode);
246			if (st >= 14) {
247			#define DEFAULT(val) (val[0] == '?') ? NAN : g_ascii_strtod(val, NULL)
248			nf.pos.lat = DEFAULT(lat);
249			nf.pos.lon = DEFAULT(lon);
250			nf.eph = DEFAULT(eph);
251			nf.track = DEFAULT(track);
252			#undef DEFAULT
253			if (st >= 15)
254			nf.mode = (mode[0] == '?') ? MODE_NOT_SEEN : atoi(mode);
255			else
256			nf.mode = (alt[0] == '?') ? MODE_2D : MODE_3D;
257			if (isnan(nf.eph)==0)
258			gpsdata->set \|= HERR_SET;
259			if (isnan(nf.track)==0)
260	harbaum	233	gpsdata->set \|= TRACK_SET;
261	harbaum	1	gpsdata->fix = nf;
262	harbaum	233	gpsdata->set \|= LATLON_SET\|MODE_SET;
263	harbaum	1	gpsdata->status = STATUS_FIX;
264			gpsdata->set \|= STATUS_SET;
265			}
266			}
267			break;
268			case 'P':
269			if (sp[2] == '?') {
270			gpsdata->fix.pos.lat = NAN;
271			gpsdata->fix.pos.lon = NAN;
272			} else {
273			char lat[20], lon[20];
274			(void)sscanf(sp, "P=%19s %19s", lat, lon);
275			gpsdata->fix.pos.lat = g_ascii_strtod(lat, NULL);
276			gpsdata->fix.pos.lon = g_ascii_strtod(lon, NULL);
277			gpsdata->set \|= LATLON_SET;
278			}
279			break;
280	harbaum	233	/* Q is not supported */
281	harbaum	1	case 'S':
282			if (sp[2] == '?') {
283			gpsdata->status = -1;
284			} else {
285			gpsdata->status = atoi(sp+2);
286			gpsdata->set \|= STATUS_SET;
287			}
288			break;
289			case 'T':
290			if (sp[2] == '?') {
291			gpsdata->fix.track = NAN;
292			} else {
293	harbaum	233	(void)sscanf(sp, "T=%f", &gpsdata->fix.track);
294	harbaum	1	gpsdata->set \|= TRACK_SET;
295			}
296			break;
297	harbaum	233	/* U - climb is not supported */
298			/* V - is not supported */
299			/* X - online is not supported */
300	harbaum	1	case 'Y':
301			if (sp[2] == '?') {
302			gpsdata->satellites = 0;
303			} else {
304			int j, i1, i2, i3, i4, i5;
305			int PRN[MAXCHANNELS];
306			int elevation[MAXCHANNELS], azimuth[MAXCHANNELS];
307			int ss[MAXCHANNELS], used[MAXCHANNELS];
308			char tag[MAXTAGLEN+1], timestamp[21];
309
310			(void)sscanf(sp, "Y=%8s %20s %d ",
311			tag, timestamp, &gpsdata->satellites);
312			for (j = 0; j < gpsdata->satellites; j++) {
313			PRN[j]=elevation[j]=azimuth[j]=ss[j]=used[j]=0;
314			}
315	harbaum	156	// printf("gps: sats = %d\n", gpsdata->satellites);
316	harbaum	1	for (j = 0, gpsdata->satellites_used = 0; j < gpsdata->satellites; j++) {
317			if ((sp != NULL) && ((sp = strchr(sp, ':')) != NULL)) {
318			sp++;
319			(void)sscanf(sp, "%d %d %d %d %d", &i1, &i2, &i3, &i4, &i5);
320			PRN[j] = i1;
321			elevation[j] = i2; azimuth[j] = i3;
322			ss[j] = i4; used[j] = i5;
323			if (i5 == 1)
324			gpsdata->satellites_used++;
325			}
326			}
327			memcpy(gpsdata->PRN, PRN, sizeof(PRN));
328			memcpy(gpsdata->elevation, elevation, sizeof(elevation));
329			memcpy(gpsdata->azimuth, azimuth,sizeof(azimuth));
330			memcpy(gpsdata->ss, ss, sizeof(ss));
331			memcpy(gpsdata->used, used, sizeof(used));
332			/@ +compdef @/
333			}
334			gpsdata->set \|= SATELLITE_SET;
335			break;
336			/* Z and $ - profiling isn't supported by gpxview */
337			}
338			}
339			}
340			}
341			}
342
343	harbaum	156	/* call one of the application provided gps callbacks */
344			static void do_app_cb(gpointer data, gpointer user_data) {
345			appdata_t appdata = (appdata_t)user_data;
346			gps_cb_t cb = (gps_cb_t)data;
347			cb->cb(appdata->gps_state, cb->data);
348			}
349
350			/* walk though list of all application provided callbacks */
351			static gboolean gps_idle_cb(gpointer data) {
352			appdata_t appdata = (appdata_t)data;
353	harbaum	193	// printf("gps: idle callback, calling app callbacks\n");
354	harbaum	156
355			g_slist_foreach(appdata->gps_state->cb, do_app_cb, appdata);
356
357			return FALSE;
358			}
359
360	harbaum	1	gpointer gps_thread(gpointer data) {
361			GnomeVFSFileSize bytes_read;
362			GnomeVFSResult vfs_result;
363			char str[512];
364			appdata_t appdata = (appdata_t)data;
365			int cnt=1000;
366
367			const char msg_pos = "o\r\n"; / pos request */
368			const char msg_sat = "y\r\n"; / sat request */
369
370			appdata->gps_state->gpsdata.set = 0;
371
372			gboolean connected = FALSE;
373
374			while(1) {
375			if(appdata->use_gps) {
376			if(!connected) {
377	harbaum	156	printf("gps: trying to connect\n");
378	harbaum	1
379			if(gps_connect(appdata->gps_state) < 0)
380			sleep(10);
381			else
382			connected = TRUE;
383			} else {
384			const char *msg;
385			if(!cnt) msg = msg_sat;
386			else msg = msg_pos;
387
388			if(GNOME_VFS_OK ==
389			(vfs_result = gnome_vfs_socket_write(appdata->gps_state->socket,
390			msg, strlen(msg)+1, &bytes_read, NULL))) {
391
392			/* update every second, wait here to make sure a complete */
393			/* reply is received */
394			if(cnt <= 1) usleep(500000);
395			else sleep(1);
396
397			if(bytes_read == (strlen(msg)+1)) {
398			vfs_result = gnome_vfs_socket_read(appdata->gps_state->socket,
399			str, sizeof(str)-1, &bytes_read, NULL);
400			if(vfs_result == GNOME_VFS_OK) {
401			str[bytes_read] = 0;
402
403	harbaum	156	// printf("gps: msg: %s (%d)\n", str, strlen(str));
404	harbaum	1
405			g_mutex_lock(appdata->gps_state->mutex);
406
407			if(!cnt) appdata->gps_state->gpsdata.set &= ~SATELLITE_SET;
408			else appdata->gps_state->gpsdata.set &=
409	harbaum	233	~(LATLON_SET\|MODE_SET\|STATUS_SET);
410	harbaum	1
411			gps_unpack(str, &appdata->gps_state->gpsdata);
412			g_mutex_unlock(appdata->gps_state->mutex);
413	harbaum	156	g_idle_add(gps_idle_cb, appdata);
414	harbaum	1	}
415			}
416			}
417			if(cnt++ >= 5) cnt = 0;
418			}
419			} else {
420			if(connected) {
421	harbaum	156	printf("gps: stopping GPS connection due to user request\n");
422	harbaum	1	gnome_vfs_inet_connection_destroy(appdata->gps_state->iconn, NULL);
423
424			#ifdef USE_MAEMO
425			gpsbt_stop(&appdata->gps_state->context);
426			#endif
427			connected = FALSE;
428			} else
429			sleep(1);
430			}
431			}
432
433	harbaum	156	printf("gps: thread ended???\n");
434	harbaum	1	return NULL;
435			}
436
437			void gps_init(appdata_t *appdata) {
438	harbaum	8	appdata->gps_state = g_new0(gps_state_t, 1);
439	harbaum	1
440			/* start a new thread to listen to gpsd */
441			appdata->gps_state->mutex = g_mutex_new();
442			appdata->gps_state->thread_p =
443			g_thread_create(gps_thread, appdata, FALSE, NULL);
444			}
445
446	harbaum	243	void gps_change_state(appdata_t *appdata) {
447			}
448
449	harbaum	1	void gps_release(appdata_t *appdata) {
450	harbaum	156	gps_unregister_all(appdata);
451	harbaum	1	#ifdef USE_MAEMO
452			gpsbt_stop(&appdata->gps_state->context);
453			#endif
454	harbaum	8	g_free(appdata->gps_state);
455	harbaum	1	}
456
457			#else
458
459			static void
460			location_changed(LocationGPSDevice device, gps_state_t gps_state) {
461
462	harbaum	8	gps_state->fields = device->fix->fields;
463
464			if(gps_state->fields & LOCATION_GPS_DEVICE_LATLONG_SET) {
465	harbaum	233	gps_state->fix.pos.lat = device->fix->latitude;
466			gps_state->fix.pos.lon = device->fix->longitude;
467			gps_state->fix.eph = device->fix->eph/100.0; // we want eph in meters
468	harbaum	8	}
469	harbaum	7
470	harbaum	8	if(gps_state->fields & LOCATION_GPS_DEVICE_TRACK_SET)
471	harbaum	233	gps_state->fix.track = device->fix->track;
472	harbaum	8
473			/* update list of sattelites */
474	harbaum	7
475	harbaum	8	/* free old list */
476			if(gps_state->sats.num) {
477			g_free(gps_state->sats.PRN);
478			g_free(gps_state->sats.used);
479			g_free(gps_state->sats.ss);
480			gps_state->sats.num = 0;
481	harbaum	1	}
482	harbaum	8
483			/* build new one */
484			if(device->satellites_in_view) {
485			gps_state->sats.PRN = g_new0(int, device->satellites_in_view);
486			gps_state->sats.used = g_new0(int, device->satellites_in_view);
487			gps_state->sats.ss = g_new0(int, device->satellites_in_view);
488
489			int i;
490			for(i=0;i<device->satellites_in_view;i++) {
491			LocationGPSDeviceSatellite *sat =
492			g_ptr_array_index(device->satellites, i);
493
494			gps_state->sats.PRN[i] = sat->prn;
495			gps_state->sats.used[i] = sat->in_use;
496			gps_state->sats.ss[i] = sat->signal_strength;
497			}
498
499			gps_state->sats.num = device->satellites_in_view;
500			}
501	harbaum	1	}
502
503			void gps_init(appdata_t *appdata) {
504			gps_state_t *gps_state = appdata->gps_state = g_new0(gps_state_t, 1);
505
506	harbaum	156	printf("gps: init: Using liblocation\n");
507	harbaum	1
508			gps_state->device = g_object_new(LOCATION_TYPE_GPS_DEVICE, NULL);
509			if(!gps_state->device) {
510	harbaum	156	printf("gps: Unable to connect to liblocation\n");
511	harbaum	1	return;
512			}
513
514			gps_state->idd_changed =
515			g_signal_connect(gps_state->device, "changed",
516			G_CALLBACK(location_changed), gps_state);
517
518	harbaum	11	gps_state->control = location_gpsd_control_get_default();
519
520	harbaum	36	if(gps_state->control
521			#if MAEMO_VERSION_MAJOR < 5
522			&& gps_state->control->can_control
523	harbaum	243	&& appdata->use_gps
524	harbaum	36	#endif
525			) {
526
527	harbaum	156	printf("gps: Having control over GPSD and GPS is to be enabled, starting it\n");
528	harbaum	11	location_gpsd_control_start(gps_state->control);
529	harbaum	243	gps_state->in_use = TRUE;
530			} else
531			gps_state->in_use = FALSE;
532			}
533
534			void gps_change_state(appdata_t *appdata) {
535			gps_state_t *gps_state = appdata->gps_state;
536
537			/* only do something if gps actually changes state */
538			if(( appdata->use_gps && gps_state->in_use) \|\|
539			(!appdata->use_gps && !gps_state->in_use))
540			return;
541
542			if(gps_state->control
543			#if MAEMO_VERSION_MAJOR < 5
544			&& gps_state->control->can_control
545			#endif
546			) {
547			printf("gps: Having control over GPSD and GPS. ");
548
549			if(appdata->use_gps) {
550			printf("Starting it!\n");
551			location_gpsd_control_start(gps_state->control);
552			gps_state->in_use = TRUE;
553			} else {
554			printf("Stopping it!\n");
555			location_gpsd_control_stop(gps_state->control);
556			gps_state->in_use = FALSE;
557			}
558	harbaum	11	}
559	harbaum	1	}
560
561			void gps_release(appdata_t *appdata) {
562			gps_state_t *gps_state = appdata->gps_state;
563	harbaum	156	gps_unregister_all(appdata);
564	harbaum	1
565			if(!gps_state->device) return;
566	harbaum	11
567	harbaum	36	if(gps_state->control
568	harbaum	11	#if MAEMO_VERSION_MAJOR < 5
569	harbaum	36	&& gps_state->control->can_control
570			#endif
571			) {
572	harbaum	156	printf("gps: Having control over GPSD, stopping it\n");
573	harbaum	11	location_gpsd_control_stop(gps_state->control);
574			}
575	harbaum	1
576			/* Disconnect signal */
577			g_signal_handler_disconnect(gps_state->device, gps_state->idd_changed);
578
579			g_free(appdata->gps_state);
580			appdata->gps_state = NULL;
581			}
582
583			pos_t gps_get_pos(appdata_t appdata) {
584			static pos_t pos;
585
586			if(!appdata->use_gps)
587			return NULL;
588
589			gps_state_t *gps_state = appdata->gps_state;
590
591	harbaum	8	if(!(gps_state->fields & LOCATION_GPS_DEVICE_LATLONG_SET))
592	harbaum	1	return NULL;
593
594	harbaum	233	pos.lat = gps_state->fix.pos.lat;
595			pos.lon = gps_state->fix.pos.lon;
596	harbaum	1
597			return &pos;
598			}
599
600	harbaum	7	float gps_get_heading(appdata_t *appdata) {
601			gps_state_t *gps_state = appdata->gps_state;
602
603	harbaum	8	if(!(gps_state->fields & LOCATION_GPS_DEVICE_TRACK_SET))
604	harbaum	7	return NAN;
605	harbaum	1
606	harbaum	233	return gps_state->fix.track;
607	harbaum	7	}
608	harbaum	1
609	harbaum	53	float gps_get_eph(appdata_t *appdata) {
610	harbaum	7	gps_state_t *gps_state = appdata->gps_state;
611
612	harbaum	8	if(!(gps_state->fields & LOCATION_GPS_DEVICE_LATLONG_SET))
613	harbaum	7	return NAN;
614
615	harbaum	233	return gps_state->fix.eph;
616	harbaum	7	}
617
618	harbaum	8	gps_sat_t gps_get_sats(appdata_t appdata) {
619			gps_sat_t *retval = NULL;
620			gps_state_t *gps_state = appdata->gps_state;
621	harbaum	1
622	harbaum	8	if(gps_state->sats.num) {
623			retval = g_new0(gps_sat_t, 1);
624			retval->num = gps_state->sats.num;
625
626			retval->PRN = g_memdup(gps_state->sats.PRN,
627			sizeof(int)*gps_state->sats.num);
628			retval->used = g_memdup(gps_state->sats.used,
629			sizeof(int)*gps_state->sats.num);
630			retval->ss = g_memdup(gps_state->sats.ss,
631			sizeof(int)*gps_state->sats.num);
632			}
633	harbaum	1
634	harbaum	8	return retval;
635			}
636
637
638	harbaum	1	#endif
639	harbaum	149
640			void gps_register_callback(appdata_t appdata, gps_cb cb, gpointer data) {
641	harbaum	156	printf("gps: register gps callback\n");
642
643			if(!appdata->gps_state)
644			return NULL;
645
646			/* allocate callback info strcuture */
647			gps_cb_t *cb_info = g_new0(gps_cb_t, 1);
648			cb_info->cb = cb;
649			cb_info->data = data;
650
651			/* and insert it into list of callbacks */
652			appdata->gps_state->cb = g_slist_append(appdata->gps_state->cb, cb_info);
653
654			return cb_info;
655	harbaum	149	}
656
657
658			void gps_unregister_callback(appdata_t appdata, void cb) {
659	harbaum	156	printf("gps: unregister gps callback\n");
660
661			if(!appdata->gps_state)
662			return;
663
664			/* the item must be in the list */
665			g_assert(g_slist_find(appdata->gps_state->cb, cb));
666
667			g_free(cb);
668			appdata->gps_state->cb = g_slist_remove(appdata->gps_state->cb, cb);
669	harbaum	149	}
670	harbaum	156
671			static void gps_unregister_all(appdata_t *appdata) {
672			printf("gps: unregister all callbacks: ");
673
674			while(appdata->gps_state->cb) {
675			printf(".");
676			g_free(appdata->gps_state->cb->data);
677			appdata->gps_state->cb = g_slist_remove(appdata->gps_state->cb,
678			appdata->gps_state->cb->data);
679			}
680			printf("\n");
681			}