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

#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] = "";

  /* 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");
    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");
    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->track = NAN;
  fixp->speed = NAN;
  fixp->climb = NAN;
  fixp->altitude = NAN;
  fixp->ept = NAN;
  fixp->eph = NAN;
  fixp->epd = NAN;
  fixp->eps = NAN;
  fixp->epc = 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 'A':
          if (sp[2] == '?') {
            gpsdata->fix.altitude = NAN;
          } else {
            (void)sscanf(sp, "A=%lf", &gpsdata->fix.altitude);
            gpsdata->set |= ALTITUDE_SET;
          }
          break;
          /* 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.ept = DEFAULT(ept);
              nf.altitude = DEFAULT(alt);
              nf.eph = DEFAULT(eph);
              nf.track = DEFAULT(track);
              nf.speed = DEFAULT(speed);
              nf.climb = DEFAULT(climb);
              nf.epd = DEFAULT(epd);
              nf.eps = DEFAULT(eps);
              nf.epc = DEFAULT(epc);
#undef DEFAULT
              if (st >= 15)
                nf.mode = (mode[0] == '?') ? MODE_NOT_SEEN : atoi(mode);
              else
                nf.mode = (alt[0] == '?') ? MODE_2D : MODE_3D;
              if (alt[0] != '?')
                gpsdata->set |= ALTITUDE_SET | CLIMB_SET;
              if (isnan(nf.eph)==0)
                gpsdata->set |= HERR_SET;
              if (isnan(nf.track)==0)
                gpsdata->set |= TRACK_SET | SPEED_SET;
              if (isnan(nf.eps)==0)
                gpsdata->set |= SPEEDERR_SET;
              if (isnan(nf.epc)==0)
                gpsdata->set |= CLIMBERR_SET;
              gpsdata->fix = nf;
              gpsdata->set |= TIME_SET|TIMERR_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;
        case 'Q':
          if (sp[2] == '?') {
            gpsdata->satellites_used = 0;
            gpsdata->pdop = 0;
            gpsdata->hdop = 0;
            gpsdata->vdop = 0;
          } else {
            (void)sscanf(sp, "Q=%d %lf %lf %lf %lf %lf",
                         &gpsdata->satellites_used,
                         &gpsdata->pdop,
                         &gpsdata->hdop,
                         &gpsdata->vdop,
                         &gpsdata->tdop,
                         &gpsdata->gdop);
            gpsdata->set |= HDOP_SET | VDOP_SET | PDOP_SET;
          }
          break;
        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=%lf", &gpsdata->fix.track);
            gpsdata->set |= TRACK_SET;
          }
          break;
        case 'U':
          if (sp[2] == '?') {
            gpsdata->fix.climb = NAN;
          } else {
            (void)sscanf(sp, "U=%lf", &gpsdata->fix.climb);
            gpsdata->set |= CLIMB_SET;
          }
          break;
        case 'V':
          if (sp[2] == '?') {
            gpsdata->fix.speed = NAN;
          } else {
            (void)sscanf(sp, "V=%lf", &gpsdata->fix.speed);
            /* V reply is in kt, fix.speed is in metres/sec */
            gpsdata->fix.speed = gpsdata->fix.speed / MPS_TO_KNOTS;
            gpsdata->set |= SPEED_SET;
          }
          break;
        case 'X':
          if (sp[2] == '?') 
            gpsdata->online = -1;
          else {
            (void)sscanf(sp, "X=%lf", &gpsdata->online);
            gpsdata->set |= ONLINE_SET;
          }
          break;
        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("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 */
        }
      }
    }
  }
}

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("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("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 &= 
                         ~(TIME_SET|TIMERR_SET|LATLON_SET|MODE_SET|STATUS_SET);
            
              gps_unpack(str, &appdata->gps_state->gpsdata);
              g_mutex_unlock(appdata->gps_state->mutex);
            }
          }
        }
        if(cnt++ >= 5) cnt = 0;
      }
    } else {
      if(connected) {
        printf("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_release(appdata_t *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->latitude = device->fix->latitude;
    gps_state->longitude = device->fix->longitude;
    gps_state->eph = device->fix->eph/100.0;  // we want eph in meters
  }

  if(gps_state->fields & LOCATION_GPS_DEVICE_TRACK_SET)
    gps_state->heading = 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("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
#endif
     ) {

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

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

  if(gps_state->control
#if MAEMO_VERSION_MAJOR < 5
     && gps_state->control->can_control
#endif     
     ) {
    printf("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->latitude;
  pos.lon = gps_state->longitude;

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

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