Diff of /trunk/src/gps.c

Parent Directory Parent Directory | Revision Log Revision Log | View Patch Patch

revision 1 by harbaum, Sat Jun 20 11:08:47 2009 UTC revision 193 by harbaum, Tue Nov 17 20:13:09 2009 UTC
# Line 32  Line 32 
32  #include <errno.h>  #include <errno.h>
33  #endif  #endif
34    
35    static void gps_unregister_all(appdata_t *appdata);
36    
37  #ifndef ENABLE_LIBLOCATION  #ifndef ENABLE_LIBLOCATION
38    
39  /* maybe user configurable later on ... */  /* maybe user configurable later on ... */
# Line 97  float gps_get_heading(appdata_t *appdata Line 99  float gps_get_heading(appdata_t *appdata
99    return retval;    return retval;
100  }  }
101    
102  float gps_get_epe(appdata_t *appdata) {  float gps_get_eph(appdata_t *appdata) {
103    float retval = NAN;    float retval = NAN;
104    
105    g_mutex_lock(appdata->gps_state->mutex);    g_mutex_lock(appdata->gps_state->mutex);
# Line 114  static int gps_connect(gps_state_t *gps_ Line 116  static int gps_connect(gps_state_t *gps_
116  #ifdef USE_MAEMO  #ifdef USE_MAEMO
117    char errstr[256] = "";    char errstr[256] = "";
118    
119      if(!gps_state) {
120        printf("No gps state\n");
121        return -1;
122      }
123    
124    /* We need to start gpsd (via gpsbt) first. */    /* We need to start gpsd (via gpsbt) first. */
125    memset(&gps_state->context, 0, sizeof(gpsbt_t));    memset(&gps_state->context, 0, sizeof(gpsbt_t));
126    errno = 0;    errno = 0;
127    
128    if(gpsbt_start(NULL, 0, 0, 0, errstr, sizeof(errstr),    if(gpsbt_start(NULL, 0, 0, 0, errstr, sizeof(errstr),
129                   0, &gps_state->context) < 0) {                   0, &gps_state->context) < 0) {
130      printf("Error connecting to GPS receiver: (%d) %s (%s)\n",      printf("gps: Error connecting to GPS receiver: (%d) %s (%s)\n",
131             errno, strerror(errno), errstr);             errno, strerror(errno), errstr);
132    }    }
133  #endif  #endif
# Line 134  static int gps_connect(gps_state_t *gps_ Line 141  static int gps_connect(gps_state_t *gps_
141    while(retries &&    while(retries &&
142          (GNOME_VFS_OK != (vfs_result = gnome_vfs_inet_connection_create(          (GNOME_VFS_OK != (vfs_result = gnome_vfs_inet_connection_create(
143                  &gps_state->iconn, GPSD_HOST, GPSD_PORT, NULL)))) {                  &gps_state->iconn, GPSD_HOST, GPSD_PORT, NULL)))) {
144      printf("Error creating connection to GPSD, retrying ...\n");      printf("gps: Error creating connection to GPSD, retrying ...\n");
145    
146      retries--;      retries--;
147      sleep(1);      sleep(1);
148    }    }
149    
150    if(!retries) {    if(!retries) {
151      printf("Finally failed ...\n");      printf("gps: Finally failed ...\n");
152      return -1;      return -1;
153    }    }
154    
155    retries = 5;    retries = 5;
156    while(retries && ((gps_state->socket =    while(retries && ((gps_state->socket =
157       gnome_vfs_inet_connection_to_socket(gps_state->iconn)) == NULL)) {       gnome_vfs_inet_connection_to_socket(gps_state->iconn)) == NULL)) {
158      printf("Error creating connecting GPSD socket, retrying ...\n");      printf("gps: Error creating connecting GPSD socket, retrying ...\n");
159    
160      retries--;      retries--;
161      sleep(1);      sleep(1);
162    }    }
163    
164    if(!retries) {    if(!retries) {
165      printf("Finally failed ...\n");      printf("gps: Finally failed ...\n");
166      return -1;      return -1;
167    }    }
168    
169    GTimeVal timeout = { 10, 0 };    GTimeVal timeout = { 10, 0 };
170    if(GNOME_VFS_OK != (vfs_result = gnome_vfs_socket_set_timeout(    if(GNOME_VFS_OK != (vfs_result = gnome_vfs_socket_set_timeout(
171          gps_state->socket, &timeout, NULL))) {          gps_state->socket, &timeout, NULL))) {
172      printf("Error setting GPSD timeout\n");      printf("gps: Error setting GPSD timeout\n");
173      return -1;      return -1;
174    }    }
175    
176    printf("GPSD connected ...\n");    printf("gps: GPSD connected ...\n");
177    
178    return 0;    return 0;
179  }  }
# Line 180  void gps_clear_fix(struct gps_fix_t *fix Line 187  void gps_clear_fix(struct gps_fix_t *fix
187    fixp->altitude = NAN;    fixp->altitude = NAN;
188    fixp->ept = NAN;    fixp->ept = NAN;
189    fixp->eph = NAN;    fixp->eph = NAN;
   fixp->epv = NAN;  
190    fixp->epd = NAN;    fixp->epd = NAN;
191    fixp->eps = NAN;    fixp->eps = NAN;
192    fixp->epc = NAN;    fixp->epc = NAN;
# Line 211  static void gps_unpack(char *buf, struct Line 217  static void gps_unpack(char *buf, struct
217            /* C - cycle isn't supported by gpxview */            /* C - cycle isn't supported by gpxview */
218            /* D - utc time isn't supported by gpxview */            /* D - utc time isn't supported by gpxview */
219          case 'E':          case 'E':
220            gpsdata->epe = gpsdata->fix.eph = gpsdata->fix.epv = NAN;            gpsdata->fix.eph = NAN;
221            /* epe should always be present if eph or epv is */            /* epe should always be present if eph or epv is */
222            if (sp[2] != '?') {            if (sp[2] != '?') {
223              char epe[20], eph[20], epv[20];              char epe[20], eph[20], epv[20];
224              (void)sscanf(sp, "E=%s %s %s", epe, eph, epv);              (void)sscanf(sp, "E=%s %s %s", epe, eph, epv);
225  #define DEFAULT(val) (val[0] == '?') ? NAN : g_ascii_strtod(val, NULL)  #define DEFAULT(val) (val[0] == '?') ? NAN : g_ascii_strtod(val, NULL)
             gpsdata->epe = DEFAULT(epe);  
226              gpsdata->fix.eph = DEFAULT(eph);              gpsdata->fix.eph = DEFAULT(eph);
             gpsdata->fix.epv = DEFAULT(epv);  
227  #undef DEFAULT  #undef DEFAULT
228            }            }
229            break;            break;
# Line 260  static void gps_unpack(char *buf, struct Line 264  static void gps_unpack(char *buf, struct
264                nf.ept = DEFAULT(ept);                nf.ept = DEFAULT(ept);
265                nf.altitude = DEFAULT(alt);                nf.altitude = DEFAULT(alt);
266                nf.eph = DEFAULT(eph);                nf.eph = DEFAULT(eph);
               nf.epv = DEFAULT(epv);  
267                nf.track = DEFAULT(track);                nf.track = DEFAULT(track);
268                nf.speed = DEFAULT(speed);                nf.speed = DEFAULT(speed);
269                nf.climb = DEFAULT(climb);                nf.climb = DEFAULT(climb);
# Line 276  static void gps_unpack(char *buf, struct Line 279  static void gps_unpack(char *buf, struct
279                  gpsdata->set |= ALTITUDE_SET | CLIMB_SET;                  gpsdata->set |= ALTITUDE_SET | CLIMB_SET;
280                if (isnan(nf.eph)==0)                if (isnan(nf.eph)==0)
281                  gpsdata->set |= HERR_SET;                  gpsdata->set |= HERR_SET;
               if (isnan(nf.epv)==0)  
                 gpsdata->set |= VERR_SET;  
282                if (isnan(nf.track)==0)                if (isnan(nf.track)==0)
283                  gpsdata->set |= TRACK_SET | SPEED_SET;                  gpsdata->set |= TRACK_SET | SPEED_SET;
284                if (isnan(nf.eps)==0)                if (isnan(nf.eps)==0)
# Line 377  static void gps_unpack(char *buf, struct Line 378  static void gps_unpack(char *buf, struct
378              for (j = 0; j < gpsdata->satellites; j++) {              for (j = 0; j < gpsdata->satellites; j++) {
379                PRN[j]=elevation[j]=azimuth[j]=ss[j]=used[j]=0;                PRN[j]=elevation[j]=azimuth[j]=ss[j]=used[j]=0;
380              }              }
381              //      printf("sats = %d\n", gpsdata->satellites);              //      printf("gps: sats = %d\n", gpsdata->satellites);
382              for (j = 0, gpsdata->satellites_used = 0; j < gpsdata->satellites; j++) {              for (j = 0, gpsdata->satellites_used = 0; j < gpsdata->satellites; j++) {
383                if ((sp != NULL) && ((sp = strchr(sp, ':')) != NULL)) {                if ((sp != NULL) && ((sp = strchr(sp, ':')) != NULL)) {
384                  sp++;                  sp++;
# Line 405  static void gps_unpack(char *buf, struct Line 406  static void gps_unpack(char *buf, struct
406    }    }
407  }  }
408    
409    /* call one of the application provided gps callbacks */
410    static void do_app_cb(gpointer data, gpointer user_data) {
411      appdata_t *appdata = (appdata_t*)user_data;
412      gps_cb_t *cb = (gps_cb_t*)data;
413      cb->cb(appdata->gps_state, cb->data);
414    }
415    
416    /* walk though list of all application provided callbacks */
417    static gboolean gps_idle_cb(gpointer data) {
418      appdata_t *appdata = (appdata_t*)data;
419      //  printf("gps: idle callback, calling app callbacks\n");
420    
421      g_slist_foreach(appdata->gps_state->cb, do_app_cb, appdata);
422    
423      return FALSE;
424    }
425    
426  gpointer gps_thread(gpointer data) {  gpointer gps_thread(gpointer data) {
427    GnomeVFSFileSize bytes_read;    GnomeVFSFileSize bytes_read;
428    GnomeVFSResult vfs_result;    GnomeVFSResult vfs_result;
# Line 422  gpointer gps_thread(gpointer data) { Line 440  gpointer gps_thread(gpointer data) {
440    while(1) {    while(1) {
441      if(appdata->use_gps) {      if(appdata->use_gps) {
442        if(!connected) {        if(!connected) {
443          printf("trying to connect\n");          printf("gps: trying to connect\n");
444    
445          if(gps_connect(appdata->gps_state) < 0)          if(gps_connect(appdata->gps_state) < 0)
446            sleep(10);            sleep(10);
# Line 448  gpointer gps_thread(gpointer data) { Line 466  gpointer gps_thread(gpointer data) {
466              if(vfs_result == GNOME_VFS_OK) {              if(vfs_result == GNOME_VFS_OK) {
467                str[bytes_read] = 0;                str[bytes_read] = 0;
468    
469                //          printf("msg: %s (%d)\n", str, strlen(str));                //          printf("gps: msg: %s (%d)\n", str, strlen(str));
470    
471                g_mutex_lock(appdata->gps_state->mutex);                g_mutex_lock(appdata->gps_state->mutex);
472    
# Line 458  gpointer gps_thread(gpointer data) { Line 476  gpointer gps_thread(gpointer data) {
476    
477                gps_unpack(str, &appdata->gps_state->gpsdata);                gps_unpack(str, &appdata->gps_state->gpsdata);
478                g_mutex_unlock(appdata->gps_state->mutex);                g_mutex_unlock(appdata->gps_state->mutex);
479                  g_idle_add(gps_idle_cb, appdata);
480              }              }
481            }            }
482          }          }
# Line 465  gpointer gps_thread(gpointer data) { Line 484  gpointer gps_thread(gpointer data) {
484        }        }
485      } else {      } else {
486        if(connected) {        if(connected) {
487          printf("stopping GPS connection due to user request\n");          printf("gps: stopping GPS connection due to user request\n");
488          gnome_vfs_inet_connection_destroy(appdata->gps_state->iconn, NULL);          gnome_vfs_inet_connection_destroy(appdata->gps_state->iconn, NULL);
489    
490  #ifdef USE_MAEMO  #ifdef USE_MAEMO
# Line 477  gpointer gps_thread(gpointer data) { Line 496  gpointer gps_thread(gpointer data) {
496      }      }
497    }    }
498    
499    printf("GPS thread ended???\n");    printf("gps: thread ended???\n");
500    return NULL;    return NULL;
501  }  }
502    
503  void gps_init(appdata_t *appdata) {  void gps_init(appdata_t *appdata) {
504    appdata->gps_state = malloc(sizeof(gps_state_t));    appdata->gps_state = g_new0(gps_state_t, 1);
   memset(appdata->gps_state, 0, sizeof(gps_state_t));  
505    
506    /* start a new thread to listen to gpsd */    /* start a new thread to listen to gpsd */
507    appdata->gps_state->mutex = g_mutex_new();    appdata->gps_state->mutex = g_mutex_new();
# Line 492  void gps_init(appdata_t *appdata) { Line 510  void gps_init(appdata_t *appdata) {
510  }  }
511    
512  void gps_release(appdata_t *appdata) {  void gps_release(appdata_t *appdata) {
513      gps_unregister_all(appdata);
514  #ifdef USE_MAEMO  #ifdef USE_MAEMO
515    gpsbt_stop(&appdata->gps_state->context);    gpsbt_stop(&appdata->gps_state->context);
516  #endif  #endif
517    free(appdata->gps_state);    g_free(appdata->gps_state);
518  }  }
519    
520  #else  #else
521    
 #warning "liblocation interface not completely implemented"  
   
522  static void  static void
523  location_changed(LocationGPSDevice *device, gps_state_t *gps_state) {  location_changed(LocationGPSDevice *device, gps_state_t *gps_state) {
524    
525    gps_state->fix =    gps_state->fields = device->fix->fields;
526      (device->fix->fields & LOCATION_GPS_DEVICE_LATLONG_SET);  
527      if(gps_state->fields & LOCATION_GPS_DEVICE_LATLONG_SET) {
   if(gps_state->fix) {  
528      gps_state->latitude = device->fix->latitude;      gps_state->latitude = device->fix->latitude;
529      gps_state->longitude = device->fix->longitude;      gps_state->longitude = device->fix->longitude;
530        gps_state->eph = device->fix->eph/100.0;  // we want eph in meters
531      }
532    
533      if(gps_state->fields & LOCATION_GPS_DEVICE_TRACK_SET)
534        gps_state->heading = device->fix->track;
535    
536      /* update list of sattelites */
537    
538      /* free old list */
539      if(gps_state->sats.num) {
540        g_free(gps_state->sats.PRN);
541        g_free(gps_state->sats.used);
542        g_free(gps_state->sats.ss);
543        gps_state->sats.num = 0;
544      }
545    
546      /* build new one */
547      if(device->satellites_in_view) {
548        gps_state->sats.PRN  = g_new0(int, device->satellites_in_view);
549        gps_state->sats.used = g_new0(int, device->satellites_in_view);
550        gps_state->sats.ss   = g_new0(int, device->satellites_in_view);
551    
552        int i;
553        for(i=0;i<device->satellites_in_view;i++) {
554          LocationGPSDeviceSatellite *sat =
555            g_ptr_array_index(device->satellites, i);
556    
557          gps_state->sats.PRN[i]  = sat->prn;
558          gps_state->sats.used[i] = sat->in_use;
559          gps_state->sats.ss[i]   = sat->signal_strength;
560        }
561    
562        gps_state->sats.num  = device->satellites_in_view;
563    }    }
564  }  }
565    
566  void gps_init(appdata_t *appdata) {  void gps_init(appdata_t *appdata) {
567    gps_state_t *gps_state = appdata->gps_state = g_new0(gps_state_t, 1);    gps_state_t *gps_state = appdata->gps_state = g_new0(gps_state_t, 1);
568    
569    printf("GPS init: Using liblocation\n");    printf("gps: init: Using liblocation\n");
570    
571    gps_state->device = g_object_new(LOCATION_TYPE_GPS_DEVICE, NULL);    gps_state->device = g_object_new(LOCATION_TYPE_GPS_DEVICE, NULL);
572    if(!gps_state->device) {    if(!gps_state->device) {
573      printf("Unable to connect to liblocation\n");      printf("gps: Unable to connect to liblocation\n");
574      return;      return;
575    }    }
576    
# Line 529  void gps_init(appdata_t *appdata) { Line 578  void gps_init(appdata_t *appdata) {
578      g_signal_connect(gps_state->device, "changed",      g_signal_connect(gps_state->device, "changed",
579                       G_CALLBACK(location_changed), gps_state);                       G_CALLBACK(location_changed), gps_state);
580    
581      gps_state->control = location_gpsd_control_get_default();
582    
583      if(gps_state->control
584    #if MAEMO_VERSION_MAJOR < 5
585         && gps_state->control->can_control
586    #endif
587         ) {
588    
589        printf("gps: Having control over GPSD and GPS is to be enabled, starting it\n");
590        location_gpsd_control_start(gps_state->control);
591      }
592  }  }
593    
594  void gps_release(appdata_t *appdata) {  void gps_release(appdata_t *appdata) {
595    gps_state_t *gps_state = appdata->gps_state;    gps_state_t *gps_state = appdata->gps_state;
596      gps_unregister_all(appdata);
597    
598    if(!gps_state->device) return;    if(!gps_state->device) return;
599    
600      if(gps_state->control
601    #if MAEMO_VERSION_MAJOR < 5
602         && gps_state->control->can_control
603    #endif
604         ) {
605        printf("gps: Having control over GPSD, stopping it\n");
606        location_gpsd_control_stop(gps_state->control);
607      }
608    
609    /* Disconnect signal */    /* Disconnect signal */
610    g_signal_handler_disconnect(gps_state->device, gps_state->idd_changed);    g_signal_handler_disconnect(gps_state->device, gps_state->idd_changed);
# Line 551  pos_t *gps_get_pos(appdata_t *appdata) { Line 621  pos_t *gps_get_pos(appdata_t *appdata) {
621    
622    gps_state_t *gps_state = appdata->gps_state;    gps_state_t *gps_state = appdata->gps_state;
623    
624    if(!gps_state->fix)    if(!(gps_state->fields & LOCATION_GPS_DEVICE_LATLONG_SET))
625      return NULL;      return NULL;
626    
627    pos.lat = gps_state->latitude;    pos.lat = gps_state->latitude;
# Line 560  pos_t *gps_get_pos(appdata_t *appdata) { Line 630  pos_t *gps_get_pos(appdata_t *appdata) {
630    return &pos;    return &pos;
631  }  }
632    
633  float gps_get_heading(appdata_t *appdata) { return NAN; }  float gps_get_heading(appdata_t *appdata) {
634      gps_state_t *gps_state = appdata->gps_state;
635    
636      if(!(gps_state->fields & LOCATION_GPS_DEVICE_TRACK_SET))
637        return NAN;
638    
639      return gps_state->heading;
640    }
641    
642  float gps_get_epe(appdata_t *appdata) { return NAN; }  float gps_get_eph(appdata_t *appdata) {
643      gps_state_t *gps_state = appdata->gps_state;
644    
645      if(!(gps_state->fields & LOCATION_GPS_DEVICE_LATLONG_SET))
646        return NAN;
647    
648  gps_sat_t *gps_get_sats(appdata_t *appdata) { return NULL; }    return gps_state->eph;
649    }
650    
651    gps_sat_t *gps_get_sats(appdata_t *appdata) {
652      gps_sat_t *retval = NULL;
653      gps_state_t *gps_state = appdata->gps_state;
654    
655      if(gps_state->sats.num) {
656        retval = g_new0(gps_sat_t, 1);
657        retval->num = gps_state->sats.num;
658    
659        retval->PRN  = g_memdup(gps_state->sats.PRN,
660                                sizeof(int)*gps_state->sats.num);
661        retval->used = g_memdup(gps_state->sats.used,
662                                sizeof(int)*gps_state->sats.num);
663        retval->ss   = g_memdup(gps_state->sats.ss,
664                                sizeof(int)*gps_state->sats.num);
665      }
666    
667      return retval;
668    }
669    
670    
671  #endif  #endif
672    
673    void *gps_register_callback(appdata_t *appdata, gps_cb cb, gpointer data) {
674      printf("gps: register gps callback\n");
675    
676      if(!appdata->gps_state)
677        return NULL;
678    
679      /* allocate callback info strcuture */
680      gps_cb_t *cb_info = g_new0(gps_cb_t, 1);
681      cb_info->cb = cb;
682      cb_info->data = data;
683    
684      /* and insert it into list of callbacks */
685      appdata->gps_state->cb = g_slist_append(appdata->gps_state->cb, cb_info);
686    
687      return cb_info;
688    }
689    
690    
691    void gps_unregister_callback(appdata_t *appdata, void *cb) {
692      printf("gps: unregister gps callback\n");
693    
694      if(!appdata->gps_state)
695        return;
696    
697      /* the item must be in the list */
698      g_assert(g_slist_find(appdata->gps_state->cb, cb));
699    
700      g_free(cb);
701      appdata->gps_state->cb = g_slist_remove(appdata->gps_state->cb, cb);
702    }
703    
704    static void gps_unregister_all(appdata_t *appdata) {
705      printf("gps: unregister all callbacks: ");
706    
707      while(appdata->gps_state->cb) {
708        printf(".");
709        g_free(appdata->gps_state->cb->data);
710        appdata->gps_state->cb = g_slist_remove(appdata->gps_state->cb,
711                                appdata->gps_state->cb->data);
712      }
713      printf("\n");
714    }
Legend:
Removed from v.1  
changed lines
  Added in v.193