geoclue-gpsd.c 12.4 KB
Newer Older
1 2
/*
 * Geoclue
Jussi Kukkonen's avatar
Jussi Kukkonen committed
3
 * geoclue-gpsd.c - Geoclue Position backend for gpsd
4 5 6 7 8
 *
 * Authors: Jussi Kukkonen <jku@o-hand.com>
 * Copyright 2007 by Garmin Ltd. or its subsidiaries
 */

9
/* TODO:
10
 * 
11 12
 * 	call to gps_set_callback blocks for a long time if 
 * 	BT device is not present.
13
 * 
14
 **/
15

16 17
#include <config.h>

18
#include <math.h>
19
#include <gps.h>
Jussi Kukkonen's avatar
Jussi Kukkonen committed
20
#include <string.h>
21

Jussi Kukkonen's avatar
Jussi Kukkonen committed
22
#include <geoclue/geoclue-error.h>
23 24 25 26
#include <geoclue/gc-provider.h>
#include <geoclue/gc-iface-position.h>
#include <geoclue/gc-iface-velocity.h>

27
typedef struct gps_data_t gps_data;
28
typedef struct gps_fix_t gps_fix;
29

Jussi Kukkonen's avatar
Jussi Kukkonen committed
30 31 32 33 34 35 36 37 38 39
/* only listing used tags */
typedef enum {
	NMEA_NONE,
	NMEA_GSA,
	NMEA_GGA,
	NMEA_GSV,
	NMEA_RMC
} NmeaTag;


40 41 42
typedef struct {
	GcProvider parent;
	
Jussi Kukkonen's avatar
Jussi Kukkonen committed
43 44 45
	char *host;
	char *port;
	
46
	pthread_t *gps_thread;
47
	gps_data *gpsdata;
48 49
	
	gps_fix *last_fix;
50
	
51
	GeoclueStatus last_status;
52 53 54
	GeocluePositionFields last_pos_fields;
	GeoclueAccuracy *last_accuracy;
	GeoclueVelocityFields last_velo_fields;
55
	
56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74
	GMainLoop *loop;
} GeoclueGpsd;

typedef struct {
	GcProviderClass parent_class;
} GeoclueGpsdClass;

static void geoclue_gpsd_position_init (GcIfacePositionClass *iface);
static void geoclue_gpsd_velocity_init (GcIfaceVelocityClass *iface);

#define GEOCLUE_TYPE_GPSD (geoclue_gpsd_get_type ())
#define GEOCLUE_GPSD(obj) (G_TYPE_CHECK_INSTANCE_CAST ((obj), GEOCLUE_TYPE_GPSD, GeoclueGpsd))

G_DEFINE_TYPE_WITH_CODE (GeoclueGpsd, geoclue_gpsd, GC_TYPE_PROVIDER,
                         G_IMPLEMENT_INTERFACE (GC_TYPE_IFACE_POSITION,
                                                geoclue_gpsd_position_init)
                         G_IMPLEMENT_INTERFACE (GC_TYPE_IFACE_VELOCITY,
                                                geoclue_gpsd_velocity_init))

75
static void geoclue_gpsd_stop_gpsd (GeoclueGpsd *self);
Jussi Kukkonen's avatar
Jussi Kukkonen committed
76
static gboolean geoclue_gpsd_start_gpsd (GeoclueGpsd *self);
77

78 79 80 81 82 83 84

/* defining global GeoclueGpsd because gpsd does not support "user_data"
 * pointers in callbacks */
GeoclueGpsd *gpsd;



85
/* Geoclue interface */
86 87
static gboolean
get_status (GcIfaceGeoclue *gc,
88
            GeoclueStatus  *status,
89 90
            GError        **error)
{
91 92 93
	GeoclueGpsd *gpsd = GEOCLUE_GPSD (gc);
	
	*status = gpsd->last_status;
94 95 96
	return TRUE;
}

97 98
static void
shutdown (GcProvider *provider)
99
{
100
	GeoclueGpsd *gpsd = GEOCLUE_GPSD (provider);
101
	
102
	g_main_loop_quit (gpsd->loop);
103 104
}

Jussi Kukkonen's avatar
Jussi Kukkonen committed
105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120
static void
geoclue_gpsd_set_status (GeoclueGpsd *self, GeoclueStatus status)
{
	if (status != self->last_status) {
		self->last_status = status;
		
		/* make position and velocity invalid if no fix */
		if (status != GEOCLUE_STATUS_AVAILABLE) {
			self->last_pos_fields = GEOCLUE_POSITION_FIELDS_NONE;
			self->last_velo_fields = GEOCLUE_VELOCITY_FIELDS_NONE;
		}
		gc_iface_geoclue_emit_status_changed (GC_IFACE_GEOCLUE (self),
		                                      status);
	}
}

121 122 123 124
static gboolean
set_options (GcIfaceGeoclue *gc,
             GHashTable     *options,
             GError        **error)
125
{
126
	GeoclueGpsd *gpsd = GEOCLUE_GPSD (gc);
Jussi Kukkonen's avatar
Jussi Kukkonen committed
127 128
	char *port, *host;
	gboolean changed = FALSE;
129 130
	
	host = g_hash_table_lookup (options, 
Jussi Kukkonen's avatar
Jussi Kukkonen committed
131
	                                  "org.freedesktop.Geoclue.GPSHost");
132
	port = g_hash_table_lookup (options, 
Jussi Kukkonen's avatar
Jussi Kukkonen committed
133 134 135
	                                  "org.freedesktop.Geoclue.GPSPort");
	
	if (port == NULL) {
136 137
		port = DEFAULT_GPSD_PORT;
	}
138
	
Jussi Kukkonen's avatar
Jussi Kukkonen committed
139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157
	/* new values? */
	if (host != NULL && gpsd->host != NULL) {
		changed = (strcmp (host, gpsd->host) != 0);
	} else if (!(host == NULL && gpsd->host == NULL)) {
		changed = TRUE;
	}
	if (!changed) {
		changed = (strcmp (port, gpsd->port) != 0);
	}
	
	if (!changed) {
		return TRUE;
	}
	
	/* update private values with new ones, restart gpsd */
	g_free (gpsd->port);
	if (gpsd->host) {
		g_free (gpsd->host);
	}
158
	geoclue_gpsd_stop_gpsd (gpsd);
Jussi Kukkonen's avatar
Jussi Kukkonen committed
159 160 161 162 163 164 165 166 167
	gpsd->port = g_strdup (port);
	gpsd->host = g_strdup (host);
	if (!geoclue_gpsd_start_gpsd (gpsd)) {
		geoclue_gpsd_set_status (gpsd, GEOCLUE_STATUS_ERROR);
		g_set_error (error, GEOCLUE_ERROR,
		             GEOCLUE_ERROR_FAILED, "Gpsd not found");
		return FALSE;
	}
	return TRUE;
168 169
}

170
static void
171
finalize (GObject *object)
172
{
173
	GeoclueGpsd *gpsd = GEOCLUE_GPSD (object);
174
	
175 176 177
	geoclue_gpsd_stop_gpsd (gpsd);
	g_free (gpsd->last_fix);
	geoclue_accuracy_free (gpsd->last_accuracy);
178
	
Jussi Kukkonen's avatar
Jussi Kukkonen committed
179 180 181 182 183 184
	g_free (gpsd->port);
	if (gpsd->host) {
		g_free (gpsd->host);
	}
	
	((GObjectClass *) geoclue_gpsd_parent_class)->finalize (object);
185 186 187 188 189 190 191
}

static void
geoclue_gpsd_class_init (GeoclueGpsdClass *klass)
{
	GObjectClass *o_class = (GObjectClass *) klass;
	GcProviderClass *p_class = (GcProviderClass *) klass;
192
	
193
	o_class->finalize = finalize;
194
	
195
	p_class->get_status = get_status;
196
	p_class->set_options = set_options;
197 198 199
	p_class->shutdown = shutdown;
}

200

201 202 203 204 205 206 207 208
static gboolean
equal_or_nan (double a, double b)
{
	if (isnan (a) && isnan (b)) {
		return TRUE;
	}
	return a == b;
}
209

210
static void
Jussi Kukkonen's avatar
Jussi Kukkonen committed
211
geoclue_gpsd_update_position (GeoclueGpsd *gpsd, NmeaTag nmea_tag)
212
{
213 214
	gps_fix *fix = &gpsd->gpsdata->fix;
	gps_fix *last_fix = gpsd->last_fix;
215
	
216 217 218 219 220
	last_fix->time = fix->time;
	
	/* If a flag is not set, bail out.*/
	if (!((gpsd->gpsdata->set & LATLON_SET) || (gpsd->gpsdata->set & ALTITUDE_SET))) {
		return;
221
	}
222
	gpsd->gpsdata->set &= ~(LATLON_SET | ALTITUDE_SET);
223
	
224 225 226 227 228 229 230 231 232 233 234 235 236
	if (equal_or_nan (fix->latitude, last_fix->latitude) &&
	    equal_or_nan (fix->longitude, last_fix->longitude) &&
	    equal_or_nan (fix->altitude, last_fix->altitude)) {
		/* position has not changed */
		return;
	}
	
	/* save values */
	last_fix->latitude = fix->latitude;
	last_fix->longitude = fix->longitude;
	last_fix->altitude = fix->altitude;
	
	/* Could use fix.eph for accuracy, but eph is 
237 238 239 240
	 * often NaN... what then? 
	 * Could also use fix mode (2d/3d) to decide vertical accuracy, 
	 * but gpsd updates that so erratically that I couldn't
	 * be arsed so far */
241 242 243 244 245 246 247 248 249 250 251
	geoclue_accuracy_set_details (gpsd->last_accuracy,
	                              GEOCLUE_ACCURACY_LEVEL_DETAILED,
	                              24, 60);
	
	gpsd->last_pos_fields = GEOCLUE_POSITION_FIELDS_NONE;
	gpsd->last_pos_fields |= (isnan (fix->latitude)) ? 
	                         0 : GEOCLUE_POSITION_FIELDS_LATITUDE;
	gpsd->last_pos_fields |= (isnan (fix->longitude)) ? 
	                         0 : GEOCLUE_POSITION_FIELDS_LONGITUDE;
	gpsd->last_pos_fields |= (isnan (fix->altitude)) ? 
	                         0 : GEOCLUE_POSITION_FIELDS_ALTITUDE;
252
	
253 254 255 256 257 258 259 260 261
	gc_iface_position_emit_position_changed 
		(GC_IFACE_POSITION (gpsd), gpsd->last_pos_fields,
		 (int)(last_fix->time+0.5), 
		 last_fix->latitude, last_fix->longitude, last_fix->altitude, 
		 gpsd->last_accuracy);
	
}

static void
Jussi Kukkonen's avatar
Jussi Kukkonen committed
262
geoclue_gpsd_update_velocity (GeoclueGpsd *gpsd, NmeaTag nmea_tag)
263 264 265 266 267
{
	gps_fix *fix = &gpsd->gpsdata->fix;
	gps_fix *last_fix = gpsd->last_fix;
	gboolean changed = FALSE;
	
268
	/* at least with my devices, gpsd updates
269 270 271 272 273 274 275
	 *  - climb on GGA, GSA and GSV messages (speed and track are set to NaN).
	 *  - speed and track on RMC message (climb is set to NaN).
	 * 
	 * couldn't think of an smart way to handle this, I don't think there is one
	 */
	
	if (((gpsd->gpsdata->set & TRACK_SET) || (gpsd->gpsdata->set & SPEED_SET)) &&
Jussi Kukkonen's avatar
Jussi Kukkonen committed
276
	    nmea_tag == NMEA_RMC) {
277
		
278
		gpsd->gpsdata->set &= ~(TRACK_SET | SPEED_SET);
279
		
280
		last_fix->time = fix->time;
281
		
282 283 284 285 286 287 288
		if (!equal_or_nan (fix->track, last_fix->track) ||
		    !equal_or_nan (fix->speed, last_fix->speed)){
			
			/* velocity has changed */
			changed = TRUE;
			last_fix->track = fix->track;
			last_fix->speed = fix->speed;
289
		}
290
	} else if ((gpsd->gpsdata->set & CLIMB_SET) &&
Jussi Kukkonen's avatar
Jussi Kukkonen committed
291 292 293
	           (nmea_tag == NMEA_GGA || 
	            nmea_tag == NMEA_GSA || 
	            nmea_tag == NMEA_GSV)) {
294 295
		
		gpsd->gpsdata->set &= ~(CLIMB_SET);
296
		
297
		last_fix->time = fix->time;
298
		
299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314
		if (!equal_or_nan (fix->climb, last_fix->climb)){
			
			/* velocity has changed */
			changed = TRUE;
			last_fix->climb = fix->climb;
		}
	}
	
	if (changed) {
		gpsd->last_velo_fields = GEOCLUE_VELOCITY_FIELDS_NONE;
		gpsd->last_velo_fields |= (isnan (last_fix->track)) ?
			0 : GEOCLUE_VELOCITY_FIELDS_DIRECTION;
		gpsd->last_velo_fields |= (isnan (last_fix->speed)) ?
			0 : GEOCLUE_VELOCITY_FIELDS_SPEED;
		gpsd->last_velo_fields |= (isnan (last_fix->climb)) ?
			0 : GEOCLUE_VELOCITY_FIELDS_CLIMB;
315 316
		
		gc_iface_velocity_emit_velocity_changed 
317 318 319
			(GC_IFACE_VELOCITY (gpsd), gpsd->last_velo_fields,
			 (int)(last_fix->time+0.5),
			 last_fix->speed, last_fix->track, last_fix->climb);
320 321 322
	}
}

Jussi Kukkonen's avatar
Jussi Kukkonen committed
323 324 325
static void
geoclue_gpsd_update_status (GeoclueGpsd *gpsd, NmeaTag nmea_tag)
{
Jussi Kukkonen's avatar
Jussi Kukkonen committed
326
	GeoclueStatus status;
Jussi Kukkonen's avatar
Jussi Kukkonen committed
327
	
328
	/* gpsdata->online is supposedly always up-to-date */
Jussi Kukkonen's avatar
Jussi Kukkonen committed
329 330 331
	if (gpsd->gpsdata->online <= 0) {
		status = GEOCLUE_STATUS_UNAVAILABLE;
	} else if (gpsd->gpsdata->set & STATUS_SET) {
Jussi Kukkonen's avatar
Jussi Kukkonen committed
332 333
		gpsd->gpsdata->set &= ~(STATUS_SET);
		
Jussi Kukkonen's avatar
Jussi Kukkonen committed
334 335 336 337
		if (gpsd->gpsdata->status > 0) {
			status = GEOCLUE_STATUS_AVAILABLE;
		} else {
			status = GEOCLUE_STATUS_ACQUIRING;
Jussi Kukkonen's avatar
Jussi Kukkonen committed
338
		}
Jussi Kukkonen's avatar
Jussi Kukkonen committed
339 340
	} else {
		return;
Jussi Kukkonen's avatar
Jussi Kukkonen committed
341
	}
Jussi Kukkonen's avatar
Jussi Kukkonen committed
342 343
	
	geoclue_gpsd_set_status (gpsd, status);
Jussi Kukkonen's avatar
Jussi Kukkonen committed
344 345
}

346 347 348
static void 
gpsd_callback (struct gps_data_t *gpsdata, char *message, size_t len, int level)
{
Jussi Kukkonen's avatar
Jussi Kukkonen committed
349 350 351 352 353 354 355 356 357 358 359 360
	char *tag_str = gpsd->gpsdata->tag;
	NmeaTag nmea_tag = NMEA_NONE;
	
	if (tag_str[0] == 'G' && tag_str[1] == 'S' && tag_str[2] == 'A') {
		nmea_tag = NMEA_GSA;
	} else if (tag_str[0] == 'G' && tag_str[1] == 'G' && tag_str[2] == 'A') {
		nmea_tag = NMEA_GGA;
	} else if (tag_str[0] == 'G' && tag_str[1] == 'S' && tag_str[2] == 'V') {
		nmea_tag = NMEA_GSV;
	} else if (tag_str[0] == 'R' && tag_str[1] == 'M' && tag_str[2] == 'C') {
		nmea_tag = NMEA_RMC;
	}
361
	
Jussi Kukkonen's avatar
Jussi Kukkonen committed
362 363 364
	geoclue_gpsd_update_status (gpsd, nmea_tag);
	geoclue_gpsd_update_position (gpsd, nmea_tag);
	geoclue_gpsd_update_velocity (gpsd, nmea_tag);
365 366
}

367 368 369 370 371
static void
geoclue_gpsd_stop_gpsd (GeoclueGpsd *self)
{
	if (self->gpsdata) {
		gps_close (self->gpsdata);
372
		g_free (self->gps_thread);
373 374 375 376 377
		self->gpsdata = NULL;
	}
}

static gboolean
Jussi Kukkonen's avatar
Jussi Kukkonen committed
378
geoclue_gpsd_start_gpsd (GeoclueGpsd *self)
379
{
Jussi Kukkonen's avatar
Jussi Kukkonen committed
380
	self->gpsdata = gps_open (self->host, self->port);
381
	if (self->gpsdata) {
Jussi Kukkonen's avatar
Jussi Kukkonen committed
382
		/* This can block for some time if device is not available */
383 384 385
		gps_set_callback (self->gpsdata, gpsd_callback, self->gps_thread);
		return TRUE;
	} else {
Jussi Kukkonen's avatar
Jussi Kukkonen committed
386
		g_warning ("gps_open() failed, is gpsd running (host=%s,port=%s)?", self->host, self->port);
387 388 389
		return FALSE;
	}
}
390 391 392 393

static void
geoclue_gpsd_init (GeoclueGpsd *self)
{
394 395
	self->gpsdata = NULL;
	self->gps_thread = g_new0 (pthread_t, 1);
396
	self->last_fix = g_new0 (gps_fix, 1);
397
	
398 399 400
	self->last_pos_fields = GEOCLUE_POSITION_FIELDS_NONE;
	self->last_velo_fields = GEOCLUE_VELOCITY_FIELDS_NONE;
	self->last_accuracy = geoclue_accuracy_new (GEOCLUE_ACCURACY_LEVEL_NONE, 0, 0);
401
	
402
	gc_provider_set_details (GC_PROVIDER (self),
403 404 405
				 "org.freedesktop.Geoclue.Providers.Gpsd",
				 "/org/freedesktop/Geoclue/Providers/Gpsd",
				 "Gpsd", "Gpsd provider");
406
	
Jussi Kukkonen's avatar
Jussi Kukkonen committed
407 408 409 410 411 412
	self->port = g_strdup (DEFAULT_GPSD_PORT);
	self->host = NULL;
	geoclue_gpsd_set_status (self, GEOCLUE_STATUS_ACQUIRING);
	if (!geoclue_gpsd_start_gpsd (self)) {
		geoclue_gpsd_set_status (self, GEOCLUE_STATUS_ERROR);
	}
413 414 415 416 417 418 419 420 421 422 423 424
}

static gboolean
get_position (GcIfacePosition       *gc,
              GeocluePositionFields *fields,
              int                   *timestamp,
              double                *latitude,
              double                *longitude,
              double                *altitude,
              GeoclueAccuracy      **accuracy,
              GError               **error)
{
425 426 427 428 429 430 431
	GeoclueGpsd *gpsd = GEOCLUE_GPSD (gc);
	
	*timestamp = (int)(gpsd->last_fix->time+0.5);
	*latitude = gpsd->last_fix->latitude;
	*longitude = gpsd->last_fix->longitude;
	*altitude = gpsd->last_fix->altitude;
	*fields = gpsd->last_pos_fields;
432
	*accuracy = geoclue_accuracy_copy (gpsd->last_accuracy);
433
	
434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451
	return TRUE;
}

static void
geoclue_gpsd_position_init (GcIfacePositionClass *iface)
{
	iface->get_position = get_position;
}

static gboolean
get_velocity (GcIfaceVelocity       *gc,
              GeoclueVelocityFields *fields,
              int                   *timestamp,
              double                *speed,
              double                *direction,
              double                *climb,
              GError               **error)
{
452 453 454 455 456 457 458 459
	GeoclueGpsd *gpsd = GEOCLUE_GPSD (gc);
	
	*timestamp = (int)(gpsd->last_fix->time+0.5);
	*speed = gpsd->last_fix->speed;
	*direction = gpsd->last_fix->track;
	*climb = gpsd->last_fix->climb;
	*fields = gpsd->last_velo_fields;
	
460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479
	return TRUE;
}

static void
geoclue_gpsd_velocity_init (GcIfaceVelocityClass *iface)
{
	iface->get_velocity = get_velocity;
}

int
main (int    argc,
      char **argv)
{
	g_type_init ();
	
	gpsd = g_object_new (GEOCLUE_TYPE_GPSD, NULL);
	
	gpsd->loop = g_main_loop_new (NULL, TRUE);
	g_main_loop_run (gpsd->loop);
	
480
	g_main_loop_unref (gpsd->loop);
481
	g_object_unref (gpsd);
482
	
483 484
	return 0;
}